# =============================================================================
# Compute target FNPEG params from nominal FNPEG run (comment out during MC)
# =============================================================================
print('\nCOMPUTING NOMINAL PARAMETERS...')
r = xxvec2[0, :]
vmag = xxvec2[3, :] / 1e3
h = (r - paramsNom_PBC.p.rad * 1e3) / 1e3
# get density and speed of sound at each altitude step
rho = []
assfun = interp.interp1d(paramsNom_PBC.p.sound[0, :],
paramsNom_PBC.p.sound[1, :])
ass = assfun(h)
for hi in h:
rho.append(getRho_from_table(paramsNom_PBC.atmdat, hi))
rho = np.asarray(rho)
# compute mach number and dynamic pressure at each step
Mvec = vmag * 1e3 / ass
Qinc = 1 / 2 * rho * (vmag * 1e3)**2
# range
lon0 = xxvec2[1, 0]
lonf = xxvec2[1, -1]
lat0 = xxvec2[2, 0]
latf = xxvec2[2, -1]
dlon = abs(lonf - lon0)
dsig = np.arccos(np.sin(lat0) * np.sin(latf)\
+ np.cos(lat0) * np.cos(latf) * np.cos(dlon))
def sphericalEntryEOMsAug(t, yy, sig0, e0, params):
'''
Like sphericalEntryEOMs from ASEN 6015, but dimensional and with GRAM dens.
Defines 3D 3DOF EOMs for lifting entry vehicle over *ellipsoidal* rotating
planet. Defined as in FNPAG (Lu et. al), then augmented with 7th state
to capture evolution of range. Uses bank angle profile as defined in
FNPEG.
ASSUMPTIONS:
- constant bank angle
- exponential atmosphere
- rotating planet
- ellipsoidal planet (J2)
params requirements:
OmP: rotation rate of planet, rad/s
radP: equatorial radius of planet, m
mu: gravitational parameter of planet, m^3/s^2
J2: zonal coefficient J2 of planet
L_D: vehicle L/D ratio
BC: vehicle ballistic coefficient, kg/m^2
INPUTS:
t: time (not used)
yy: state vector:
r: radial distance, m
lon: longitude, radians
lat: latitude, radians
v: planet-relative velocity magnitude, m/s
gam: flight path angle, negative-down, radians
hda: heading angle, clockwise from north, radians
sig: bank angle, rad
OUTPUTS:
dydt: time derivative of each state
'''
# extract constants from params
OmP = params.p.om
radP = params.p.rad * 1e3
mu = params.p.mu * 1e9 # convert from km^3/s^2 to m^3/s^2
J2 = params.p.J2
L_D = params.LD
BC = params.BC
ef = params.ef
sigf = params.sigf
# extract state variables
r = yy[0]
# lon = yy[1] # not used in EOMs
lat = yy[2]
v = yy[3]
gam = yy[4]
hda = yy[5]
# get gravity terms from J2 model
gr = mu / r**2 * (1 + J2 * (radP / r)**2 * (1.5 - 4.5 * np.sin(lat)**2))
gphi = mu / r**2 * (J2 * (radP / r)**2 * (3 * np.sin(lat) * np.cos(lat)))
# get density at this altitude
if params.dMode == 'fun':
rho = params.dFun((r - radP) / 1e3)
elif params.dMode == 'table':
rho = getRho_from_table(params.atmdat, (r - radP) / 1e3)
else:
sys.exit('atm mode not recognized')
# #### TROUBLESHOOTING CODE ####
# rho0 = 0.0263 # kg/m^3
# H = 10153 # m
# h = r - radP
# rho = rho0 * np.exp(-h / H)
# #### TROUBLESHOOTING CODE ####
# compute e
e = mu / r - v**2 / 2
# compute current bank angle
sig0 = (sig0 + 180) % (360) - 180
sig = sig0 + (e - e0) / (ef - e0) * (sigf - sig0)
sig = np.radians(sig)
# compute lift and drag accelerations
D = rho * v**2 / (2 * BC)
L = L_D * D
# EOMs
dydt = np.empty(7)
dydt[:] = np.NaN
dydt[0] = v * np.sin(gam)
dydt[1] = v * np.cos(gam) * np.sin(hda) / (r * np.cos(lat))
dydt[2] = v * np.cos(gam) * np.cos(hda) / r
dydt[3] = -D\
- gr * np.sin(gam)\
- gphi * np.cos(gam) * np.cos(hda)\
+ OmP**2 * r * np.cos(lat) * (np.sin(gam) * np.cos(lat)\
- np.cos(gam) * np.sin(lat)\
* np.cos(hda))
dydt[4] = 1/v * (L * np.cos(sig)\
+ (v**2/r - gr) * np.cos(gam)\
+ gphi * np.sin(gam) * np.cos(hda)\
+ 2 * OmP * v * np.cos(lat) * np.sin(hda)\
+ OmP**2 * r * np.cos(lat) * (np.cos(gam) * np.cos(lat)\
+ np.sin(gam) * np.cos(hda)\
* np.sin(lat)))
dydt[5] = 1/v * (L * np.sin(sig) / np.cos(gam)\
+ v**2/r * np.cos(gam) * np.sin(hda) * np.tan(lat)\
+ gphi * np.sin(hda) / np.cos(gam)\
- 2 * OmP * v * (np.tan(gam) * np.cos(hda) * np.cos(lat)\
- np.sin(lat))\
+ OmP**2 * r / np.cos(gam) * np.sin(hda)\
* np.sin(lat) * np.cos(lat))
dydt[6] = -v * np.cos(gam) / r
return dydt
def dynamics(t,
yy,
sig,
params,
returnForces=False,
returnAccelerations=False):
'''
ODEs for full dynamics acting on the vehicle
assume:
- just one vehicle
- yy is [position; velocity] in INERTIAL frame
- sig is bank angle in deg
'''
dydt = np.empty(6)
dydt[:] = np.NaN
# extract inertial state components
x = yy[0]
y = yy[1]
z = yy[2]
dx = yy[3]
dy = yy[4]
dz = yy[5]
xvec_N = np.array([x, y, z])
vvec_N = np.array([dx, dy, dz])
r = norm(xvec_N)
mu_r3 = params.p.mu / r**3
## Get gravitational force
agvec_N = -mu_r3 * xvec_N
## Get aerodynamics forces
# TODO: carefully replace below with VN2Vinf function call
SN = DCMs.getSN(t, params)
NS = DCMs.getNS(t, params)
OMvec = np.array([0, 0, -params.p.om])
vvec_S = SN @ (vvec_N + cross(OMvec, xvec_N))
h = r - params.p.rad
if params.dMode == 'fun':
rho = params.dFun(h)
elif params.dMode == 'table':
rho = getRho_from_table(params.atmdat, h)
else:
sys.exit('atm mode not recognized')
wvec_S = getWind(h)
vInfvec_S = vvec_S + wvec_S
vInf = norm(vInfvec_S)
vInfvec_N = NS @ vInfvec_S
# Lmag = 1/2 * rho * (vInf*1e3)**2 * params.CL * params.A / 1e3
# Dmag = 1/2 * rho * (vInf*1e3)**2 * params.CD * params.A / 1e3
Dmag = rho * (vInf * 1e3)**2 / (2 * params.BC) / 1e3
Lmag = Dmag * params.LD
hvec_N = cross(xvec_N, vInfvec_N)
Lupvec_N = cross(vInfvec_N, hvec_N)
LupvecU_N = Lupvec_N / norm(Lupvec_N)
# TODO - following code check is a little sketchy. can improve w/ 6 DOF
c1 = cross(xvec_N, params.v0)
c2 = cross(xvec_N, vInfvec_N)
# if we are "flying upside down"
if np.dot(c1, c2) < 0:
# then change the sign on the lift vector to be upside down
LupvecU_N = -LupvecU_N
# print(LupvecU_N)
# print(t)
vInfvecU_N = vInfvec_N / norm(vInfvec_N)
LvecU_N = LupvecU_N * np.cos(np.radians(sig)) + \
cross(vInfvecU_N, LupvecU_N) * np.sin(np.radians(sig))
DvecU_N = -vInfvecU_N
aLvec_N = Lmag * LvecU_N
aDvec_N = Dmag * DvecU_N
dydt[0] = dx
dydt[1] = dy
dydt[2] = dz
dydt[3:6] = (agvec_N + aLvec_N + aDvec_N)
if returnForces:
return agvec_N * params.m, aLvec_N, aDvec_N
elif returnAccelerations:
return agvec_N, aLvec_N, aDvec_N
else:
return dydt
# =============================================================================
# Compute target parameters for FNPEG from probe
# =============================================================================
# compute Mach number profile
rvec = outsProbe.rvec_N
vvec = outsProbe.vvec_N
h = np.linalg.norm(rvec, axis=0) - params.p.rad
vmag = np.linalg.norm(vvec, axis=0)
# get density and speed of sound at each altitude step
rho = []
assfun = interp.interp1d(params.p.sound[0, :], params.p.sound[1, :])
ass = assfun(h)
for hi in h:
rho.append(getRho_from_table(params.atmdat, hi))
rho = np.asarray(rho)
# compute mach number and dynamic pressure at each step
Mvec = vmag * 1e3 / ass
Qinc = 1 / 2 * rho * (vmag * 1e3)**2
# compute range
lon0 = np.radians(outsProbe.lon0)
lonf = np.radians(outsProbe.lonf)
lat0 = np.radians(outsProbe.lat0)
latf = np.radians(outsProbe.latf)
dlon = abs(lonf - lon0)
dsig = np.arccos(np.sin(lat0) * np.sin(latf)\
+ np.cos(lat0) * np.cos(latf) * np.cos(dlon))
def mainAD(params, tspan, events, outs, verbose=True):
### CONVERT GIVEN INPUT TO INERTIAL VECTORS (and other input types as well)
# NOTE - no matter what the input type is, vectors should be in N frame
if params.inputType == 'inertial vectors':
params.vInfvec_N = VN2Vinf(params.x0, params.v0, params, tspan[0])
params.lat, params.lon, params.alt, params.efpa, params.hda,\
params.vmag = RV2LLAEHV(params.x0, params.v0, params, tspan[0])
_, _, _, params.efpaWR, params.hdaWR, params.vmagWR = \
RV2LLAEHV(params.x0, params.vInfvec_N, params, tspan[0])
elif params.inputType == 'wind-relative vectors':
params.v0 = Vinf2VN(params.x0, params.vInfvec_N, params, tspan[0])
params.lat, params.lon, params.alt, params.efpa, params.hda,\
params.vmag = RV2LLAEHV(params.x0, params.v0, params, tspan[0])
_, _, _, params.efpaWR, params.hdaWR, params.vmagWR = \
RV2LLAEHV(params.x0, params.vInfvec_N, params, tspan[0])
elif params.inputType == 'inertial angles':
params.x0, params.v0 = LLAEHV2RV(params.lat, params.lon, params.alt,
params.efpa, params.hda, params.vmag,
params, tspan[0])
params.vInfvec_N = VN2Vinf(params.x0, params.v0, params, tspan[0])
_, _, _, params.efpaWR, params.hdaWR, params.vmagWR = \
RV2LLAEHV(params.x0, params.vInfvec_N, params, tspan[0])
elif params.inputType == 'wind-relative angles':
params.x0, params.vInfvec_N = LLAEHV2RV(params.lat, params.lon,
params.alt, params.efpaWR,
params.hdaWR, params.vmagWR,
params, tspan[0])
params.v0 = Vinf2VN(params.x0, params.vInfvec_N, params, tspan[0])
_, _, _, params.efpa, params.hda, params.vmag = \
RV2LLAEHV(params.x0, params.v0, params, tspan[0])
else:
sys.exit('input type not recognized')
### CALL SIMRUN
sol = simRun(params, tspan, events, verbose=verbose)
rvec_N = sol.y[0:3, :]
vvec_N = sol.y[3:6, :]
### GET OUTPUT PARAMS OF INTEREST
# final inertial state
rfvec_N = rvec_N[:, -1]
vfvec_N = vvec_N[:, -1]
tf = sol.t[-1]
# convert final state params (these are inertial)
lat, lon, alt, fpa, hda, vmag = RV2LLAEHV(rfvec_N, vfvec_N, params, tf)
## Compute peak heat rate, add to output
# get airspeed at each time, vInf
vInfvec_N = []
for i in range(vvec_N.shape[1]):
vInfvec_N.append(VN2Vinf(rvec_N[:, i], vvec_N[:, i], params, sol.t[i]))
vInfvec_N = np.asarray(vInfvec_N).T
vInf = np.linalg.norm(vInfvec_N, axis=0)
# get density at each time
r = np.linalg.norm(rvec_N, axis=0)
h = r - params.p.rad
if params.dMode == 'fun':
rho = params.dFun(h)
elif params.dMode == 'table':
rho = getRho_from_table(params.atmdat, h)
else:
sys.exit('atm mode not recognized')
# calculate S-G heat rate without coefficient, SG
SG = []
for i in range(len(rho)):
SG.append(np.sqrt(rho[i] / params.Rn) * vInf[i]**3)
SG = np.asarray(SG)
SGpeak = SG.max()
# calculate peak heat rate
qpeak = params.p.k * SGpeak * 1e5 # puts q in W/cm^2 units
q = params.p.k * SG * 1e5
# now integrate numerically to get heat load
Qload = trapz(q, sol.t) # J / cm^2
## Compute max g, add to output
# run through dynamics again to get forces output
agvec_N = np.empty((3, len(sol.t)))
agvec_N[:] = np.NaN
aLvec_N = np.empty((3, len(sol.t)))
aLvec_N[:] = np.NaN
aDvec_N = np.empty((3, len(sol.t)))
aDvec_N[:] = np.NaN
for i, (ti, xi, vi) in enumerate(zip(sol.t, rvec_N.T, vvec_N.T)):
yyi = np.block([xi, vi])
agvec_N[:, i], aLvec_N[:, i], aDvec_N[:, i] = ODEs.dynamics(
ti, yyi, params.bank, params, returnAccelerations=True)
# compute g-load at each time
gload = np.linalg.norm(aLvec_N + aDvec_N, axis=0)\
/ (constants.G0 / 1e3) # divide g0 by 1000 to get it in km/s^2 units
gpeak = gload.max()
## Compute apoapsis at final time, add to output
vf = np.linalg.norm(vfvec_N)
rf = np.linalg.norm(rfvec_N)
engf = vf**2 / 2 - params.p.mu / rf
hfvec_N = np.cross(rfvec_N, vfvec_N)
hf = np.linalg.norm(hfvec_N)
af = -params.p.mu / (2 * engf)
eccf = np.sqrt(1 + 2 * engf * hf**2 / params.p.mu**2)
raf = af * (1 + eccf)
haf = raf - params.p.rad
### ASSIGN OUTPUTS TO outs CLASS
# initial state
outs.lat0 = params.lat
outs.lon0 = params.lon
outs.alt0 = params.alt
outs.efpa0 = params.efpa
outs.hda0 = params.hda
outs.vmag0 = params.vmag
outs.efpaWR0 = params.efpaWR
outs.hdaWR0 = params.hdaWR
outs.vmagWR0 = params.vmagWR
outs.rvec_N0 = params.x0
outs.vvec_N0 = params.v0
outs.vInfvec_N0 = params.vInfvec_N
outs.tspan = tspan
# vehicle
outs.BC = params.BC
outs.Rn = params.Rn
outs.bank = params.bank
# final state
outs.latf = lat
outs.lonf = lon
outs.altf = alt
outs.fpaf = fpa
outs.hdaf = hda
outs.vmagf = vmag
outs.rvec_Nf = rfvec_N
outs.vvec_Nf = vfvec_N
outs.raf = raf
outs.haf = haf
outs.engf = engf
outs.af = af
outs.eccf = eccf
outs.t = tf
# peak values and total loads
outs.SGpeak = SGpeak
outs.qpeak = qpeak
outs.Qload = Qload
outs.gpeak = gpeak
if params.returnTimeVectors:
outs.rvec_N = rvec_N
outs.vvec_N = vvec_N
outs.tvec = sol.t
outs.q = q
outs.gload = gload
return outs
'''
# get index of first element in altitude array great than h
ind = np.argmax(params.atmdat[0,:] > h)
x0 = params.atmdat[0, ind-1]
x1 = params.atmdat[0, ind]
y0 = params.atmdat[1, ind-1]
y1 = params.atmdat[1, ind]
rho = (y0 * (x1 - h) + y1 * (h - x0)) / (x1 - x0)
return rho
h = 96.15498715
tic1 = time.time()
rho1 = getRho_from_table(params.atmdat, h)
toc1 = time.time()
tic2 = time.time()
rho2 = myinterp(h, params)
toc2 = time.time()
tic3 = time.time()
rho3 = scipyfun(h)
toc3 = time.time()
print(rho1)
print(rho2)
print(rho3)
# NOTE: just do timeit _____ for each rho command, in the IPython prompts