def calcI(self):
N,s = self.N,self.s
_, fOrig, fPF = self.calcF()
IvecOrig = numpy.empty(s)
IvecPF = numpy.empty(s)
for arc in range(s):
IvecOrig[arc] = simp(fOrig[:,arc],N)
IvecPF[arc] = simp(fPF[:,arc],N)
#
IOrig, IPF = IvecOrig.sum(), IvecPF.sum()
IOrig = self.x[0, 2, 0] - self.x[-1, 2, -1]
return IOrig+IPF, IOrig, IPF
def calcI(self):
f, _, _ = self.calcF()
N = self.N
I = 0.0
for arc in range(self.s):
I += simp(f[:, arc], N)
return I, I, 0.0
def calcI(self):
#N,s = self.N,self.s
#f, _, _ = self.calcF()
#Ivec = self.pi
# for arc in range(s):
# Ivec[arc] = .5*(f[0,arc]+f[N-1,arc])
# Ivec[arc] += f[1:(N-1),arc].sum()
# Ivec *= 1.0/(N-1)
Ivec = numpy.empty(self.s)
f, _, _ = self.calcF()
for arc in range(self.s):
Ivec[arc] = utils.simp(f[:, arc], self.N)
I = Ivec.sum()
return I, I, 0.0
def calcJ(self):
N, s = self.N, self.s
#x = self.x
#phi = self.calcPhi()
psi = self.calcPsi()
lam = self.lam
mu = self.mu
#dx = ddt(x,N)
I, Iorig, Ipf = self.calcI()
func = -self.calcErr() #like this, it's dx-phi
vetL = numpy.empty((N, s))
#vetIL = numpy.empty((N,s))
for arc in range(s):
for t in range(N):
vetL[t, arc] = lam[t, :, arc].transpose().dot(func[t, :, arc])
# Perform the integration of Lint array by Simpson's method
Lint = 0.0
for arc in range(self.s):
Lint += simp(vetL[:, arc], N)
#Lint = vetIL[N-1,:].sum()
Lpsi = mu.transpose().dot(psi)
L = Lint + Lpsi
J_Lint = Lint
J_Lpsi = Lpsi
J_I = I
J = L + J_I
strJs = "J = {:.6E}".format(J)+", J_Lint = {:.6E}".format(J_Lint)+\
", J_Lpsi = {:.6E}".format(J_Lpsi)+", J_I = {:.6E}".format(J_I)
self.log.printL(strJs)
return J, J_Lint, J_Lpsi, I, Iorig, Ipf
def try_lambdaR(lr, K=1., vL=.5, l=1., a=1., mustPrint=False, mustPlot=False):
"""Generates a solution with the same "shape" as the optimal solution,
from a given value of lr (lambda_R), returning the value of the
residual of the time equation. Hence the solution yielded is not
necessarily the optimal one, but the closer the residual is to zero,
the closer the solution is to the optimal one.
"""
# maximum theoretical speed
vMaxTheo = numpy.sqrt(a * l)
if vL > vMaxTheo:
raise Exception("Limit speed is useless (too high).")
# this is the speed in which the cart "coasts"
vs = vL + lr / 2. / K
if vs > vMaxTheo:
vs = vMaxTheo
# total time
pi = l / vs + vs / a
# (non-dimensional) time until vL speed is achieved
tvL = vL / pi / a
# (non-dimensional) time until vs speed is achieved
t1s = vs / pi / a
# speed limit violation
viol = 100. * (vs / vL - 1.)
# preparing the array for numerical integration
N = 10001
tVec = numpy.linspace(0., 1., num=N)
v = tVec * 0.
# this loop assembles the speed profile array
for k, t in enumerate(tVec):
if t < t1s:
v[k] = pi * a * t
elif t < 1. - t1s:
v[k] = vs
else:
v[k] = vs - pi * a * (t + t1s - 1.)
# residual computation
int1 = K * simp((v - vL)**2 * (v >= vL), N)
int2 = K * simp(v * (v - vL) * (v >= vL), N)
res = 1. + int1 + 2 * int2 - 2. * lr * l / pi
# cost function value for this solution proposal
cost = pi * (1. + int1)
strVio = "Speed violation = {:.2G}%, costFunc = {:.4E}".format(viol, cost)
if mustPrint:
strP = "vs = {:.4G}, pi = {:.4G}, tvL = {:.4G}, t1s = {:.4G}".format(
vs, pi, tvL, t1s)
print(strP)
print(strVio)
if mustPlot:
plt.plot(tVec, v, label='v')
plt.plot(tVec, vL + tVec * 0., label='vLim')
plt.xlabel("Non-dimensional time")
plt.ylabel("Speed")
plt.title(strVio)
plt.grid(True)
plt.legend()
plt.show()
return res
def intg_land(pi,t1,lam0,aL,K,pars,mustPlot=False,N=100001):
"""This is the innermost function for the lander problem with acceleration
limitation. It yields (numerically, of course) a solution to the lander
problem, assuming the total time pi>0, waiting fraction 0<t1<1, initial
conditions on costates lam0, limiting acceleration aL and penalty function
gain K.
N is the number of elements for discretization of time array, should be
kept high because the differential equations are solved with the simplest
(first order) Euler method.
This function returns the four residuals that must become zero (only) for
the exact optimal solution, corresponding to the conditions:
h(1) = 0,
v(1) = 0,
lambda_M(1) = 0,
integral of (H_pi) dt = 0
where lambda_M is the Mass costate and H = f - lam * phi is the
Hamiltonian.
"""
lh, lv0, lm0 = lam0
g0Isp = pars['g0Isp']
g = pars['g']
h0 = pars['h0']
v0 = pars['v0']
M0 = pars['M0']
T = pars['T']
# initial value for the crossing function xi
xi0 = (1. + lm0) / g0Isp - lv0 / M0
msg = "pi = {:.5G}, xi0 = {:.4G}, t1 = {:.4G}, "\
"lh = {:.4G}, lv0 = {:.4G}".format(pi, xi0, t1, lh, lv0)
print(msg)
t = numpy.linspace(0., 1., num=N)
dtd = t[1] * pi
# speed costate
lv = lv0 - pi * lh * t
# These are the exact solutions assuming free fall
v = v0 - pi * g * t
h = h0 + v0 * pi * t - .5 * g * (pi * t) ** 2
M = M0 + numpy.zeros(N)#0. * t
lm = lm0 + numpy.zeros(N)#0. * t
# Perform the integration (for the propulsed part)
for k,t_ in enumerate(t):
if t_ > t1:
M_ = M[k-1]
# xi equation
xi_ = (1.+lm[k-1])/g0Isp - lv[k-1]/M_
xiM2 = xi_ * M_**2/2./K/T
# fraction of maximum thrust
b_ = M_ * aL * (1. - xiM2) / T
# perform obvious saturations
if b_ > 1.:
b_ = 1.
elif b_ < 0.:
b_ = 0.
# acceleration provoked by that thrust
a_ = b_ * T / M_
# everything is ready, integrate the ODEs
h[k] = h[k-1] + v[k-1] * dtd
v[k] = v[k-1] + (a_ - g) * dtd
M[k] = M_ - b_ * T * dtd/ g0Isp
lm[k] = lm[k-1] + dtd * (a_ / M_) * \
(lv[k-1] - 2. * K * (a_-aL) * (a_>aL))
# assemble complete arrays for integration
xi = (1.+lm)/g0Isp - lv/M
b = M * aL * (1. - xi * M**2 / 2. / K / T) * (t>=t1) / T
# saturate again
for k, b_ in enumerate(b):
if b_ > 1.:
b[k] = 1.
elif b_ < 0.:
b[k] = 0.
a = b * T / M
# this is the term for the integral equation
intg = (M0-M[-1])/pi + K * simp((a-aL)**2 * (a>=aL),N) + \
2. * lh * h0 / pi + lv0 * v0 / pi + (T/g0Isp) * simp(lm * b,N)
# assemble residuals
resV = numpy.array([h[-1], v[-1], lm[-1], intg])
res = sum(resV ** 2)
#if mustPrint:
print("resV =", resV)
print("res =", res)
if mustPlot:
fig, axs = plt.subplots(3, 2, constrained_layout=True)
axs[0,0].plot(t*pi,h,label='h')
#axs[0].set_title('subplot 1')
axs[0,0].set_ylabel('Height [km]')
axs[0,0].set_xlabel('Time [s]')
axs[0, 0].grid()
msg = 'Analytical solution for K = {:.1G}, aLim = {:.1G}g (Res = {:.3E})'.format(K,aL/g,res)
fig.suptitle(msg, fontsize=12)
axs[0,1].plot(t*pi, v, label='v')
axs[0,1].set_ylabel('Speed [km/s]')
axs[0,1].set_xlabel('Time [s]')
axs[0, 1].grid()
axs[1,0].plot(t*pi, a/aL, label='acc/accLim')
axs[1,0].set_ylabel('acc/accLim [-]')
axs[1,0].set_xlabel('Time [s]')
axs[1, 0].grid()
axs[1,1].plot(t*pi, b, label='beta')
axs[1,1].set_ylabel('beta [-]')
axs[1,1].set_xlabel('Time [s]')
axs[1, 1].grid()
axs[2,0].plot(t*pi, M, label='M')
axs[2,0].set_ylabel('Mass [kg]')
axs[2,0].set_xlabel('Time [s]')
axs[2, 0].grid()
axs[2,1].plot(t*pi, xi, label='xi')
axs[2,1].set_ylabel('xi [s/km]')
axs[2,1].set_xlabel('Time [s]')
axs[2, 1].grid()
# calculate maximum violation, mean violation, etc
maxVio = (max(a) / aL - 1.) * 100.
meanVio = (simp((a - aL) * (a >= aL), N) / (1. - t1) / aL ) * 100.
plt.figure()
plt.plot(t*pi,a/aL)
plt.grid()
plt.xlabel("Time [s]")
plt.ylabel("Acc/AccLim")
plt.title("K = {:.2G}. Violation: max = {:.2G}%, mean = {:.2G}%".format(K,maxVio,meanVio))
plt.show()
return resV
def LMPBVP(self,rho=0.0,isParallel=False):
helper = LMPBVPhelp(self,rho)
# get proper range according to grad or rest and omit or not
if rho > .5 and self.omit:
# Grad and omit: use only the elements from the omitted list
rang = self.omitVarList
else:
# Rest or no omit: use all the elements
rang = list(range(self.Ns+1))
if isParallel:
pool = Pool()
res = pool.map(helper.propagate, rang)
pool.close()
pool.join()
else:
if rho>0.5:
self.log.printL("\nRunning GRAD in sequential " + \
"(non-parallel) mode...\n")
else:
self.log.printL("\nRunning REST in sequential " + \
"(non-parallel) mode...\n")
res = list()
for j in rang:
outp = helper.propagate(j)
res.append(outp)
#
#
A,B,C,lam,mu = helper.getCorr(res,self.log)
corr = {'x':A, 'u':B, 'pi':C}
# these are all non-essential for the algorithm itself
if rho > 0.5:
if self.save.get('eig',False):
helper.showEig(self.N,self.n,self.s)#,mustShow=True)
self.savefig(keyName='eig',fullName='eigenvalues')
if self.save.get('lambda', False):
self.plotSol(opt={'mode':'lambda'})
self.plotSol(opt={'mode':'lambda'},piIsTime=False)
BBvec = numpy.empty((self.N,self.s))
BB = 0.0
for arc in range(self.s):
for k in range(self.N):
BBvec[k,arc] = B[k,:,arc].transpose().dot(B[k,:,arc])
#
BB += simp(BBvec[:,arc],self.N)
#
CC = C.transpose().dot(C)
dJdStep = -BB-CC; corr['dJdStepTheo'] = dJdStep
self.log.printL("\nBB = {:.4E}".format(BB) + \
", CC = {:.4E},".format(CC) + \
" dJ/dAlfa = {:.4E}".format(dJdStep))
if self.save.get('var', False):
self.plotSol(opt={'mode':'var','x':A,'u':B,'pi':C})
self.plotSol(opt={'mode':'var','x':A,'u':B,'pi':C},
piIsTime=False)
#if self.NIterGrad > 380:
# raise Exception("Mandou parar, parei.")
#self.log.printL("\nWaiting 5.0 seconds for lambda/corrections check...")
#time.sleep(5.0)
#input("\n@Grad: Waiting for lambda/corrections check...")
#
return corr,lam,mu
def calcP(self, mustPlotPint=False):
N, s = self.N, self.s
psi = self.calcPsi()
#print("psi = "+str(psi))
func = self.calcErr()
vetP = numpy.empty((N, s))
vetIP = numpy.empty((N, s))
for arc in range(s):
for t in range(N):
vetP[t, arc] = func[t, :, arc].dot(func[t, :, arc].transpose())
coefList = simp([], N, onlyCoef=True)
for arc in range(s):
vetIP[0, arc] = coefList[0] * vetP[0, arc]
for t in range(1, N):
vetIP[t, arc] = vetIP[t - 1, arc] + coefList[t] * vetP[t, arc]
#
#vetIP *= self.dt # THIS IS WRONG!! REMOVE IT!!
# TEST FOR VIOLATIONS!
PiCondVio = False
piLowLim = self.restrictions['pi_min']
piHighLim = self.restrictions['pi_max']
for i in range(self.s):
# violated here in lower limit condition
if piLowLim[i] is not None and self.pi[i] < piLowLim[i]:
PiCondVio = True
break # already violated, no need to continue
# violated here in upper limit condition
if piHighLim[i] is not None and self.pi[i] > piHighLim[i]:
PiCondVio = True
break # already violated, no need to continue
#
if PiCondVio:
vetIP *= 1e300
# for arc in range(s):
# vetIP[0,arc] = (17.0/48.0) * vetP[0,arc]
# vetIP[1,arc] = vetIP[0,arc] + (59.0/48.0) * vetP[1,arc]
# vetIP[2,arc] = vetIP[1,arc] + (43.0/48.0) * vetP[2,arc]
# vetIP[3,arc] = vetIP[2,arc] + (49.0/48.0) * vetP[3,arc]
# for t in range(4,N-4):
# vetIP[t] = vetIP[t-1,arc] + vetP[t,arc]
# vetIP[N-4,arc] = vetIP[N-5,arc] + (49.0/48.0) * vetP[N-4,arc]
# vetIP[N-3,arc] = vetIP[N-4,arc] + (43.0/48.0) * vetP[N-3,arc]
# vetIP[N-2,arc] = vetIP[N-3,arc] + (59.0/48.0) * vetP[N-2,arc]
# vetIP[N-1,arc] = vetIP[N-2,arc] + (17.0/48.0) * vetP[N-1,arc]
# for arc in range(s):
# vetIP[0,arc] = coefList[0] * vetP[0,arc]
# for t in range(1,N):
# vetIP[t] = vetIP[t-1,arc] + coefList[t] * vetP[t,arc]
#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()
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(event #" + str(int((self.EvntIndx+1)/2)) + \
", 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 calcQ(self, mustPlotQs=False):
# Q expression from (15).
# FYI: Miele (2003) is wrong in oh so many ways...
self.log.printL("In calcQ.")
N, n, m, p, s = self.N, self.n, self.m, self.p, self.s
dt = 1.0 / (N - 1)
lam, mu = self.lam, 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']
Qx, Qu, Qp, Qt, Q = 0.0, 0.0, 0.0, 0.0, 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)
coefList = simp([], N, onlyCoef=True)
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]
# calculate Qx separately. In this way, the derivative avaliation is
# adequate with the trapezoidal integration method
med = (lam[1,:,arc]-lam[0,:,arc])/dt -.5*(fx[0,:,arc]+fx[1,:,arc]) + \
.5 * phix[0,:,:,arc].transpose().dot(lam[0,:,arc]) + \
.5 * phix[1,:,:,arc].transpose().dot(lam[1,:,arc])
errQx[0, :, arc] = med
errQx[1, :, arc] = med
for k in range(2, N):
errQx[k,:,arc] = 2.0 * (lam[k,:,arc]-lam[k-1,:,arc]) / dt + \
-fx[k,:,arc] - fx[k-1,:,arc] + \
phix[k,:,:,arc].transpose().dot(lam[k,:,arc]) + \
phix[k-1,:,:,arc].transpose().dot(lam[k-1,:,arc]) + \
-errQx[k-1,:,arc]
for k in range(N):
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] * coefList[k]
Qu += normErrQu[k, arc] * coefList[k]
auxVecIntQp[:, arc] += errQp[k, :, arc] * coefList[k]
#
#
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 getCorr(self, res, log):
""" Computes the actual correction for this grad/rest step, by linear
combination of the solutions generated by method 'propagate'."""
# Get sizes
Ns, N, n, m, p, q, s = self.Ns, self.N, self.n, self.m, self.p, self.q, self.s
# Declare matrices Ctilde, Dtilde, Etilde, and the integral term
if self.omit:
# Grad and omit: proceed to omit
NsRed = len(self.omitVarList)
q_ = q + NsRed - Ns - 1
omit = self.omitEqMat
else:
# nothing is omitted
NsRed = Ns + 1
q_ = q
Ct = numpy.empty((p, NsRed))
Dt = numpy.empty((2 * n * s, NsRed))
Et = numpy.empty((2 * n * s, NsRed))
phiLamInt = numpy.empty((p, NsRed))
# Unpack outputs from 'propagate' into proper matrices Ct, Dt, etc.
for j in range(NsRed):
Ct[:, j] = res[j]['C']
Dt[:, j] = res[j]['Dt']
Et[:, j] = res[j]['Et']
phiLamInt[:, j] = res[j]['phiLam']
# Assembly of matrix M and column 'Col' for the linear system
# Matrix for linear system involving k's and mu's
M = numpy.zeros((NsRed + q, NsRed + q))
# from eq (34d) - k term
M[0, :NsRed] = numpy.ones(NsRed)
# from eq (34b) - mu term
M[(q_ + 1):(q_ + 1 + p), NsRed:] = self.psip.transpose()
# from eq (34c) - mu term
M[(p + q_ + 1):, NsRed:] = self.psiy.transpose()
# from eq (34a) - k term
if self.omit:
# under omission, less equations are needed
M[1:(q_ +
1), :NsRed] = omit.dot(self.psiy.dot(Dt) + self.psip.dot(Ct))
else:
# no omission, all the equations are needed
M[1:(q_ + 1), :NsRed] = self.psiy.dot(Dt) + self.psip.dot(Ct)
# from eq (34b) - k term
M[(q_ + 1):(q_ + p + 1), :NsRed] = Ct - phiLamInt
# from eq (34c) - k term
M[(q_ + p + 1):, :NsRed] = Et
# column vector for linear system involving k's and mu's [eqs (34)]
col = numpy.zeros(NsRed + q)
col[0] = 1.0 # eq (34d)
# Integral term
if self.rho > 0.5:
# eq (34b) - only applicable for grad
# sumIntFpi = numpy.zeros(p)
# for arc in range(s):
# for ind in range(p):
# sumIntFpi[ind] += self.fp[:,ind,arc].sum()
# sumIntFpi[ind] -= .5 * ( self.fp[0,ind,arc] + \
# self.fp[-1,ind,arc])
# sumIntFpi *= self.dt
sumIntFpi = numpy.zeros(p)
for arc in range(s):
for ind in range(p):
sumIntFpi[ind] += simp(self.fp[:, ind, arc], N)
#
#
col[(q_ + 1):(q_ + p + 1)] = -sumIntFpi
else:
# eq (34a) - only applicable for rest
col[1:(q + 1)] = -self.psi
# Calculations of weights k:
KMi = numpy.linalg.solve(M, col)
Res = M.dot(KMi) - col
log.printL("LMPBVP: Residual of the Linear System: " + \
str(Res.transpose().dot(Res)))
K, mu = KMi[:NsRed], KMi[NsRed:]
log.printL("LMPBVP: coefficients of particular solutions: " + \
str(K))
# summing up linear combinations
A = numpy.zeros((N, n, s))
B = numpy.zeros((N, m, s))
C = numpy.zeros(p)
lam = numpy.zeros((N, n, s))
for j in range(NsRed):
A += K[j] * res[j]['A'] #self.arrayA[j,:,:,:]
B += K[j] * res[j]['B'] #self.arrayB[j,:,:,:]
C += K[j] * res[j]['C'] #self.arrayC[j,:]
lam += K[j] * res[j]['L'] #self.arrayL[j,:,:,:]
###############################################################################
if (self.rho > 0.5 and self.dbugOptGrad['plotCorrFin']) or \
(self.rho < 0.5 and self.dbugOptRest['plotCorrFin']):
log.printL(
"\n------------------------------------------------------------"
)
log.printL("Final corrections:\n")
for arc in range(s):
log.printL("> Corrections for arc =", arc)
plt.plot(self.t, A[:, 0, arc])
plt.grid(True)
plt.ylabel('A: pos')
plt.show()
plt.clf()
plt.close('all')
if n > 1:
plt.plot(self.t, A[:, 1, arc])
plt.grid(True)
plt.ylabel('A: vel')
plt.show()
plt.clf()
plt.close('all')
if n > 2:
plt.plot(self.t, A[:, 2, arc])
plt.grid(True)
plt.ylabel('A: gama')
plt.show()
plt.clf()
plt.close('all')
if n > 3:
plt.plot(self.t, A[:, 3, arc])
plt.grid(True)
plt.ylabel('A: m')
plt.show()
plt.clf()
plt.close('all')
plt.plot(self.t, B[:, 0, arc])
plt.grid(True)
plt.ylabel('B0')
plt.show()
plt.clf()
plt.close('all')
if m > 1:
plt.plot(self.t, B[:, 1, arc])
plt.grid(True)
plt.ylabel('B1')
plt.show()
plt.clf()
plt.close('all')
log.printL("C[arc] =", C[arc])
#input(" > ")
###############################################################################
return A, B, C, lam, mu
def propagate(self, j):
"""This method computes each solution, via propagation of the
applicable Linear System of Ordinary Differential Equations."""
# Load data (sizes, common matrices, etc)
rho = self.rho
rho1 = self.rho - 1.0
Ns, N, n, m, p, s = self.Ns, self.N, self.n, self.m, self.p, self.s
dt = self.dt
InitCondMat = self.InitCondMat
phip = self.phip
err = self.err
phiuFu = self.phiuFu
fx = self.fx
if rho > .5:
rhoFu = self.fu
else:
rhoFu = numpy.zeros((N, m, s))
phiuTr = self.phiuTr
phipTr = self.phipTr
DynMat = self.DynMat
I = numpy.eye(2 * n)
# Declare matrices for corrections
phiLamIntCol = numpy.zeros(p)
DtCol = numpy.empty(2 * n * s)
EtCol = numpy.empty(2 * n * s)
A = numpy.zeros((N, n, s))
B = numpy.zeros((N, m, s))
C = numpy.zeros((p, 1))
lam = numpy.zeros((N, n, s))
# the vector that will be integrated is Xi = [A; lam]
Xi = numpy.zeros((N, 2 * n, s))
# Initial conditions for the LSODE:
for arc in range(s):
A[0, :, arc] = InitCondMat[2 * n * arc:(2 * n * arc + n), j]
lam[0, :, arc] = InitCondMat[(2 * n * arc + n):(2 * n * (arc + 1)),
j]
Xi[0, :n, arc], Xi[0, n:, arc] = A[0, :, arc], lam[0, :, arc]
C = InitCondMat[(2 * n * s):, j]
# Non-homogeneous terms for the LSODE:
nonHom = numpy.empty((N, 2 * n, s))
for arc in range(s):
for k in range(N):
# minus sign in rho1 (rho-1) is on purpose!
nonHA = phip[k,:,:,arc].dot(C) + \
-rho1*err[k,:,arc] - rho*phiuFu[k,:,arc]
nonHL = rho * fx[k, :, arc]
nonHom[k, :n, arc] = nonHA #.copy()
nonHom[k, n:, arc] = nonHL #.copy()
# This command probably broke the compatibility with other integration
# methods. They weren't working anyway, so...
coefList = simp([], N, onlyCoef=True)
for arc in range(s):
if self.solver == 'heun':
###############################################################################
# Integrate the LSODE (by Heun's method):
B[0,:,arc] = -rhoFu[0,:,arc] + \
phiuTr[0,:,:,arc].dot(lam[0,:,arc])
phiLamIntCol += .5 * (phipTr[0, :, :, arc].dot(lam[0, :, arc]))
# First point: simple propagation
derXik = DynMat[0,:,:,arc].dot(Xi[0,:,arc]) + \
nonHom[0,:,arc]
Xi[1, :, arc] = Xi[0, :, arc] + dt * derXik
#A[1,:,arc] = Xi[1,:n,arc]
lam[1, :, arc] = Xi[1, n:, arc]
B[1,:,arc] = -rhoFu[1,:,arc] + \
phiuTr[1,:,:,arc].dot(lam[1,:,arc])
phiLamIntCol += phipTr[1, :, :, arc].dot(lam[1, :, arc])
# "Middle" points: original Heun propagation
for k in range(1, N - 2):
derXik = DynMat[k,:,:,arc].dot(Xi[k,:,arc]) + \
nonHom[k,:,arc]
aux = Xi[k, :, arc] + dt * derXik
Xi[k+1,:,arc] = Xi[k,:,arc] + .5 * dt * (derXik + \
DynMat[k+1,:,:,arc].dot(aux) + \
nonHom[k+1,:,arc])
#A[k+1,:,arc] = Xi[k+1,:n,arc]
lam[k + 1, :, arc] = Xi[k + 1, n:, arc]
B[k+1,:,arc] = -rhoFu[k+1,:,arc] + \
phiuTr[k+1,:,:,arc].dot(lam[k+1,:,arc])
phiLamIntCol += phipTr[k + 1, :, :, arc].dot(lam[k + 1, :,
arc])
#
# Last point: simple propagation, but based on the last point
derXik = DynMat[N-1,:,:,arc].dot(Xi[N-2,:,arc]) + \
nonHom[N-1,:,arc]
Xi[N - 1, :, arc] = Xi[N - 2, :, arc] + dt * derXik
#A[N-1,:,arc] = Xi[N-1,:n,arc]
lam[N - 1, :, arc] = Xi[N - 1, n:, arc]
B[N-1,:,arc] = -rhoFu[N-1,:,arc] + \
phiuTr[N-1,:,:,arc].dot(lam[N-1,:,arc])
phiLamIntCol += .5 * phipTr[N - 1, :, :, arc].dot(lam[N - 1, :,
arc])
###############################################################################
elif self.solver == 'trap':
# Integrate the LSODE by trapezoidal (implicit) method
B[0,:,arc] = -rhoFu[0,:,arc] + \
phiuTr[0,:,:,arc].dot(lam[0,:,arc])
phiLamIntCol += coefList[0] * \
(phipTr[0,:,:,arc].dot(lam[0,:,arc]))
for k in range(N - 1):
Xi[k+1,:,arc] = self.InvDynMat[k+1,:,:,arc].dot(
(I + .5 * dt * DynMat[k,:,:,arc]).dot(Xi[k,:,arc]) + \
.5 * dt * (nonHom[k+1,:,arc]+nonHom[k,:,arc]))
lam[k + 1, :, arc] = Xi[k + 1, n:, arc]
B[k+1,:,arc] = -rhoFu[k+1,:,arc] + \
phiuTr[k+1,:,:,arc].dot(lam[k+1,:,arc])
phiLamIntCol += coefList[k+1] * \
phipTr[k+1,:,:,arc].dot(lam[k+1,:,arc])
#
###############################################################################
elif self.solver == 'BEI':
# Integrate the LSODE by "original" Euler Backwards implicit
B[0,:,arc] = -rhoFu[0,:,arc] + \
phiuTr[0,:,:,arc].dot(lam[0,:,arc])
phiLamIntCol += .5 * (phipTr[0, :, :, arc].dot(lam[0, :, arc]))
for k in range(N - 1):
Xi[k+1,:,arc] = numpy.linalg.solve(I - dt*DynMat[k+1,:,:,arc],\
Xi[k,:,arc] + dt*nonHom[k+1,:,arc])
lam[k + 1, :, arc] = Xi[k + 1, n:, arc]
B[k+1,:,arc] = -rhoFu[k+1,:,arc] + \
phiuTr[k+1,:,:,arc].dot(lam[k+1,:,arc])
phiLamIntCol += phipTr[k + 1, :, :, arc].dot(lam[k + 1, :,
arc])
phiLamIntCol -= .5 * phipTr[N - 1, :, :, arc].dot(lam[N - 1, :,
arc])
###############################################################################
if self.solver == 'leapfrog':
# Integrate the LSODE by "leapfrog" with special start and end
# with special 1st step...
B[0,:,arc] = -rhoFu[0,:,arc] + \
phiuTr[0,:,:,arc].dot(lam[0,:,arc])
phiLamIntCol += coefList[0] * (phipTr[0, :, :, arc].dot(
lam[0, :, arc]))
Xi[1,:,arc] = Xi[0,:,arc] + dt * \
(DynMat[0,:,:,arc].dot(Xi[0,:,arc])+nonHom[0,:,arc])
lam[1, :, arc] = Xi[1, n:, arc]
B[1,:,arc] = -rhoFu[1,:,arc] + \
phiuTr[1,:,:,arc].dot(lam[1,:,arc])
phiLamIntCol += coefList[1] * \
phipTr[1,:,:,arc].dot(lam[1,:,arc])
for k in range(1, N - 2):
Xi[k+1,:,arc] = Xi[k-1,:,arc] + 2. * dt * \
(DynMat[k,:,:,arc].dot(Xi[k,:,arc]) + nonHom[k,:,arc])
lam[k + 1, :, arc] = Xi[k + 1, n:, arc]
B[k+1,:,arc] = -rhoFu[k+1,:,arc] + \
phiuTr[k+1,:,:,arc].dot(lam[k+1,:,arc])
phiLamIntCol += coefList[k+1] * \
phipTr[k+1,:,:,arc].dot(lam[k+1,:,arc])
# # with special last step...
Xi[N-1,:,arc] = numpy.linalg.solve(I - dt*DynMat[N-1,:,:,arc],\
Xi[N-2,:,arc] + dt*nonHom[N-1,:,arc])
# # with special last step...
# Xi[N-1,:,arc] = Xi[N-2,:,arc] + dt * \
# (DynMat[N-1,:,:,arc].dot(Xi[N-2,:,arc])+nonHom[N-1,:,arc])
# with special last step...
# derXik = DynMat[N-2,:,:,arc].dot(Xi[N-2,:,arc]) + \
# nonHom[N-2,:,arc]
# aux = Xi[N-2,:,arc] + dt * derXik
# Xi[N-1,:,arc] = Xi[N-2,:,arc] + .5 * dt * (derXik + \
# DynMat[N-1,:,:,arc].dot(aux) + \
# nonHom[N-1,:,arc])
lam[N - 1, :, arc] = Xi[N - 1, n:, arc]
B[N-1,:,arc] = -rhoFu[N-1,:,arc] + \
phiuTr[N-1,:,:,arc].dot(lam[N-1,:,arc])
phiLamIntCol += coefList[N-1] * \
phipTr[N-1,:,:,arc].dot(lam[N-1,:,arc])
###############################################################################
if self.solver == 'BEI_spec':
# Integrate the LSODE by Euler Backwards implicit,
# with special 1st step...
B[0,:,arc] = -rhoFu[0,:,arc] + \
phiuTr[0,:,:,arc].dot(lam[0,:,arc])
phiLamIntCol += coefList[0] * (phipTr[0, :, :, arc].dot(
lam[0, :, arc]))
Xi[1,:,arc] = Xi[0,:,arc] + dt * \
(DynMat[0,:,:,arc].dot(Xi[0,:,arc])+nonHom[0,:,arc])
lam[1, :, arc] = Xi[1, n:, arc]
B[1,:,arc] = -rhoFu[1,:,arc] + \
phiuTr[1,:,:,arc].dot(lam[1,:,arc])
phiLamIntCol += coefList[1] * \
phipTr[1,:,:,arc].dot(lam[1,:,arc])
for k in range(1, N - 1):
Xi[k+1,:,arc] = numpy.linalg.solve(I - dt*DynMat[k+1,:,:,arc],\
Xi[k,:,arc] + dt*nonHom[k+1,:,arc])
lam[k + 1, :, arc] = Xi[k + 1, n:, arc]
B[k+1,:,arc] = -rhoFu[k+1,:,arc] + \
phiuTr[k+1,:,:,arc].dot(lam[k+1,:,arc])
phiLamIntCol += coefList[k+1] * \
phipTr[k+1,:,:,arc].dot(lam[k+1,:,arc])
###############################################################################
if self.solver == 'hamming_mod':
# Integrate the LSODE by Hamming's mod predictor-corrector method
B[0,:,arc] = -rhoFu[0,:,arc] + \
phiuTr[0,:,:,arc].dot(lam[0,:,arc])
phiLamIntCol += coefList[0] * (phipTr[0, :, :, arc].dot(
lam[0, :, arc]))
# First points: RKF4
Xi[1,:,arc] = Xi[0,:,arc] + dt * \
(DynMat[0,:,:,arc].dot(Xi[0,:,arc]) + nonHom[0,:,arc])
lam[1, :, arc] = Xi[1, n:, arc]
B[1,:,arc] = -rhoFu[1,:,arc] + \
phiuTr[1,:,:,arc].dot(lam[1,:,arc])
phiLamIntCol += coefList[1] * \
phipTr[1,:,:,arc].dot(lam[1,:,arc])
for k in range(1, 3):
Xik = Xi[k, :, arc]
DM13 = DynMat[k, :, :, arc] * (
2. / 3.) + DynMat[k + 1, :, :, arc] * (1. / 3.)
NH13 = nonHom[k, :, arc] * (
2. / 3.) + nonHom[k + 1, :, arc] * (1. / 3.)
DM23 = DynMat[k, :, :, arc] * (
1. / 3.) + DynMat[k + 1, :, :, arc] * (2. / 3.)
NH23 = nonHom[k, :, arc] * (
1. / 3.) + nonHom[k + 1, :, arc] * (2. / 3.)
f1 = DynMat[k, :, :, arc].dot(Xi[k, :, arc]) + nonHom[k, :,
arc]
f2 = DM13.dot(Xik + (1. / 3.) * dt * f1) + NH13
f3 = DM23.dot(Xik + dt * (-(1. / 3.) * f1 + f2)) + NH23
f4 = DynMat[k + 1, :, :,
arc].dot(Xik + dt *
(f1 - f2 + f3)) + nonHom[k + 1, :,
arc]
Xi[k + 1, :,
arc] = Xik + dt * (f1 + 3. * f2 + 3. * f3 + f4) / 8.
# Xik = Xi[k,:,arc]
# DM14 = DynMat[k,:,:,arc]*.75 + DynMat[k+1,:,:,arc]*.25
# NH14 = nonHom[k,:,arc] * .75 + nonHom[k+1,:,arc] * .25
# DM38 = DynMat[k,:,:,arc]*.625 + DynMat[k+1,:,:,arc]*.375
# NH38 = nonHom[k,:,arc] * .625 + nonHom[k+1,:,arc] * .375
# DM12 = DynMat[k,:,:,arc]*.5 + DynMat[k+1,:,:,arc]*.5
# NH12 = nonHom[k,:,arc] * .5 + nonHom[k+1,:,arc] * .5
# DM1213 = DynMat[k,:,:,arc]*(1./13.) + DynMat[k+1,:,:,arc]*(12./13.)
# NH1213 = nonHom[k,:,arc] * (1./13.) + nonHom[k+1,:,arc] * (1./13.)
# f1 = DynMat[k,:,:,arc].dot(Xi[k,:,arc]) + nonHom[k,:,arc]
# f2 = DM14.dot(Xik+.25*dt*f1) + NH14
# f3 = DM38.dot(Xik + dt*(3.*f1 + 9.*f2)/32.) + NH38
# f4 = DM1213.dot(Xik + dt*(1932.*f1 - 7200.*f2 + 7296.)/2197.) + NH1213
# f5 = DynMat[k+1,:,:,arc].dot(Xik + dt*((439./216.)*f1-8.*f2+(3680./513.)*f3-(845./4104.)*f4)) + nonHom[k+1,:,arc]
# f6 = DM12.dot(Xik+dt*(-(8./27.)*f1 + 2.*f2 -(3544./2565.)*f3 +(1859./4104.)*f4 -(11./40.)*f5)) + NH12
#
# Xi[k+1,:,arc] = Xik + dt * ((16./135.)*f1 + \
# (6656./12825.)*f3 + \
# (28561./56430.)*f4 + \
# -(9./50.)*f5 + \
# (2./55.)*f6)
lam[k + 1, :, arc] = Xi[k + 1, n:, arc]
B[k+1,:,arc] = -rhoFu[k+1,:,arc] + \
phiuTr[k+1,:,:,arc].dot(lam[k+1,:,arc])
phiLamIntCol += coefList[k+1] * \
phipTr[k+1,:,:,arc].dot(lam[k+1,:,arc])
#
# Now, Hamming's...
pk = numpy.zeros_like(Xi[0, :, arc])
ck = pk
for k in range(3, N - 1):
fk = DynMat[k, :, :, arc].dot(Xi[k, :, arc]) + nonHom[k, :,
arc]
fkm1 = DynMat[k - 1, :, :, arc].dot(
Xi[k - 1, :, arc]) + nonHom[k - 1, :, arc]
fkm2 = DynMat[k - 2, :, :, arc].dot(
Xi[k - 2, :, arc]) + nonHom[k - 2, :, arc]
pkp1 = Xi[k-3,:,arc] + (4.0*dt/3.0) * \
(2.0 * fk - fkm1 + 2.0 * fkm2)
mkp1 = pkp1 + (112.0 / 121.0) * (ck - pk)
rdkp1 = DynMat[k + 1, :, :, arc].dot(mkp1)
ckp1 = (9.0/8.0) * Xi[k,:,arc] -(1.0/8.0) * Xi[k-2,:,arc] + \
(3.0*dt/8.0) * (rdkp1 + 2.0 * fk - fkm1)
Xi[k + 1, :, arc] = ckp1 + (9.0 / 121.0) * (pkp1 - ckp1)
lam[k + 1, :, arc] = Xi[k + 1, n:, arc]
B[k+1,:,arc] = -rhoFu[k+1,:,arc] + \
phiuTr[k+1,:,:,arc].dot(lam[k+1,:,arc])
phiLamIntCol += coefList[k+1] * \
phipTr[k+1,:,:,arc].dot(lam[k+1,:,arc])
pk, ck = pkp1, ckp1
#
###############################################################################
if self.solver == 'hamming':
# Integrate the LSODE by Hamming's predictor-corrector method
B[0,:,arc] = -rhoFu[0,:,arc] + \
phiuTr[0,:,:,arc].dot(lam[0,:,arc])
phiLamIntCol += coefList[0] * (phipTr[0, :, :, arc].dot(
lam[0, :, arc]))
# first point: simple propagation?
# Xi[1,:,arc] = numpy.linalg.solve(I - dt*DynMat[1,:,:,arc],\
# Xi[0,:,arc] + dt*nonHom[1,:,arc])
# First points: Heun...
for k in range(3):
Xi[k+1,:,arc] = numpy.linalg.solve(I - .5*dt*DynMat[k+1,:,:,arc],\
Xi[k,:,arc] + .5 * dt * \
(DynMat[k,:,:,arc].dot(Xi[k,:,arc]) + \
nonHom[k,:,arc] + nonHom[k+1,:,arc]))
# derXik = DynMat[k,:,:,arc].dot(Xi[k,:,arc]) + \
# nonHom[k,:,arc]
# aux = Xi[k,:,arc] + dt * derXik
# Xi[k+1,:,arc] = Xi[k,:,arc] + .5 * dt * (derXik + \
# DynMat[k+1,:,:,arc].dot(aux) + \
# nonHom[k+1,:,arc])
lam[k + 1, :, arc] = Xi[k + 1, n:, arc]
B[k+1,:,arc] = -rhoFu[k+1,:,arc] + \
phiuTr[k+1,:,:,arc].dot(lam[k+1,:,arc])
phiLamIntCol += coefList[k+1] * \
phipTr[k+1,:,:,arc].dot(lam[k+1,:,arc])
#
# Now, Hamming's...
for k in range(3, N - 1):
fk = DynMat[k, :, :, arc].dot(Xi[k, :, arc]) + nonHom[k, :,
arc]
fkm1 = DynMat[k - 1, :, :, arc].dot(
Xi[k - 1, :, arc]) + nonHom[k - 1, :, arc]
fkm2 = DynMat[k - 2, :, :, arc].dot(
Xi[k - 2, :, arc]) + nonHom[k - 2, :, arc]
Xikp1 = Xi[k-3,:,arc] + (4.0*dt/3.0) * \
(2.0 * fk - fkm1 + 2.0 * fkm2)
fkp1 = DynMat[k + 1, :, :,
arc].dot(Xikp1) + nonHom[k + 1, :, arc]
Xi[k+1,:,arc] = (9.0/8.0) * Xi[k,:,arc] + \
-(1.0/8.0) * Xi[k-2,:,arc] + (3.0*dt/8.0) * \
(fkp1 - fkm1 + 2.0 * fk)
lam[k + 1, :, arc] = Xi[k + 1, n:, arc]
B[k+1,:,arc] = -rhoFu[k+1,:,arc] + \
phiuTr[k+1,:,:,arc].dot(lam[k+1,:,arc])
phiLamIntCol += coefList[k+1] * \
phipTr[k+1,:,:,arc].dot(lam[k+1,:,arc])
#
###############################################################################
if self.solver == 'expm':
# Integrate the LSODE by matrix exponentiation
B[0,:,arc] = -rhoFu[0,:,arc] + \
phiuTr[0,:,:,arc].dot(lam[0,:,arc])
phiLamIntCol += .5 * (phipTr[0, :, :, arc].dot(lam[0, :, arc]))
for k in range(N - 1):
expDM = expm(DynMat[k, :, :, arc] * dt)
NHterm = expDM.dot(nonHom[k, :, arc]) - nonHom[k, :, arc]
Xi[k+1,:,arc] = expDM.dot(Xi[k,:,arc]) + \
numpy.linalg.solve(DynMat[k,:,:,arc], NHterm)
lam[k + 1, :, arc] = Xi[k + 1, n:, arc]
B[k+1,:,arc] = -rhoFu[k+1,:,arc] + \
phiuTr[k+1,:,:,arc].dot(lam[k+1,:,arc])
phiLamIntCol += phipTr[k + 1, :, :, arc].dot(lam[k + 1, :,
arc])
#
phiLamIntCol -= .5 * phipTr[N - 1, :, :, arc].dot(lam[N - 1, :,
arc])
###############################################################################
# Get the A values from Xi
A[:, :, arc] = Xi[:, :n, arc]
# Put initial and final conditions of A and Lambda into matrices
# DtCol and EtCol, which represent the columns of Dtilde(Dt) and
# Etilde(Et)
DtCol[(2 * arc) * n:(2 * arc + 1) * n] = A[0, :, arc] # eq (32a)
DtCol[(2 * arc + 1) * n:(2 * arc + 2) * n] = A[N - 1, :,
arc] # eq (32a)
EtCol[(2 * arc) * n:(2 * arc + 1) *
n] = -lam[0, :, arc] # eq (32b)
EtCol[(2 * arc + 1) * n:(2 * arc + 2) * n] = lam[N - 1, :,
arc] # eq (32b)
#
# All integrations ready!
# no longer used, because coefList from simp already includes dt
#phiLamIntCol *= dt
###############################################################################
if (rho > 0.5 and self.dbugOptGrad['plotCorr']) or \
(rho < 0.5 and self.dbugOptRest['plotCorr']):
print("\nHere are the corrections for iteration " + str(j+1) + \
" of " + str(Ns+1) + ":\n")
for arc in range(s):
print("> Corrections for arc =", arc)
plt.plot(self.t, A[:, 0, arc])
plt.grid(True)
plt.ylabel('A: pos')
plt.show()
plt.clf()
plt.close('all')
plt.plot(self.t, lam[:, 0, arc])
plt.grid(True)
plt.ylabel('lambda: pos')
plt.show()
plt.clf()
plt.close('all')
if n > 1:
plt.plot(self.t, A[:, 1, arc])
plt.grid(True)
plt.ylabel('A: vel')
plt.show()
plt.clf()
plt.close('all')
plt.plot(self.t, lam[:, 1, arc])
plt.grid(True)
plt.ylabel('lambda: vel')
plt.show()
plt.clf()
plt.close('all')
if n > 2:
plt.plot(self.t, A[:, 2, arc])
plt.grid(True)
plt.ylabel('A: gama')
plt.show()
plt.clf()
plt.close('all')
plt.plot(self.t, lam[:, 2, arc])
plt.grid(True)
plt.ylabel('lambda: gamma')
plt.show()
plt.clf()
plt.close('all')
if n > 3:
plt.plot(self.t, A[:, 3, arc])
plt.grid(True)
plt.ylabel('A: m')
plt.show()
plt.clf()
plt.close('all')
plt.plot(self.t, lam[:, 3, arc])
plt.grid(True)
plt.ylabel('lambda: m')
plt.show()
plt.clf()
plt.close('all')
plt.plot(self.t, B[:, 0, arc])
plt.grid(True)
plt.ylabel('B0')
plt.show()
plt.clf()
plt.close('all')
if m > 1:
plt.plot(self.t, B[:, 1, arc])
plt.grid(True)
plt.ylabel('B1')
plt.show()
plt.clf()
plt.close('all')
print("C[arc] =", C[arc])
#input(" > ")
###############################################################################
# All the outputs go to main output dictionary; the final solution is
# computed by the next method, 'getCorr'.
outp = {
'A': A,
'B': B,
'C': C,
'L': lam,
'Dt': DtCol,
'Et': EtCol,
'phiLam': phiLamIntCol
}
return outp