def calcP(sizes, x, u, pi, constants, boundary, restrictions):
print("\nIn calcP.")
N = sizes['N']
dt = 1.0 / (N - 1)
phi = calcPhi(sizes, x, u, pi, constants, restrictions)
psi = calcPsi(sizes, x, boundary)
dx = ddt(sizes, x)
func = dx - phi
vetP = numpy.empty(N)
vetIP = numpy.empty(N)
P = .5 * (func[0, :].dot(func[0, :].transpose())) #norm(func[0,:])**2
vetP[0] = P
vetIP[0] = P
for t in range(1, N - 1):
vetP[t] = func[t, :].dot(func[t, :].transpose()) #norm(func[t,:])**2
P += vetP[t]
vetIP[t] = P
vetP[N - 1] = .5 * (func[N - 1, :].dot(func[N - 1, :].transpose())
) #norm(func)**2
P += vetP[N - 1]
vetIP[N - 1] = P
P *= dt
vetP *= dt
vetIP *= dt
# tPlot = numpy.arange(0,1.0+dt,dt)
# plt.plot(tPlot,vetP)
# plt.grid(True)
# plt.title("Integrand of P")
# plt.show()
# plt.plot(tPlot,vetIP)
# plt.grid(True)
# plt.title("Partially integrated P")
# plt.show()
Pint = P
Ppsi = norm(psi)**2
P += Ppsi
print("P = {:.4E}".format(P) + ": P_int = {:.4E}".format(Pint) +
", P_psi = {:.4E}".format(Ppsi))
return P, Pint, Ppsi
def oderest(sizes,x,u,pi,t,constants,boundary,restrictions):
#print("\nIn oderest.")
# get sizes
N = sizes['N']
n = sizes['n']
m = sizes['m']
p = sizes['p']
q = sizes['q']
#print("Calc phi...")
# calculate phi and psi
phi = calcPhi(sizes,x,u,pi,constants,restrictions)
# err: phi - dx/dt
err = phi - ddt(sizes,x)
# get gradients
#print("Calc grads...")
Grads = calcGrads(sizes,x,u,pi,constants,restrictions)
dt = Grads['dt']
phix = Grads['phix']
lam = 0*x; mu = numpy.zeros(q)
B = numpy.zeros((N,m))
C = numpy.zeros(p)
#print("Integrating ODE for A...")
# integrate equation for A:
A = numpy.zeros((N,n))
for k in range(N-1):
derk = phix[k,:].dot(A[k,:]) + err[k,:]
aux = A[k,:] + dt*derk
A[k+1,:] = A[k,:] + .5*dt*( derk + phix[k+1,:].dot(aux) + err[k+1,:])
# optPlot = dict()
# optPlot['mode'] = 'var'
# plotSol(sizes,t,A,B,C,constants,restrictions,optPlot)
#print("Calculating step...")
alfa = calcStepOdeRest(sizes,t,x,u,pi,A,B,C,constants,boundary,restrictions)
nx = x + alfa * A
print("Leaving oderest with alfa =",alfa)
return nx,u,pi,lam,mu
def calcP(sizes, x, u, pi, constants, boundary, restrictions):
N = sizes['N']
phi = calcPhi(sizes, x, u, pi, constants, restrictions)
psi = calcPsi(sizes, x, boundary)
dx = ddt(sizes, x)
#dx = x.copy()
P = 0.0
for t in range(1, N - 1):
# dx[t,:] = x[t,:]-x[t-1,:]
P += norm(dx[t, :] - phi[t, :])**2
P += .5 * norm(dx[0, :] - phi[0, :])**2
P += .5 * norm(dx[N - 1, :] - phi[N - 1, :])**2
P *= 1.0 / (N - 1)
P += norm(psi)
return P
def oderest(sizes, x, u, pi, t, constants, boundary, restrictions):
print("\nIn oderest.")
# get sizes
N = sizes['N']
n = sizes['n']
m = sizes['m']
p = sizes['p']
print("Calc phi...")
# calculate phi and psi
phi = calcPhi(sizes, x, u, pi, constants, restrictions)
# aux: phi - dx/dt
aux = phi - ddt(sizes, x)
# get gradients
print("Calc grads...")
Grads = calcGrads(sizes, x, u, pi, constants, restrictions)
phix = Grads['phix']
lam = 0 * x
B = numpy.zeros((N, m))
C = numpy.zeros(p)
print("Integrating ODE for A...")
# integrate equation for A:
A = odeint(calcADotOdeRest, numpy.zeros(n), t, args=(t, phix, aux))
#optPlot = dict()
#optPlot['mode'] = 'var'
#plotSol(sizes,t,A,B,C,constants,restrictions,optPlot)
print("Calculating step...")
alfa = calcStepOdeRest(sizes, t, x, u, pi, A, B, C, constants, boundary,
restrictions)
nx = x + alfa * A
print("Leaving oderest with alfa =", alfa)
return nx, u, pi, lam, mu
def rest(sizes,x,u,pi,t,constants,boundary,restrictions,mustPlot=False):
#print("\nIn rest.")
# get sizes
N = sizes['N']
n = sizes['n']
m = sizes['m']
p = sizes['p']
q = sizes['q']
#print("Calc phi...")
# calculate phi and psi
phi = calcPhi(sizes,x,u,pi,constants,restrictions)
#print("Calc psi...")
psi = calcPsi(sizes,x,boundary)
# aux: phi - dx/dt
err = phi - ddt(sizes,x)
# get gradients
#print("Calc grads...")
Grads = calcGrads(sizes,x,u,pi,constants,restrictions)
dt = Grads['dt']
#dt6 = dt/6
phix = Grads['phix']
phiu = Grads['phiu']
phip = Grads['phip']
psix = Grads['psix']
psip = Grads['psip']
#print("Preparing matrices...")
psixTr = psix.transpose()
phixInv = phix.copy()
phiuTr = numpy.empty((N,m,n))
phipTr = numpy.empty((N,p,n))
psipTr = psip.transpose()
for k in range(N):
phixInv[k,:,:] = phix[N-k-1,:,:].transpose()
phiuTr[k,:,:] = phiu[k,:,:].transpose()
phipTr[k,:,:] = phip[k,:,:].transpose()
mu = numpy.zeros(q)
# Matrix for linear system involving k's
M = numpy.ones((q+1,q+1))
# column vector for linear system involving k's [eqs (88-89)]
col = numpy.zeros(q+1)
col[0] = 1.0 # eq (88)
col[1:] = -psi # eq (89)
arrayA = numpy.empty((q+1,N,n))
arrayB = numpy.empty((q+1,N,m))
arrayC = numpy.empty((q+1,p))
arrayL = arrayA.copy()
arrayM = numpy.empty((q+1,q))
optPlot = dict()
#print("Beginning loop for solutions...")
for i in range(q+1):
# print("\nIntegrating solution "+str(i+1)+" of "+str(q+1)+"...\n")
mu = 0.0*mu
if i<q:
mu[i] = 1.0e-10
# integrate equation (75-2) backwards
auxLamInit = - psixTr.dot(mu)
auxLam = numpy.empty((N,n))
auxLam[0,:] = auxLamInit
# Euler implicit
I = numpy.eye(n)
for k in range(N-1):
auxLam[k+1,:] = numpy.linalg.solve(I-dt*phixInv[k+1,:],auxLam[k,:])
# Euler's method
# for k in range(N-1):
# auxLam[k+1,:] = auxLam[k,:] + dt * phixInv[k,:,:].dot(auxLam[k,:])
# Heun's method
#for k in range(N-1):
# derk = phixInv[k,:,:].dot(auxLam[k,:])
# aux = auxLam[k,:] + dt*derk
# auxLam[k+1,:] = auxLam[k,:] + \
# .5*dt*(derk + phixInv[k+1,:,:].dot(aux))
# RK4 method (with interpolation...)
# for k in range(N-1):
# phixInv_k = phixInv[k,:,:]
# phixInv_kp1 = phixInv[k+1,:,:]
# phixInv_kpm = .5*(phixInv_k+phixInv_kp1)
# k1 = phixInv_k.dot(auxLam[k,:])
# k2 = phixInv_kpm.dot(auxLam[k,:]+.5*dt*k1)
# k3 = phixInv_kpm.dot(auxLam[k,:]+.5*dt*k2)
# k4 = phixInv_kp1.dot(auxLam[k,:]+dt*k3)
# auxLam[k+1,:] = auxLam[k,:] + dt6 * (k1+k2+k2+k3+k3+k4)
# equation for Bi (75-3)
B = numpy.empty((N,m))
lam = 0*x
for k in range(N):
lam[k,:] = auxLam[N-k-1,:]
B[k,:] = phiuTr[k,:,:].dot(lam[k,:])
##################################################################
# TESTING LAMBDA DIFFERENTIAL EQUATION
dlam = ddt(sizes,lam)
erroLam = numpy.empty((N,n))
normErroLam = numpy.empty(N)
for k in range(N):
erroLam[k,:] = dlam[k,:]+phix[k,:,:].transpose().dot(lam[k,:])
normErroLam[k] = erroLam[k,:].transpose().dot(erroLam[k,:])
maxNormErroLam = normErroLam.max()
print("maxNormErroLam =",maxNormErroLam)
#print("\nLambda Error:")
#
#optPlot['mode'] = 'states:LambdaError'
#plotSol(sizes,t,erroLam,numpy.zeros((N,m)),numpy.zeros(p),\
# constants,restrictions,optPlot)
#optPlot['mode'] = 'states:LambdaError (zoom)'
#N1 = 0#int(N/100)-10
#N2 = 20##N1+20
#plotSol(sizes,t[N1:N2],erroLam[N1:N2,:],numpy.zeros((N2-N1,m)),\
# numpy.zeros(p),constants,restrictions,optPlot)
#plt.semilogy(normErroLam)
#plt.grid()
#plt.title("ErroLam")
#plt.show()
#plt.semilogy(normErroLam[N1:N2])
#plt.grid()
#plt.title("ErroLam (zoom)")
#plt.show()
##################################################################
scal = 1.0/((numpy.absolute(B)).max())
lam *= scal
mu *= scal
B *= scal
# equation for Ci (75-4)
C = numpy.zeros(p)
for k in range(1,N-1):
C += phipTr[k,:,:].dot(lam[k,:])
C += .5*(phipTr[0,:,:].dot(lam[0,:]))
C += .5*(phipTr[N-1,:,:].dot(lam[N-1,:]))
C *= dt
C -= -psipTr.dot(mu)
# optPlot['mode'] = 'states:Lambda'
# plotSol(sizes,t,lam,B,C,constants,restrictions,optPlot)
#
# optPlot['mode'] = 'states:Lambda (zoom)'
# plotSol(sizes,t[N1:N2],lam[N1:N2,:],B[N1:N2,:],C,constants,restrictions,optPlot)
#print("Integrating ODE for A ["+str(i)+"/"+str(q)+"] ...")
# integrate equation for A:
A = numpy.zeros((N,n))
for k in range(N-1):
derk = phix[k,:,:].dot(A[k,:]) + phiu[k,:,:].dot(B[k,:]) + \
phip[k,:,:].dot(C) + err[k,:]
aux = A[k,:] + dt*derk
A[k+1,:] = A[k,:] + .5*dt*(derk + \
phix[k+1,:,:].dot(aux) + \
phiu[k+1,:,:].dot(B[k+1,:]) + \
phip[k+1,:,:].dot(C) + \
err[k+1,:])
# for k in range(N-1):
# phix_k = phix[k,:,:]
# phix_kp1 = phix[k+1,:,:]
# phix_kpm = .5*(phix_k+phix_kp1)
# add_k = phiu[k,:,:].dot(B[k,:]) + phip[k,:,:].dot(C) + err[k,:]
# add_kp1 = phiu[k+1,:,:].dot(B[k+1,:]) + phip[k+1,:,:].dot(C) + err[k+1,:]
# add_kpm = .5*(add_k+add_kp1)
#
# k1 = phix_k.dot(A[k,:]) + add_k
# k2 = phix_kpm.dot(A[k,:]+.5*dt*k1) + add_kpm
# k3 = phix_kpm.dot(A[k,:]+.5*dt*k2) + add_kpm
# k4 = phix_kp1.dot(A[k,:]+dt*k3) + add_kp1
# A[k+1,:] = A[k,:] + dt6 * (k1+k2+k2+k3+k3+k4)
else:
# integrate equation (75-2) backwards
lam *= 0.0
# equation for Bi (75-3)
B *= 0.0
# equation for Ci (75-4)
C *= 0.0
#print("Integrating ODE for A ["+str(i)+"/"+str(q)+"] ...")
# integrate equation for A:
A = numpy.zeros((N,n))
for k in range(N-1):
derk = phix[k,:,:].dot(A[k,:]) + err[k,:]
aux = A[k,:] + dt*derk
A[k+1,:] = A[k,:] + .5*dt*(derk + \
phix[k+1,:,:].dot(aux) + err[k+1,:])
# for k in range(N-1):
# phix_k = phix[k,:,:]
# phix_kp1 = phix[k+1,:,:]
# phix_kpm = .5*(phix_k+phix_kp1)
# add_k = err[k,:]
# add_kp1 = err[k+1,:]
# add_kpm = .5*(add_k+add_kp1)
#
# k1 = phix_k.dot(A[k,:]) + add_k
# k2 = phix_kpm.dot(A[k,:]+.5*dt*k1) + add_kpm
# k3 = phix_kpm.dot(A[k,:]+.5*dt*k2) + add_kpm
# k4 = phix_kp1.dot(A[k,:]+dt*k3) + add_kp1
# A[k+1,:] = A[i,:] + dt6 * (k1+k2+k2+k3+k3+k4)
#
# store solution in arrays
arrayA[i,:,:] = A
arrayB[i,:,:] = B
arrayC[i,:] = C
arrayL[i,:,:] = lam
arrayM[i,:] = mu
#optPlot['mode'] = 'var'
#plotSol(sizes,t,A,B,C,constants,restrictions,optPlot)
# Matrix for linear system (89)
M[1:,i] = psix.dot(A[N-1,:]) + psip.dot(C)
#
# Calculations of weights k:
print("M =",M)
#print("col =",col)
K = numpy.linalg.solve(M,col)
print("K =",K)
# summing up linear combinations
A = 0.0*A
B = 0.0*B
C = 0.0*C
lam = 0.0*lam
mu = 0.0*mu
for i in range(q+1):
A += K[i]*arrayA[i,:,:]
B += K[i]*arrayB[i,:,:]
C += K[i]*arrayC[i,:]
lam += K[i]*arrayL[i,:,:]
mu += K[i]*arrayM[i,:]
if mustPlot:
optPlot['mode'] = 'var'
plotSol(sizes,t,A,B,C,constants,restrictions,optPlot)
optPlot['mode'] = 'proposed (states: lambda)'
plotSol(sizes,t,lam,B,C,constants,restrictions,optPlot)
#print("Calculating step...")
alfa = calcStepRest(sizes,t,x,u,pi,A,B,C,constants,boundary,restrictions,mustPlot)
nx = x + alfa * A
nu = u + alfa * B
np = pi + alfa * C
print("Leaving rest with alfa =",alfa)
return nx,nu,np,lam,mu
def calcP(sizes,x,u,pi,constants,boundary,restrictions,mustPlot=False):
#print("\nIn calcP.")
N = sizes['N']
dt = 1.0/(N-1)
phi = calcPhi(sizes,x,u,pi,constants,restrictions)
psi = calcPsi(sizes,x,boundary)
dx = ddt(sizes,x)
func = dx-phi
vetP = numpy.empty(N)
vetIP = numpy.empty(N)
P = .5*(func[0,:].dot(func[0,:].transpose()))#norm(func[0,:])**2
vetP[0] = P
vetIP[0] = P
for t in range(1,N-1):
vetP[t] = func[t,:].dot(func[t,:].transpose())#norm(func[t,:])**2
P += vetP[t]
vetIP[t] = P
# P += func[t,:].dot(func[t,:].transpose())
vetP[N-1] = .5*(func[N-1,:].dot(func[N-1,:].transpose()))#norm(func[N-1,:])**2
P += vetP[N-1]
vetIP[N-1] = P
# P += .5*(func[N-1,:].dot(func[N-1,:].transpose()))
P *= dt
#vetP *= dt
vetIP *= dt
if mustPlot:
tPlot = numpy.arange(0,1.0+dt,dt)
plt.plot(tPlot,vetP)
plt.grid(True)
plt.title("Integrand of P")
plt.show()
# for zoomed version:
indMaxP = vetP.argmax()
ind1 = numpy.array([indMaxP-20,0]).max()
ind2 = numpy.array([indMaxP+20,N]).min()
plt.plot(tPlot[ind1:ind2],vetP[ind1:ind2],'o')
plt.grid(True)
plt.title("Integrand of P (zoom)")
plt.show()
n = sizes['n']; m = sizes['m']
if n==4 and m==2:
print("\nStates and controls on the region of maxP:")
plt.plot(tPlot[ind1:ind2],x[ind1:ind2,0])
plt.grid(True)
plt.ylabel("h [km]")
plt.show()
plt.plot(tPlot[ind1:ind2],x[ind1:ind2,1],'g')
plt.grid(True)
plt.ylabel("V [km/s]")
plt.show()
plt.plot(tPlot[ind1:ind2],x[ind1:ind2,2]*180/numpy.pi,'r')
plt.grid(True)
plt.ylabel("gamma [deg]")
plt.show()
plt.plot(tPlot[ind1:ind2],x[ind1:ind2,3],'m')
plt.grid(True)
plt.ylabel("m [kg]")
plt.show()
plt.plot(tPlot[ind1:ind2],u[ind1:ind2,0],'k')
plt.grid(True)
plt.ylabel("u1 [-]")
plt.show()
plt.plot(tPlot[ind1:ind2],u[ind1:ind2,1],'c')
plt.grid(True)
plt.xlabel("t")
plt.ylabel("u2 [-]")
plt.show()
plt.plot(tPlot[ind1:ind2],numpy.tanh(u[ind1:ind2,0])*2.0,'k')
plt.grid(True)
plt.ylabel("alfa [deg]")
plt.show()
plt.plot(tPlot[ind1:ind2],numpy.tanh(u[ind1:ind2,1])*.5+.5,'c')
plt.grid(True)
plt.xlabel("t")
plt.ylabel("beta [-]")
plt.show()
print("\nResidual on the region of maxP:")
plt.plot(tPlot[ind1:ind2],func[ind1:ind2,0])
plt.grid(True)
plt.ylabel("res_hDot [km/s]")
plt.show()
plt.plot(tPlot[ind1:ind2],func[ind1:ind2,1],'g')
plt.grid(True)
plt.ylabel("res_vDot [km/s/s]")
plt.show()
plt.plot(tPlot[ind1:ind2],func[ind1:ind2,2]*180/numpy.pi,'r')
plt.grid(True)
plt.ylabel("res_gammaDot [deg/s]")
plt.show()
plt.plot(tPlot[ind1:ind2],func[ind1:ind2,3],'m')
plt.grid(True)
plt.ylabel("res_mDot [kg/s]")
plt.show()
print("\nState time derivatives on the region of maxP:")
plt.plot(tPlot[ind1:ind2],dx[ind1:ind2,0])
plt.grid(True)
plt.ylabel("hDot [km/s]")
plt.show()
plt.plot(tPlot[ind1:ind2],dx[ind1:ind2,1],'g')
plt.grid(True)
plt.ylabel("vDot [km/s/s]")
plt.show()
plt.plot(tPlot[ind1:ind2],dx[ind1:ind2,2]*180/numpy.pi,'r')
plt.grid(True)
plt.ylabel("gammaDot [deg/s]")
plt.show()
plt.plot(tPlot[ind1:ind2],dx[ind1:ind2,3],'m')
plt.grid(True)
plt.ylabel("mDot [kg/s]")
plt.show()
print("\nPHI on the region of maxP:")
plt.plot(tPlot[ind1:ind2],phi[ind1:ind2,0])
plt.grid(True)
plt.ylabel("hDot [km/s]")
plt.show()
plt.plot(tPlot[ind1:ind2],phi[ind1:ind2,1],'g')
plt.grid(True)
plt.ylabel("vDot [km/s/s]")
plt.show()
plt.plot(tPlot[ind1:ind2],phi[ind1:ind2,2]*180/numpy.pi,'r')
plt.grid(True)
plt.ylabel("gammaDot [deg/s]")
plt.show()
plt.plot(tPlot[ind1:ind2],phi[ind1:ind2,3],'m')
plt.grid(True)
plt.ylabel("mDot [kg/s]")
plt.show()
plt.plot(tPlot,vetIP)
plt.grid(True)
plt.title("Partially integrated P")
plt.show()
Pint = P
Ppsi = psi.transpose().dot(psi)#norm(psi)**2
P += Ppsi
#print("P = {:.4E}".format(P)+": P_int = {:.4E}".format(Pint)+", P_psi = {:.4E}".format(Ppsi))
return P,Pint,Ppsi
def rest(sizes, x, u, pi, t, constants, boundary, restrictions):
#print("\nIn rest.")
# get sizes
N = sizes['N']
n = sizes['n']
m = sizes['m']
p = sizes['p']
q = sizes['q']
#print("Calc phi...")
# calculate phi and psi
phi = calcPhi(sizes, x, u, pi, constants, restrictions)
#print("Calc psi...")
psi = calcPsi(sizes, x, boundary)
# aux: phi - dx/dt
err = phi - ddt(sizes, x)
# get gradients
#print("Calc grads...")
Grads = calcGrads(sizes, x, u, pi, constants, restrictions)
dt = Grads['dt']
# dt6 = dt/6
phix = Grads['phix']
phiu = Grads['phiu']
phip = Grads['phip']
psix = Grads['psix']
psip = Grads['psip']
#print("Preparing matrices...")
psixTr = psix.transpose()
phixInv = phix.copy()
phiuTr = numpy.empty((N, m, n))
phipTr = numpy.empty((N, p, n))
psipTr = psip.transpose()
for k in range(N):
phixInv[k, :, :] = phix[N - k - 1, :, :].transpose()
phiuTr[k, :, :] = phiu[k, :, :].transpose()
phipTr[k, :, :] = phip[k, :, :].transpose()
mu = numpy.zeros(q)
# Matrix for linear system involving k's
M = numpy.ones((q + 1, q + 1))
# column vector for linear system involving k's [eqs (88-89)]
col = numpy.zeros(q + 1)
col[0] = 1.0 # eq (88)
col[1:] = -psi # eq (89)
arrayA = numpy.empty((q + 1, N, n))
arrayB = numpy.empty((q + 1, N, m))
arrayC = numpy.empty((q + 1, p))
arrayL = arrayA.copy()
arrayM = numpy.empty((q + 1, q))
optPlot = dict()
#print("Beginning loop for solutions...")
for i in range(q + 1):
print("\nIntegrating solution " + str(i + 1) + " of " + str(q + 1) +
"...\n")
mu = 0.0 * mu
if i < q:
mu[i] = 1.0
# integrate equation (75-2) backwards
auxLamInit = -psixTr.dot(mu)
auxLam = numpy.empty((N, n))
auxLam[0, :] = auxLamInit
for k in range(N - 1):
derk = phixInv[k, :, :].dot(auxLam[k, :])
aux = auxLam[k, :] + dt * derk
auxLam[k+1,:] = auxLam[k,:] + \
.5*dt*(derk + phixInv[k+1,:,:].dot(aux))
# for k in range(N-1):
# phixInv_k = phixInv[k,:,:]
# phixInv_kp1 = phixInv[k+1,:,:]
# phixInv_kpm = .5*(phixInv_k+phixInv_kp1)
# k1 = phixInv_k.dot(auxLam[k,:])
# k2 = phixInv_kpm.dot(auxLam[k,:]+.5*dt*k1)
# k3 = phixInv_kpm.dot(auxLam[k,:]+.5*dt*k2)
# k4 = phixInv_kp1.dot(auxLam[k,:]+dt*k3)
# auxLam[k+1,:] = auxLam[k,:] + dt6 * (k1+k2+k2+k3+k3+k4)
# equation for Bi (75-3)
B = numpy.empty((N, m))
lam = 0 * x
for k in range(N):
lam[k, :] = auxLam[N - k - 1, :]
B[k, :] = phiuTr[k, :, :].dot(lam[k, :])
scal = 1.0 / ((numpy.absolute(B)).max())
lam *= scal
mu *= scal
B *= scal
# equation for Ci (75-4)
C = numpy.zeros(p)
for k in range(1, N - 1):
C += phipTr[k, :, :].dot(lam[k, :])
C += .5 * (phipTr[0, :, :].dot(lam[0, :]))
C += .5 * (phipTr[N - 1, :, :].dot(lam[N - 1, :]))
C *= dt
C -= -psipTr.dot(mu)
optPlot['mode'] = 'states:Lambda'
plotSol(sizes, t, lam, B, C, constants, restrictions, optPlot)
optPlot['mode'] = 'states:Lambda (zoom)'
plotSol(sizes, t[0:10], lam[0:10, :], B[0:10, :], C, constants,
restrictions, optPlot)
#print("Integrating ODE for A ["+str(i)+"/"+str(q)+"] ...")
# integrate equation for A:
A = numpy.zeros((N, n))
for k in range(N - 1):
derk = phix[k,:,:].dot(A[k,:]) + phiu[k,:,:].dot(B[k,:]) + \
phip[k,:,:].dot(C) + err[k,:]
aux = A[k, :] + dt * derk
A[k+1,:] = A[k,:] + .5*dt*(derk + \
phix[k+1,:,:].dot(aux) + \
phiu[k+1,:,:].dot(B[k+1,:]) + \
phip[k+1,:,:].dot(C) + \
err[k+1,:])
# for k in range(N-1):
# phix_k = phix[k,:,:]
# phix_kp1 = phix[k+1,:,:]
# phix_kpm = .5*(phix_k+phix_kp1)
# add_k = phiu[k,:,:].dot(B[k,:]) + phip[k,:,:].dot(C) + err[k,:]
# add_kp1 = phiu[k+1,:,:].dot(B[k+1,:]) + phip[k+1,:,:].dot(C) + err[k+1,:]
# add_kpm = .5*(add_k+add_kp1)
#
# k1 = phix_k.dot(A[k,:]) + add_k
# k2 = phix_kpm.dot(A[k,:]+.5*dt*k1) + add_kpm
# k3 = phix_kpm.dot(A[k,:]+.5*dt*k2) + add_kpm
# k4 = phix_kp1.dot(A[k,:]+dt*k3) + add_kp1
# A[k+1,:] = A[k,:] + dt6 * (k1+k2+k2+k3+k3+k4)
else:
# integrate equation (75-2) backwards
lam *= 0.0
# equation for Bi (75-3)
B *= 0.0
# equation for Ci (75-4)
C *= 0.0
#print("Integrating ODE for A ["+str(i)+"/"+str(q)+"] ...")
# integrate equation for A:
A = numpy.zeros((N, n))
for k in range(N - 1):
derk = phix[k, :, :].dot(A[k, :]) + err[k, :]
aux = A[k, :] + dt * derk
A[k+1,:] = A[k,:] + .5*dt*(derk + \
phix[k+1,:,:].dot(aux) + err[k+1,:])
# for k in range(N-1):
# phix_k = phix[k,:,:]
# phix_kp1 = phix[k+1,:,:]
# phix_kpm = .5*(phix_k+phix_kp1)
# add_k = err[k,:]
# add_kp1 = err[k+1,:]
# add_kpm = .5*(add_k+add_kp1)
#
# k1 = phix_k.dot(A[k,:]) + add_k
# k2 = phix_kpm.dot(A[k,:]+.5*dt*k1) + add_kpm
# k3 = phix_kpm.dot(A[k,:]+.5*dt*k2) + add_kpm
# k4 = phix_kp1.dot(A[k,:]+dt*k3) + add_kp1
# A[k+1,:] = A[i,:] + dt6 * (k1+k2+k2+k3+k3+k4)
#
# store solution in arrays
arrayA[i, :, :] = A
arrayB[i, :, :] = B
arrayC[i, :] = C
arrayL[i, :, :] = lam
arrayM[i, :] = mu
optPlot['mode'] = 'var'
plotSol(sizes, t, A, B, C, constants, restrictions, optPlot)
# Matrix for linear system (89)
M[1:, i] = psix.dot(A[N - 1, :]) + psip.dot(C)
#
# Calculations of weights k:
print("M =", M)
print("col =", col)
K = numpy.linalg.solve(M, col)
print("K =", K)
# summing up linear combinations
A = 0.0 * A
B = 0.0 * B
C = 0.0 * C
lam = 0.0 * lam
mu = 0.0 * mu
for i in range(q + 1):
A += K[i] * arrayA[i, :, :]
B += K[i] * arrayB[i, :, :]
C += K[i] * arrayC[i, :]
lam += K[i] * arrayL[i, :, :]
mu += K[i] * arrayM[i, :]
optPlot['mode'] = 'var'
plotSol(sizes, t, A, B, C, constants, restrictions, optPlot)
optPlot['mode'] = 'states: lambda'
plotSol(sizes, t, lam, B, C, constants, restrictions, optPlot)
#print("Calculating step...")
alfa = calcStepRest(sizes, t, x, u, pi, A, B, C, constants, boundary,
restrictions)
nx = x + alfa * A
nu = u + alfa * B
np = pi + alfa * C
print("Leaving rest with alfa =", alfa)
return nx, nu, np, lam, mu
def calcP(sizes, x, u, pi, constants, boundary, restrictions, mustPlot=False):
#print("\nIn calcP.")
N = sizes['N']
dt = 1.0 / (N - 1)
phi = calcPhi(sizes, x, u, pi, constants, restrictions)
psi = calcPsi(sizes, x, boundary)
dx = ddt(sizes, x)
func = dx - phi
vetP = numpy.empty(N)
vetIP = numpy.empty(N)
P = .5 * (func[0, :].dot(func[0, :].transpose())) #norm(func[0,:])**2
vetP[0] = P
vetIP[0] = P
for t in range(1, N - 1):
vetP[t] = func[t, :].dot(func[t, :].transpose()) #norm(func[t,:])**2
P += vetP[t]
vetIP[t] = P
# P += func[t,:].dot(func[t,:].transpose())
vetP[N - 1] = .5 * (func[N - 1, :].dot(func[N - 1, :].transpose())
) #norm(func[N-1,:])**2
P += vetP[N - 1]
vetIP[N - 1] = P
# P += .5*(func[N-1,:].dot(func[N-1,:].transpose()))
P *= dt
vetP *= dt
vetIP *= dt
if mustPlot:
tPlot = numpy.arange(0, 1.0 + dt, dt)
plt.plot(tPlot, vetP)
plt.grid(True)
plt.title("Integrand of P")
plt.show()
# for zoomed version:
indMaxP = vetP.argmax()
ind1 = numpy.array([indMaxP - 10, 0]).max()
ind2 = numpy.array([indMaxP + 10, N - 1]).min()
plt.plot(tPlot[ind1:ind2], vetP[ind1:ind2], 'o')
plt.grid(True)
plt.title("Integrand of P (zoom)")
plt.show()
print("\nStates and controls on the region of maxP:")
# plt.subplot2grid((8,4),(0,0),colspan=5)
plt.plot(tPlot[ind1:ind2], x[ind1:ind2, 0])
plt.grid(True)
plt.ylabel("h [km]")
plt.show()
# plt.subplot2grid((8,4),(1,0),colspan=5)
plt.plot(tPlot[ind1:ind2], x[ind1:ind2, 1], 'g')
plt.grid(True)
plt.ylabel("V [km/s]")
plt.show()
# plt.subplot2grid((8,4),(2,0),colspan=5)
plt.plot(tPlot[ind1:ind2], x[ind1:ind2, 2] * 180 / numpy.pi, 'r')
plt.grid(True)
plt.ylabel("gamma [deg]")
plt.show()
# plt.subplot2grid((8,4),(3,0),colspan=5)
plt.plot(tPlot[ind1:ind2], x[ind1:ind2, 3], 'm')
plt.grid(True)
plt.ylabel("m [kg]")
plt.show()
# plt.subplot2grid((8,4),(4,0),colspan=5)
plt.plot(tPlot[ind1:ind2], u[ind1:ind2, 0], 'k')
plt.grid(True)
plt.ylabel("u1 [-]")
plt.show()
# plt.subplot2grid((8,4),(5,0),colspan=5)
plt.plot(tPlot[ind1:ind2], u[ind1:ind2, 1], 'c')
plt.grid(True)
plt.xlabel("t")
plt.ylabel("u2 [-]")
# plt.subplots_adjust(0.0125,0.0,0.9,2.5,0.2,0.2)
plt.show()
plt.plot(tPlot, vetIP)
plt.grid(True)
plt.title("Partially integrated P")
plt.show()
Pint = P
Ppsi = norm(psi)**2
P += Ppsi
#print("P = {:.4E}".format(P)+": P_int = {:.4E}".format(Pint)+", P_psi = {:.4E}".format(Ppsi))
return P, Pint, Ppsi
print(
'--------------------------------------------------------------------------------'
)
print('\nThis is SGRA_SIMPLE_ROCKET_ALT.py!')
print(datetime.datetime.now())
print('\n')
opt = dict()
opt['initMode'] = 'extSol' #'default'#'extSol'
# declare problem:
sizes, t, x, u, pi, lam, mu, tol, constants, boundary, restrictions = declProb(
opt)
Grads = calcGrads(sizes, x, u, pi, constants, restrictions)
phi = calcPhi(sizes, x, u, pi, constants, restrictions)
psi = calcPsi(sizes, x, boundary)
# phix = Grads['phix']
# phiu = Grads['phiu']
# psix = Grads['psix']
# psip = Grads['psip']
print("\nProposed initial guess:\n")
P, Pint, Ppsi = calcP(sizes, x, u, pi, constants, boundary, restrictions,
True)
print("P = {:.4E}".format(P)+", Pint = {:.4E}".format(Pint)+\
", Ppsi = {:.4E}".format(Ppsi)+"\n")
Q = calcQ(sizes, x, u, pi, lam, mu, constants, restrictions)
optPlot = dict()
optPlot['P'] = P
def rest(sizes, x, u, pi, t, constants, boundary, restrictions):
print("In rest.")
P0 = calcP(sizes, x, u, pi, constants, boundary, restrictions)
print("P0 =", P0)
# get sizes
N = sizes['N']
n = sizes['n']
m = sizes['m']
p = sizes['p']
q = sizes['q']
print("Calc phi...")
# calculate phi and psi
phi = calcPhi(sizes, x, u, pi, constants, restrictions)
print("Calc psi...")
psi = calcPsi(sizes, x, boundary)
# aux: phi - dx/dt
aux = phi.copy()
aux -= ddt(sizes, x)
# get gradients
print("Calc grads...")
Grads = calcGrads(sizes, x, u, pi, constants, restrictions)
dt = Grads['dt']
phix = Grads['phix']
phiu = Grads['phiu']
phip = Grads['phip']
fx = Grads['fx']
psix = Grads['psix']
psip = Grads['psip']
print("Preparing matrices...")
psixTr = psix.transpose()
fxInv = fx.copy()
phixInv = phix.copy()
phiuTr = numpy.empty((N, m, n))
phipTr = numpy.empty((N, p, n))
psipTr = psip.transpose()
for k in range(N):
fxInv[k, :] = fx[N - k - 1, :]
phixInv[k, :, :] = phix[N - k - 1, :, :].transpose()
phiuTr[k, :, :] = phiu[k, :, :].transpose()
phipTr[k, :, :] = phip[k, :, :].transpose()
mu = numpy.zeros(q)
# Matrix for linear system involving k's
M = numpy.ones((q + 1, q + 1))
# column vector for linear system involving k's [eqs (88-89)]
col = numpy.zeros(q + 1)
col[0] = 1.0 # eq (88)
col[1:] = -psi # eq (89)
arrayA = numpy.empty((q + 1, N, n))
arrayB = numpy.empty((q + 1, N, m))
arrayC = numpy.empty((q + 1, p))
arrayL = arrayA.copy()
arrayM = numpy.empty((q + 1, q))
print("Beginning loop for solutions...")
for i in range(q + 1):
mu = 0.0 * mu
if i < q:
mu[i] = 1.0
# integrate equation (75-2) backwards
auxLamInit = -psixTr.dot(mu)
auxLam = odeint(calcLamDotRest, auxLamInit, t, args=(t, phixInv))
# equation for Bi (75-3)
B = numpy.empty((N, m))
lam = auxLam.copy()
for k in range(N):
lam[k, :] = auxLam[N - k - 1, :]
B[k, :] = phiuTr[k, :, :].dot(lam[k, :])
# plt.plot(t,lam)
# plt.grid(True)
# plt.xlabel("t")
# plt.ylabel("lambda")
# plt.show()
# equation for Ci (75-4)
C = numpy.zeros(p)
for k in range(1, N - 1):
C += phipTr[k, :, :].dot(lam[k, :])
C += .5 * (phipTr[0, :, :].dot(lam[0, :]))
C += .5 * (phipTr[N - 1, :, :].dot(lam[N - 1, :]))
C *= dt
C -= -psipTr.dot(mu)
print("Integrating ODE for A [i = " + str(i) + "] ...")
# integrate equation for A:
A = odeint(calcADotRest,
numpy.zeros(n),
t,
args=(t, phix, phiu, phip, B, C, aux))
# store solution in arrays
arrayA[i, :, :] = A
arrayB[i, :, :] = B
arrayC[i, :] = C
arrayL[i, :, :] = lam
arrayM[i, :] = mu
# Matrix for linear system (89)
M[1:, i] = psix.dot(A[N - 1, :])
M[1:, i] += psip.dot(C) #psip * C
#
# Calculations of weights k:
K = numpy.linalg.solve(M, col)
print("K =", K)
# summing up linear combinations
A = 0.0 * A
B = 0.0 * B
C = 0.0 * C
lam = 0.0 * lam
mu = 0.0 * mu
for i in range(q + 1):
A += K[i] * arrayA[i, :, :]
B += K[i] * arrayB[i, :, :]
C += K[i] * arrayC[i, :]
lam += K[i] * arrayL[i, :, :]
mu += K[i] * arrayM[i, :]
# alfa = 1.0#2.0#
print("Calculating step...")
alfa = calcStepRest(x, u, p, A, B, C, constants, boundary, restrictions)
nx = x + alfa * A
nu = u + alfa * B
np = pi + alfa * C
# while calcP(sizes,nx,nu,np) > P0:
# alfa *= .8#.5
# nx = x + alfa * A
# nu = u + alfa * B
# np = pi + alfa * C
# print("alfa =",alfa)
# incorporation of A, B into the solution
# x += alfa * A
# u += alfa * B
print("Leaving rest with alfa =", alfa)
return nx, nu, np, lam, mu
def rest(sizes, x, u, pi, t, constants, boundary, restrictions):
print("\nIn rest.")
# get sizes
N = sizes['N']
n = sizes['n']
m = sizes['m']
p = sizes['p']
q = sizes['q']
print("Calc phi...")
# calculate phi and psi
phi = calcPhi(sizes, x, u, pi, constants, restrictions)
print("Calc psi...")
psi = calcPsi(sizes, x, boundary)
# aux: phi - dx/dt
aux = phi - ddt(sizes, x)
# get gradients
print("Calc grads...")
Grads = calcGrads(sizes, x, u, pi, constants, restrictions)
dt = Grads['dt']
phix = Grads['phix']
phiu = Grads['phiu']
phip = Grads['phip']
fx = Grads['fx']
psix = Grads['psix']
psip = Grads['psip']
print("Preparing matrices...")
psixTr = psix.transpose()
fxInv = fx.copy()
phixInv = phix.copy()
phiuTr = numpy.empty((N, m, n))
phipTr = numpy.empty((N, p, n))
psipTr = psip.transpose()
for k in range(N):
fxInv[k, :] = fx[N - k - 1, :]
phixInv[k, :, :] = phix[N - k - 1, :, :].transpose()
phiuTr[k, :, :] = phiu[k, :, :].transpose()
phipTr[k, :, :] = phip[k, :, :].transpose()
mu = numpy.zeros(q)
# Matrix for linear system involving k's
M = numpy.ones((q + 1, q + 1))
# column vector for linear system involving k's [eqs (88-89)]
col = numpy.zeros(q + 1)
col[0] = 1.0 # eq (88)
col[1:] = -psi # eq (89)
arrayA = numpy.empty((q + 1, N, n))
arrayB = numpy.empty((q + 1, N, m))
arrayC = numpy.empty((q + 1, p))
arrayL = arrayA.copy()
arrayM = numpy.empty((q + 1, q))
optPlot = dict()
print("Beginning loop for solutions...")
for i in range(q + 1):
mu = 0.0 * mu
if i < q:
mu[i] = 1.0
# integrate equation (75-2) backwards
auxLamInit = -psixTr.dot(mu)
auxLam = odeint(calcLamDotRest, auxLamInit, t, args=(t, phixInv))
# equation for Bi (75-3)
B = numpy.empty((N, m))
lam = 0 * x
for k in range(N):
lam[k, :] = auxLam[N - k - 1, :]
B[k, :] = phiuTr[k, :, :].dot(lam[k, :])
while B.max() > numpy.pi or B.min() < -numpy.pi:
lam *= .1
mu *= .1
B *= .1
print("mu =", mu)
# equation for Ci (75-4)
C = numpy.zeros(p)
for k in range(1, N - 1):
C += phipTr[k, :, :].dot(lam[k, :])
C += .5 * (phipTr[0, :, :].dot(lam[0, :]))
C += .5 * (phipTr[N - 1, :, :].dot(lam[N - 1, :]))
C *= dt
C -= -psipTr.dot(mu)
optPlot['mode'] = 'states:Lambda'
#plotSol(sizes,t,lam,B,C,constants,restrictions,optPlot)
print("Integrating ODE for A [" + str(i) + "/" + str(q) + "] ...")
# integrate equation for A:
A = odeint(calcADotRest,
numpy.zeros(n),
t,
args=(t, phix, phiu, phip, B, C, aux))
else:
# integrate equation (75-2) backwards
lam *= 0.0
# equation for Bi (75-3)
B *= 0.0
# equation for Ci (75-4)
C *= 0.0
print("Integrating ODE for A [" + str(i) + "/" + str(q) + "] ...")
# integrate equation for A:
A = odeint(calcADotOdeRest, numpy.zeros(n), t, args=(t, phix, aux))
#
# store solution in arrays
arrayA[i, :, :] = A
arrayB[i, :, :] = B
arrayC[i, :] = C
arrayL[i, :, :] = lam
arrayM[i, :] = mu
optPlot['mode'] = 'var'
#plotSol(sizes,t,A,B,C,constants,restrictions,optPlot)
# Matrix for linear system (89)
M[1:, i] = psix.dot(A[N - 1, :]) + psip.dot(C)
#
# Calculations of weights k:
K = numpy.linalg.solve(M, col)
print("K =", K)
# summing up linear combinations
A = 0.0 * A
B = 0.0 * B
C = 0.0 * C
lam = 0.0 * lam
mu = 0.0 * mu
for i in range(q + 1):
A += K[i] * arrayA[i, :, :]
B += K[i] * arrayB[i, :, :]
C += K[i] * arrayC[i, :]
lam += K[i] * arrayL[i, :, :]
mu += K[i] * arrayM[i, :]
optPlot['mode'] = 'var'
plotSol(sizes, t, A, B, C, constants, restrictions, optPlot)
print("Calculating step...")
alfa = calcStepRest(sizes, t, x, u, pi, A, B, C, constants, boundary,
restrictions)
nx = x + alfa * A
nu = u + alfa * B
np = pi + alfa * C
print("Leaving rest with alfa =", alfa)
return nx, nu, np, lam, mu
# -*- coding: utf-8 -*-
"""
Created on Tue Mar 14 21:28:20 2017
@author: levi
"""
import utils
from rockProp import getRockTraj
from prob_rocket_sgra import declProb, calcPhi, calcPsi, calcGrads, calcI
from sgra_simple_rocket import calcP, plotSol
opt = dict()
opt['initMode'] = 'extSol'
# declare problem:
sizes, t, x, u, pi, lam, mu, tol, constants = declProb(opt)
dx = utils.ddt(sizes, x)
phi = calcPhi(sizes, x, u, pi, constants)
#print("Proposed initial guess:")
#P = calcP(sizes,x,u,pi,constants)
#plotSol(sizes,t,x,u,pi,lam,mu,constants)
#print("P =",P)
""" Created on Thu Apr 27 14:09:24 2017 @author: levi """ import numpy import matplotlib.pyplot as plt from prob_rocket_sgra import declProb, calcGrads, calcPhi from sgra_simple_rocket_alt import calcP opt = dict() opt['initMode'] = 'extSol'#'default'#'extSol' # declare problem: sizes,t,x0,u0,pi0,lam,mu,tol,constants,boundary,restrictions = declProb(opt) phi0 = calcPhi(sizes,x0,u0,pi0,constants,restrictions) calcP(sizes,x0,u0,pi0,constants,boundary,restrictions,mustPlot=True) Grads = calcGrads(sizes,x0,u0,pi0,constants,restrictions) phix = Grads['phix'] N = sizes['N'] n = sizes['n'] phixNum = phix.copy() E = numpy.zeros((n,N,n)) #for i in range(N): # for j in range(n): # E[j,i,j] = 1.0