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 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 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 grad(sizes, x, u, pi, t, Q0, restrictions):
print("In grad.")
print("Q0 =", Q0)
# get sizes
N = sizes['N']
n = sizes['n']
m = sizes['m']
p = sizes['p']
q = sizes['q']
# get gradients
Grads = calcGrads(sizes, x, u, pi, constants, restrictions)
phix = Grads['phix']
phiu = Grads['phiu']
phip = Grads['phip']
fx = Grads['fx']
fu = Grads['fu']
fp = Grads['fp']
psix = Grads['psix']
psip = Grads['psip']
dt = Grads['dt']
# prepare time reversed/transposed versions of the arrays
psixTr = psix.transpose()
fxInv = fx.copy()
phixInv = phix.copy()
phiuTr = numpy.empty((N, m, n))
phipTr = numpy.empty((N, p, n))
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()
psipTr = psip.transpose()
# Prepare array mu and arrays for linear combinations of A,B,C,lam
mu = numpy.zeros(q)
M = numpy.ones((q + 1, q + 1))
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))
for i in range(q + 1):
mu = 0.0 * mu
if i < q:
mu[i] = 1.0
#print("mu =",mu)
# integrate equation (38) backwards for lambda
auxLamInit = -psixTr.dot(mu)
#auxLam = numpy.empty((N,n))
#auxLam[0,:] = auxLamInit
#for k in range(N-1):
# auxLam[k+1,:] = auxLam[k,:] + dt*(phixInv[k,:,:].dot(auxLam[k-1,:]))
auxLam = odeint(calcLamDotGrad,
auxLamInit,
t,
args=(t, fxInv, phixInv))
# Calculate B
B = -fu
lam = auxLam.copy()
for k in range(N):
lam[k, :] = auxLam[N - k - 1, :]
B[k, :] += phiuTr[k, :, :].dot(lam[k, :])
# Calculate C
C = numpy.zeros(p)
for k in range(1, N - 1):
C += fp[k, :] - phipTr[k, :, :].dot(lam[k, :])
C += .5 * (fp[0, :] - phipTr[0, :, :].dot(lam[0, :]))
C += .5 * (fp[N - 1, :] - phipTr[N - 1, :, :].dot(lam[N - 1, :]))
C *= -dt
C -= -psipTr.dot(mu)
# plt.plot(t,B)
# plt.grid(True)
# plt.xlabel("t")
# plt.ylabel("B")
# plt.show()
#
# integrate diff equation for A
# A = numpy.zeros((N,n))
# for k in range(N-1):
# A[k+1,:] = A[k,:] + dt*(phix[k,:,:].dot(A[k,:]) + \
# phiu[k,:,:].dot(B[k,:]) + \
# phip[k,:].dot(C[k,:]))
A = odeint(calcADotGrad,
numpy.zeros(n),
t,
args=(t, phix, phiu, phip, B, C))
# plt.plot(t,A)
# plt.grid(True)
# plt.xlabel("t")
# plt.ylabel("A")
# plt.show()
arrayA[i, :, :] = A
arrayB[i, :, :] = B
arrayC[i, :] = C
arrayL[i, :, :] = lam
arrayM[i, :] = mu
M[1:, i] = psix.dot(A[N - 1, :]) + psip.dot(C)
#
# Calculations of weights k:
col = numpy.zeros(q + 1)
col[0] = 1.0
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, :]
# Calculation of alfa
alfa = calcStepGrad(x, u, pi, lam, mu, A, B, C, restrictions)
nx = x + alfa * A
nu = u + alfa * B
np = pi + alfa * C
Q = calcQ(sizes, nx, nu, np, lam, mu, constants, restrictions)
print("Leaving grad with alfa =", alfa)
return nx, nu, np, lam, mu, Q
def calcQ(sizes, x, u, pi, lam, mu, constants, restrictions):
# Q expression from (15)
N = sizes['N']
p = sizes['p']
dt = 1.0 / (N - 1)
# get gradients
Grads = calcGrads(sizes, x, u, pi, constants, restrictions)
phix = Grads['phix']
phiu = Grads['phiu']
phip = Grads['phip']
fx = Grads['fx']
fu = Grads['fu']
fp = Grads['fp']
psix = Grads['psix']
#psip = Grads['psip']
dlam = ddt(sizes, lam)
Qx = 0.0
Qu = 0.0
Qp = 0.0
Qt = 0.0
Q = 0.0
auxVecIntQ = numpy.zeros(p)
isnan = 0
for k in range(1, N - 1):
# dlam[k,:] = lam[k,:]-lam[k-1,:]
Qx += norm(dlam[k, :] - fx[k, :] +
phix[k, :, :].transpose().dot(lam[k, :]))**2
Qu += norm(fu[k, :] - phiu[k, :, :].transpose().dot(lam[k, :]))**2
auxVecIntQ += fp[k, :] - phip[k, :, :].transpose().dot(lam[k, :])
if numpy.math.isnan(Qx) or numpy.math.isnan(Qu):
isnan += 1
if isnan == 1:
print("k_nan=", k)
#
Qx += .5 * (norm(dlam[0, :] - fx[0, :] +
phix[0, :, :].transpose().dot(lam[0, :]))**2)
Qx += .5 * (norm(dlam[N - 1, :] - fx[N - 1, :] +
phix[N - 1, :, :].transpose().dot(lam[N - 1, :]))**2)
Qu += .5 * norm(fu[0, :] - phiu[0, :, :].transpose().dot(lam[0, :]))**2
Qu += .5 * norm(fu[N - 1, :] -
phiu[N - 1, :, :].transpose().dot(lam[N - 1, :]))**2
Qx *= dt
Qu *= dt
auxVecIntQ += .5 * (fp[0, :] - phip[0, :, :].transpose().dot(lam[0, :]))
auxVecIntQ += .5 * (fp[N - 1, :] -
phip[N - 1, :, :].transpose().dot(lam[N - 1, :]))
auxVecIntQ *= dt
Qp = norm(auxVecIntQ)
Qt = norm(lam[N - 1, :] + psix.transpose().dot(mu))
#"Qx =",Qx)
Q = Qx + Qu + Qp + Qt
print("Q = {:.4E}".format(Q) + ": Qx = {:.4E}".format(Qx) +
", Qu = {:.4E}".format(Qu) + ", Qp = {:.4E}".format(Qp) +
", Qt = {:.4E}".format(Qt))
return Q
if __name__ == "__main__":
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()
def grad(sizes,
x,
u,
pi,
t,
Q0,
constants,
boundary,
restrictions,
mustPlot=False):
print("In grad.")
print("Q0 =", Q0, "\n")
# get sizes
N = sizes['N']
n = sizes['n']
m = sizes['m']
p = sizes['p']
q = sizes['q']
# get gradients
Grads = calcGrads(sizes, x, u, pi, constants, restrictions)
phix = Grads['phix']
phiu = Grads['phiu']
phip = Grads['phip']
fx = Grads['fx']
fu = Grads['fu']
fp = Grads['fp']
psix = Grads['psix']
psip = Grads['psip']
dt = Grads['dt']
# prepare time reversed/transposed versions of the arrays
psixTr = psix.transpose()
fxInv = fx.copy()
phixInv = phix.copy()
phiuTr = numpy.empty((N, m, n))
phipTr = numpy.empty((N, p, n))
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()
psipTr = psip.transpose()
# Prepare array mu and arrays for linear combinations of A,B,C,lam
mu = numpy.zeros(q)
auxLam = 0 * x
lam = 0 * x
M = numpy.ones((q + 1, q + 1))
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()
#ind1 = [1,2,0]
#ind2 = [2,0,1]
for i in range(q + 1):
print("\n>Integrating solution " + str(i + 1) + " of " + str(q + 1) +
"...\n")
mu = 0.0 * mu
if i < q:
mu[i] = 1.0e-7
#mu[i] = ((-1)**i)*1.0e-8#10#1.0#e-5#
#mu[ind1[i]] = 1.0e-8
#mu[ind2[i]] = -1.0e-8
# integrate equation (38) backwards for lambda
auxLam[0, :] = -psixTr.dot(mu)
# 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,:]-fxInv[k,:]*dt)
#
#auxLam = numpy.empty((N,n))
#auxLam[0,:] = auxLamInit
#for k in range(N-1):
# auxLam[k+1,:] = auxLam[k,:] + dt*(phixInv[k,:,:].dot(auxLam[k-1,:]))
# Calculate B
B = -fu.copy()
for k in range(N):
lam[k, :] = auxLam[N - k - 1, :]
B[k, :] += phiuTr[k, :, :].dot(lam[k, :])
##################################################################
# TESTING LAMBDA DIFFERENTIAL EQUATION
if i < q: #otherwise, there's nothing to test here...
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, :]) - fx[k, :]
normErroLam[k] = erroLam[k, :].transpose().dot(erroLam[k, :])
if mustPlot:
print("\nLambda Error:")
optPlot['mode'] = 'states:LambdaError'
plotSol(sizes,t,erroLam,numpy.zeros((N,m)),numpy.zeros(p),\
constants,restrictions,optPlot)
maxNormErroLam = normErroLam.max()
print("maxNormErroLam =", maxNormErroLam)
if mustPlot and (maxNormErroLam > 0):
plt.semilogy(normErroLam)
plt.grid()
plt.title("ErroLam")
plt.show()
##################################################################
# Calculate C
C = numpy.zeros(p)
for k in range(1, N - 1):
C += fp[k, :] - phipTr[k, :, :].dot(lam[k, :])
C += .5 * (fp[0, :] - phipTr[0, :, :].dot(lam[0, :]))
C += .5 * (fp[N - 1, :] - phipTr[N - 1, :, :].dot(lam[N - 1, :]))
C *= -dt #yes, the minus sign is on purpose!
C -= -psipTr.dot(mu)
if mustPlot:
optPlot['mode'] = 'states:Lambda'
plotSol(sizes, t, lam, B, C, constants, restrictions, optPlot)
# integrate diff 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)
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))
# for k in range(N-1):
# A[k+1,:] = numpy.linalg.solve(I-dt*phix[k+1,:,:],\
# A[k,:] + dt*(phiu[k,:,:].dot(B[k,:]) + phip[k,:,:].dot(C)))
#A = numpy.zeros((N,n))
#for k in range(N-1):
# A[k+1,:] = A[k,:] + dt*(phix[k,:,:].dot(A[k,:]) + \
# phiu[k,:,:].dot(B[k,:]) + \
# phip[k,:].dot(C[k,:]))
dA = ddt(sizes, A)
erroA = numpy.empty((N, n))
normErroA = numpy.empty(N)
for k in range(N):
erroA[k, :] = dA[k, :] - phix[k, :, :].dot(
A[k, :]) - phiu[k, :, :].dot(B[k, :]) - phip[k, :, :].dot(C)
normErroA[k] = erroA[k, :].dot(erroA[k, :])
if mustPlot:
print("\nA Error:")
optPlot['mode'] = 'states:AError'
plotSol(sizes,t,erroA,B,C,\
constants,restrictions,optPlot)
maxNormErroA = normErroA.max()
print("maxNormErroA =", maxNormErroA)
if mustPlot and (maxNormErroA > 0):
plt.semilogy(normErroA)
plt.grid()
plt.title("ErroA")
plt.show()
arrayA[i, :, :] = A
arrayB[i, :, :] = B
arrayC[i, :] = C
arrayL[i, :, :] = lam
arrayM[i, :] = mu
if mustPlot:
optPlot['mode'] = 'var'
plotSol(sizes, t, A, B, C, constants, restrictions, optPlot)
M[1:, i] = psix.dot(A[N - 1, :]) + psip.dot(C)
#
# Calculations of weights k:
col = numpy.zeros(q + 1)
col[0] = 1.0
print("M =", M)
print("col =", col)
K = numpy.linalg.solve(M, col)
print("K =", K)
print("Residual =", M.dot(K) - col)
# 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, :]
##########################################
dlam = ddt(sizes, lam)
dA = ddt(sizes, A)
erroLam = numpy.empty((N, n))
erroA = numpy.empty((N, n))
normErroLam = numpy.empty(N)
normErroA = numpy.empty(N)
for k in range(N):
erroLam[k, :] = dlam[k, :] + phix[k, :, :].transpose().dot(
lam[k, :]) - fx[k, :]
normErroLam[k] = erroLam[k, :].transpose().dot(erroLam[k, :])
erroA[k, :] = dA[k, :] - phix[k, :, :].dot(
A[k, :]) - phiu[k, :, :].dot(B[k, :]) - phip[k, :, :].dot(C)
normErroA[k] = erroA[k, :].dot(erroA[k, :])
if mustPlot:
print("\nFINAL A Error:")
optPlot['mode'] = 'states:AError'
plotSol(sizes,t,erroA,B,C,\
constants,restrictions,optPlot)
maxNormErroA = normErroA.max()
print("FINAL maxNormErroA =", maxNormErroA)
if mustPlot and (maxNormErroA > 0):
plt.semilogy(normErroA)
plt.grid()
plt.title("ErroA")
plt.show()
if mustPlot:
print("\nFINAL Lambda Error:")
optPlot['mode'] = 'states:LambdaError'
plotSol(sizes,t,erroLam,B,C,\
constants,restrictions,optPlot)
maxNormErroLam = normErroLam.max()
print("FINAL maxNormErroLam =", maxNormErroLam)
if mustPlot and (maxNormErroLam > 0):
plt.semilogy(normErroLam)
plt.grid()
plt.title("ErroLam")
plt.show()
##########################################
#if (B>numpy.pi).any() or (B<-numpy.pi).any():
# print("\nProblems in grad: corrections will result in control overflow.")
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)
# Calculation of alfa
alfa = calcStepGrad(sizes, x, u, pi, lam, mu, A, B, C, constants, boundary,
restrictions)
nx = x + alfa * A
nu = u + alfa * B
np = pi + alfa * C
Q = calcQ(sizes, nx, nu, np, lam, mu, constants, restrictions, mustPlot)
print("Leaving grad with alfa =", alfa)
return nx, nu, np, lam, mu, Q
def calcQ(sizes, x, u, pi, lam, mu, constants, restrictions, mustPlot=False):
# Q expression from (15)
N = sizes['N']
n = sizes['n']
m = sizes['m']
p = sizes['p']
dt = 1.0 / (N - 1)
# get gradients
Grads = calcGrads(sizes, x, u, pi, constants, restrictions)
phix = Grads['phix']
phiu = Grads['phiu']
phip = Grads['phip']
fx = Grads['fx']
fu = Grads['fu']
fp = Grads['fp']
psix = Grads['psix']
psip = Grads['psip']
dlam = ddt(sizes, lam)
Qx = 0.0
Qu = 0.0
Qp = 0.0
Qt = 0.0
Q = 0.0
auxVecIntQp = numpy.zeros(p)
errQx = numpy.empty((N, n))
normErrQx = numpy.empty(N)
errQu = numpy.empty((N, m))
normErrQu = numpy.empty(N)
errQp = numpy.empty((N, p))
#normErrQp = numpy.empty(N)
for k in range(N):
errQx[k, :] = dlam[k, :] - fx[k, :] + phix[k, :, :].transpose().dot(
lam[k, :])
errQu[k, :] = fu[k, :] - phiu[k, :, :].transpose().dot(lam[k, :])
errQp[k, :] = fp[k, :] - phip[k, :, :].transpose().dot(lam[k, :])
normErrQx[k] = errQx[k, :].transpose().dot(errQx[k, :])
normErrQu[k] = errQu[k, :].transpose().dot(errQu[k, :])
Qx += normErrQx[k]
Qu += normErrQu[k]
auxVecIntQp += errQp[k, :]
#
Qx -= .5 * (normErrQx[0] + normErrQx[N - 1])
Qu -= .5 * (normErrQu[0] + normErrQu[N - 1])
Qx *= dt
Qu *= dt
auxVecIntQp -= .5 * (errQp[0, :] + errQp[N - 1, :])
auxVecIntQp *= dt
auxVecIntQp += psip.transpose().dot(mu)
Qp = auxVecIntQp.transpose().dot(auxVecIntQp)
errQt = lam[N - 1, :] + psix.transpose().dot(mu)
Qt = errQt.transpose().dot(errQt)
Q = Qx + Qu + Qp + Qt
print("Q = {:.4E}".format(Q) + ": Qx = {:.4E}".format(Qx) +
", Qu = {:.4E}".format(Qu) + ", Qp = {:.4E}".format(Qp) +
", Qt = {:.4E}".format(Qt))
if mustPlot:
tPlot = numpy.arange(0, 1.0 + dt, dt)
plt.plot(tPlot, normErrQx)
plt.grid(True)
plt.title("Integrand of Qx")
plt.show()
plt.plot(tPlot, normErrQu)
plt.grid(True)
plt.title("Integrand of Qu")
plt.show()
# for zoomed version:
indMaxQx = normErrQx.argmax()
ind1 = numpy.array([indMaxQx - 20, 0]).max()
ind2 = numpy.array([indMaxQx + 20, N - 1]).min()
plt.plot(tPlot[ind1:ind2], normErrQx[ind1:ind2], 'o')
plt.grid(True)
plt.title("Integrand of Qx (zoom)")
plt.show()
n = sizes['n']
m = sizes['m']
if n == 4 and m == 2:
plt.plot(tPlot[ind1:ind2], errQx[ind1:ind2, 0])
plt.grid(True)
plt.ylabel("Qx_h")
plt.show()
plt.plot(tPlot[ind1:ind2], errQx[ind1:ind2, 1], 'g')
plt.grid(True)
plt.ylabel("Qx_V")
plt.show()
plt.plot(tPlot[ind1:ind2], errQx[ind1:ind2, 2], 'r')
plt.grid(True)
plt.ylabel("Qx_gamma")
plt.show()
plt.plot(tPlot[ind1:ind2], errQx[ind1:ind2, 3], 'm')
plt.grid(True)
plt.ylabel("Qx_m")
plt.show()
print("\nStates, controls, lambda on the region of maxQx:")
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()
print("Lambda:")
plt.plot(tPlot[ind1:ind2], lam[ind1:ind2, 0])
plt.grid(True)
plt.ylabel("lam_h")
plt.show()
plt.plot(tPlot[ind1:ind2], lam[ind1:ind2, 1], 'g')
plt.grid(True)
plt.ylabel("lam_V")
plt.show()
plt.plot(tPlot[ind1:ind2], lam[ind1:ind2, 2], 'r')
plt.grid(True)
plt.ylabel("lam_gamma")
plt.show()
plt.plot(tPlot[ind1:ind2], lam[ind1:ind2, 3], 'm')
plt.grid(True)
plt.ylabel("lam_m")
plt.show()
# print("dLambda/dt:")
#
# plt.plot(tPlot[ind1:ind2],dlam[ind1:ind2,0])
# plt.grid(True)
# plt.ylabel("dlam_h")
# plt.show()
#
# plt.plot(tPlot[ind1:ind2],dlam[ind1:ind2,1])
# plt.grid(True)
# plt.ylabel("dlam_V")
# plt.show()
#
# plt.plot(tPlot[ind1:ind2],dlam[ind1:ind2,2],'r')
# plt.grid(True)
# plt.ylabel("dlam_gamma")
# plt.show()
#
# plt.plot(tPlot[ind1:ind2],dlam[ind1:ind2,3],'m')
# plt.grid(True)
# plt.ylabel("dlam_m")
# plt.show()
#
# print("-phix*lambda:")
#
# plt.plot(tPlot[ind1:ind2],dlam[ind1:ind2,0]-errQx[ind1:ind2,0])
# plt.grid(True)
# plt.ylabel("-phix*lambda_h")
# plt.show()
#
# plt.plot(tPlot[ind1:ind2],dlam[ind1:ind2,1]-errQx[ind1:ind2,1],'g')
# plt.grid(True)
# plt.ylabel("-phix*lambda_V")
# plt.show()
#
# plt.plot(tPlot[ind1:ind2],dlam[ind1:ind2,2]-errQx[ind1:ind2,2],'r')
# plt.grid(True)
# plt.ylabel("-phix*lambda_gamma")
# plt.show()
#
# plt.plot(tPlot[ind1:ind2],dlam[ind1:ind2,3]-errQx[ind1:ind2,3],'m')
# plt.grid(True)
# plt.ylabel("-phix*lambda_m")
# plt.show()
print("\nBlue: dLambda/dt; Black: -phix*lam")
plt.plot(tPlot[ind1:ind2], dlam[ind1:ind2, 0])
plt.plot(tPlot[ind1:ind2],
dlam[ind1:ind2, 0] - errQx[ind1:ind2, 0], 'k')
plt.grid(True)
plt.ylabel("z_h")
plt.show()
plt.plot(tPlot[ind1:ind2], dlam[ind1:ind2, 1])
plt.plot(tPlot[ind1:ind2],
dlam[ind1:ind2, 1] - errQx[ind1:ind2, 1], 'k')
plt.grid(True)
plt.ylabel("z_V")
plt.show()
plt.plot(tPlot[ind1:ind2], dlam[ind1:ind2, 2])
plt.plot(tPlot[ind1:ind2],
dlam[ind1:ind2, 2] - errQx[ind1:ind2, 2], 'k')
plt.grid(True)
plt.ylabel("z_gamma")
plt.show()
plt.plot(tPlot[ind1:ind2], dlam[ind1:ind2, 3])
plt.plot(tPlot[ind1:ind2],
dlam[ind1:ind2, 3] - errQx[ind1:ind2, 3], 'k')
plt.grid(True)
plt.ylabel("z_m")
plt.show()
return Q
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
@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 delta = numpy.array([1e-6,1e-7,1e-8,1e-6]) for j in range(n):