def unit_test_twobody():
# Orbit Parameter Setup
params = {}
params['GM'] = GME
params['J2'] = J2E
params['wE'] = wE # rad/s
params['Re'] = Re # km
params['Cd'] = 2.2
params['A_m'] = 1e-8 # km^2/kg
# Integration times
# tin = np.array([0., 86400.*2.])
tin = np.arange(0., 86400. * 2 + 1., 10.)
# Initial orbit - Molniya
Xo = np.array([
2.88824880e3, -7.73903934e2, -5.97116199e3, 2.64414431, 9.86808092, 0.0
])
# Integration function and additional settings
int_params = {}
int_params['integrator'] = 'ode'
int_params['ode_integrator'] = 'dop853'
int_params['intfcn'] = ode_twobody
int_params['step'] = 10.
int_params['rtol'] = 1e-12
int_params['atol'] = 1e-12
int_params['local_extrap'] = True
# Run integrator
tout, Xout = general_dynamics(Xo, tin, params, int_params)
print(len(tout))
print(tout[-1])
# Analytic solution
elem = cart2kep(Xo, GM=params['GM'])
a = elem[0]
e = elem[1]
i = elem[2]
RAAN = elem[3]
w = elem[4]
theta0 = elem[5] * pi / 180.
E0 = true2ecc(theta0, e)
M0 = ecc2mean(E0, e)
n = np.sqrt(params['GM'] / a**3.)
kk = 0
a_diff = []
e_diff = []
i_diff = []
RAAN_diff = []
w_diff = []
theta_diff = []
energy_diff = []
pos_diff = []
vel_diff = []
for t in tout:
# Compute new mean anomaly [rad]
M = M0 + n * (t - tout[0])
while M > 2 * pi:
M -= 2 * pi
# Convert to true anomaly [rad]
E = mean2ecc(M, e)
theta = ecc2true(E, e)
# Convert anomaly angles to deg
M *= 180. / pi
E *= 180. / pi
theta *= 180. / pi
X_true = kep2cart([a, e, i, RAAN, w, theta], GM=params['GM'])
elem_true = [a, e, i, RAAN, w, theta]
# Convert numeric to elements
elem_num = cart2kep(Xout[kk, :], GM=params['GM'])
a_diff.append(elem_num[0] - elem_true[0])
e_diff.append(elem_num[1] - elem_true[1])
i_diff.append(elem_num[2] - elem_true[2])
RAAN_diff.append(elem_num[3] - elem_true[3])
w_diff.append(elem_num[4] - elem_true[4])
theta_diff.append(elem_num[5] - elem_true[5])
pos_diff.append(
np.linalg.norm(X_true[0:3].flatten() - Xout[kk, 0:3].flatten()))
vel_diff.append(
np.linalg.norm(X_true[3:6].flatten() - Xout[kk, 3:6].flatten()))
if RAAN_diff[kk] < 0:
RAAN_diff[kk] += 360.
if RAAN_diff[kk] > 180:
RAAN_diff[kk] -= 360.
energy_diff.append(params['GM'] / (2 * elem_true[0]) - params['GM'] /
(2 * elem_num[0]))
kk += 1
plt.figure()
plt.subplot(3, 1, 1)
plt.plot(tout / 3600., energy_diff, 'k.')
plt.ylabel('Energy [km^2/s^2]')
plt.title('Size and Shape Parameters')
plt.subplot(3, 1, 2)
plt.plot(tout / 3600., np.asarray(a_diff), 'k.')
plt.ylabel('SMA [km]')
plt.subplot(3, 1, 3)
plt.plot(tout / 3600., e_diff, 'k.')
plt.ylabel('Eccentricity')
plt.xlabel('Time [hours]')
plt.figure()
plt.subplot(3, 1, 1)
plt.plot(tout / 3600., i_diff, 'k.')
plt.ylabel('Inclination [deg]')
plt.title('Orientation Parameters')
plt.subplot(3, 1, 2)
plt.plot(tout / 3600., RAAN_diff, 'k.')
plt.ylabel('RAAN [deg]')
plt.subplot(3, 1, 3)
plt.plot(tout / 3600., w_diff, 'k.')
plt.ylabel('AoP [deg]')
plt.xlabel('Time [hours]')
plt.figure()
plt.subplot(2, 1, 1)
plt.plot(tout / 3600., np.asarray(pos_diff), 'k.')
plt.ylabel('3D Pos [km]')
plt.title('Position and Velocity')
plt.subplot(2, 1, 2)
plt.plot(tout / 3600., np.asarray(vel_diff), 'k.')
plt.ylabel('3D Vel [km/s]')
plt.xlabel('Time [hours]')
plt.show()
return
def twobody_geo_setup():
arcsec2rad = pi / (3600. * 180.)
# Retrieve latest EOP data from celestrak.com
eop_alldata = get_celestrak_eop_alldata()
# Retrieve polar motion data from file
XYs_df = get_XYs2006_alldata()
# Define state parameters
state_params = {}
state_params['GM'] = GME
state_params['radius_m'] = 1.
state_params['albedo'] = 0.1
# Integration function and additional settings
int_params = {}
int_params['integrator'] = 'ode'
int_params['ode_integrator'] = 'dop853'
int_params['intfcn'] = ode_twobody
int_params['rtol'] = 1e-12
int_params['atol'] = 1e-12
int_params['time_system'] = 'datetime'
# Time vector
tk_list = []
for hr in range(24):
UTC = datetime(2021, 6, 21, hr, 0, 0)
tvec = np.arange(0., 601., 60.)
tk_list.extend([UTC + timedelta(seconds=ti) for ti in tvec])
# Inital State
elem = [42164.1, 0.001, 0., 90., 0., 0.]
X_true = np.reshape(kep2cart(elem), (6, 1))
P = np.diag([1., 1., 1., 1e-6, 1e-6, 1e-6])
pert_vect = np.multiply(np.sqrt(np.diag(P)), np.random.randn(6))
X_init = X_true + np.reshape(pert_vect, (6, 1))
state_dict = {}
state_dict[tk_list[0]] = {}
state_dict[tk_list[0]]['X'] = X_init
state_dict[tk_list[0]]['P'] = P
# Sensor and measurement parameters
sensor_id_list = ['UNSW Falcon']
sensor_params = define_sensors(sensor_id_list)
sensor_params['eop_alldata'] = eop_alldata
sensor_params['XYs_df'] = XYs_df
for sensor_id in sensor_id_list:
sensor_params[sensor_id]['meas_types'] = ['ra', 'dec']
sigma_dict = {}
sigma_dict['ra'] = 5. * arcsec2rad # rad
sigma_dict['dec'] = 5. * arcsec2rad # rad
sensor_params[sensor_id]['sigma_dict'] = sigma_dict
print(sensor_params)
# Generate truth and measurements
truth_dict = {}
meas_fcn = H_radec
meas_dict = {}
meas_dict['tk_list'] = []
meas_dict['Yk_list'] = []
meas_dict['sensor_id_list'] = []
X = X_true.copy()
for kk in range(len(tk_list)):
if kk > 0:
tin = [tk_list[kk - 1], tk_list[kk]]
tout, Xout = general_dynamics(X, tin, state_params, int_params)
X = Xout[-1, :].reshape(6, 1)
truth_dict[tk_list[kk]] = X
# Check visibility conditions and compute measurements
UTC = tk_list[kk]
EOP_data = get_eop_data(eop_alldata, UTC)
for sensor_id in sensor_id_list:
sensor = sensor_params[sensor_id]
if check_visibility(X, state_params, sensor, UTC, EOP_data,
XYs_df):
# Compute measurements
Yk = compute_measurement(X,
state_params,
sensor,
UTC,
EOP_data,
XYs_df,
meas_types=sensor['meas_types'])
Yk[0] += np.random.randn() * sigma_dict['ra']
Yk[1] += np.random.randn() * sigma_dict['dec']
meas_dict['tk_list'].append(UTC)
meas_dict['Yk_list'].append(Yk)
meas_dict['sensor_id_list'].append(sensor_id)
# Plot data
tplot = [(tk - tk_list[0]).total_seconds() / 3600. for tk in tk_list]
xplot = []
yplot = []
zplot = []
for tk in tk_list:
X = truth_dict[tk]
xplot.append(X[0])
yplot.append(X[1])
zplot.append(X[2])
meas_tk = meas_dict['tk_list']
meas_tplot = [(tk - tk_list[0]).total_seconds() / 3600. for tk in meas_tk]
meas_sensor_id = meas_dict['sensor_id_list']
meas_sensor_index = [
sensor_id_list.index(sensor_id) for sensor_id in meas_sensor_id
]
plt.figure()
plt.subplot(3, 1, 1)
plt.plot(tplot, xplot, 'k.')
plt.ylabel('X [km]')
plt.subplot(3, 1, 2)
plt.plot(tplot, yplot, 'k.')
plt.ylabel('Y [km]')
plt.subplot(3, 1, 3)
plt.plot(tplot, zplot, 'k.')
plt.ylabel('Z [km]')
plt.xlabel('Time [hours]')
plt.figure()
plt.plot(meas_tplot, meas_sensor_index, 'k.')
plt.xlabel('Time [hours]')
plt.ylabel('Sensor ID')
plt.show()
print(meas_dict)
pklFile = open('twobody_geo_setup.pkl', 'wb')
pickle.dump([
state_dict, state_params, int_params, meas_fcn, meas_dict,
sensor_params, truth_dict
], pklFile, -1)
pklFile.close()
return
def ls_batch(state_dict, truth_dict, meas_dict, meas_fcn, state_params,
sensor_params, int_params):
'''
This function implements the linearized batch estimator for the least
squares cost function.
Parameters
------
state_dict : dictionary
initial state and covariance for filter execution
meas_dict : dictionary
measurement data over time for the filter and parameters (noise, etc)
meas_fcn : function handle
function for measurements
Returns
------
filter_output : dictionary
output state, covariance, and post-fit residuals over time
'''
# State information
state_tk = sorted(state_dict.keys())[-1]
Xo_ref = state_dict[state_tk]['X']
Po_bar = state_dict[state_tk]['P']
# Setup
cholPo = np.linalg.inv(np.linalg.cholesky(Po_bar))
invPo_bar = np.dot(cholPo.T, cholPo)
n = len(Xo_ref)
# Initialize output
filter_output = {}
# Measurement times
tk_list = meas_dict['tk_list']
Yk_list = meas_dict['Yk_list']
sensor_id_list = meas_dict['sensor_id_list']
# Number of epochs
L = len(tk_list)
# Initialize
maxiters = 10
xo_bar = np.zeros((n, 1))
xo_hat = np.zeros((n, 1))
phi0 = np.identity(n)
phi0_v = np.reshape(phi0, (n**2, 1))
# Begin Loop
iters = 0
xo_hat_mag = 1
conv_crit = 1e-5
while xo_hat_mag > conv_crit:
# Increment loop counter and exit if necessary
iters += 1
if iters > maxiters:
iters -= 1
print('Solution did not converge in ', iters, ' iterations')
print('Last xo_hat magnitude: ', xo_hat_mag)
break
# Initialze values for this iteration
Xref_list = []
phi_list = []
resids_list = []
phi_v = phi0_v.copy()
Xref = Xo_ref.copy()
Lambda = invPo_bar.copy()
N = np.dot(Lambda, xo_bar)
# Loop over times
for kk in range(L):
# Current and previous time
if kk == 0:
tk_prior = state_tk
else:
tk_prior = tk_list[kk - 1]
tk = tk_list[kk]
# Read the next observation
Yk = Yk_list[kk]
sensor_id = sensor_id_list[kk]
# Initialize
Xref_prior = Xref.copy()
# Initial Conditions for Integration Routine
int0 = np.concatenate((Xref_prior, phi_v))
# Integrate Xref and STM
if tk_prior == tk:
intout = int0.T
else:
int0 = int0.flatten()
tin = [tk_prior, tk]
tout, intout = general_dynamics(int0, tin, state_params,
int_params)
# Extract values for later calculations
xout = intout[-1, :]
Xref = xout[0:n].reshape(n, 1)
phi_v = xout[n:].reshape(n**2, 1)
phi = np.reshape(phi_v, (n, n))
# Accumulate the normal equations
Hk_til, Gk, Rk = meas_fcn(tk, Xref, state_params, sensor_params,
sensor_id)
yk = Yk - Gk
Hk = np.dot(Hk_til, phi)
cholRk = np.linalg.inv(np.linalg.cholesky(Rk))
invRk = np.dot(cholRk.T, cholRk)
Lambda += np.dot(Hk.T, np.dot(invRk, Hk))
N += np.dot(Hk.T, np.dot(invRk, yk))
# Store output
resids_list.append(yk)
Xref_list.append(Xref)
phi_list.append(phi)
# print(kk)
# print(tk)
# print(int0)
# print(Xref)
# print(Yk)
# print(Gk)
# print(yk)
#
# if kk > 0:
# mistake
# Solve the normal equations
cholLam_inv = np.linalg.inv(np.linalg.cholesky(Lambda))
Po = np.dot(cholLam_inv.T, cholLam_inv)
xo_hat = np.dot(Po, N)
xo_hat_mag = np.linalg.norm(xo_hat)
# Update for next batch iteration
Xo_ref = Xo_ref + xo_hat
xo_bar = xo_bar - xo_hat
print('Iteration Number: ', iters)
print('xo_hat_mag = ', xo_hat_mag)
# Form output
for kk in range(L):
tk = tk_list[kk]
X = Xref_list[kk]
resids = resids_list[kk]
phi = phi_list[kk]
P = np.dot(phi, np.dot(Po, phi.T))
filter_output[tk] = {}
filter_output[tk]['X'] = X
filter_output[tk]['P'] = P
filter_output[tk]['resids'] = resids
# Integrate over full time
tk_truth = list(truth_dict.keys())
phi_v = phi0_v.copy()
Xref = Xo_ref.copy()
full_state_output = {}
for kk in range(len(tk_truth)):
# Current and previous time
if kk == 0:
tk_prior = state_tk
else:
tk_prior = tk_truth[kk - 1]
tk = tk_truth[kk]
# Initial Conditions for Integration Routine
Xref_prior = Xref.copy()
int0 = np.concatenate((Xref_prior, phi_v))
# Integrate Xref and STM
if tk_prior == tk:
intout = int0.T
else:
int0 = int0.flatten()
tin = [tk_prior, tk]
tout, intout = general_dynamics(int0, tin, state_params,
int_params)
# Extract values for later calculations
xout = intout[-1, :]
Xref = xout[0:n].reshape(n, 1)
phi_v = xout[n:].reshape(n**2, 1)
phi = np.reshape(phi_v, (n, n))
P = np.dot(phi, np.dot(Po, phi.T))
full_state_output[tk] = {}
full_state_output[tk]['X'] = Xref
full_state_output[tk]['P'] = P
return filter_output, full_state_output
def balldrop_setup():
# Define state parameters
acc = 9.81 #m/s^2
state_params = {}
state_params['acc'] = acc
state_params['Q'] = np.diag([1e-12, 1e-12])
# Integration function and additional settings
int_params = {}
int_params['integrator'] = 'ode'
int_params['ode_integrator'] = 'dop853'
int_params['intfcn'] = ode_balldrop
int_params['rtol'] = 1e-12
int_params['atol'] = 1e-12
int_params['time_system'] = 'seconds'
# Time vector
tk_list = np.arange(0., 100.1, 1.)
# Inital State
X_true = np.array([[0.], [0.]])
P = np.array([[4., 0.], [0., 1.]])
pert_vect = np.multiply(np.sqrt(np.diag(P)), np.random.randn(2))
X_init = X_true + np.reshape(pert_vect, (2, 1))
state_dict = {}
state_dict[tk_list[0]] = {}
state_dict[tk_list[0]]['X'] = X_init
state_dict[tk_list[0]]['P'] = P
# Generate Truth and Measurements
truth_dict = {}
meas_dict = {}
meas_dict['tk_list'] = []
meas_dict['Yk_list'] = []
meas_dict['sensor_id_list'] = []
sensor_params = {}
sensor_params[1] = {}
sig_y = 0.01
sig_dy = 0.001
sensor_params[1]['sigma_dict'] = {}
sensor_params[1]['sigma_dict']['y'] = sig_y
sensor_params[1]['sigma_dict']['dy'] = sig_dy
sensor_params[1]['meas_types'] = ['y', 'dy']
meas_fcn = H_balldrop
outlier_inds = []
X = X_true.copy()
for kk in range(len(tk_list)):
if kk > 0:
tin = [tk_list[kk - 1], tk_list[kk]]
tout, Xout = general_dynamics(X, tin, state_params, int_params)
X = Xout[-1, :].reshape(2, 1)
truth_dict[tk_list[kk]] = X
if kk in outlier_inds:
y_noise = 100. * sig_y * np.random.randn()
else:
y_noise = sig_y * np.random.randn()
dy_noise = sig_dy * np.random.randn()
y_meas = float(X[0]) + y_noise
dy_meas = float(X[1]) + dy_noise
meas_dict['tk_list'].append(tk_list[kk])
meas_dict['Yk_list'].append(np.array([[y_meas], [dy_meas]]))
meas_dict['sensor_id_list'].append(1)
return state_dict, state_params, int_params, meas_fcn, meas_dict, sensor_params, truth_dict