def calcQ(sizes, x, u, pi, lam, mu):
# Q expression from (15)
N = sizes['N']
p = sizes['p']
dt = 1.0 / (N - 1)
# get gradients
Grads = calcGrads(sizes, x, u, pi)
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
auxVecIntQ = numpy.zeros(p)
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, :])
#
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
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):
N = sizes['N']
phi = calcPhi(sizes, x, u, pi)
psi = calcPsi(sizes, x)
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 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
# -*- 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)
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 calcQ(self, mustPlotQs=False):
# Q expression from (15).
# FYI: Miele (2003) is wrong in oh so many ways...
N, n, m, p, s = self.N, self.n, self.m, self.p, self.s
dt = 1.0 / (N - 1)
# x = self.x
# u = self.u
lam = self.lam
mu = self.mu
# get gradients
Grads = self.calcGrads()
phix = Grads['phix']
phiu = Grads['phiu']
phip = Grads['phip']
fx = Grads['fx']
fu = Grads['fu']
fp = Grads['fp']
psiy = Grads['psiy']
psip = Grads['psip']
dlam = ddt(lam, N)
Qx = 0.0
Qu = 0.0
Qp = 0.0
Qt = 0.0
Q = 0.0
auxVecIntQp = numpy.zeros((p, s))
errQx = numpy.empty((N, n, s))
normErrQx = numpy.empty((N, s))
errQu = numpy.empty((N, m, s))
normErrQu = numpy.empty((N, s))
errQp = numpy.empty((N, p, s))
#normErrQp = numpy.empty(N)
z = numpy.empty(2 * n * s)
for arc in range(s):
z[2 * arc * n:(2 * arc + 1) * n] = -lam[0, :, arc]
z[(2 * arc + 1) * n:(2 * arc + 2) * n] = lam[N - 1, :, arc]
for k in range(N):
errQx[k,:,arc] = dlam[k,:,arc] - fx[k,:,arc] + \
phix[k,:,:,arc].transpose().dot(lam[k,:,arc])
errQu[k,:,arc] = fu[k,:,arc] + \
- phiu[k,:,:,arc].transpose().dot(lam[k,:,arc])
errQp[k,:,arc] = fp[k,:,arc] + \
- phip[k,:,:,arc].transpose().dot(lam[k,:,arc])
normErrQx[k, arc] = errQx[k, :, arc].transpose().dot(errQx[k, :,
arc])
normErrQu[k, arc] = errQu[k, :, arc].transpose().dot(errQu[k, :,
arc])
Qx += normErrQx[k, arc]
Qu += normErrQu[k, arc]
auxVecIntQp[:, arc] += errQp[k, :, arc]
#
Qx -= .5 * (normErrQx[0, arc] + normErrQx[N - 1, arc])
Qu -= .5 * (normErrQu[0, arc] + normErrQu[N - 1, arc])
auxVecIntQp[:, arc] -= .5 * (errQp[0, :, arc] + errQp[N - 1, :, arc])
auxVecIntQp *= dt
Qx *= dt
Qu *= dt
resVecIntQp = numpy.zeros(p)
for arc in range(s):
resVecIntQp += auxVecIntQp[:, arc]
resVecIntQp += psip.transpose().dot(mu)
Qp = resVecIntQp.transpose().dot(resVecIntQp)
errQt = z + psiy.transpose().dot(mu)
Qt = errQt.transpose().dot(errQt)
Q = Qx + Qu + Qp + Qt
self.log.printL("Q = {:.4E}".format(Q)+": Qx = {:.4E}".format(Qx)+\
", Qu = {:.4E}".format(Qu)+", Qp = {:.7E}".format(Qp)+\
", Qt = {:.4E}".format(Qt))
self.Q = Q
###############################################################################
if mustPlotQs:
args = {
'errQx': errQx,
'errQu': errQu,
'errQp': errQp,
'Qx': Qx,
'Qu': Qu,
'normErrQx': normErrQx,
'normErrQu': normErrQu,
'resVecIntQp': resVecIntQp
}
self.plotQRes(args)
###############################################################################
somePlot = False
for key in self.dbugOptGrad.keys():
if ('plotQ' in key) or ('PlotQ' in key):
if self.dbugOptGrad[key]:
somePlot = True
break
if somePlot:
self.log.printL("\nDebug plots for this calcQ run:\n")
self.plotSol()
indMaxQu = numpy.argmax(normErrQu, axis=0)
for arc in range(s):
self.log.printL("\nArc =", arc, "\n")
ind1 = numpy.array([indMaxQu[arc] - 20, 0]).max()
ind2 = numpy.array([indMaxQu[arc] + 20, N]).min()
if self.dbugOptGrad['plotQu']:
plt.plot(self.t, normErrQu[:, arc])
plt.grid(True)
plt.title("Integrand of Qu")
plt.show()
#for zoomed version:
if self.dbugOptGrad['plotQuZoom']:
plt.plot(self.t[ind1:ind2], normErrQu[ind1:ind2, arc], 'o')
plt.grid(True)
plt.title("Integrand of Qu (zoom)")
plt.show()
# if self.dbugOptGrad['plotCtrl']:
# if self.m==2:
# alfa,beta = self.calcDimCtrl()
# plt.plot(self.t,alfa[:,arc]*180.0/numpy.pi)
# plt.title("Ang. of attack")
# plt.show()
#
# plt.plot(self.t,beta[:,arc]*180.0/numpy.pi)
# plt.title("Thrust profile")
# plt.show()
if self.dbugOptGrad['plotQuComp']:
plt.plot(self.t, errQu[:, 0, arc])
plt.grid(True)
plt.title("Qu: component 1")
plt.show()
if m > 1:
plt.plot(self.t, errQu[:, 1, arc])
plt.grid(True)
plt.title("Qu: component 2")
plt.show()
if self.dbugOptGrad['plotQuCompZoom']:
plt.plot(self.t[ind1:ind2], errQu[ind1:ind2, 0, arc])
plt.grid(True)
plt.title("Qu: component 1 (zoom)")
plt.show()
if m > 1:
plt.plot(self.t[ind1:ind2], errQu[ind1:ind2, 1, arc])
plt.grid(True)
plt.title("Qu: component 2 (zoom)")
plt.show()
if self.dbugOptGrad['plotLam']:
plt.plot(self.t, lam[:, 0, arc])
plt.grid(True)
plt.title("Lambda_h")
plt.show()
if n > 1:
plt.plot(self.t, lam[:, 1, arc])
plt.grid(True)
plt.title("Lambda_v")
plt.show()
if n > 2:
plt.plot(self.t, lam[:, 2, arc])
plt.grid(True)
plt.title("Lambda_gama")
plt.show()
if n > 3:
plt.plot(self.t, lam[:, 3, arc])
plt.grid(True)
plt.title("Lambda_m")
plt.show()
# TODO: break these plots into more conditions
# if numpy.array(self.dbugOptGrad.values()).any:
# print("\nDebug plots for this calcQ run:")
#
# if self.dbugOptGrad['plotQx']:
# plt.plot(self.t,normErrQx)
# plt.grid(True)
# plt.title("Integrand of Qx")
# plt.show()
#
# if self.dbugOptGrad['plotQu']:
# plt.plot(self.t,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()
#
# if self.dbugOptGrad['plotQxZoom']:
# plt.plot(self.t[ind1:ind2],normErrQx[ind1:ind2],'o')
# plt.grid(True)
# plt.title("Integrand of Qx (zoom)")
# plt.show()
#
# if self.dbugOptGrad['plotSolQxMax']:
# print("\nSolution on the region of MaxQx:")
# self.plotSol(intv=numpy.arange(ind1,ind2,1,dtype='int'))
#
# # for zoomed version:
# indMaxQu = normErrQu.argmax()
# ind1 = numpy.array([indMaxQu-20,0]).max()
# ind2 = numpy.array([indMaxQu+20,N-1]).min()
#
# if self.dbugOptGrad['plotQuZoom']:
# plt.plot(self.t[ind1:ind2],normErrQu[ind1:ind2],'o')
# plt.grid(True)
# plt.title("Integrand of Qu (zoom)")
# plt.show()
#
# if self.dbugOptGrad['plotSolQuMax']:
# print("\nSolution on the region of MaxQu:")
# self.plotSol(intv=numpy.arange(ind1,ind2,1,dtype='int'))
#
#
## 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()
if self.dbugOptGrad['pausCalcQ']:
input("calcQ in debug mode. Press any key to continue...")
return Q, Qx, Qu, Qp, Qt
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
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 calcP(self, mustPlotPint=False):
N, s, dt = self.N, self.s, self.dt
x = self.x
phi = self.calcPhi()
psi = self.calcPsi()
dx = ddt(x, N)
func = dx - phi
vetP = numpy.empty((N, s))
vetIP = numpy.empty((N, s))
for arc in range(s):
P = .5 * (func[0, :, arc].dot(func[0, :, arc].transpose()))
vetP[0, arc] = P
vetIP[0, arc] = P
for t in range(1, N - 1):
vetP[t, arc] = func[t, :, arc].dot(func[t, :, arc].transpose())
P += vetP[t, arc]
vetIP[t, arc] = P
vetP[N - 1, arc] = .5 * (func[N - 1, :, arc].dot(
func[N - 1, :, arc].transpose()))
P += vetP[N - 1, arc]
vetIP[N - 1, arc] = P
#P *= dt
vetIP *= dt
# Look for some debug plot
someDbugPlot = False
for key in self.dbugOptRest.keys():
if ('plot' in key) or ('Plot' in key):
if self.dbugOptRest[key]:
someDbugPlot = True
break
if someDbugPlot:
self.log.printL("\nDebug plots for this calcP run:")
indMaxP = numpy.argmax(vetP, axis=0)
self.log.printL(indMaxP)
for arc in range(s):
self.log.printL("\nArc =", arc, "\n")
ind1 = numpy.array([indMaxP[arc] - 20, 0]).max()
ind2 = numpy.array([indMaxP[arc] + 20, N]).min()
if self.dbugOptRest['plotP_int']:
plt.plot(self.t, vetP[:, arc])
plt.grid(True)
plt.title("Integrand of P")
plt.show()
if self.dbugOptRest['plotIntP_int']:
plt.plot(self.t, vetIP[:, arc])
plt.grid(True)
plt.title("Partially integrated P")
plt.show()
#for zoomed version:
if self.dbugOptRest['plotP_intZoom']:
plt.plot(self.t[ind1:ind2], vetP[ind1:ind2, arc], 'o')
plt.grid(True)
plt.title("Integrand of P (zoom)")
plt.show()
if self.dbugOptRest['plotSolMaxP']:
self.log.printL(
"rest_sgra: plotSol @ MaxP region: not implemented yet!")
#self.log.printL("\nSolution on the region of MaxP:")
#self.plotSol(intv=numpy.arange(ind1,ind2,1,dtype='int'))
# # TODO: extend these debug plots
# if self.dbugOptRest['plotRsidMaxP']:
#
# print("\nResidual on the region of maxP:")
#
# if self.n==4 and self.m ==2:
# plt.plot(self.t[ind1:ind2],func[ind1:ind2,0])
# plt.grid(True)
# plt.ylabel("res_hDot [km/s]")
# plt.show()
#
# plt.plot(self.t[ind1:ind2],func[ind1:ind2,1],'g')
# plt.grid(True)
# plt.ylabel("res_vDot [km/s/s]")
# plt.show()
#
# plt.plot(self.t[ind1:ind2],func[ind1:ind2,2]*180/numpy.pi,'r')
# plt.grid(True)
# plt.ylabel("res_gammaDot [deg/s]")
# plt.show()
#
# plt.plot(self.t[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()
#
#
# # else:
# # print("Not implemented (yet).")
# #
# #
Pint = vetIP[N - 1, :].sum() #P
Ppsi = psi.transpose().dot(psi)
P = Ppsi + Pint
strPs = "P = {:.6E}".format(P)+", Pint = {:.6E}".format(Pint)+\
", Ppsi = {:.6E}.".format(Ppsi)
self.log.printL(strPs)
self.P = P
if mustPlotPint:
#plt.subplots_adjust(wspace=1.0,hspace=1.0)
#plt.subplots_adjust(hspace=.5)
plt.subplots_adjust(0.0125, 0.0, 0.9, 2.5, 0.2, 0.2)
Np = self.n + 2
for arc in range(1, s):
vetIP[:, arc] += vetIP[-1, arc - 1]
plt.subplot2grid((Np, 1), (0, 0))
self.plotCat(vetIP, piIsTime=False)
plt.grid(True)
plt.title("P_int: Accumulated value, integrand and error components\n"+\
"P = {:.4E}, ".format(P)+\
"P_int = {:.4E}, ".format(Pint)+\
"P_psi = {:.4E}".format(Ppsi)+\
"\n(rest. iter. #"+str(self.NIterRest+1)+")\n")
plt.ylabel('Accum.')
plt.subplot2grid((Np, 1), (1, 0))
self.plotCat(vetP, piIsTime=False, color='k')
plt.grid(True)
plt.ylabel('Integrand')
colorList = ['b', 'g', 'r', 'm']
for i in range(self.n):
plt.subplot2grid((Np, 1), (i + 2, 0))
self.plotCat(func[:, i, :], piIsTime=False, color=colorList[i % 4])
plt.grid(True)
plt.ylabel('State ' + str(i))
self.savefig(keyName='Pint', fullName='integrand of P')
return P, Pint, Ppsi
def rest(sizes, x, u, pi, t):
print("In rest.")
P0 = calcP(sizes, x, u, pi)
print("P0 =", P0)
# get sizes
N = sizes['N']
n = sizes['n']
m = sizes['m']
p = sizes['p']
q = sizes['q']
# calculate phi and psi
phi = calcPhi(sizes, x, u, pi)
psi = calcPsi(sizes, x)
# aux: phi - dx/dt
aux = phi.copy()
aux -= ddt(sizes, x)
# get gradients
Grads = calcGrads(sizes, x, u, pi)
dt = Grads['dt']
phix = Grads['phix']
phiu = Grads['phiu']
phip = Grads['phip']
fx = Grads['fx']
psix = Grads['psix']
psip = Grads['psip']
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))
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)
# 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#
alfa = calcStepRest(x, u, p, A, B, C)
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 __init__(self, sol, rho):
"""Initialization method. Comprises all the "common" calculations for
each independent solution to be added over.
According to the Miele (2003) convention,
rho = 0 for rest, and rho = 1 for grad.
"""
# debug options...
self.dbugOptGrad = sol.dbugOptGrad
self.dbugOptRest = sol.dbugOptRest
self.t = sol.t
# get sizes
Ns, N, m, n, p, q, s = sol.Ns, sol.N, sol.m, sol.n, sol.p, sol.q, sol.s
self.Ns, self.N, self.m, self.n, self.p, self.q, self.s = Ns, N, m, n, p, q, s
self.dt = 1.0 / (N - 1)
self.rho = rho
# calculate integration error (only if necessary)
psi = sol.calcPsi()
self.psi = psi
if rho < .5:
# calculate phi and psi
phi = sol.calcPhi()
err = phi - ddt(sol.x, N)
else:
err = numpy.zeros((N, n, s))
self.err = err
#######################################################################
if rho < 0.5 and self.dbugOptRest['plotErr']:
print("\nThis is err:")
for arc in range(s):
plt.plot(self.t, err[:, 0, arc])
plt.ylabel("errPos")
plt.grid(True)
plt.show()
plt.clf()
plt.close('all')
if n > 1:
plt.plot(self.t, err[:, 1, arc])
plt.ylabel("errVel")
plt.grid(True)
plt.show()
plt.clf()
plt.close('all')
#######################################################################
# Get gradients
Grads = sol.calcGrads(calcCostTerm=(rho > 0.5))
#dt6 = dt/6
phix = Grads['phix']
phiu = Grads['phiu']
phip = Grads['phip']
psiy = Grads['psiy']
psip = Grads['psip']
fx = Grads['fx']
fu = Grads['fu']
fp = Grads['fp']
self.phip = phip
self.psiy = psiy
self.psip = psip
self.fx = fx
self.fu = fu
self.fp = fp
# Prepare matrices with derivatives:
phixTr = numpy.empty_like(phix)
phiuTr = numpy.empty((N, m, n, s))
phipTr = numpy.empty((N, p, n, s))
phiuFu = numpy.empty((N, n, s))
for arc in range(s):
for k in range(N):
phixTr[k, :, :, arc] = phix[k, :, :, arc].transpose()
phiuTr[k, :, :, arc] = phiu[k, :, :, arc].transpose()
phipTr[k, :, :, arc] = phip[k, :, :, arc].transpose()
phiuFu[k, :, arc] = phiu[k, :, :, arc].dot(fu[k, :, arc])
self.phiuFu = phiuFu
self.phiuTr = phiuTr
self.phipTr = phipTr
InitCondMat = numpy.eye(Ns, Ns + 1)
self.InitCondMat = InitCondMat
# Dynamics matrix for propagating the LSODE:
DynMat = numpy.zeros((N, 2 * n, 2 * n, s))
for arc in range(s):
for k in range(N):
DynMat[k, :n, :n, arc] = phix[k, :, :, arc]
DynMat[k, :n, n:, arc] = phiu[k, :, :, arc].dot(phiuTr[k, :, :,
arc])
DynMat[k, n:, n:, arc] = -phixTr[k, :, :, arc]
self.DynMat = DynMat
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
