def doBallisticProbe(params, t0, xx0vec, plotsOn=True):
'''
main function for passive ballistic probe (no control or guidance).
Uses augmented spherical planet-relative EOMs.
'''
print('\nRUNNING PASSIVE PROBE...')
# set dummy sig0 and e0 values
sig0 = 90
e0 = 0
# =========================================================================
# Set up events
# =========================================================================
event1 = lambda t, y: ODEs.above_max_alt_sph(t, y, params)
event1.terminal = True
event1.direction = 1
event2 = lambda t, y: ODEs.below_min_alt_sph(t, y, params)
event2.terminal = True
# =========================================================================
# Main integration loop
# =========================================================================
tspan = (t0, params.tf)
sol = solve_ivp(
lambda t, y: ODEs.sphericalEntryEOMsAug(t, y, sig0, e0, params),
tspan,
xx0vec,
rtol=params.rtol,
atol=params.atol,
events=(event1, event2))
xxvec = sol.y
tvec = sol.t
if plotsOn:
# uses global variable ax3 for plotting!
ax3.plot(xxvec[3, :] / 1e3, xxvec[0, :] / 1e3 - params.p.rad)
# =========================================================================
# Compute target errors
# =========================================================================
xxfvec = xxvec[:, -1]
sfErr = np.degrees(xxfvec[6])
hfErr = xxfvec[0] - params.rf
vfErr = xxfvec[3] - params.vf
print('Final range error: {0:.6e} deg'.format(sfErr))
print('Final altitude error: {0:.6f} m'.format(hfErr))
print('Final velocity error: {0:.6f} m/s'.format(vfErr))
return xxvec, tvec, sfErr, hfErr, vfErr
def simRun(params, tspan, events, **options):
# now do the setup and call dynamics
y0 = np.block([params.x0, params.v0])
sol = solve_ivp(lambda t, y: ODEs.dynamics(t, y, params.bank, params),
[tspan[0], tspan[-1]],
y0.flatten(),
rtol=1e-10,
atol=1e-10,
events=events)
# 1e-10 maintains about 5 sig figs of accuracy
if 'verbose' in options:
if options['verbose']:
# print('BC: %.2f, EFPA_WR: %.2f' % (params.BC, params.efpaWR))
print('EFPA_WR: {0:.2f} deg, vmag_WR: {1:.3f} m/s'\
.format(params.efpaWR, params.vmagWR*1e3))
print(sol.message)
if not sol.success:
sys.exit('integration failed')
if not sol.status:
print('Got stuck at:')
print('BC: %.2f' % params.BC)
print('EFPA: %.2f' % params.efpa)
sys.exit('no termination event reached')
return sol
def marvelProbe(params, verbose = False, MC = False):
'''
main function for MARVEL probes.
1) back-propagate probes for params.tback seconds
2) split probe into N separate probes
3) apply delta-V of params.DV to each probe in hard-coded directions
4) forward propagate to entry interface
5) call mainAD for each probe from entry interface to landing
'''
# get inertial cartesian state from given planet-relative spherical state
params.x0, params.vInfvec_N = LLAEHV2RV(params.lat, params.lon,
params.alt, params.efpaWR,
params.hdaWR, params.vmagWR,
params, params.tspan[0])
params.v0 = Vinf2VN(params.x0, params.vInfvec_N, params, params.tspan[0])
# propagate vehicle backwards in time by params.tback seconds
yy0 = np.block([params.x0, params.v0])
solBack = solve_ivp(lambda t, y: ODEs.TBP(t, y, params),
(params.tback, 0), yy0.flatten(), rtol = params.rtol,
atol = params.atol)
xx0vecCenter = solBack.y[:,-1]
x0vecCenter = xx0vecCenter[:3]
v0vecCenter = xx0vecCenter[3:]
# split probe into 4 separate probes
rDir = x0vecCenter / np.linalg.norm(x0vecCenter)
vDir = v0vecCenter / np.linalg.norm(v0vecCenter)
hDir = np.cross(rDir, vDir)
hDir = hDir / np.linalg.norm(hDir)
thDir = np.cross(hDir, rDir)
thDir = thDir / np.linalg.norm(thDir)
phi = np.radians(0)
eta = np.radians(0)
aDir = np.cos(phi) * thDir + np.sin(phi) * hDir
bDir = -aDir
cDir = np.cos(eta) * hDir - np.sin(eta) * thDir
dDir = -cDir
v0vecDown = v0vecCenter + params.DVDown * aDir
v0vecUp = v0vecCenter + params.DVUp * bDir
v0vecLeft = v0vecCenter + params.DVLeft * cDir
v0vecRight = v0vecCenter + params.DVRight * dDir
xx0vecDown = np.hstack([x0vecCenter, v0vecDown])
xx0vecUp = np.hstack([x0vecCenter, v0vecUp])
xx0vecLeft = np.hstack([x0vecCenter, v0vecLeft])
xx0vecRight = np.hstack([x0vecCenter, v0vecRight])
# pdb.set_trace()
# propagate each probe forward until atm interface
eventEI = lambda t, y: ODEs.above_max_alt(t, y, params)
eventEI.terminal = True
paramsDown = copy.deepcopy(params)
solDown = solve_ivp(lambda t, y: ODEs.TBP(t, y, params),
(0, 10 * params.tback), xx0vecDown, rtol = params.rtol,
atol = params.atol, events = (eventEI))
paramsDown.x0 = solDown.y[:3,-1]
paramsDown.v0 = solDown.y[3:,-1]
paramsDown.inputType = 'inertial vectors'
paramsUp = copy.deepcopy(params)
solUp = solve_ivp(lambda t, y: ODEs.TBP(t, y, params),
(0, 10 * params.tback), xx0vecUp, rtol = params.rtol,
atol = params.atol, events = (eventEI))
paramsUp.x0 = solUp.y[:3,-1]
paramsUp.v0 = solUp.y[3:,-1]
paramsUp.inputType = 'inertial vectors'
paramsLeft = copy.deepcopy(params)
solLeft = solve_ivp(lambda t, y: ODEs.TBP(t, y, params),
(0, 10 * params.tback), xx0vecLeft, rtol = params.rtol,
atol = params.atol, events = (eventEI))
paramsLeft.x0 = solLeft.y[:3,-1]
paramsLeft.v0 = solLeft.y[3:,-1]
paramsLeft.inputType = 'inertial vectors'
paramsRight = copy.deepcopy(params)
solRight = solve_ivp(lambda t, y: ODEs.TBP(t, y, params),
(0, 10 * params.tback), xx0vecRight, rtol = params.rtol,
atol = params.atol, events = (eventEI))
paramsRight.x0 = solRight.y[:3,-1]
paramsRight.v0 = solRight.y[3:,-1]
paramsRight.inputType = 'inertial vectors'
# propagate all 4 probes to the surface
outsDown = Outs()
outsUp = Outs()
outsLeft = Outs()
outsRight = Outs()
outsDown = mainAD(paramsDown, params.tspan, params.events, outsDown,
verbose = verbose)
outsUp = mainAD(paramsUp, params.tspan, params.events, outsUp,
verbose = verbose)
outsLeft = mainAD(paramsLeft, params.tspan, params.events, outsLeft,
verbose = verbose)
outsRight = mainAD(paramsRight, params.tspan, params.events, outsRight,
verbose = verbose)
# make sure that all probes enter atm at least 1 km away from orbiter
dista = greatCircleDistDeg(params.lon, params.lat,
outsDown.lon0, outsDown.lat0, params)
distb = greatCircleDistDeg(params.lon, params.lat,
outsUp.lon0, outsUp.lat0, params)
distc = greatCircleDistDeg(params.lon, params.lat,
outsLeft.lon0, outsLeft.lat0, params)
distd = greatCircleDistDeg(params.lon, params.lat,
outsRight.lon0, outsRight.lat0, params)
if min(dista, distb, distc, distd) < 1:
# sys.exit('a probe entered the atmosphere within {0:.3f} km of the'\
# ' orbtier'.format(min(dista, distb, distc, distd)))
print('a probe entered the atmosphere within {0:.3f} km of the'\
' orbtier'.format(min(dista, distb, distc, distd)))
# find minimum distance between two landing locations
distAB = greatCircleDistDeg(outsDown.lonf, outsDown.latf,
outsUp.lonf, outsUp.latf, params)
distAC = greatCircleDistDeg(outsDown.lonf, outsDown.latf,
outsLeft.lonf, outsLeft.latf, params)
distAD = greatCircleDistDeg(outsDown.lonf, outsDown.latf,
outsRight.lonf, outsRight.latf, params)
distBC = greatCircleDistDeg(outsUp.lonf, outsUp.latf,
outsLeft.lonf, outsLeft.latf, params)
distBD = greatCircleDistDeg(outsUp.lonf, outsUp.latf,
outsRight.lonf, outsRight.latf, params)
distCD = greatCircleDistDeg(outsLeft.lonf, outsLeft.latf,
outsRight.lonf, outsRight.latf, params)
minDist = min(distAB, distAC, distAD, distBC, distBD, distCD)
maxDist = max(distAB, distAC, distAD, distBC, distBD, distCD)
return outsDown, outsUp, outsLeft, outsRight, (minDist, maxDist)
params.hdaWR = 0 params.vmagWR = 6 ### SET DEFAULT SEPARATION DELTA-V AND TIMING DVnom = 1e-4 # km/s tbacknom = 1 * 24 * 60 * 60 # s params.tback = tbacknom ### TIME VECTOR AND EXIT CONDITIONS params.tspan = (tbacknom, 5000000) # EXIT CONDITIONS: params.hmin = 10 params.hmax = params.p.halt + 1e-7 event1 = lambda t, y: ODEs.below_min_alt(t, y, params) event1.terminal = True event2 = lambda t, y: ODEs.above_max_alt(t, y, params) event2.terminal = True event2.direction = 1 events = (event1, event2) params.events = events ### INTEGRATION TOLERANCE params.rtol = 1e-11 params.atol = 1e-11 outsCenter = Outs() outsCenter = mainAD(params, params.tspan, params.events, outsCenter)
def doFNPAG(mode,
paramsTrue,
paramsNom,
t0,
xx0vec,
sig0,
sigd,
ts,
ts1,
ts2,
verbose=True,
plotsOn=True,
updatesOn=True):
'''
main function for FNPAG with variable mode (supports modes 1 and 2).
paramsTrue holds real values used in propagation, paramsNom holds nominal
values used in prediction for guidance updates.
'''
# =========================================================================
# Set up events
# =========================================================================
event1 = lambda t, y: ODEs.above_max_alt(t, y, paramsTrue)
event1.terminal = True
event1.direction = 1
event2 = lambda t, y: ODEs.below_min_alt(t, y, paramsTrue)
event2.terminal = True
event3 = lambda t, y: ODEs.switchEvent(t, y, ts)
event3.terminal = True
# =========================================================================
# Main integration loop
# =========================================================================
# make sure correct bank angles are set
paramsTrue.bank = sig0
paramsNom.bank = sig0
# initialize
tvec = np.arange(t0, paramsNom.tf + paramsTrue.dtGdn, paramsTrue.dtGdn)
tvecEval = np.empty(1)
tvecEval[0] = t0
xxvec = np.empty((len(xx0vec), 1))
xxvec[:] = np.NaN
xxvec[:, 0] = xx0vec
tsList = []
sigdList = []
t = t0
ind = 0
xx0veci = xx0vec
## PHASE 1 ##
phase = 1
for ind, t in enumerate(tvec):
# update guidance
if updatesOn:
ts = updateFNPAG(xxvec[:, -1], t, ts, sig0, sigd, ts1, ts2, phase,
mode, paramsNom)
# update event with new ts value
event3 = lambda t, y: ODEs.switchEvent(t, y, ts)
event3.terminal = True
if verbose:
print('PHASE 1: updating guidance at time {0:.3f} ts = {1:.3f}'\
.format(t, ts))
tsList.append(ts)
# propagate until next guidance update or switching time
tspan = (t, t + paramsTrue.dtGdn)
soli = solve_ivp(lambda t, y: ODEs.dynamics(t, y, paramsTrue),
tspan,
xx0veci,
rtol=paramsTrue.rtol,
atol=paramsTrue.atol,
events=(event1, event2, event3))
# append results
xxvec = np.append(xxvec, soli.y, axis=1)
tvecEval = np.append(tvecEval, soli.t)
xx0veci = xxvec[:, -1]
# either proceed to next guidance update or enter phase 2
if soli.status == 0:
# reached next guidance update
continue
elif len(soli.t_events[0]) > 0:
# reached switching time
break
else:
sys.exit('propagator never reached next guidance update or'\
'switching time in phase 1')
tvecP1 = tvecEval * 1
## create new tvec for phase 2
tvec = np.arange(tvecEval[-1], paramsNom.tf + paramsTrue.dtGdn,
paramsTrue.dtGdn)
## PHASE 2 ##
phase = 2
paramsTrue.bank = sigd
paramsNom.bank = sigd
print()
for ind, t in enumerate(tvec):
# update guidance
if updatesOn:
sigd = updateFNPAG(xxvec[:, -1], t, ts, sig0, sigd, ts1, ts2,
phase, mode, paramsNom)
paramsTrue.bank = sigd
paramsNom.bank = sigd
# make sure sigd is in [-180, 180] deg range
sigd = (sigd + 180) % (360) - 180
if verbose:
print('PHASE 2: updating guidance at time {0:.3f}'\
' sigd = {1:.3f} deg'.format(t, sigd))
sigdList.append(sigd)
# propagate until next guidance update or final state
tspan = (t, t + paramsTrue.dtGdn)
soli = solve_ivp(lambda t, y: ODEs.dynamics(t, y, paramsTrue),
tspan,
xx0veci,
rtol=paramsTrue.rtol,
atol=paramsTrue.atol,
events=(event1, event2))
# append results
xxvec = np.append(xxvec, soli.y, axis=1)
tvecEval = np.append(tvecEval, soli.t)
xx0veci = xxvec[:, -1]
if soli.status == 1:
# reached terminal state
break
# trim off first array elements
xxvec = xxvec[:, 1:]
tvecEval = tvecEval[1:]
tvecP2 = tvecEval * 1
if plotsOn:
print()
# TODO: add some plotting here
# =========================================================================
# Compute apoapsis error and DV magnitudes
# =========================================================================
raf, rpf = getApses(xxvec[:3, -1], xxvec[3:, -1], paramsTrue)
raErr = raf - paramsTrue.raStar
DV1 = np.sqrt(2 * paramsTrue.mu)\
* abs((np.sqrt(1/paramsTrue.raStar - 1\
/ (paramsTrue.raStar + paramsTrue.rpStar))\
- np.sqrt(1/paramsTrue.raStar - 1 / (raf + rpf))))
DV2 = np.sqrt(2 * paramsTrue.mu)\
* abs((np.sqrt(1/paramsTrue.rpStar - 1\
/ (paramsTrue.raStar + paramsTrue.rpStar))\
- np.sqrt(1/paramsTrue.rpStar - 1 / (raf + paramsTrue.rpStar))))
DV = DV1 + DV2
print('\n\nfinal apoapsis error: {0:.3e} m'.format(raErr))
print('delta-V for periapsis raise: {0:.3f} m/s'.format(DV1))
print('delta-V for apoapsis correction: {0:.3f} m/s'.format(DV2))
print('total delta-V: {0:.3f} m/s'.format(DV))
return xxvec, tvecEval, raf, rpf, raErr, DV,\
tsList, sigdList, tvecP1, tvecP2
def NonLinSystem(t, x, power_yaw, Ybus):
"""
Objective:
Describes the system of equations for a coupled machine-turbine system
dictated by a given network. We use the Kundur 2-area system as a
test-case
Inputs:
x := state vector
t := time
fi := struct containing flow info
Ybus := Admittance matrix
Outputs:
F := d/dts
Note:
In this function, we will work with 2 systems
1) Turbine := No_mach*No_states_mach : No_mach*No_states_mach + 11
2) Synchronous Machines := 0:No_mach*No_states_mach
The network gives us voltage info of the buses and we use this to
simulate the dynamics of turbine and synch mach
"""
# So now we have things written in different functions
# This is sorta the main function where we get the ODEs
# Initialization:
Nfull = No_mach * No_states_mach + No_states_turb * No_turb
#==========================================================================
# 0) Get the currents from the turbine to feed into the grid
Iturb = TurbineCurrentsInGlobal(t, x[No_mach * No_states_mach:])
#==========================================================================
# 1) Get voltage from the network
Vd, Vq, Id, Iq = GridInterconnection(Ybus, x[:No_mach * No_states_mach],
Iturb)
#==========================================================================
# 2) Use this voltage to simulate SM
omega_slack = x[3]
DXsyncDT = np.zeros(No_mach * No_states_mach)
# Loop over machines
l = 0
for mach in range(No_mach):
if mach == 0:
omega_slack_1 = 377
else:
omega_slack_1 = x[3]
mach_dyn = ODEs.SyncMachineEqns(t, x[:No_mach*No_states_mach],\
omega_slack, omega_slack_1,\
Id[mach], Iq[mach])
DXsyncDT[l:l + No_states_mach] = mach_dyn
l = l + No_states_mach
#==========================================================================
# 3) Use this voltage to simulate turbine
DXturbDT = np.zeros(No_states_turb * No_turb)
# Loop over turbines
k = 0
for turb in range(No_turb):
turb_dyn = ODEs.TurbineEqns(t, x[No_mach*No_states_mach:],\
power_yaw[turb]/5e6, Vd[-1],Vq[-1])
DXturbDT[k:k + No_states_turb] = turb_dyn
k = k + No_states_turb
#==========================================================================
# 4) Stack 'em
F = np.hstack((DXsyncDT, DXturbDT))
return F
raf, rpf = getApsesSphPR(xxvecs2[:, -1], paramsNom)
print(raf / 1e3 - paramsNom.p.rad)
h = np.linalg.norm(xxvecs2[:3, :], axis=0) / 1e3
vmag = np.linalg.norm(xxvecs2[3:, :], axis=0) / 1e3
ax.plot(vmag, h, label='spherical EOMs')
ax.legend()
# spherical aug
xx0vec3 = np.append(xx0vec2, 0)
tic3 = time.time()
e0 = 0
paramsNom.ef = 100
paramsNom.sigf = sigd
event1 = lambda t, y: ODEs.above_max_alt_sph(t, y, paramsNom)
event1.terminal = True
event1.direction = 1
event2 = lambda t, y: ODEs.below_min_alt_sph(t, y, paramsNom)
event2.terminal = True
tspan = (t0, paramsNom.tf)
sol = solve_ivp(
lambda t, y: ODEs.sphericalEntryEOMsAug(t, y, sig0, e0, paramsNom),
tspan,
xx0vec3,
rtol=paramsNom.rtol,
atol=paramsNom.atol,
events=(event1, event2))
xxvecs3 = sol.y
toc3 = time.time()
raf, rpf = getApsesSphPR(xxvecs3[:, -1], paramsNom)
params1.hmin = 30
params1.hmax = 100
tspan = np.linspace(0, 1800, 10000) # integrate
# r0vec_N = np.array([-6402,-1809,1065]) * 1e3
# v0vec_N = np.array([0.999,-6.471,-4.302]) * 1e3
r0vec_N = np.array([6478100, 0, 0]) / 1e3
# v0vec_N = np.array([-671.533934883426, 472.3899576546, 10979.4827826405]) / 1e3
v0vec_N = np.array([-0.67153393, 0.47238996, 10.97948278])
y0 = np.block([r0vec_N, v0vec_N])
event1 = lambda t, y: ODEs.below_min_alt(t, y, params1)
event1.terminal = True
event2 = lambda t, y: ODEs.above_max_alt(t, y, params1)
event2.terminal = True
sol = solve_ivp(lambda t, y: ODEs.dynamics(t, y, params1),
[tspan[0], tspan[-1]],
y0.flatten(),
rtol=1e-9,
atol=1e-9,
t_eval=tspan,
events=(event1, event2))
# normally set tols to 1e-12
print(sol.message)
def doFNPAG(mode,
paramsTrue,
paramsNom,
t0,
xx0vec,
sig0,
sigd,
ts,
verbose=True,
plotsOn=True,
updatesOn=True):
'''
main function for FNPAG with variable mode (supports modes 1 and 2).
paramsTrue holds real values used in propagation, paramsNom holds nominal
values used in prediction for guidance updates.
Uses spherical planet-relative EOMs.
'''
print('\nRUNNING FNPAG...')
# =========================================================================
# Set up events
# =========================================================================
event1 = lambda t, y: ODEs.above_max_alt_sph(t, y, paramsTrue)
event1.terminal = True
event1.direction = 1
event2 = lambda t, y: ODEs.below_min_alt_sph(t, y, paramsTrue)
event2.terminal = True
event3 = lambda t, y: ODEs.switchEvent(t, y, ts)
event3.terminal = True
# =========================================================================
# Main integration loop
# =========================================================================
# initialize
tvec = np.arange(t0, paramsNom.tf + paramsTrue.dtGdn, paramsTrue.dtGdn)
tvecEval = np.empty(1)
tvecEval[0] = t0
xxvec = np.empty((len(xx0vec), 1))
xxvec[:] = np.NaN
xxvec[:, 0] = xx0vec
tsList = []
sigdList = []
t = t0
ind = 0
xx0veci = xx0vec
## PHASE 1 ##
phase = 1
for ind, t in enumerate(tvec):
# update guidance
if updatesOn:
ts = updateFNPAG(xx0veci,
t,
ts,
sig0,
sigd,
phase,
mode,
paramsNom,
sphericalEOMs=True)
# update event with new ts value
event3 = lambda t, y: ODEs.switchEvent(t, y, ts)
event3.terminal = True
if verbose:
print('PHASE 1: updating guidance at time {0:.3f} ts = {1:.3f}'\
.format(t, ts))
tsList.append(ts)
# propagate until next guidance update or switching time
tspan = (t, t + paramsTrue.dtGdn)
soli = solve_ivp(lambda t, y: ODEs.sphericalEntryEOMs(
t, y, np.radians(sig0), paramsTrue),
tspan,
xx0veci,
rtol=paramsTrue.rtol,
atol=paramsTrue.atol,
events=(event1, event2, event3))
# append results
xxvec = np.append(xxvec, soli.y, axis=1)
tvecEval = np.append(tvecEval, soli.t)
xx0veci = xxvec[:, -1]
# either proceed to next guidance update or enter phase 2
if soli.status == 0:
# reached next guidance update
continue
elif len(soli.t_events[2]) > 0:
# reached switching time
break
else:
sys.exit('propagator never reached next guidance update or'\
' switching time in phase 1')
xswitchvec = xxvec[:, -1] # save state at switching time
tvecP1 = tvecEval * 1
## create new tvec for phase 2
tvec = np.arange(tvecEval[-1], paramsNom.tf + paramsTrue.dtGdn,
paramsTrue.dtGdn)
## PHASE 2 ##
phase = 2
if verbose:
print()
for ind, t in enumerate(tvec):
# update guidance
if updatesOn:
sigd = updateFNPAG(xxvec[:, -1], t, ts, sig0, sigd, phase, mode,
paramsNom)
# make sure sigd is in [-180, 180] deg range
sigd = (sigd + 180) % (360) - 180
# if verbose:
# print('PHASE 2: updating guidance at time {0:.3f}'\
# ' sigd = {1:.3f} deg'.format(t, sigd))
sigdList.append(sigd)
# propagate until next guidance update or final state
tspan = (t, t + paramsTrue.dtGdn)
soli = solve_ivp(lambda t, y: ODEs.sphericalEntryEOMs(
t, y, np.radians(sigd), paramsTrue),
tspan,
xx0veci,
rtol=paramsTrue.rtol,
atol=paramsTrue.atol,
events=(event1, event2))
# append results
xxvec = np.append(xxvec, soli.y, axis=1)
tvecEval = np.append(tvecEval, soli.t)
xx0veci = xxvec[:, -1]
if soli.status == 1:
# reached terminal state
break
# trim off first array elements
xxvec = xxvec[:, 1:]
tvecEval = tvecEval[1:]
tvecP2 = tvecEval * 1
if plotsOn:
# uses global variable ax for plotting!
ax.plot(xxvec[3, :] / 1e3, xxvec[0, :] / 1e3 - paramsNom.p.rad)
ax.plot(xswitchvec[3] / 1e3,
xswitchvec[0] / 1e3 - paramsNom.p.rad,
'o',
markersize=6,
color=plt.gca().lines[-1].get_color())
# =========================================================================
# Compute apoapsis error and DV magnitudes
# =========================================================================
raf, rpf, engf = getApsesSphPR(xxvec[:, -1], paramsTrue, returnEng=True)
raErr = raf - paramsTrue.raStar
mu = paramsTrue.p.mu * 1e9
# if hyperbolic orbit, assign NaN to both DV values
if raf < 0:
DV1 = np.NaN
DV2 = np.NaN
else:
DV1 = np.sqrt(2 * mu)\
* abs((np.sqrt(1/raf - 1\
/ (raf + paramsTrue.rpStar))\
- np.sqrt(1/raf - 1 / (raf + rpf))))
DV2 = np.sqrt(2 * mu)\
* abs((np.sqrt(1/paramsTrue.rpStar - 1\
/ (paramsTrue.raStar + paramsTrue.rpStar))\
- np.sqrt(1/paramsTrue.rpStar - 1\
/ (raf + paramsTrue.rpStar))))
DV = DV1 + DV2 # m/s
print('final apoapsis error: {0:.3e} m'.format(raErr))
print('delta-V for periapsis raise: {0:.3f} m/s'.format(DV1))
print('delta-V for apoapsis correction: {0:.3f} m/s'.format(DV2))
print('total delta-V: {0:.3f} m/s'.format(DV))
return xxvec, tvecEval, raf, rpf, engf, raErr, DV,\
tsList, sigdList, tvecP1, tvecP2, xswitchvec
def doFNPEG(paramsTrue,
paramsNom,
t0,
xx0vec,
sig0,
e0,
verbose=True,
plotsOn=True,
updatesOn=True):
'''
main function for FNPEG. paramsTrue holds real values used in propagation,
paramsNom holds nominal values used in prediction for guidance updates.
Uses augmented spherical planet-relative EOMs.
'''
print('\nRUNNING FNPEG...')
# =========================================================================
# Set up events
# =========================================================================
event1 = lambda t, y: ODEs.above_max_alt_sph(t, y, paramsTrue)
event1.terminal = True
event1.direction = 1
event2 = lambda t, y: ODEs.below_min_alt_sph(t, y, paramsTrue)
event2.terminal = True
event3 = lambda t, y: engEvent(t, y, paramsTrue)
event3.terminal = True
# =========================================================================
# Main integration loop
# =========================================================================
# initialize
tvec = np.arange(t0, paramsNom.tf + paramsTrue.dtGdn, paramsTrue.dtGdn)
tvecEval = np.empty(1)
tvecEval[0] = t0
xxvec = np.empty((len(xx0vec), 1))
xxvec[:] = np.NaN
xxvec[:, 0] = xx0vec
evec = np.empty(1)
evec[:] = np.NaN
sigvec = np.empty(1)
sigvec[:] = np.NaN
sig0List = []
t = t0
ind = 0
xx0veci = xx0vec * 1
for ind, t in enumerate(tvec):
# update guidance
if updatesOn:
sig0 = updateFNPEG(xx0veci, t, sig0, e0, paramsNom)
if verbose:
print('FNPEG: updating guidance at time {0:.3f} '\
' sig0 = {1:.3f} deg'.format(t, sig0))
sig0List.append(sig0)
# propagate until next guidance update or switching time
tspan = (t, t + paramsTrue.dtGdn)
soli = solve_ivp(lambda t, y: ODEs.sphericalEntryEOMsAug(
t, y, sig0, e0, paramsTrue),
tspan,
xx0veci,
rtol=paramsTrue.rtol,
atol=paramsTrue.atol,
events=(event1, event2, event3))
# get bank angle history
eveci = paramsTrue.p.mu * 1e9 / soli.y[0, :] - soli.y[3, :]**2 / 2
sigveci = sig0 + (eveci - e0) /\
(paramsTrue.ef - e0) * (paramsTrue.sigf - sig0)
# append results
xxvec = np.append(xxvec, soli.y, axis=1)
tvecEval = np.append(tvecEval, soli.t)
evec = np.append(evec, eveci)
sigvec = np.append(sigvec, sigveci)
xx0veci = xxvec[:, -1]
if soli.status == 1:
break
# trim off first array elements
xxvec = xxvec[:, 1:]
tvecEval = tvecEval[1:]
evec = evec[1:]
sigvec = sigvec[1:]
if plotsOn:
# uses global variable ax2 for plotting!
ax2.plot(xxvec[3, :] / 1e3, xxvec[0, :] / 1e3 - paramsNom.p.rad)
# =========================================================================
# Compute target errors
# =========================================================================
xxfvec = xxvec[:, -1]
sfErr = np.degrees(xxfvec[6])
hfErr = xxfvec[0] - paramsTrue.rf
vfErr = xxfvec[3] - paramsTrue.vf
print('Final range error: {0:.6e} deg'.format(sfErr))
print('Final altitude error: {0:.6f} m'.format(hfErr))
print('Final velocity error: {0:.6f} m/s'.format(vfErr))
return xxvec, tvecEval, sfErr, hfErr, vfErr, evec, sigvec, sig0List
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