By expanding a trajectory to second order, the gradient of the covariance matrix (or any function thereof) with respect to nominal design parameters can be computed. This requires expanding in both initial states (and/or parametric uncertainties) and the design parameters. Due to the quadratic nature of the covariance matrix, a second order expansion of the trajectory is required, but this has the additional benefit of enabling Newton-type methods rather than being limited to gradient-based solutions or quasi-Newton approximations.
Much more care must be taken in a closed loop setting; first the nominal trajectory must be expanded, then any reference variables and gains must be constructed with DA variables. Then, the closed loop trajectory must be expanded in terms of initial conditions and other uncertain terms.
\begin{align} \Phi = \frac{\partial}{\partial {x_0}}x_f({x_0}) \\ P_f = \Phi P_0 \Phi^T \end{align}Goals:
In [1]:
import sys
sys.path.append("./EntryGuidance")
from scipy.interpolate import interp1d
from scipy.integrate import odeint, cumtrapz
from pyaudi import gdual_double, gdual_vdouble
import pyaudi as pa
import numpy as np
# import seaborn
import matplotlib.pyplot as plt
# from mpl_toolkits.mplot3d import Axes3D
# print(plt.style.available)
# plt.style.use("seaborn-whitegrid")
# from Utils.boxgrid import boxgrid
import Utils.DA as da
from Utils.RK4 import RK4
from Utils.submatrix import submatrix
from Utils.gpops import srp
from EntryGuidance.EntryEquations import Entry
from EntryGuidance.Simulation import Simulation, Cycle, EntrySim
from EntryGuidance.InitialState import InitialState
# from ParametrizedPlanner import profile
from EntryGuidance.Apollo import gains
In [38]:
x0 = InitialState(vehicle='heavy')
print("m0 = {} kg".format(x0[-1]))
Vf = 530
dasim = Simulation(cycle=Cycle(1), output=True, use_da=True, **EntrySim(Vf=Vf), )
names = ['r', 'theta', 'phi', 'V', 'fpa', 'psi', 's', 'm']
da_names = names + ['u']
order = 2
x0d = da.make(x0, names, order)
u = gdual_double(0.15, 'u', order)
ref_profile = lambda **args: u
res = dasim.run(x0d, [ref_profile])
In [44]:
dasim.history[-1, 3]
Out[44]:
In [30]:
print(da.hessian(dasim.history[-1,3], da_names))
In [8]:
da_names =['r', 'theta', 'phi', 'V', 'fpa', 'psi', 'u']
stmf = np.array([da.differentiate(x, da_names) for x in dasim.history[-1][0:6]])
In [9]:
P0 = np.diag([100, 0.0001, 0.0001, 1.5, np.radians(0.025), np.radians(0.02), 0])
Pf = stmf.dot(P0).dot(stmf.T)
# W = np.diag(np.ones()
# C = np.trace(Pf)
# dCdu = da.gradient(C, ['u'])
# dCdu
# dPfdu = np.array([da.gradient(x, ['u']) for x in np.diag(Pf)])
dPfdu = np.array([da.jacobian(x, ['u']) for x in Pf[:6]])
print(dPfdu)
Out[9]:
In [5]:
np.diag(Pf)
Out[5]:
In [6]:
H = np.array([da.hessian(x, ['u']) for x in np.diag(Pf)])
In [7]:
dPfdu/H.squeeze() # Newton step update
Out[7]:
In [3]:
import time
def da_integration_timer(order, cycle=1, steps=10):
x0 = InitialState(vehicle='heavy')
P0 = np.diag([100, 0.0001, 0.0001, 1.5, np.radians(0.025), np.radians(0.02)])
Vf = 530
dasim = Simulation(cycle=Cycle(cycle), output=False, use_da=True, **EntrySim(Vf=Vf), )
names = ['r', 'theta', 'phi', 'V', 'fpa', 'psi', 's', 'm']
da_names = names + ['u']
x0d = da.make(x0, names, order)
u = gdual_double(0.15, 'u', order)
ref_profile = lambda **args: u
t0 = time.time()
dasim.run(x0d, [ref_profile], StepsPerCycle=steps)
t = time.time() - t0
print("order {} DA integration: {:.2f} s".format(order, t))
# stmf = np.array([da.differentiate(x, da_names[0:6]) for x in dasim.history[-1][0:6]])
# Pf = stmf.dot(P0).dot(stmf.T)
return t, dasim
In [ ]:
times = []
for i in range(1, 5):
print(f"Order: {i}")
t,sim = da_integration_timer(i)
times.append(t)
times = np.array(times)
In [63]:
plt.plot(list(range(1,5)), times, 'o-')
plt.xlabel('DA Order, 9 variables')
plt.ylabel('Time, seconds')
plt.savefig('DA_time_vs_order.png')
# plt.show()
In [41]:
times = []
xf = []
# dts = [1, 5]
dts = [0.1, 0.5, 1, 5, 10]
for dt in dts:
print(f"dt: {dt}")
t,sim = da_integration_timer(order=2, cycle=dt, steps=2)
xf.append(sim.history)
times.append(t)
times = np.array(times)
# xf = np.array(xf)
In [42]:
plt.plot(dts, times, 'o-')
plt.ylabel('Time')
plt.xlabel('Step size (s)')
Out[42]:
In [43]:
# print(da.const(xf[:,3]))
names = ['r', 'theta', 'phi', 'V', 'fpa', 'psi', 'u']
for xfi in xf[1:]:
# print(da.const(xfi[:,3], True)[-1])
# interpolate the coarser integrations onto the same final velocity
xfv = da.interp([xf[0][-1,3].constant_cf], da.const(xfi[::-1,3], True), xfi[::-1])[0]
# for state in xfv:
# display(state.constant_cf)
# for state in xf[0][-1]:
# display(state.constant_cf)
delta = xfv-xf[0][-1]
h,lon,lat,v,fpa,psi,s,m = delta
for d,state,unit in zip(delta, ['radius','lon','lat'], ['m','rad','rad']):
print(f"Error in Constant Term of final {state}: {d.constant_cf} {unit}")
for d,state,unit in zip(delta[4:], ['fpa','heading'], ['deg']*2):
print(f"Error in Constant Term of final {state}: {np.degrees(d.constant_cf)} {unit}")
# display(da.jacobian(delta, names))
pdiff = da.jacobian(delta, names)/da.jacobian(xf[0][-1], names) * 100
pdiff[np.isnan(pdiff)] = 0
pdiff_max = np.max(np.abs(pdiff))
print("Maximum % error over the terminal jacobian = {:.3f}%".format(pdiff_max))
# display() # Percent diff, lots of nans anywhere with zero gradient
# for d in delta:
# display(da.gradient(delta[0], names))
# display(da.gradient(delta[0], names)/da.gradient(xf[0][-1]))
print("\n")
print("Somewhat surprisingly, large steps can be take with minimal loss, at least for a constant bank angle")
In [50]:
print(dasim.history[-1])
In [24]:
gradient = []
for unew in [0.149, 0.151,]:
ref_profile = lambda **args: unew
t0 = time.time()
res = dasim.run(x0d, [ref_profile])
print("\nFirst order DA integration: {:.2f} s".format(time.time()-t0))
stmf = np.array([da.differentiate(x, da_names[0:6]) for x in dasim.history[-1][0:6]])
Pf_new = stmf.dot(P0).dot(stmf.T)
dPf_du = ( Pf_new - Pf ) / (unew-u)
gradient.append(dPf_du)
print(dPf_du)
dPf_du_central = np.mean(gradient, axis=0)
print("\nCentral difference:")
print(dPf_du_central)
In [12]:
print("Using differential algebra, dPf/du = ")
print(dPfdu)
In [25]:
np.abs(dPfdu-da.const(dPf_du_central))/dPfdu * 100
Out[25]:
That weird saturation used in desensitizing the minimum fuel powered descent paper
In [40]:
umin = 0
umax = 1
u = np.linspace(0,1,300)
def sat(u, umin, umax):
return 4*(u-umin)*(umax-u)/(umax-umin)**2
plt.figure(figsize=(10,10))
plt.plot(u, sat(u, umin, umax)**0.5)
plt.grid(True)
plt.show()
In [8]:
[display(x) for x in dasim.history[2:4, 0]]
Out[8]:
In [96]:
da.interp([0.5],np.array([0,1]),dasim.history[2:4, 0])[0]
Out[96]:
In [16]:
np.mean(dasim.history[2:4, 0])
Out[16]:
In [43]:
dasim.history[100, 3].extract_terms(2).subs('dm', 0).subs('du', 0)
Out[43]:
In [2]:
from Triggers import AltitudeTrigger
x0 = InitialState(vehicle='heavy')
print("m0 = {} kg".format(x0[-1]))
Vf = 330
sim_conditions = EntrySim(Vf=Vf)
sim_conditions['conditions'] = [AltitudeTrigger(3)]
sim = Simulation(cycle=Cycle(1), output=True, **sim_conditions, )
ref_profile = lambda **args: np.sin(args['velocity']/1000.)*1.5
# v = np.linspace(5000, Vf, 1000)
# plt.plot(v, np.degrees(np.sin(v/500.)))
# plt.show()
res = sim.run(x0, [ref_profile])
sim.plot()
In [4]:
# target = (h, lon, lat)
target = [0e3, np.radians(15.4), 0.0]
def spherical_to_cartesian(x, target):
r,th,ph,v,fpa,azi,s,m = x
x,y = sim.edlModel.planet.range(th,ph,azi, target[1], target[2], km=False)
z = sim.edlModel.altitude(r, km=False) - target[0]
vx = v*np.cos(fpa)*np.cos(azi)
vy = v*np.cos(fpa)*np.sin(azi)
vz = v*np.sin(fpa)
return [x, y, z, vx, vy, vz]
In [6]:
def solve_srp(x_cart, m0, display=False):
x_cart.append(m0)
x_cart = [float(x) for x in x_cart]
srp_sol = srp((x_cart, 0)) # srp takes cartesian states, with the target at the origin
srp_x = np.array(srp_sol['state'])
srp_u = np.array(srp_sol['control'])
srp_t = np.array(srp_sol['time']).squeeze()
if display:
print("Optimal ToF = {:.1f} s".format(srp_t[-1]))
print("Optimal Prop = {:.1f} s".format(srp_x[0,-1]-srp_x[-1,-1]))
return srp_t, srp_x, srp_u
In [ ]:
print(sim.history.shape)
mprop = []
sol = []
for x in sim.history[240:-1:2]:
x_cart = spherical_to_cartesian(x, target)
print(x_cart)
t,x,u = solve_srp(x_cart, 8500, display=True)
sol.append((t,x,u))
mprop.append(x[-1,-1])
In [54]:
def srp_plot(srp_t, srp_x, srp_u):
figsize = (10,10)
plt.figure(figsize=figsize)
plt.plot(srp_t, srp_x[:,:3])
plt.xlabel('Time since ignition (s)')
plt.figure(figsize=figsize)
plt.plot(srp_t, srp_x[:,3:6])
plt.xlabel('Time since ignition (s)')
plt.figure(figsize=figsize)
plt.plot(srp_t, srp_x[:,-1])
plt.xlabel('Time since ignition (s)')
plt.figure(figsize=figsize)
plt.plot(srp_t, srp_u[:,0]/(70*8500))
plt.xlabel('Time since ignition (s)')
Out[54]: