def calcADotRest(A, t, tVec, phixVec, phiuVec, phipVec, B, C, aux):
phixt = interpM(t, tVec, phixVec)
phiut = interpM(t, tVec, phiuVec)
phipt = interpM(t, tVec, phipVec)
auxt = interpV(t, tVec, aux)
Bt = interpV(t, tVec, B)
return phixt.dot(A) + phiut.dot(Bt) + phipt.dot(C) + auxt
def calcADotRest(A, t, tVec, phixVec, phiuVec, phipVec, B, C, aux):
#print("In calcADot!")
phixt = interpM(t, tVec, phixVec)
phiut = interpM(t, tVec, phiuVec)
phipt = interpM(t, tVec, phipVec)
auxt = interpV(t, tVec, aux)
Bt = interpV(t, tVec, B)
#print('phixt =',phixt)
#print('phiut =',phiut)
#print('Bt =',Bt)
return phixt.dot(A) + phiut.dot(Bt) + phipt.dot(C) + auxt
def mdlDer(x, t, tVec, alfaProg, betaProg, T, Isp, g0, R):
h, v, gama, M = x[0], x[1], x[2], x[3]
betat = interpV(t, tVec, betaProg)
alfat = interpV(t, tVec, alfaProg)
btm = betat * T / M
sinGama = numpy.sin(gama)
g = g0 * (R / (R + h))**2
return numpy.array([v*sinGama,\
btm*numpy.cos(alfat) - g*sinGama,\
btm*numpy.sin(alfat)/v + (v/(h+R)-g/v)*numpy.cos(gama),\
-btm*M/g0/Isp])
def calcADotGrad(A, t, tVec, phixVec, phiuVec, phipVec, B, C):
phixt = interpM(t, tVec, phixVec)
phiut = interpM(t, tVec, phiuVec)
phipt = interpM(t, tVec, phipVec)
Bt = interpV(t, tVec, B)
return phixt.dot(A) + phiut.dot(Bt) + phipt.dot(C)
def mdlDer(x, t, tVec, u1Prog, u2Prog, Thrust, Isp, grav_e, r_e, alpha_min,
alpha_max, beta_min, beta_max):
h, v, gama, M = x[0], x[1], x[2], x[3]
# betat = interpV(t,tVec,betaProg)
# alphat = interpV(t,tVec,alphaProg)
u1t = interpV(t, tVec, u1Prog)
u2t = interpV(t, tVec, u2Prog)
alphat = (alpha_max + alpha_min) / 2 + numpy.sin(u1t) * (alpha_max -
alpha_min) / 2
betat = (beta_max + beta_min) / 2 + numpy.sin(u2t) * (beta_max -
beta_min) / 2
btm = betat * Thrust / M
sinGama = numpy.sin(gama)
g = grav_e * (r_e / (r_e + h))**2
return numpy.array([v*sinGama,\
btm*numpy.cos(alphat) - g*sinGama,\
btm*numpy.sin(alphat)/v + (v/(h+r_e)-g/v)*numpy.cos(gama),\
-btm*M/grav_e/Isp])
def calcLamDotGrad(lam, t, tVec, fxInv, phixInv):
fxt = interpV(t, tVec, fxInv)
phixt = interpM(t, tVec, phixInv)
return phixt.dot(lam) - fxt
def calcADotOdeRest(A, t, tVec, phixVec, aux):
phixt = interpM(t, tVec, phixVec)
auxt = interpV(t, tVec, aux)
return phixt.dot(A) + auxt