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)
# 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
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 rest(sizes,x,u,pi,t,constants,boundary,restrictions,mustPlot=False):
#print("\nIn rest.")
# get sizes
N = sizes['N']
n = sizes['n']
m = sizes['m']
p = sizes['p']
q = sizes['q']
#print("Calc phi...")
# calculate phi and psi
phi = calcPhi(sizes,x,u,pi,constants,restrictions)
#print("Calc psi...")
psi = calcPsi(sizes,x,boundary)
# aux: phi - dx/dt
err = phi - ddt(sizes,x)
# get gradients
#print("Calc grads...")
Grads = calcGrads(sizes,x,u,pi,constants,restrictions)
dt = Grads['dt']
#dt6 = dt/6
phix = Grads['phix']
phiu = Grads['phiu']
phip = Grads['phip']
psix = Grads['psix']
psip = Grads['psip']
#print("Preparing matrices...")
psixTr = psix.transpose()
phixInv = phix.copy()
phiuTr = numpy.empty((N,m,n))
phipTr = numpy.empty((N,p,n))
psipTr = psip.transpose()
for k in range(N):
phixInv[k,:,:] = phix[N-k-1,:,:].transpose()
phiuTr[k,:,:] = phiu[k,:,:].transpose()
phipTr[k,:,:] = phip[k,:,:].transpose()
mu = numpy.zeros(q)
# Matrix for linear system involving k's
M = numpy.ones((q+1,q+1))
# column vector for linear system involving k's [eqs (88-89)]
col = numpy.zeros(q+1)
col[0] = 1.0 # eq (88)
col[1:] = -psi # eq (89)
arrayA = numpy.empty((q+1,N,n))
arrayB = numpy.empty((q+1,N,m))
arrayC = numpy.empty((q+1,p))
arrayL = arrayA.copy()
arrayM = numpy.empty((q+1,q))
optPlot = dict()
#print("Beginning loop for solutions...")
for i in range(q+1):
# print("\nIntegrating solution "+str(i+1)+" of "+str(q+1)+"...\n")
mu = 0.0*mu
if i<q:
mu[i] = 1.0e-10
# integrate equation (75-2) backwards
auxLamInit = - psixTr.dot(mu)
auxLam = numpy.empty((N,n))
auxLam[0,:] = auxLamInit
# Euler implicit
I = numpy.eye(n)
for k in range(N-1):
auxLam[k+1,:] = numpy.linalg.solve(I-dt*phixInv[k+1,:],auxLam[k,:])
# Euler's method
# for k in range(N-1):
# auxLam[k+1,:] = auxLam[k,:] + dt * phixInv[k,:,:].dot(auxLam[k,:])
# Heun's method
#for k in range(N-1):
# derk = phixInv[k,:,:].dot(auxLam[k,:])
# aux = auxLam[k,:] + dt*derk
# auxLam[k+1,:] = auxLam[k,:] + \
# .5*dt*(derk + phixInv[k+1,:,:].dot(aux))
# RK4 method (with interpolation...)
# for k in range(N-1):
# phixInv_k = phixInv[k,:,:]
# phixInv_kp1 = phixInv[k+1,:,:]
# phixInv_kpm = .5*(phixInv_k+phixInv_kp1)
# k1 = phixInv_k.dot(auxLam[k,:])
# k2 = phixInv_kpm.dot(auxLam[k,:]+.5*dt*k1)
# k3 = phixInv_kpm.dot(auxLam[k,:]+.5*dt*k2)
# k4 = phixInv_kp1.dot(auxLam[k,:]+dt*k3)
# auxLam[k+1,:] = auxLam[k,:] + dt6 * (k1+k2+k2+k3+k3+k4)
# equation for Bi (75-3)
B = numpy.empty((N,m))
lam = 0*x
for k in range(N):
lam[k,:] = auxLam[N-k-1,:]
B[k,:] = phiuTr[k,:,:].dot(lam[k,:])
##################################################################
# TESTING LAMBDA DIFFERENTIAL EQUATION
dlam = ddt(sizes,lam)
erroLam = numpy.empty((N,n))
normErroLam = numpy.empty(N)
for k in range(N):
erroLam[k,:] = dlam[k,:]+phix[k,:,:].transpose().dot(lam[k,:])
normErroLam[k] = erroLam[k,:].transpose().dot(erroLam[k,:])
maxNormErroLam = normErroLam.max()
print("maxNormErroLam =",maxNormErroLam)
#print("\nLambda Error:")
#
#optPlot['mode'] = 'states:LambdaError'
#plotSol(sizes,t,erroLam,numpy.zeros((N,m)),numpy.zeros(p),\
# constants,restrictions,optPlot)
#optPlot['mode'] = 'states:LambdaError (zoom)'
#N1 = 0#int(N/100)-10
#N2 = 20##N1+20
#plotSol(sizes,t[N1:N2],erroLam[N1:N2,:],numpy.zeros((N2-N1,m)),\
# numpy.zeros(p),constants,restrictions,optPlot)
#plt.semilogy(normErroLam)
#plt.grid()
#plt.title("ErroLam")
#plt.show()
#plt.semilogy(normErroLam[N1:N2])
#plt.grid()
#plt.title("ErroLam (zoom)")
#plt.show()
##################################################################
scal = 1.0/((numpy.absolute(B)).max())
lam *= scal
mu *= scal
B *= scal
# equation for Ci (75-4)
C = numpy.zeros(p)
for k in range(1,N-1):
C += phipTr[k,:,:].dot(lam[k,:])
C += .5*(phipTr[0,:,:].dot(lam[0,:]))
C += .5*(phipTr[N-1,:,:].dot(lam[N-1,:]))
C *= dt
C -= -psipTr.dot(mu)
# optPlot['mode'] = 'states:Lambda'
# plotSol(sizes,t,lam,B,C,constants,restrictions,optPlot)
#
# optPlot['mode'] = 'states:Lambda (zoom)'
# plotSol(sizes,t[N1:N2],lam[N1:N2,:],B[N1:N2,:],C,constants,restrictions,optPlot)
#print("Integrating ODE for A ["+str(i)+"/"+str(q)+"] ...")
# integrate equation for A:
A = numpy.zeros((N,n))
for k in range(N-1):
derk = phix[k,:,:].dot(A[k,:]) + phiu[k,:,:].dot(B[k,:]) + \
phip[k,:,:].dot(C) + err[k,:]
aux = A[k,:] + dt*derk
A[k+1,:] = A[k,:] + .5*dt*(derk + \
phix[k+1,:,:].dot(aux) + \
phiu[k+1,:,:].dot(B[k+1,:]) + \
phip[k+1,:,:].dot(C) + \
err[k+1,:])
# for k in range(N-1):
# phix_k = phix[k,:,:]
# phix_kp1 = phix[k+1,:,:]
# phix_kpm = .5*(phix_k+phix_kp1)
# add_k = phiu[k,:,:].dot(B[k,:]) + phip[k,:,:].dot(C) + err[k,:]
# add_kp1 = phiu[k+1,:,:].dot(B[k+1,:]) + phip[k+1,:,:].dot(C) + err[k+1,:]
# add_kpm = .5*(add_k+add_kp1)
#
# k1 = phix_k.dot(A[k,:]) + add_k
# k2 = phix_kpm.dot(A[k,:]+.5*dt*k1) + add_kpm
# k3 = phix_kpm.dot(A[k,:]+.5*dt*k2) + add_kpm
# k4 = phix_kp1.dot(A[k,:]+dt*k3) + add_kp1
# A[k+1,:] = A[k,:] + dt6 * (k1+k2+k2+k3+k3+k4)
else:
# integrate equation (75-2) backwards
lam *= 0.0
# equation for Bi (75-3)
B *= 0.0
# equation for Ci (75-4)
C *= 0.0
#print("Integrating ODE for A ["+str(i)+"/"+str(q)+"] ...")
# integrate equation for A:
A = numpy.zeros((N,n))
for k in range(N-1):
derk = phix[k,:,:].dot(A[k,:]) + err[k,:]
aux = A[k,:] + dt*derk
A[k+1,:] = A[k,:] + .5*dt*(derk + \
phix[k+1,:,:].dot(aux) + err[k+1,:])
# for k in range(N-1):
# phix_k = phix[k,:,:]
# phix_kp1 = phix[k+1,:,:]
# phix_kpm = .5*(phix_k+phix_kp1)
# add_k = err[k,:]
# add_kp1 = err[k+1,:]
# add_kpm = .5*(add_k+add_kp1)
#
# k1 = phix_k.dot(A[k,:]) + add_k
# k2 = phix_kpm.dot(A[k,:]+.5*dt*k1) + add_kpm
# k3 = phix_kpm.dot(A[k,:]+.5*dt*k2) + add_kpm
# k4 = phix_kp1.dot(A[k,:]+dt*k3) + add_kp1
# A[k+1,:] = A[i,:] + dt6 * (k1+k2+k2+k3+k3+k4)
#
# store solution in arrays
arrayA[i,:,:] = A
arrayB[i,:,:] = B
arrayC[i,:] = C
arrayL[i,:,:] = lam
arrayM[i,:] = mu
#optPlot['mode'] = 'var'
#plotSol(sizes,t,A,B,C,constants,restrictions,optPlot)
# Matrix for linear system (89)
M[1:,i] = psix.dot(A[N-1,:]) + psip.dot(C)
#
# Calculations of weights k:
print("M =",M)
#print("col =",col)
K = numpy.linalg.solve(M,col)
print("K =",K)
# summing up linear combinations
A = 0.0*A
B = 0.0*B
C = 0.0*C
lam = 0.0*lam
mu = 0.0*mu
for i in range(q+1):
A += K[i]*arrayA[i,:,:]
B += K[i]*arrayB[i,:,:]
C += K[i]*arrayC[i,:]
lam += K[i]*arrayL[i,:,:]
mu += K[i]*arrayM[i,:]
if mustPlot:
optPlot['mode'] = 'var'
plotSol(sizes,t,A,B,C,constants,restrictions,optPlot)
optPlot['mode'] = 'proposed (states: lambda)'
plotSol(sizes,t,lam,B,C,constants,restrictions,optPlot)
#print("Calculating step...")
alfa = calcStepRest(sizes,t,x,u,pi,A,B,C,constants,boundary,restrictions,mustPlot)
nx = x + alfa * A
nu = u + alfa * B
np = pi + alfa * C
print("Leaving rest with alfa =",alfa)
return nx,nu,np,lam,mu
def rest(sizes, x, u, pi, t, constants, boundary, restrictions):
#print("\nIn rest.")
# get sizes
N = sizes['N']
n = sizes['n']
m = sizes['m']
p = sizes['p']
q = sizes['q']
#print("Calc phi...")
# calculate phi and psi
phi = calcPhi(sizes, x, u, pi, constants, restrictions)
#print("Calc psi...")
psi = calcPsi(sizes, x, boundary)
# aux: phi - dx/dt
err = phi - ddt(sizes, x)
# get gradients
#print("Calc grads...")
Grads = calcGrads(sizes, x, u, pi, constants, restrictions)
dt = Grads['dt']
# dt6 = dt/6
phix = Grads['phix']
phiu = Grads['phiu']
phip = Grads['phip']
psix = Grads['psix']
psip = Grads['psip']
#print("Preparing matrices...")
psixTr = psix.transpose()
phixInv = phix.copy()
phiuTr = numpy.empty((N, m, n))
phipTr = numpy.empty((N, p, n))
psipTr = psip.transpose()
for k in range(N):
phixInv[k, :, :] = phix[N - k - 1, :, :].transpose()
phiuTr[k, :, :] = phiu[k, :, :].transpose()
phipTr[k, :, :] = phip[k, :, :].transpose()
mu = numpy.zeros(q)
# Matrix for linear system involving k's
M = numpy.ones((q + 1, q + 1))
# column vector for linear system involving k's [eqs (88-89)]
col = numpy.zeros(q + 1)
col[0] = 1.0 # eq (88)
col[1:] = -psi # eq (89)
arrayA = numpy.empty((q + 1, N, n))
arrayB = numpy.empty((q + 1, N, m))
arrayC = numpy.empty((q + 1, p))
arrayL = arrayA.copy()
arrayM = numpy.empty((q + 1, q))
optPlot = dict()
#print("Beginning loop for solutions...")
for i in range(q + 1):
print("\nIntegrating solution " + str(i + 1) + " of " + str(q + 1) +
"...\n")
mu = 0.0 * mu
if i < q:
mu[i] = 1.0
# integrate equation (75-2) backwards
auxLamInit = -psixTr.dot(mu)
auxLam = numpy.empty((N, n))
auxLam[0, :] = auxLamInit
for k in range(N - 1):
derk = phixInv[k, :, :].dot(auxLam[k, :])
aux = auxLam[k, :] + dt * derk
auxLam[k+1,:] = auxLam[k,:] + \
.5*dt*(derk + phixInv[k+1,:,:].dot(aux))
# for k in range(N-1):
# phixInv_k = phixInv[k,:,:]
# phixInv_kp1 = phixInv[k+1,:,:]
# phixInv_kpm = .5*(phixInv_k+phixInv_kp1)
# k1 = phixInv_k.dot(auxLam[k,:])
# k2 = phixInv_kpm.dot(auxLam[k,:]+.5*dt*k1)
# k3 = phixInv_kpm.dot(auxLam[k,:]+.5*dt*k2)
# k4 = phixInv_kp1.dot(auxLam[k,:]+dt*k3)
# auxLam[k+1,:] = auxLam[k,:] + dt6 * (k1+k2+k2+k3+k3+k4)
# equation for Bi (75-3)
B = numpy.empty((N, m))
lam = 0 * x
for k in range(N):
lam[k, :] = auxLam[N - k - 1, :]
B[k, :] = phiuTr[k, :, :].dot(lam[k, :])
scal = 1.0 / ((numpy.absolute(B)).max())
lam *= scal
mu *= scal
B *= scal
# equation for Ci (75-4)
C = numpy.zeros(p)
for k in range(1, N - 1):
C += phipTr[k, :, :].dot(lam[k, :])
C += .5 * (phipTr[0, :, :].dot(lam[0, :]))
C += .5 * (phipTr[N - 1, :, :].dot(lam[N - 1, :]))
C *= dt
C -= -psipTr.dot(mu)
optPlot['mode'] = 'states:Lambda'
plotSol(sizes, t, lam, B, C, constants, restrictions, optPlot)
optPlot['mode'] = 'states:Lambda (zoom)'
plotSol(sizes, t[0:10], lam[0:10, :], B[0:10, :], C, constants,
restrictions, optPlot)
#print("Integrating ODE for A ["+str(i)+"/"+str(q)+"] ...")
# integrate equation for A:
A = numpy.zeros((N, n))
for k in range(N - 1):
derk = phix[k,:,:].dot(A[k,:]) + phiu[k,:,:].dot(B[k,:]) + \
phip[k,:,:].dot(C) + err[k,:]
aux = A[k, :] + dt * derk
A[k+1,:] = A[k,:] + .5*dt*(derk + \
phix[k+1,:,:].dot(aux) + \
phiu[k+1,:,:].dot(B[k+1,:]) + \
phip[k+1,:,:].dot(C) + \
err[k+1,:])
# for k in range(N-1):
# phix_k = phix[k,:,:]
# phix_kp1 = phix[k+1,:,:]
# phix_kpm = .5*(phix_k+phix_kp1)
# add_k = phiu[k,:,:].dot(B[k,:]) + phip[k,:,:].dot(C) + err[k,:]
# add_kp1 = phiu[k+1,:,:].dot(B[k+1,:]) + phip[k+1,:,:].dot(C) + err[k+1,:]
# add_kpm = .5*(add_k+add_kp1)
#
# k1 = phix_k.dot(A[k,:]) + add_k
# k2 = phix_kpm.dot(A[k,:]+.5*dt*k1) + add_kpm
# k3 = phix_kpm.dot(A[k,:]+.5*dt*k2) + add_kpm
# k4 = phix_kp1.dot(A[k,:]+dt*k3) + add_kp1
# A[k+1,:] = A[k,:] + dt6 * (k1+k2+k2+k3+k3+k4)
else:
# integrate equation (75-2) backwards
lam *= 0.0
# equation for Bi (75-3)
B *= 0.0
# equation for Ci (75-4)
C *= 0.0
#print("Integrating ODE for A ["+str(i)+"/"+str(q)+"] ...")
# integrate equation for A:
A = numpy.zeros((N, n))
for k in range(N - 1):
derk = phix[k, :, :].dot(A[k, :]) + err[k, :]
aux = A[k, :] + dt * derk
A[k+1,:] = A[k,:] + .5*dt*(derk + \
phix[k+1,:,:].dot(aux) + err[k+1,:])
# for k in range(N-1):
# phix_k = phix[k,:,:]
# phix_kp1 = phix[k+1,:,:]
# phix_kpm = .5*(phix_k+phix_kp1)
# add_k = err[k,:]
# add_kp1 = err[k+1,:]
# add_kpm = .5*(add_k+add_kp1)
#
# k1 = phix_k.dot(A[k,:]) + add_k
# k2 = phix_kpm.dot(A[k,:]+.5*dt*k1) + add_kpm
# k3 = phix_kpm.dot(A[k,:]+.5*dt*k2) + add_kpm
# k4 = phix_kp1.dot(A[k,:]+dt*k3) + add_kp1
# A[k+1,:] = A[i,:] + dt6 * (k1+k2+k2+k3+k3+k4)
#
# store solution in arrays
arrayA[i, :, :] = A
arrayB[i, :, :] = B
arrayC[i, :] = C
arrayL[i, :, :] = lam
arrayM[i, :] = mu
optPlot['mode'] = 'var'
plotSol(sizes, t, A, B, C, constants, restrictions, optPlot)
# Matrix for linear system (89)
M[1:, i] = psix.dot(A[N - 1, :]) + psip.dot(C)
#
# Calculations of weights k:
print("M =", M)
print("col =", col)
K = numpy.linalg.solve(M, col)
print("K =", K)
# summing up linear combinations
A = 0.0 * A
B = 0.0 * B
C = 0.0 * C
lam = 0.0 * lam
mu = 0.0 * mu
for i in range(q + 1):
A += K[i] * arrayA[i, :, :]
B += K[i] * arrayB[i, :, :]
C += K[i] * arrayC[i, :]
lam += K[i] * arrayL[i, :, :]
mu += K[i] * arrayM[i, :]
optPlot['mode'] = 'var'
plotSol(sizes, t, A, B, C, constants, restrictions, optPlot)
optPlot['mode'] = 'states: lambda'
plotSol(sizes, t, lam, B, C, constants, restrictions, optPlot)
#print("Calculating step...")
alfa = calcStepRest(sizes, t, x, u, pi, A, B, C, constants, boundary,
restrictions)
nx = x + alfa * A
nu = u + alfa * B
np = pi + alfa * C
print("Leaving rest with alfa =", alfa)
return nx, nu, np, lam, mu
# psip = Grads['psip']
print("\nProposed initial guess:\n")
P, Pint, Ppsi = calcP(sizes, x, u, pi, constants, boundary, restrictions,
True)
print("P = {:.4E}".format(P)+", Pint = {:.4E}".format(Pint)+\
", Ppsi = {:.4E}".format(Ppsi)+"\n")
Q = calcQ(sizes, x, u, pi, lam, mu, constants, restrictions)
optPlot = dict()
optPlot['P'] = P
optPlot['Q'] = Q
optPlot['mode'] = 'sol'
optPlot['dispP'] = True
optPlot['dispQ'] = False
plotSol(sizes, t, x, u, pi, constants, restrictions, optPlot)
psi = calcPsi(sizes, x, boundary)
print("psi =", psi)
#input("Everything ok?")
tolP = tol['P']
tolQ = tol['Q']
MaxIterRest = 10000
histP = numpy.zeros(MaxIterRest)
histPint = histP.copy()
histPpsi = histP.copy()
uman = numpy.linspace(0, MaxIterRest, MaxIterRest + 1) #um a n
# first restoration rounds:
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 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