def H_radec(tk, Xref, state_params, sensor_params, sensor_id):
# Compute sensor position in GCRF
eop_alldata = sensor_params['eop_alldata']
XYs_df = sensor_params['XYs_df']
EOP_data = get_eop_data(eop_alldata, tk)
sensor_kk = sensor_params[sensor_id]
sensor_itrf = sensor_kk['site_ecef']
sensor_gcrf, dum = itrf2gcrf(sensor_itrf, np.zeros((3, 1)), tk, EOP_data,
XYs_df)
# Measurement noise
meas_types = sensor_kk['meas_types']
sigma_dict = sensor_kk['sigma_dict']
p = len(meas_types)
Rk = np.zeros((p, p))
for ii in range(p):
mtype = meas_types[ii]
sig = sigma_dict[mtype]
Rk[ii, ii] = sig**2.
# Object location in GCRF
r_gcrf = Xref[0:3].reshape(3, 1)
# Compute range and line of sight vector
rho_gcrf = r_gcrf - sensor_gcrf
rg = np.linalg.norm(rho_gcrf)
rho_hat_gcrf = rho_gcrf / rg
ra = atan2(rho_hat_gcrf[1], rho_hat_gcrf[0]) #rad
dec = asin(rho_hat_gcrf[2]) #rad
# Calculate partials of rho
drho_dx = rho_hat_gcrf[0]
drho_dy = rho_hat_gcrf[1]
drho_dz = rho_hat_gcrf[2]
# Calculate partials of right ascension
d_atan = 1. / (1. + (rho_gcrf[1] / rho_gcrf[0])**2.)
dra_dx = d_atan * (-(rho_gcrf[1]) / ((rho_gcrf[0])**2.))
dra_dy = d_atan * (1. / (rho_gcrf[0]))
# Calculate partials of declination
d_asin = 1. / np.sqrt(1. - ((rho_gcrf[2]) / rg)**2.)
ddec_dx = d_asin * (-(rho_gcrf[2]) / rg**2.) * drho_dx
ddec_dy = d_asin * (-(rho_gcrf[2]) / rg**2.) * drho_dy
ddec_dz = d_asin * (1. / rg - ((rho_gcrf[2]) / rg**2.) * drho_dz)
# Hk_til and Gi
Gk = np.reshape([ra, dec], (2, 1))
Hk_til = np.zeros((2, 6))
Hk_til[0, 0] = dra_dx
Hk_til[0, 1] = dra_dy
Hk_til[1, 0] = ddec_dx
Hk_til[1, 1] = ddec_dy
Hk_til[1, 2] = ddec_dz
return Hk_til, Gk, Rk
def generate_sensor_location_file():
fdir = r'D:\documents\teaching\unsw_ssa_undergrad\2021\lab\telescope\orbit_determination\student_data'
fname = 'Pass-Data-SSA-3.csv'
meas_file_in = os.path.join(fdir, fname)
df = pd.read_csv(meas_file_in, header=None)
print(df)
t0 = datetime(2019, 9, 3, 10, 9, 42)
eop_alldata = get_celestrak_eop_alldata()
times = df.iloc[0,:]
lat_gs = -35.29
lon_gs = 149.17
ht_gs = 0.606 # km
stat_ecef = latlonht2ecef(lat_gs, lon_gs, ht_gs)
output = np.zeros((4,len(times)))
for ii in range(len(times)):
print(times[ii])
# ti = datetime.strptime(times[ii], '%Y-%m-%dT%H:%M:%S.%f')
ti = t0 + timedelta(seconds=times[ii])
ti_sec = (ti - t0).total_seconds()
EOP_data = get_eop_data(eop_alldata, ti)
stat_eci, dum = itrf2gcrf(stat_ecef, np.zeros((3,1)), ti, EOP_data)
print(stat_eci)
output[0,ii] = ti_sec
output[1,ii] = float(stat_eci[0])
output[2,ii] = float(stat_eci[1])
output[3,ii] = float(stat_eci[2])
print(output)
sensor_df = pd.DataFrame(output)
csv_name = os.path.join(fdir, 'sensor_eci_Mortaza.csv')
sensor_df.to_csv(csv_name, index=False)
return
ra_geo = []
dec_geo = []
ra_topo = []
dec_topo = []
for ii in range(len(UTC_list)):
# Retrieve object position vector
UTC = UTC_list[ii]
r_GCRF = output_state[obj_id_list[0]]['r_GCRF'][ii]
x = float(r_GCRF[0])
y = float(r_GCRF[1])
z = float(r_GCRF[2])
r = np.linalg.norm(r_GCRF)
# Retrieve sensor location in ECI
EOP_data = get_eop_data(eop_alldata, UTC)
stat_eci, dum = itrf2gcrf(stat_ecef, np.zeros((3, 1)), UTC, EOP_data)
xs = float(stat_eci[0])
ys = float(stat_eci[1])
zs = float(stat_eci[2])
rho = np.linalg.norm(r_GCRF - stat_eci)
print(UTC)
print(stat_eci)
# Compute measurements
ra_geo.append(atan2(y, x) * 180. / pi)
dec_geo.append(asin(z / r) * 180. / pi)
ra_topo.append(atan2((y - ys), (x - xs)) * 180. / pi)
dec_topo.append(asin((z - zs) / rho) * 180. / pi)
def multiple_model_filter(model_params_file,
sensor_file,
meas_file,
filter_output_file,
ephemeris,
ts,
method='mmae',
alpha=1.):
'''
'''
# Load model parameters
pklFile = open(model_params_file, 'rb')
data = pickle.load(pklFile)
model_bank = data[0]
eop_alldata = data[1]
XYs_df = data[2]
pklFile.close()
print(model_bank)
# 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_times = data[0]
meas = data[1]
pklFile.close()
# Sun and earth data
earth = ephemeris['earth']
sun = ephemeris['sun']
# Sensor and measurement parameters
sensor_id = list(sensor_dict.keys())[0]
sensor = sensor_dict[sensor_id]
# Save initial state in output
output_dict = {}
model_id0 = list(model_bank.keys())[0]
t0 = model_bank[model_id0]['spacecraftConfig']['time']
X0 = model_bank[model_id0]['spacecraftConfig']['X']
n = len(X0)
X0 = np.reshape(X0, (n, 1))
P0 = model_bank[model_id0]['spacecraftConfig']['covar']
extracted_model = {}
extracted_model['est_weights'] = np.array([1.])
extracted_model['est_means'] = np.reshape(X0[0:6], (6, 1))
extracted_model['est_covars'] = P0.copy()
UTC_JD = dt2jd(t0)
output_dict[UTC_JD] = {}
output_dict[UTC_JD]['extracted_model'] = copy.deepcopy(extracted_model)
output_dict[UTC_JD]['model_bank'] = copy.deepcopy(model_bank)
# Loop over times
for ii in range(len(meas_times)):
# Retrieve current and previous times
ti = meas_times[ii]
UTC_JD = dt2jd(ti)
print('Current time: ', ti)
# Predictor step
model_bank = multiple_model_predictor(model_bank, ti, alpha)
print('predictor')
print(model_bank)
# Read the next observation
Yi = meas[ii, :].reshape(len(meas[ii, :]), 1)
print('Yi', Yi)
# 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)
# Corrector step
model_bank = multiple_model_corrector(model_bank, Yi, ti, sun_gcrf,
sensor, EOP_data, XYs_df, method,
alpha)
print('corrector')
# Estimate extractor
extracted_model, model_bank = estimate_extractor(model_bank, method)
print('extracted model')
print(extracted_model)
print('model bank')
print(model_bank)
# Output
output_dict[UTC_JD] = {}
output_dict[UTC_JD]['extracted_model'] = copy.deepcopy(extracted_model)
output_dict[UTC_JD]['model_bank'] = copy.deepcopy(model_bank)
# if ii > 3:
# mistake
# Save data
pklFile = open(filter_output_file, 'wb')
pickle.dump([output_dict], pklFile, -1)
pklFile.close()
return output_dict
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
def truth_vs_tle():
plt.close('all')
# Setup
eop_alldata = get_celestrak_eop_alldata()
sensor_dict = define_sensors(['UNSW Falcon'])
latlonht = sensor_dict['UNSW Falcon']['geodetic_latlonht']
lat = latlonht[0]
lon = latlonht[1]
ht = latlonht[2]
stat_ecef = latlonht2ecef(lat, lon, ht)
# Object ID
norad_id = 42917
sp3_obj_id = 'PJ07'
# Measurement Times
t0 = datetime(2019, 9, 3, 10, 9, 42)
tf = datetime(2019, 9, 3, 18, 4, 42)
# Read SP3 file
fdir = 'D:\documents\\teaching\\unsw_ssa_undergrad\lab\\telescope\\truth_data'
fname = 'gbm20692.sp3'
sp3_fname = os.path.join(fdir, fname)
sp3_dict = read_sp3_file(sp3_fname)
gps_time_list = sp3_dict[sp3_obj_id]['datetime']
sp3_ecef_list = sp3_dict[sp3_obj_id]['r_ecef']
UTC_list = []
for ii in range(len(gps_time_list)):
gps_time = gps_time_list[ii]
sp3_ecef = sp3_ecef_list[ii]
EOP_data = get_eop_data(eop_alldata, gps_time)
# Convert to UTC
utc_time = gpsdt2utcdt(gps_time, EOP_data['TAI_UTC'])
UTC_list.append(utc_time) # + timedelta(seconds=ti) for ti in range(0,101,10)]
print(UTC_list[0:10])
print(UTC_list[0])
obj_id_list = [norad_id]
tle_dict, tle_df = get_spacetrack_tle_data(obj_id_list, [UTC_list[0]])
print(tle_dict)
output_state = propagate_TLE(obj_id_list, UTC_list, tle_dict)
ric_err = np.zeros((3, len(UTC_list)))
ti_hrs = []
truth_out = []
tle_ric_err = []
for ii in range(len(UTC_list)):
UTC = UTC_list[ii]
EOP_data = get_eop_data(eop_alldata, UTC)
tle_r_eci = output_state[obj_id]['r_GCRF'][ii]
tle_v_eci = output_state[obj_id]['v_GCRF'][ii]
# r_ecef, v_ecef = gcrf2itrf(r_eci, v_eci, UTC, EOP_data)
sp3_ecef = sp3_ecef_list[ii]
sp3_r_eci, dum = itrf2gcrf(sp3_ecef, np.zeros((3,1)), UTC, EOP_data)
rho_eci = sp3_r_eci - tle_r_eci # TLE data as chief
rho_ric = eci2ric(tle_r_eci, tle_v_eci, rho_eci)
rho_ric = -rho_ric # treats SP3 data as truth
ric_err[:,ii] = rho_ric.flatten()
ti_hrs.append((UTC - UTC_list[0]).total_seconds()/3600.)
if UTC >= t0 and UTC <= tf:
ti_output = (UTC - t0).total_seconds()
x_output = float(sp3_r_eci[0])
y_output = float(sp3_r_eci[1])
z_output = float(sp3_r_eci[2])
next_out = np.array([[ti_output], [x_output], [y_output], [z_output]])
if len(truth_out) == 0:
truth_out = next_out.copy()
else:
truth_out = np.concatenate((truth_out, next_out), axis=1)
ric_out = np.insert(ric_err[:,ii], 0, ti_output)
ric_out = np.reshape(ric_out, (4,1))
if len(tle_ric_err) == 0:
tle_ric_err = ric_out
else:
tle_ric_err = np.concatenate((tle_ric_err, ric_out), axis=1)
print(truth_out)
truth_df = pd.DataFrame(truth_out)
print(truth_df)
# csv_name = os.path.join(fdir, 'truth.csv')
# truth_df.to_csv(csv_name, index=False)
# csv_name2 = os.path.join(fdir, 'tle_ric_err.csv')
# tle_ric_df = pd.DataFrame(tle_ric_err)
# tle_ric_df.to_csv(csv_name2, index=False)
print('RMS Values')
n = len(UTC_list)
rms_r = np.sqrt((1/n) * np.dot(ric_err[0,:], ric_err[0,:]))
rms_i = np.sqrt((1/n) * np.dot(ric_err[1,:], ric_err[1,:]))
rms_c = np.sqrt((1/n) * np.dot(ric_err[2,:], ric_err[2,:]))
print('Radial RMS [km]:', rms_r)
print('In-Track RMS [km]:', rms_i)
print('Cross-Track RMS [km]:', rms_c)
plt.figure()
plt.subplot(3,1,1)
plt.plot(ti_hrs, ric_err[0,:], 'k.')
plt.ylabel('Radial [km]')
plt.ylim([-2, 2])
plt.title('RIC Error TLE vs SP3')
plt.subplot(3,1,2)
plt.plot(ti_hrs, ric_err[1,:], 'k.')
plt.ylabel('In-Track [km]')
plt.ylim([-5, 5])
plt.subplot(3,1,3)
plt.plot(ti_hrs, ric_err[2,:], 'k.')
plt.ylabel('Cross-Track [km]')
plt.ylim([-2, 2])
plt.xlabel('Time Since ' + UTC_list[0].strftime('%Y-%m-%d %H:%M:%S') + ' [hours]')
plt.show()
# print(UTC)
# print('ECI \n', r_eci)
# print('ECEF \n', r_ecef)
#
# sp3_ecef = np.array([[-25379.842058],[33676.622067],[51.528803]])
#
# print(sp3_ecef - r_ecef)
# print(np.linalg.norm(sp3_ecef - r_ecef))
#
# stat_eci, dum = itrf2gcrf(stat_ecef, np.zeros((3,1)), UTC, EOP_data)
#
# print(stat_eci)
# print(r_eci)
#
#
# rho_eci = np.reshape(r_eci, (3,1)) - np.reshape(stat_eci, (3,1))
#
# print(rho_eci)
# print(r_eci)
# print(stat_eci)
#
# ra = atan2(rho_eci[1], rho_eci[0]) * 180./pi
#
# dec = asin(rho_eci[2]/np.linalg.norm(rho_eci)) * 180./pi
#
# print('tle data')
# print(ra)
# print(dec)
#
#
# sp3_eci, dum = itrf2gcrf(sp3_ecef, np.zeros((3,1)), UTC, EOP_data)
#
# rho_eci2 = sp3_eci - stat_eci
#
# ra2 = atan2(rho_eci2[1], rho_eci2[0]) * 180./pi
#
# dec2 = asin(rho_eci2[2]/np.linalg.norm(rho_eci2)) * 180./pi
#
# print('sp3 data')
# print(ra2)
# print(dec2)
return