def plot_sensor_map(sensor_id_list):
sensor_dict = define_sensors(sensor_id_list)
plt.figure()
m = Basemap(projection='cyl',
llcrnrlat=-90,
urcrnrlat=90,
llcrnrlon=-180,
urcrnrlon=180,
resolution='c')
m.drawcoastlines()
for sensor_id in sensor_dict:
latlonht = sensor_dict[sensor_id]['geodetic_latlonht']
plt.plot(latlonht[1], latlonht[0], 'bo', ms=8)
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
get_tle_UTC = [datetime(2018, 10, 29, 0, 0, 0)]
tle_dict, tle_df = get_spacetrack_tle_data(obj_id_list, get_tle_UTC)
print(tle_dict)
fdir = Path(
'D:/documents/research/sensor_management/reports/2018_asrc_ROO/data')
fname = fdir / 'QZSS_OD_angles.csv'
UTC_list, ra_list, dec_list = read_csv_angles_meas(fname)
output_state = propagate_TLE(obj_id_list,
UTC_list,
tle_dict,
offline_flag=True)
sensor_dict = define_sensors(sensor_id_list)
latlonht = sensor_dict[sensor_id_list[0]]['geodetic_latlonht']
lat = latlonht[0]
lon = latlonht[1]
ht = latlonht[2]
stat_ecef = latlonht2ecef(lat, lon, ht)
eop_alldata = get_celestrak_eop_alldata(offline_flag=True)
ra_geo = []
dec_geo = []
ra_topo = []
dec_topo = []
for ii in range(len(UTC_list)):
# Retrieve object position vector
UTC = UTC_list[ii]
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