def multiple_model_corrector(model_bank_in,
Yi,
ti,
sun_gcrf,
sensor,
EOP_data,
XYs_df,
method='mmae',
alpha=1e-4):
# Initialize Output
model_bank = copy.deepcopy(model_bank_in)
# Compute update components
wbar = []
beta_list = []
for model_id in sorted(model_bank_in.keys()):
# Retrieve model parameters
spacecraftConfig = model_bank_in[model_id]['spacecraftConfig']
surfaces = model_bank_in[model_id]['surfaces']
Xbar = spacecraftConfig['X']
Pbar = spacecraftConfig['covar']
n = len(Xbar)
print(model_id)
print(Pbar)
print(np.linalg.eig(Pbar))
# Corrector
if spacecraftConfig['type'] == '3DoF':
X, P, beta = ukf_3dof_corrector(Xbar, Pbar, Yi, ti, n, alpha,
sun_gcrf, sensor, EOP_data, XYs_df,
spacecraftConfig, surfaces)
elif spacecraftConfig['type'] == '3att':
X, P, beta = ukf_3att_corrector(Xbar, Pbar, Yi, ti, n, alpha,
sun_gcrf, sensor, EOP_data, XYs_df,
spacecraftConfig, surfaces)
elif spacecraftConfig['type'] == '6DoF':
X, P, beta = ukf_6dof_corrector(Xbar, Pbar, qmean, Yi, ti, n,
alpha, sun_gcrf, sensor, EOP_data,
XYs_df, spacecraftConfig, surfaces)
else:
print('Spacecraft Type Error')
print(spacecraftConfig)
break
print('\n\n Corrector Step')
print(ti)
print(X)
print(P)
print(beta)
# Compute post-fit residuals
Ybar_post = compute_measurement(X, sun_gcrf, sensor, spacecraftConfig,
surfaces, ti, EOP_data,
sensor['meas_types'], XYs_df)
resids = Yi - Ybar_post
print('post')
print(model_id)
print('Ybar_post', Ybar_post)
print('resids', resids)
# Store weights and likelihoods for weight updates
wbar.append(model_bank_in[model_id]['weight'])
beta_list.append(beta)
# Update outputs for each model
model_bank[model_id]['spacecraftConfig']['time'] = ti
model_bank[model_id]['spacecraftConfig']['X'] = X
model_bank[model_id]['spacecraftConfig']['covar'] = P
model_bank[model_id]['resids'] = resids
# Update weights per mulitple model method
wf_list = multiple_model_weights(wbar, beta_list, method)
print('Update Weights')
print('wbar', wbar)
print('beta_list', beta_list)
print('wf_list', wf_list)
# Update model bank
ii = 0
for model_id in sorted(model_bank_in.keys()):
model_bank[model_id]['weight'] = np.array(wf_list[ii])
ii += 1
return model_bank
def ukf_6dof_corrector(Xgrp, Pbar, qmean, Yi, ti, n, alpha, sun_gcrf, sensor,
EOP_data, XYs_df, spacecraftConfig, surfaces):
#Compute Weights
beta = 2.
kappa = 3. - 12.
lam = alpha**2 * (12. + kappa) - 12.
gam = np.sqrt(12. + lam)
Wm = 1. / (2. * (12. + lam)) * np.ones((1, 2 * 12))
Wm = list(Wm.flatten())
Wc = copy.copy(Wm)
Wm.insert(0, lam / (12. + lam))
Wc.insert(0, lam / (12. + lam) + (1 - alpha**2 + beta))
Wm = np.asarray(Wm)
diagWc = np.diag(Wc)
# Sensor parameters
meas_types = sensor['meas_types']
sigma_dict = sensor['sigma_dict']
p = len(meas_types)
# Measurement noise
var = []
for mt in meas_types:
var.append(sigma_dict[mt]**2.)
Rk = np.diag(var)
# Recompute sigma points to incorporate process noise
sqP = np.linalg.cholesky(Pbar)
Xrep = np.tile(Xgrp, (1, 12))
chi_grp = np.concatenate((Xgrp, Xrep + (gam * sqP), Xrep - (gam * sqP)),
axis=1)
chi_diff = chi_grp - np.dot(Xgrp, np.ones((1, 25)))
# Compute sigma points with quaternions
chi_quat = np.zeros((13, 25))
chi_quat[0:6, :] = chi_grp[0:6, :]
chi_quat[10:13, :] = chi_grp[9:12, :]
chi_quat[6:10, 0] = qmean.flatten()
for jj in range(24):
dp = chi_grp[6:9, jj + 1].reshape(3, 1)
dq = att.grp2quat(dp, 1)
qj = att.quat_composition(dq, qmean)
chi_quat[6:10, jj + 1] = qj.flatten()
# Computed measurements
meas_bar = np.zeros((p, 25))
for jj in range(chi_quat.shape[1]):
Xj = chi_quat[:, jj]
Yj = compute_measurement(Xj, sun_gcrf, sensor, spacecraftConfig,
surfaces, ti, EOP_data, sensor['meas_types'],
XYs_df)
meas_bar[:, jj] = Yj.flatten()
# print(jj)
# print(Yj)
# print(Wm)
Ybar = np.dot(meas_bar, Wm.T)
Ybar = np.reshape(Ybar, (p, 1))
Y_diff = meas_bar - np.dot(Ybar, np.ones((1, 25)))
Pyy = np.dot(Y_diff, np.dot(diagWc, Y_diff.T))
Pxy = np.dot(chi_diff, np.dot(diagWc, Y_diff.T))
Pyy += Rk
# print(meas_bar)
# print(Yi)
# print(Ybar)
# print(Pyy)
# Measurement Update
K = np.dot(Pxy, np.linalg.inv(Pyy))
Xgrp = Xgrp + np.dot(K, Yi - Ybar)
# Compute updated quaternion and full state vector
dp = Xgrp[6:9].reshape(3, 1)
dq = att.grp2quat(dp, 1)
q = att.quat_composition(dq, qmean)
X = np.zeros((13, 1))
X[0:6] = Xgrp[0:6]
X[6:10] = q
X[10:13] = Xgrp[9:12]
# # Regular update
# P = Pbar - np.dot(K, np.dot(Pyy, K.T))
#
# # Re-symmetric pos def
# P = 0.5 * (P + P.T)
# Joseph Form
cholPbar = np.linalg.inv(np.linalg.cholesky(Pbar))
invPbar = np.dot(cholPbar.T, cholPbar)
P1 = (np.identity(12) - np.dot(np.dot(K, np.dot(Pyy, K.T)), invPbar))
P = np.dot(P1, np.dot(Pbar, P1.T)) + np.dot(K, np.dot(Rk, K.T))
# print('posterior')
# print(X)
# print(P)
# print(Ybar)
# print(Yi - Ybar)
# print(Pyy)
# print(Rk)
# print(Pxy)
# # Gaussian Likelihood
beta = compute_gaussian(Yi, Ybar, Pyy)
# beta_list.append(beta)
return X, P, beta
def ukf_3att_corrector(Xbar, Pbar, Yi, ti, n, alpha, sun_gcrf, sensor,
EOP_data, XYs_df, spacecraftConfig, surfaces):
#Compute Weights
n = 6
beta = 2.
kappa = 3. - n
lam = alpha**2 * (n + kappa) - n
gam = np.sqrt(n + lam)
Wm = 1. / (2. * (n + lam)) * np.ones((1, 2 * n))
Wm = list(Wm.flatten())
Wc = copy.copy(Wm)
Wm.insert(0, lam / (n + lam))
Wc.insert(0, lam / (n + lam) + (1 - alpha**2 + beta))
Wm = np.asarray(Wm)
diagWc = np.diag(Wc)
# Sensor parameters
meas_types = sensor['meas_types']
sigma_dict = sensor['sigma_dict']
p = len(meas_types)
# Measurement noise
var = []
for mt in meas_types:
var.append(sigma_dict[mt]**2.)
Rk = np.diag(var)
# Recompute sigma points to incorporate process noise
sqP = np.linalg.cholesky(Pbar)
Xpv = Xbar[0:6].reshape(6, 1)
Xatt = Xbar[6:13].reshape(7, 1)
Xrep = np.tile(Xpv, (1, n))
chi_bar = np.concatenate((Xpv, Xrep + (gam * sqP), Xrep - (gam * sqP)),
axis=1)
chi_diff = chi_bar - np.dot(Xpv, np.ones((1, (2 * n + 1))))
# Computed measurements
meas_bar = np.zeros((p, 2 * n + 1))
for jj in range(chi_bar.shape[1]):
Xj = chi_bar[:, jj].reshape(6, 1)
Xj_full = np.concatenate((Xj, Xatt), axis=0)
Yj = compute_measurement(Xj_full, sun_gcrf, sensor, spacecraftConfig,
surfaces, ti, EOP_data, sensor['meas_types'],
XYs_df)
meas_bar[:, jj] = Yj.flatten()
Ybar = np.dot(meas_bar, Wm.T)
Ybar = np.reshape(Ybar, (p, 1))
Y_diff = meas_bar - np.dot(Ybar, np.ones((1, (2 * n + 1))))
Pyy = np.dot(Y_diff, np.dot(diagWc, Y_diff.T))
Pxy = np.dot(chi_diff, np.dot(diagWc, Y_diff.T))
Pyy += Rk
# Measurement Update
K = np.dot(Pxy, np.linalg.inv(Pyy))
X = Xpv + np.dot(K, Yi - Ybar)
X = np.concatenate((X, Xatt), axis=0)
# # Regular update
# P = Pbar - np.dot(K, np.dot(Pyy, K.T))
#
# # Re-symmetric pos def
# P = 0.5 * (P + P.T)
# Joseph Form
cholPbar = np.linalg.inv(np.linalg.cholesky(Pbar))
invPbar = np.dot(cholPbar.T, cholPbar)
P1 = (np.identity(6) - np.dot(np.dot(K, np.dot(Pyy, K.T)), invPbar))
P = np.dot(P1, np.dot(Pbar, P1.T)) + np.dot(K, np.dot(Rk, K.T))
print('posterior')
print(Xbar)
print(X)
print(P)
print(Ybar)
print(Yi - Ybar)
# print(Pyy)
# print(Rk)
# print(Pxy)
# # Gaussian Likelihood
beta = compute_gaussian(Yi, Ybar, Pyy)
# beta_list.append(beta)
return X, P, beta
def unscented_kalman_filter(model_params_file,
sensor_file,
meas_file,
ephemeris,
ts,
alpha=1.):
'''
'''
# Load model parameters
pklFile = open(model_params_file, 'rb')
data = pickle.load(pklFile)
spacecraftConfig = data[0]
forcesCoeff = data[1]
surfaces = data[2]
eop_alldata = data[3]
XYs_df = data[4]
pklFile.close()
# Load sensor data
pklFile = open(sensor_file, 'rb')
data = pickle.load(pklFile)
sensor_dict = data[0]
pklFile.close()
# Load measurement data
pklFile = open(meas_file, 'rb')
data = pickle.load(pklFile)
meas_dict = data[0]
pklFile.close()
meas_times = sorted(list(meas_dict.keys()))
# # Force model parameters
# dragCoeff = forcesCoeff['dragCoeff']
# order = forcesCoeff['order']
# degree = forcesCoeff['degree']
# emissivity = forcesCoeff['emissivity']
# Sun and earth data
earth = ephemeris['earth']
sun = ephemeris['sun']
#Number of states and observations per epoch
n = len(spacecraftConfig['X'])
# Initial state parameters
X = spacecraftConfig['X'].reshape(n, 1)
P = spacecraftConfig['covar']
print(spacecraftConfig)
print(X)
# Loop over times
# beta_list = []
filter_output = {}
filter_output['time'] = []
filter_output['X'] = []
filter_output['P'] = []
filter_output['resids'] = []
for ii in range(len(meas_times)):
# Retrieve current and previous times
ti = meas_times[ii]
print('Current time: ', ti)
ti_prior = spacecraftConfig['time']
print(ti_prior)
delta_t = (ti - ti_prior).total_seconds()
print('delta_t', delta_t)
# Predictor
if spacecraftConfig['type'] == '3DoF':
Xbar, Pbar = \
ukf_3dof_predictor(X, P, delta_t, n, alpha,
spacecraftConfig, forcesCoeff, surfaces)
elif spacecraftConfig['type'] == '3att':
Xbar, Pbar = \
ukf_3att_predictor(X, P, delta_t, n, alpha,
spacecraftConfig, forcesCoeff, surfaces)
elif spacecraftConfig['type'] == '6DoF':
Xbar, Pbar, qmean = \
ukf_6dof_predictor(X, P, delta_t, n, alpha,
spacecraftConfig, forcesCoeff, surfaces)
else:
print('Spacecraft Type Error')
print(spacecraftConfig)
break
print('\n\n Predictor Step')
print(ti)
print(Xbar)
print(Pbar)
# Skyfield time and sun position
UTC_skyfield = ts.utc(ti.replace(tzinfo=utc))
sun_gcrf = earth.at(UTC_skyfield).observe(sun).position.km
sun_gcrf = np.reshape(sun_gcrf, (3, 1))
# EOPs at current time
EOP_data = get_eop_data(eop_alldata, ti)
# Loop over sensors
# Yi = meas[ii,:].reshape(len(meas[ii,:]),1)
sensor_id_list = list(meas_dict[ti].keys())
for sensor_id in sensor_id_list:
sensor = sensor_dict[sensor_id]
Yi = meas_dict[ti][sensor_id]
print('Yi', Yi)
# Corrector
if spacecraftConfig['type'] == '3DoF':
X, P, beta = ukf_3dof_corrector(Xbar, Pbar, Yi, ti, n, alpha,
sun_gcrf, sensor, EOP_data,
XYs_df, spacecraftConfig,
surfaces)
elif spacecraftConfig['type'] == '3att':
X, P, beta = ukf_3att_corrector(Xbar, Pbar, Yi, ti, n, alpha,
sun_gcrf, sensor, EOP_data,
XYs_df, spacecraftConfig,
surfaces)
elif spacecraftConfig['type'] == '6DoF':
X, P, beta = ukf_6dof_corrector(Xbar, Pbar, qmean, Yi, ti, n,
alpha, sun_gcrf, sensor,
EOP_data, XYs_df,
spacecraftConfig, surfaces)
else:
print('Spacecraft Type Error')
print(spacecraftConfig)
break
print('\n\n Corrector Step')
print(ti)
print(sensor_id)
print(X)
print(P)
print(beta)
# Update with post-fit solution
spacecraftConfig['time'] = ti
# Compute post-fit residuals
Ybar_post = compute_measurement(X, sun_gcrf, sensor, spacecraftConfig,
surfaces, ti, EOP_data,
sensor['meas_types'], XYs_df)
resids = Yi - Ybar_post
print('post')
print('Ybar_post', Ybar_post)
print('resids', resids)
# if ii > 3:
# mistake
# Append data to output
filter_output['time'].append(ti)
filter_output['X'].append(X)
filter_output['P'].append(P)
filter_output['resids'].append(resids)
return filter_output
def check_visibility(state, UTC_times, sun_gcrf_array, moon_gcrf_array, sensor,
spacecraftConfig, surfaces, eop_alldata, XYs_df=[]):
# Sensor parameters
mapp_lim = sensor['mapp_lim']
az_lim = sensor['az_lim']
el_lim = sensor['el_lim']
rg_lim = sensor['rg_lim']
sun_elmask = sensor['sun_elmask']
moon_angle_lim = sensor['moon_angle_lim']
geodetic_latlonht = sensor['geodetic_latlonht']
meas_types = ['rg', 'az', 'el']
# Sensor coordinates
lat = geodetic_latlonht[0]
lon = geodetic_latlonht[1]
ht = geodetic_latlonht[2]
sensor_itrf = latlonht2ecef(lat, lon, ht)
start = time.time()
# Loop over times and check visiblity conditions
vis_inds = []
for ii in range(len(UTC_times)):
# Retrieve time and current sun and object locations in ECI
UTC = UTC_times[ii]
Xi = state[ii,:]
sun_gcrf = sun_gcrf_array[:,ii].reshape(3,1)
# print(UTC)
if ii % 100 == 0:
print(ii)
print(UTC)
print('time elapsed:', time.time() - start)
# Compute measurements
EOP_data = get_eop_data(eop_alldata, UTC)
Yi = compute_measurement(Xi, sun_gcrf, sensor, spacecraftConfig,
surfaces, UTC, EOP_data, meas_types,
XYs_df)
# print(Yi)
rg = float(Yi[0])
az = float(Yi[1])
el = float(Yi[2])
# Compute sun elevation angle
sun_itrf, dum = gcrf2itrf(sun_gcrf, np.zeros((3,1)), UTC, EOP_data,
XYs_df)
sun_az, sun_el, sun_rg = ecef2azelrange_rad(sun_itrf, sensor_itrf)
# Check against constraints
vis_flag = True
if el < el_lim[0]:
vis_flag = False
if el > el_lim[1]:
vis_flag = False
if az < az_lim[0]:
vis_flag = False
if az > az_lim[1]:
vis_flag = False
if rg < rg_lim[0]:
vis_flag = False
if rg > rg_lim[1]:
vis_flag = False
if sun_el > sun_elmask:
vis_flag = False
# If passed constraints, check for eclipse and moon angle
if vis_flag:
# Compute angles
rso_gcrf = Xi[0:3].reshape(3,1)
moon_gcrf = moon_gcrf_array[:,ii].reshape(3,1)
sensor_gcrf, dum = \
itrf2gcrf(sensor_itrf, np.zeros((3,1)), UTC, EOP_data,
XYs_df)
phase_angle, sun_angle, moon_angle = \
compute_angles(rso_gcrf, sun_gcrf, moon_gcrf, sensor_gcrf)
# Check for eclipse - if sun angle is less than half cone angle
# the sun is behind the earth
# First check valid orbit - radius greater than Earth radius
r = np.linalg.norm(rso_gcrf)
if r < Re:
vis_flag = False
else:
half_cone = asin(Re/r)
if sun_angle < half_cone:
vis_flag = False
# Check too close to moon
if moon_angle < moon_angle_lim:
vis_flag = False
#TODO Moon Limits based on phase of moon (Meeus algorithm?)
# If still good, compute apparent mag
if vis_flag:
# print('az', az*180/pi)
# print('el', el*180/pi)
# print('sun az', sun_az*180/pi)
# print('sun el', sun_el*180/pi)
meas_types_mapp = ['mapp']
Yi = compute_measurement(Xi, sun_gcrf, sensor, spacecraftConfig,
surfaces, UTC, EOP_data, meas_types_mapp,
XYs_df)
if float(Yi[0]) > mapp_lim:
vis_flag = False
# If passed all checks, append to list
if vis_flag:
vis_inds.append(ii)
return vis_inds
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