def calcXdot(sizes, t, x, u, constants, restrictions):
n = sizes['n']
grav_e = constants['grav_e']
Thrust = constants['Thrust']
Isp = constants['Isp']
r_e = constants['r_e']
GM = constants['GM']
CL0 = constants['CL0']
CL1 = constants['CL1']
CD0 = constants['CD0']
CD2 = constants['CD2']
s_ref = constants['s_ref']
DampCent = constants['DampCent']
DampSlop = constants['DampSlop']
#alpha_min = restrictions['alpha_min']
#alpha_max = restrictions['alpha_max']
#beta_min = restrictions['beta_min']
#beta_max = restrictions['beta_max']
sin = numpy.sin
cos = numpy.cos
u1 = u[0]
u2 = u[1]
# calculate variables alpha and beta
alpha = u1 #(alpha_max + alpha_min)/2 + tanh(u1)*(alpha_max - alpha_min)/2
beta = u2 #(beta_max + beta_min)/2 + tanh(u2)*(beta_max - beta_min)/2
# calculate variables CL and CD
CL = CL0 + CL1 * alpha
CD = CD0 + CD2 * (alpha)**2
# calculate L and D
dens = rho(x[0])
pDynTimesSref = .5 * dens * (x[1]**2) * s_ref
L = CL * pDynTimesSref
D = CD * pDynTimesSref
# calculate r
r = r_e + x[0]
# calculate grav
grav = GM / r / r
# calculate phi:
dx = numpy.empty(n)
# example rocket single stage to orbit with Lift and Drag
sinGama = sin(x[2])
dx[0] = x[1] * sinGama
dx[1] = (beta * Thrust * cos(alpha) - D) / x[3] - grav * sinGama
dx[2] = (beta * Thrust * sin(alpha) + L)/(x[3] * x[1]) + \
cos(x[2]) * ( x[1]/r - grav/x[1] )
dx[2] *= .5 * (1.0 + numpy.tanh(DampSlop * (t - DampCent)))
dx[3] = -(beta * Thrust) / (grav_e * Isp)
#if t < 3.0:
# print(t)
# dx[2] = 0.0
return dx
def __calcAedTab(self, tt, xx, uu) -> tuple:
con = self.con
LL = tt.copy()
DD = tt.copy()
CCL = tt.copy()
CCD = tt.copy()
QQ = tt.copy()
for ii in range(0, len(tt)):
h = xx[ii, 0]
v = xx[ii, 1]
alfat = uu[ii, 0]
# Aerodynamics
CL = con['CL0'] + con['CL1'] * alfat
CD = con['CD0'] + con['CD2'] * (alfat**2)
qdin = 0.5 * rho(h) * (v**2)
L = qdin * con['s_ref'] * CL
D = qdin * con['s_ref'] * CD
LL[ii] = L
DD[ii] = D
CCL[ii] = CL
CCD[ii] = CD
QQ[ii] = qdin
return LL, DD, CCL, CCD, QQ
def mdlDer(t: float, x: list, alfaProg: callable, betaProg: callable,
aed: callable, earth: callable) -> list:
# initialization
h = x[0]
v = x[1]
gamma = x[2]
M = x[3]
if numpy.isnan(h):
print('t: ', t)
# print('x: ', x)
raise Exception('itsme saying: h is not a number')
# Alpha control calculation
alfat = alfaProg(t)
# other calculations
# btm = betaProg(t)*con['T']/M
beta, Isp, T = betaProg(t)
btm = beta * T / M
sinGamma = numpy.sin(gamma)
g = earth.g0 * (earth.R / (earth.R + h))**2 - (earth.we**2) * (earth.R + h)
# aerodynamics
qdinSrefM = 0.5 * rho(h) * (v**2) * aed.s_ref / M
LM = qdinSrefM * (aed.CL0 + aed.CL1 * alfat)
DM = qdinSrefM * (aed.CD0 + aed.CD2 * (alfat**2))
if v < 1e-8:
v = 1e-8
# states derivatives
return [
v * sinGamma, # coefficient
btm * numpy.cos(alfat) - g * sinGamma - DM, # coefficient
btm * numpy.sin(alfat) / v +
(v / (h + earth.R) - g / v) * numpy.cos(gamma) + (LM / v) +
2 * earth.we, # coefficient
-btm * M / (earth.g0 * Isp)
] # coefficient
def calcGrads(sizes, x, u, pi, constants, restrictions):
Grads = dict()
N = sizes['N']
n = sizes['n']
m = sizes['m']
p = sizes['p']
#q = sizes['q']
#N0 = sizes['N0']
# Pre-assign functions
sin = numpy.sin
cos = numpy.cos
tanh = numpy.tanh
array = numpy.array
grav_e = constants['grav_e']
Thrust = constants['Thrust']
Isp = constants['Isp']
scal = constants['costScalingFactor']
r_e = constants['r_e']
GM = constants['GM']
s_f = constants['s_f']
CL0 = constants['CL0']
CL1 = constants['CL1']
CD0 = constants['CD0']
CD2 = constants['CD2']
s_ref = constants['s_ref']
DampCent = constants['DampCent']
DampSlop = constants['DampSlop']
alpha_min = restrictions['alpha_min']
alpha_max = restrictions['alpha_max']
beta_min = restrictions['beta_min']
beta_max = restrictions['beta_max']
u1 = u[:, 0]
u2 = u[:, 1]
Grads['dt'] = 1.0 / (N - 1)
phix = numpy.zeros((N, n, n))
phiu = numpy.zeros((N, n, m))
if p > 0:
phip = numpy.zeros((N, n, p))
else:
phip = numpy.zeros((N, n, 1))
fx = numpy.zeros((N, n))
fu = numpy.zeros((N, m))
fp = numpy.zeros((N, p))
# Gradients from example rocket single stage to orbit with Lift and Drag
psix = array([[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0],
[0.0, 0.0, 1.0, 0.0]])
psip = array([[0.0], [0.0], [0.0]])
# Calculate variables (arrays) alpha and beta
aExp = .5 * (alpha_max - alpha_min)
alpha = (alpha_max + alpha_min) / 2 + tanh(u1) * aExp
bExp = .5 * (beta_max - beta_min)
beta = (beta_max + beta_min) / 2 + tanh(u2) * bExp
# calculate variables CL and CD
CL = CL0 + CL1 * alpha
CD = CD0 + CD2 * (alpha)**2
# calculate L and D; atmosphere: numerical gradient
dens = numpy.empty(N)
del_rho = numpy.empty(N)
for k in range(N):
dens[k] = rho(x[k, 0])
del_rho[k] = (rho(x[k, 0] + .1) - dens[k]) / .1
pDynTimesSref = .5 * dens * (x[:, 1]**2) * s_ref
L = CL * pDynTimesSref
D = CD * pDynTimesSref
# calculate r
r = r_e + x[:, 0]
# calculate grav
grav = GM / r / r
#==============================================================================
for k in range(N):
sinGama = sin(x[k, 2])
cosGama = cos(x[k, 2])
sinAlpha = sin(alpha[k])
cosAlpha = cos(alpha[k])
#cosu1 = cos(u1[k])
#cosu2 = cos(u2[k])
r2 = r[k]**2
r3 = r2 * r[k]
V = x[k, 1]
V2 = V * V
m = x[k, 3]
m2 = m * m
fVel = beta[k] * Thrust * cosAlpha - D[
k] # forces on velocity direction
fNor = beta[k] * Thrust * sinAlpha + L[k] # forces normal to velocity
fdg = .5 * (1.0 + numpy.tanh(DampSlop * (k * pi[0] /
(N - 1) - DampCent)))
# print("k =",k,", fdg =",fdg)
# input("?")
# Expanded notation:
DAlfaDu1 = aExp * (1 - tanh(u1[k])**2)
DBetaDu2 = bExp * (1 - tanh(u2[k])**2)
# if k < N0:# calculated for 3s of no maneuver
# phix[k,:,:] = pi[0]*array([[0.0 ,sinGama ,V*cosGama ,0.0 ],
# [2*GM*sinGama/r3 - (0.5*CD[k]*del_rho[k]*s_ref*V2)/m ,-CD[k]*dens[k]*s_ref*V/m ,-grav[k]*cosGama ,-fVel/m2 ],
# [0.0 ,0.0 ,0.0 ,0.0 ],
# [0.0 ,0.0 ,0.0 ,0.0 ]])
#
# phiu[k,:,:] = pi[0]*array([[0.0 ,0.0 ],
# [-beta[k]*Thrust*sinAlpha*DAlfaDu1/m ,Thrust*cosAlpha*DBetaDu2/m ],
# [0.0 ,0.0 ],
# [0.0 ,-Thrust*DBetaDu2/(grav_e*Isp)]])
#
# else:
# phix[k,:,:] = pi[0]*array([[0.0 ,sinGama ,V*cosGama ,0.0 ],
# [2*GM*sinGama/r3 - (0.5*CD[k]*del_rho[k]*s_ref*V2)/m ,-CD[k]*dens[k]*s_ref*V/m ,-grav[k]*cosGama ,-fVel/m2 ],
# [cosGama*(-V/r2+2*GM/(V*r3)) + (0.5*CL[k]*del_rho[k]*s_ref*V)/m ,-beta[k]*Thrust*sinAlpha/(m*V2) + cosGama*((1/r[k])+grav[k]/(V2)) + 0.5*CL[k]*dens[k]*s_ref/m ,-sinGama*((V/r[k])-grav[k]/V) ,-fNor/(m2*V) ],
# [0.0 ,0.0 ,0.0 ,0.0 ]])
#
# phiu[k,:,:] = pi[0]*array([[0.0 ,0.0 ],
# [(-beta[k]*Thrust*sinAlpha*DAlfaDu1 - CD2*alpha[k]*dens[k]*s_ref*V2*DAlfaDu1)/m ,Thrust*cosAlpha*DBetaDu2/m ],
# [(beta[k]*Thrust*cosAlpha*DAlfaDu1/V + 0.5*CL1*dens[k]*s_ref*(V)*DAlfaDu1)/m ,Thrust*sinAlpha*DBetaDu2/(m*V)],
# [0.0 ,-Thrust*DBetaDu2/(grav_e*Isp) ]])
#
phix[k, :, :] = pi[0] * array(
[[0.0, sinGama, V * cosGama, 0.0],
[
2 * GM * sinGama / r3 -
(0.5 * CD[k] * del_rho[k] * s_ref * V2) / m, -CD[k] *
dens[k] * s_ref * V / m, -grav[k] * cosGama, -fVel / m2
],
[
cosGama * (-V / r2 + 2 * GM / (V * r3)) +
(0.5 * CL[k] * del_rho[k] * s_ref * V) / m,
-beta[k] * Thrust * sinAlpha / (m * V2) + cosGama *
((1 / r[k]) + grav[k] /
(V2)) + 0.5 * CL[k] * dens[k] * s_ref / m, -sinGama *
((V / r[k]) - grav[k] / V), -fNor / (m2 * V)
], [0.0, 0.0, 0.0, 0.0]])
phix[k, 2, :] *= fdg
phiu[k, :, :] = pi[0] * array(
[[0.0, 0.0],
[(-beta[k] * Thrust * sinAlpha * DAlfaDu1 -
CD2 * alpha[k] * dens[k] * s_ref * V2 * DAlfaDu1) / m,
Thrust * cosAlpha * DBetaDu2 / m],
[(beta[k] * Thrust * cosAlpha * DAlfaDu1 / V +
0.5 * CL1 * dens[k] * s_ref *
(V) * DAlfaDu1) / m, Thrust * sinAlpha * DBetaDu2 /
(m * V)], [0.0, -Thrust * DBetaDu2 / (grav_e * Isp)]])
phiu[k, 2, :] *= fdg
phip[k, :, :] = array(
[[V * sinGama], [fVel / m - grav[k] * sinGama],
[fNor / (m * V) + cosGama * ((V / r[k]) - (grav[k] / V))],
[-(beta[k] * Thrust) / (grav_e * Isp)]])
phip[k, 2, :] *= fdg
fu[k, :] = array(
[0.0, (pi[0] * Thrust * DBetaDu2) / (grav_e * Isp * (1 - s_f))])
fp[k, 0] = (Thrust * beta[k]) / (grav_e * Isp * (1 - s_f))
#==============================================================================
Grads['phix'] = phix
Grads['phiu'] = phiu
Grads['phip'] = phip
Grads['fx'] = fx * scal
Grads['fu'] = fu * scal
Grads['fp'] = fp * scal
# Grads['gx'] = gx
# Grads['gp'] = gp
Grads['psix'] = psix
Grads['psip'] = psip
return Grads
def calcPhi(sizes, x, u, pi, constants, restrictions):
N = sizes['N']
#N0 = sizes['N0']
n = sizes['n']
grav_e = constants['grav_e']
Thrust = constants['Thrust']
Isp = constants['Isp']
r_e = constants['r_e']
GM = constants['GM']
CL0 = constants['CL0']
CL1 = constants['CL1']
CD0 = constants['CD0']
CD2 = constants['CD2']
s_ref = constants['s_ref']
DampCent = constants['DampCent']
DampSlop = constants['DampSlop']
alpha_min = restrictions['alpha_min']
alpha_max = restrictions['alpha_max']
beta_min = restrictions['beta_min']
beta_max = restrictions['beta_max']
sin = numpy.sin
cos = numpy.cos
tanh = numpy.tanh
u1 = u[:, 0]
u2 = u[:, 1]
# calculate variables alpha and beta
alpha = (alpha_max + alpha_min) / 2 + tanh(u1) * (alpha_max -
alpha_min) / 2
beta = (beta_max + beta_min) / 2 + tanh(u2) * (beta_max - beta_min) / 2
# calculate variables CL and CD
CL = CL0 + CL1 * alpha
CD = CD0 + CD2 * (alpha)**2
# calculate L and D
# TODO: making atmosphere.rho vectorized (array compatible) would increase
# performance significantly!
dens = numpy.empty(N)
for k in range(N):
dens[k] = rho(x[k, 0])
pDynTimesSref = .5 * dens * (x[:, 1]**2) * s_ref
L = CL * pDynTimesSref
D = CD * pDynTimesSref
# calculate r
r = r_e + x[:, 0]
# calculate grav
grav = GM / r / r
# calculate phi:
phi = numpy.empty((N, n))
# example rocket single stage to orbit with Lift and Drag
sinGama = sin(x[:, 2])
phi[:, 0] = pi[0] * x[:, 1] * sinGama
phi[:, 1] = pi[0] * (
(beta * Thrust * cos(alpha) - D) / x[:, 3] - grav * sinGama)
phi[:, 2] = pi[0] * ((beta * Thrust * sin(alpha) + L) /
(x[:, 3] * x[:, 1]) + cos(x[:, 2]) *
(x[:, 1] / r - grav / x[:, 1]))
# phi[0,2] = 0.0
# for k in range(N0):
# phi[k,2] = 0.0
dt = 1.0 / (N - 1)
t = pi[0] * numpy.arange(0, 1.0 + dt, dt)
phi[:,2] = pi[0] * ( (beta * Thrust * sin(alpha) + L)/(x[:,3] * x[:,1]) +
cos(x[:,2]) * ( x[:,1]/r - grav/x[:,1] )) * \
.5*(1.0+numpy.tanh(DampSlop*(t-DampCent)))
phi[:, 3] = -(pi[0] * beta * Thrust) / (grav_e * Isp)
return phi
def mdlDer(t: float, x: list, alfaProg: callable, betaProg: callable,
aed: callable, earth: callable, allResults: bool) -> list:
# initialization
h = x[0]
v = x[1]
gamma = x[2]
M = x[3]
if numpy.isnan(h):
print('t: ', t)
# print('x: ', x)
raise Exception('itsme saying: h is not a number')
# Alpha control calculation
alfat = alfaProg(t)
# other calculations
# btm = betaProg(t)*con['T']/M
beta, Isp, T = betaProg(t)
btm = beta * T / M
sinGamma = numpy.sin(gamma)
g = earth.g0 * (earth.R / (earth.R + h))**2 - (earth.we**2) * (earth.R + h)
# aerodynamics
qdinSrefM = 0.5 * rho(h) * (v**2) * aed.s_ref / M
LM = qdinSrefM * (aed.CL0 + aed.CL1 * alfat)
DM = qdinSrefM * (aed.CD0 + aed.CD2 * (alfat**2))
if v < 1e-8:
v = 1e-8
# states derivatives
ans = [
v * sinGamma, # coefficient
btm * numpy.cos(alfat) - g * sinGamma - DM, # coefficient
btm * numpy.sin(alfat) / v +
(v / (h + earth.R) - g / v) * numpy.cos(gamma) + (LM / v) +
2 * earth.we, # coefficient
-btm * M / (earth.g0 * Isp)
] # coefficient
if allResults:
sol = dict()
sol['t [s]'] = t
sol['h [km]'] = h
sol['v [km]'] = v
sol['gamma [rad]'] = gamma
sol['M [kg]'] = M
sol['dhdt [km/s]'] = ans[0]
sol['a [km/s2]'] = ans[1]
sol['dgdt [rad/s]'] = ans[2]
sol['dmdt [kg/s]'] = ans[3]
sol['L [kN]'] = LM * M
sol['D [kN]'] = DM * M
dens, Patm, Tatm, asound = atm(h)
sol['rho [kg/km3]'] = dens
sol['Patm [kPa]'] = Patm
sol['Tatm [k]'] = Tatm
sol['v_{sound} [km/s]'] = asound
sol['Mach [-]'] = v / asound
sol['qdin [kPa]'] = 0.5 * dens * (v**2)
sol['Peff [kN]'] = g * M
sol['Cl [-]'] = aed.CL0 + aed.CL1 * alfat
sol['Cd [-]'] = aed.CD0 + aed.CD2 * (alfat**2)
sol['theta [rad]'] = alfat + gamma
sol['btm [km/s2]'] = btm
a, e, E, momAng, ph, ah, h = orbitCalculation(x, earth)
sol['semi-major axis [km]'] = a
sol['eccentricity [-]'] = e
sol['E [GJ]'] = E
sol['momAng [GNm]'] = momAng
sol['ph [km]'] = ph
sol['ah [km]'] = ah
ans = sol
return ans