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