def test_PropagatorSGP4_cart_polar_motion00(self):
prop = propagator_sgp4.PropagatorSGP4(polar_motion=True, polar_motion_model='00')
t = np.arange(0,24*360, dtype=np.float)*10.0
ecefs = self.prop.get_orbit_cart(
t=t, mjd0=self.mjd0,
x=-5339.76186573000e3, y=5721.43584226500e3, z=921.276953805000e3,
vx=-4.88969089550000e3, vy=-3.83304653050000e3, vz=3.18013811100000e3,
C_D=self.C_D, m=self.m, A=self.A,
)
assert ecefs.shape == (6, t.size)
assert isinstance(ecefs, np.ndarray)
def test_PropagatorSGP4_polar_motion00(self):
prop = propagator_sgp4.PropagatorSGP4(polar_motion=True, polar_motion_model='00')
t = np.arange(0,24*360, dtype=np.float)*10.0
ecefs = self.prop.get_orbit(
t=t, mjd0=self.mjd0,
a=7000e3, e=0.0, inc=90.0,
raan=10, aop=10, mu0=40.0,
C_D=self.C_D, m=self.m, A=self.A,
)
assert ecefs.shape == (6, t.size)
assert isinstance(ecefs, np.ndarray)
import os
import pymc3 as pm
import scipy
import numpy as np
import matplotlib.pyplot as plt
import theano.tensor as tt
import propagator_sgp4
prop = propagator_sgp4.PropagatorSGP4(
polar_motion = False,
out_frame = 'TEME',
)
# define a theano Op for our likelihood function
class LogLike(tt.Op):
"""
Specify what type of object will be passed and returned to the Op when it is
called.
"""
itypes = [tt.dvector] # expects a vector of parameter values when called
otypes = [tt.dscalar] # outputs a single scalar value (the log likelihood)
def __init__(self, likelihood, r, sigma_r, station, dt, mjd0):
"""
Initialise the Op
"""
# add inputs as class attributes
def setUp(self):
self.prop = propagator_sgp4.PropagatorSGP4(out_frame='ITRF')
def test_PropagatorSGP4_cart_kep_inverse_cases(self):
R_E = 6353.0e3
mjd0 = dpt.jd_to_mjd(2457126.2729)
a = R_E*2.0
orb_init_list = []
orb_range = np.array([a, 0.9, 180, 360, 360, 360], dtype=np.float)
orb_offset = np.array([R_E*1.1, 0, 0.0, 0.0, 0.0, 0.0], dtype=np.float)
test_n = 5000
while len(orb_init_list) < test_n:
orb = np.random.rand(6)
orb = orb_offset + orb*orb_range
if orb[0]*(1.0 - orb[1]) > R_E+200e3:
orb_init_list.append(orb)
orb_init_list.append(np.array([R_E*1.2, 0, 0.0, 0.0, 0.0, 0.0], dtype=np.float))
orb_init_list.append(np.array([R_E*1.2, 0, 0.0, 0.0, 0.0, 270], dtype=np.float))
orb_init_list.append(np.array([R_E*1.2, 1e-9, 0.0, 0.0, 0.0, 0.0], dtype=np.float))
orb_init_list.append(np.array([R_E*1.2, 0.1, 0.0, 0.0, 0.0, 0.0], dtype=np.float))
orb_init_list.append(np.array([R_E*1.2, 0.1, 75.0, 0.0, 0.0, 0.0], dtype=np.float))
orb_init_list.append(np.array([R_E*1.2, 0.1, 0.0, 120.0, 0.0, 0.0], dtype=np.float))
orb_init_list.append(np.array([R_E*1.2, 0.1, 0.0, 0.0, 35.0, 0.0], dtype=np.float))
orb_init_list.append(np.array([R_E*1.2, 0.1, 75.0, 120.0, 0.0, 0.0], dtype=np.float))
orb_init_list.append(np.array([R_E*1.2, 0.1, 75.0, 0.0, 35.0, 0.0], dtype=np.float))
orb_init_list.append(np.array([R_E*1.2, 0.1, 75.0, 120.0, 35.0, 0.0], dtype=np.float))
prop = propagator_sgp4.PropagatorSGP4(polar_motion=False)
t = np.array([0], dtype=np.float)
M_earth = propagator_sgp4.SGP4.GM*1e9/consts.G
fail_inds = []
errs = np.empty((len(orb_init_list),2), dtype=np.float)
for ind, kep in enumerate(orb_init_list):
state_TEME = dpt.kep2cart(kep, m=self.m, M_cent=M_earth, radians=False)
ecefs_kep = self.prop.get_orbit(
t=t, mjd0=mjd0,
a=kep[0], e=kep[1], inc=kep[2],
raan=kep[4], aop=kep[3], mu0=dpt.true2mean(kep[5],kep[1],radians=False),
C_D=self.C_D, m=self.m, A=self.A,
)
p = ecefs_kep[:3]*1e-3
p.shape=(3,1)
v = ecefs_kep[3:]*1e-3
v.shape=(3,1)
kep_TEME = propagator_sgp4.ecef2teme(np.array([0.0]), p, v, mjd0=mjd0)*1e3
kep_TEME.shape = (6,)
ecefs_cart = self.prop.get_orbit_cart(
t=t, mjd0=mjd0,
x=kep_TEME[0], y=kep_TEME[1], z=kep_TEME[2],
vx=kep_TEME[3], vy=kep_TEME[4], vz=kep_TEME[5],
C_D=self.C_D, m=self.m, A=self.A,
)
p = ecefs_cart[:3]*1e-3
p.shape=(3,1)
v = ecefs_cart[3:]*1e-3
v.shape=(3,1)
cart_TEME = propagator_sgp4.ecef2teme(np.array([0.0]), p, v, mjd0=mjd0)*1e3
cart_TEME.shape = (6,)
state_diff1 = np.abs(kep_TEME - state_TEME)
try:
nt.assert_array_less(state_diff1[:3], np.full((3,), 1e-2, dtype=state_diff1.dtype))
nt.assert_array_less(state_diff1[3:], np.full((3,), 1e-5, dtype=state_diff1.dtype))
except AssertionError as err:
if ind not in fail_inds:
fail_inds.append(ind)
state_diff2 = np.abs(cart_TEME - state_TEME)
try:
nt.assert_array_less(state_diff2[:3], np.full((3,), 1e-2, dtype=state_diff2.dtype))
nt.assert_array_less(state_diff2[3:], np.full((3,), 1e-5, dtype=state_diff2.dtype))
except AssertionError as err:
if ind not in fail_inds:
fail_inds.append(ind)
state_diff = np.abs(cart_TEME - kep_TEME)
try:
nt.assert_array_less(state_diff[:3], np.full((3,), 1e-2, dtype=state_diff.dtype))
nt.assert_array_less(state_diff[3:], np.full((3,), 1e-5, dtype=state_diff.dtype))
except AssertionError as err:
if ind not in fail_inds:
fail_inds.append(ind)
er_r = np.linalg.norm(state_diff1[:3])
er_v = np.linalg.norm(state_diff1[3:])
errs[ind,0] = er_r
errs[ind,1] = er_v
if len(fail_inds) > 0:
print('FAIL / TOTAL: {} / {}'.format(len(fail_inds), len(orb_init_list)))
assert np.median(errs[:,0]) < 1e-2
assert np.median(errs[:,1]) < 1e-4
assert len(fail_inds) < float(test_n)/100.
def test_PropagatorSGP4_cart_kep_cases(self):
R_E = 6353.0e3
mjd0 = dpt.jd_to_mjd(2457126.2729)
a = R_E*2.0
orb_init_list = []
orb_range = np.array([a, 0.9, 180, 360, 360, 360], dtype=np.float)
orb_offset = np.array([R_E*1.1, 0, 0.0, 0.0, 0.0, 0.0], dtype=np.float)
test_n = 100
while len(orb_init_list) < test_n:
orb = np.random.rand(6)
orb = orb_offset + orb*orb_range
if orb[0]*(1.0 - orb[1]) > R_E+200e3:
orb_init_list.append(orb)
orb_init_list.append(np.array([R_E*1.2, 0, 0.0, 0.0, 0.0, 0.0], dtype=np.float))
orb_init_list.append(np.array([R_E*1.2, 0, 0.0, 0.0, 0.0, 270], dtype=np.float))
orb_init_list.append(np.array([R_E*1.2, 1e-9, 0.0, 0.0, 0.0, 0.0], dtype=np.float))
orb_init_list.append(np.array([R_E*1.2, 0.1, 0.0, 0.0, 0.0, 0.0], dtype=np.float))
orb_init_list.append(np.array([R_E*1.2, 0.1, 75.0, 0.0, 0.0, 0.0], dtype=np.float))
orb_init_list.append(np.array([R_E*1.2, 0.1, 0.0, 120.0, 0.0, 0.0], dtype=np.float))
orb_init_list.append(np.array([R_E*1.2, 0.1, 0.0, 0.0, 35.0, 0.0], dtype=np.float))
orb_init_list.append(np.array([R_E*1.2, 0.1, 75.0, 120.0, 0.0, 0.0], dtype=np.float))
orb_init_list.append(np.array([R_E*1.2, 0.1, 75.0, 0.0, 35.0, 0.0], dtype=np.float))
orb_init_list.append(np.array([R_E*1.2, 0.1, 75.0, 120.0, 35.0, 0.0], dtype=np.float))
prop = propagator_sgp4.PropagatorSGP4(polar_motion=False)
t = np.linspace(0, 12*3600, num=100, dtype=np.float)
M_earth = propagator_sgp4.SGP4.GM*1e9/consts.G
for kep in orb_init_list:
state_TEME = dpt.kep2cart(kep, m=self.m, M_cent=M_earth, radians=False)
ecefs_kep = prop.get_orbit(
t=t, mjd0=mjd0,
a=kep[0], e=kep[1], inc=kep[2],
raan=kep[4], aop=kep[3], mu0=dpt.true2mean(kep[5], kep[1], radians=False),
C_D=self.C_D, m=self.m, A=self.A,
radians=False,
)
ecefs_cart = prop.get_orbit_cart(
t=t, mjd0=mjd0,
x=state_TEME[0], y=state_TEME[1], z=state_TEME[2],
vx=state_TEME[3], vy=state_TEME[4], vz=state_TEME[5],
C_D=self.C_D, m=self.m, A=self.A,
)
p = ecefs_kep[:3,:]*1e-3
v = ecefs_kep[3:,:]*1e-3
kep_TEME = propagator_sgp4.ecef2teme(t, p, v, mjd0=mjd0)*1e3
p = ecefs_kep[:3,:]*1e-3
v = ecefs_kep[3:,:]*1e-3
cart_TEME = propagator_sgp4.ecef2teme(t, p, v, mjd0=mjd0)*1e3
state_diff1 = np.abs(kep_TEME - cart_TEME)
nt.assert_array_less(state_diff1[:3,:], np.full((3,t.size), 1e-5, dtype=state_diff1.dtype))
nt.assert_array_less(state_diff1[3:,:], np.full((3,t.size), 1e-7, dtype=state_diff1.dtype))
def test_PropagatorSGP4_cart_kep_inverse(self):
prop = propagator_sgp4.PropagatorSGP4(polar_motion=False)
t = np.array([0], dtype=np.float)
reci = np.array([
-5339.76186573000,
5721.43584226500,
921.276953805000,
], dtype=np.float)
veci = np.array([
-4.88969089550000,
-3.83304653050000,
3.18013811100000,
], dtype=np.float)
state_TEME = np.concatenate((reci*1e3, veci*1e3), axis=0)
M_earth = propagator_sgp4.SGP4.GM*1e9/consts.G
kep = dpt.cart2kep(state_TEME, m=self.m, M_cent=M_earth, radians=False)
kep[5] = dpt.true2mean(kep[5], kep[1], radians=False)
ecefs_kep = prop.get_orbit(
t=t, mjd0=self.mjd0,
a=kep[0], e=kep[1], inc=kep[2],
raan=kep[4], aop=kep[3], mu0=kep[5],
C_D=self.C_D, m=self.m, A=self.A,
)
p = ecefs_kep[:3]*1e-3
p.shape=(3,1)
v = ecefs_kep[3:]*1e-3
v.shape=(3,1)
kep_TEME = propagator_sgp4.ecef2teme(np.array([0.0]), p, v, mjd0=self.mjd0)*1e3
kep_TEME.shape = (6,)
state_diff1 = np.abs(kep_TEME - state_TEME)
nt.assert_array_less(state_diff1[:3], np.full((3,), 1e-2, dtype=state_diff1.dtype))
nt.assert_array_less(state_diff1[3:], np.full((3,), 1e-5, dtype=state_diff1.dtype))
ecefs_cart = prop.get_orbit_cart(
t=t, mjd0=self.mjd0,
x=-5339.76186573000e3, y=5721.43584226500e3, z=921.276953805000e3,
vx=-4.88969089550000e3, vy=-3.83304653050000e3, vz=3.18013811100000e3,
C_D=self.C_D, m=self.m, A=self.A,
)
p = ecefs_cart[:3]*1e-3
p.shape=(3,1)
v = ecefs_cart[3:]*1e-3
v.shape=(3,1)
cart_TEME = propagator_sgp4.ecef2teme(np.array([0.0]), p, v, mjd0=self.mjd0)*1e3
cart_TEME.shape = (6,)
state_diff2 = np.abs(cart_TEME - state_TEME)
nt.assert_array_less(state_diff2[:3], np.full((3,), 1e-2, dtype=state_diff2.dtype))
nt.assert_array_less(state_diff2[3:], np.full((3,), 1e-5, dtype=state_diff2.dtype))
state_diff = np.abs(cart_TEME - kep_TEME)
nt.assert_array_less(state_diff[:3], np.full((3,), 1e-2, dtype=state_diff.dtype))
nt.assert_array_less(state_diff[3:], np.full((3,), 1e-5, dtype=state_diff.dtype))
def setUp(self):
self.mjd0 = dpt.jd_to_mjd(2457126.2729) # 2015-04-13T18:32:58.560019
self.C_D = 2.3
self.m = 8000
self.A = 1.0
self.prop = propagator_sgp4.PropagatorSGP4()
import dpt_tools as dpt
import propagator_sgp4
import numpy as np
import matplotlib.pyplot as plt
import scipy.constants as consts
M_earth = propagator_sgp4.SGP4.GM * 1e9 / consts.G
prop = propagator_sgp4.PropagatorSGP4()
def get_orbit(o):
#print('{:.2f}, {:.2f}, {:.2f}, {:.2f}, {:.2f}, {:.2f}'.format(*o.tolist()))
ecef = prop.get_orbit(
t=0.0,
a=o[0],
e=o[1],
inc=o[2],
raan=o[4],
aop=o[3],
mu0=dpt.true2mean(o[5], o[1], radians=False),
mjd0=dpt.jd_to_mjd(2457126.2729),
C_D=2.3,
A=1.0,
m=1.0,
)
return ecef
#dpt.plot_ref_orbit(get_orbit)
dpt.plot_ref_orbit(get_orbit,
orb_init=np.array([10000e3, 0, 0, 0, 0], dtype=np.float))
def test_OD(root, sub_path):
import orbit_determination
import TLE_tools as tle
import dpt_tools as dpt
import radar_library as rlib
import propagator_sgp4
#import propagator_orekit
#import propagator_neptune
import ccsds_write
radar = rlib.eiscat_3d(beam='interp', stage=1)
radar.set_FOV(max_on_axis=90.0, horizon_elevation=10.0)
radar.set_SNR_limits(min_total_SNRdb=10.0, min_pair_SNRdb=1.0)
radar.set_TX_bandwith(bw = 1.0e6)
#prop = propagator_neptune.PropagatorNeptune()
prop = propagator_sgp4.PropagatorSGP4()
mass=0.8111E+04
diam=0.8960E+01
m_to_A=128.651
params = dict(
A = {
'dist': None,
'val': mass/m_to_A,
},
d = {
'dist': None,
'val': diam,
},
m = {
'dist': None,
'val': mass,
},
C_D = {
'dist': None,
'val': 2.3,
},
)
fname = glob.glob(root + sub_path + '*.oem')[0]
prior_data, prior_meta = ccsds_write.read_oem(fname)
prior_sort = np.argsort(prior_data['date'])
prior_data = prior_data[prior_sort][0]
prior_mjd = dpt.npdt2mjd(prior_data['date'])
prior_jd = dpt.mjd_to_jd(prior_mjd)
state0 = np.empty((6,), dtype=np.float64)
state0[0] = prior_data['x']
state0[1] = prior_data['y']
state0[2] = prior_data['z']
state0[3] = prior_data['vx']
state0[4] = prior_data['vy']
state0[5] = prior_data['vz']
#state0_ITRF = state0.copy()
state0 = tle.ITRF_to_TEME(state0, prior_jd, 0.0, 0.0)
#state0_TEME = state0.copy()
#state0_ITRF_ref = tle.TEME_to_ITRF(state0_TEME, prior_jd, 0.0, 0.0)
#print(state0_ITRF_ref - state0_ITRF)
#exit()
data_folder = root + sub_path
data_h5 = glob.glob(data_folder + '*.h5')
data_h5_sort = np.argsort(np.array([int(_h.split('/')[-1].split('-')[1]) for _h in data_h5])).tolist()
true_prior_h5 = data_h5[data_h5_sort[0]]
true_obs_h5 = data_h5[data_h5_sort[1]]
print(true_prior_h5)
print(true_obs_h5)
with h5py.File(true_prior_h5, 'r') as hf:
true_prior = hf['true_state'].value.T*1e3
true_prior_jd = dpt.unix_to_jd(hf['true_time'].value)
print('-- True time diff prior [s] --')
prior_match_ind = np.argmin(np.abs(true_prior_jd-prior_jd))
jd_diff = prior_jd - true_prior_jd[prior_match_ind]
state0_true = true_prior[:,prior_match_ind]
state0_true = tle.ITRF_to_TEME(state0_true, true_prior_jd[prior_match_ind], 0.0, 0.0)
print(prior_match_ind)
print(jd_diff*3600.0*24.0)
with h5py.File(true_obs_h5, 'r') as hf:
true_obs = hf['true_state'].value.T*1e3
true_obs_jd = dpt.unix_to_jd(hf['true_time'].value)
data_tdm = glob.glob(data_folder + '*.tdm')
#this next line i wtf, maybe clean up
data_tdm_sort = np.argsort(np.array([int(_h.split('/')[-1].split('-')[-1][2]) for _h in data_tdm])).tolist()
ccsds_files = [data_tdm[_tdm] for _tdm in data_tdm_sort]
print('prior true vs prior mean')
print(state0_true - state0)
for _fh in ccsds_files:
print(_fh)
r_obs_v = []
r_sig_v = []
v_obs_v = []
v_sig_v = []
t_obs_v = []
for ccsds_file in ccsds_files:
obs_data = ccsds_write.read_ccsds(ccsds_file)
sort_obs = np.argsort(obs_data['date'])
obs_data = obs_data[sort_obs]
jd_obs = dpt.mjd_to_jd(dpt.npdt2mjd(obs_data['date']))
date_obs = obs_data['date']
sort_obs = np.argsort(date_obs)
date_obs = date_obs[sort_obs]
r_obs = obs_data['range'][sort_obs]*1e3 #to m
v_obs = -obs_data['doppler_instantaneous'][sort_obs]*1e3 #to m/s
#v_obs = obs_data['doppler_instantaneous'][sort_obs]*1e3 #to m/s
r_sig = 2.0*obs_data['range_err'][sort_obs]*1e3 #to m
v_sig = 2.0*obs_data['doppler_instantaneous_err'][sort_obs]*1e3 #to m/s
#TRUNCATE FOR DEBUG
inds = np.linspace(0,len(jd_obs)-1,num=10,dtype=np.int64)
jd_obs = jd_obs[inds]
r_obs = r_obs[inds]
v_obs = v_obs[inds]
r_sig = r_sig[inds]
v_sig = v_sig[inds]
if ccsds_file.split('/')[-1].split('.')[0] == true_obs_h5.split('/')[-1].split('.')[0]:
print('-- True time diff obs [s] --')
jd_diff = jd_obs - true_obs_jd[inds]
print(jd_diff*3600.0*24.0)
#r_sig = np.full(r_obs.shape, 100.0, dtype=r_obs.dtype)
#v_sig = np.full(v_obs.shape, 10.0, dtype=v_obs.dtype)
r_obs_v.append(r_obs)
r_sig_v.append(r_sig)
v_obs_v.append(v_obs)
v_sig_v.append(v_sig)
t_obs = (jd_obs - prior_jd)*(3600.0*24.0)
#correct for light time approximently
lt_correction = r_obs*0.5/scipy.constants.c
t_obs -= lt_correction
t_obs_v.append(t_obs)
print('='*10 + 'Dates' + '='*10)
print('{:<8}: {} JD'.format('Prior', prior_jd))
for ind, _jd in enumerate(jd_obs):
print('Obs {:<4}: {} JD'.format(ind, _jd))
print('='*10 + 'Observations' + '='*10)
print(len(jd_obs))
prior = {}
prior['cov'] = np.diag([1e3, 1e3, 1e3, 1e1, 1e1, 1e1])*1.0
prior['mu'] = state0
print('='*10 + 'Prior Mean' + '='*10)
print(prior['mu'])
print('='*10 + 'Prior Covariance' + '='*10)
print(prior['cov'])
rx_ecef = []
for rx in radar._rx:
rx_ecef.append(rx.ecef)
tx_ecef = radar._tx[0].ecef
tune = 0
trace = orbit_determination.determine_orbit(
num = 2000,
r = r_obs_v,
sd_r = r_sig_v,
v = v_obs_v,
sd_v = v_sig_v,
grad_dx = [10.0]*3 + [1.0]*3,
rx_ecef = rx_ecef,
tx_ecef = tx_ecef,
t = t_obs_v,
mjd0 = prior_mjd,
params = params,
prior = prior,
propagator = prop,
step = 'Metropolis',
step_opts = {
'scaling': 0.75,
},
pymc_opts = {
'tune': tune,
'discard_tuned_samples': True,
'cores': 1,
'chains': 1,
'parallelize': True,
},
)
#if comm.rank != 0:
# exit()
var = ['$X$ [km]', '$Y$ [km]', '$Z$ [km]', '$V_X$ [km/s]', '$V_Y$ [km/s]', '$V_Z$ [km/s]']
fig = plt.figure(figsize=(15,15))
for ind in range(6):
ax = fig.add_subplot(231+ind)
ax.plot(trace['state'][:,ind]*1e-3)
ax.set(
xlabel='Iteration',
ylabel='{}'.format(var[ind]),
)
state1 = np.mean(trace['state'], axis=0)
print('='*10 + 'Trace summary' + '='*10)
print(pm.summary(trace))
_form = '{:<10}: {}'
print('='*10 + 'Prior Mean' + '='*10)
for ind in range(6):
print(_form.format(var[ind], state0[ind]*1e-3))
print('='*10 + 'Posterior state mean' + '='*10)
for ind in range(6):
print(_form.format(var[ind], state1[ind]*1e-3))
stated = state1 - state0
print('='*10 + 'State shift' + '='*10)
for ind in range(6):
print(_form.format(var[ind], stated[ind]*1e-3))
print('='*10 + 'True posterior' + '='*10)
for ind in range(6):
print(_form.format(var[ind], state0_true[ind]*1e-3))
print('='*10 + 'Posterior error' + '='*10)
for ind in range(6):
print(_form.format(var[ind],(state1[ind] - state0_true[ind])*1e-3))
print('='*10 + 'Parameter shift' + '='*10)
theta0 = {}
theta1 = {}
for key, val in params.items():
if val['dist'] is not None:
theta0[key] = val['mu']
theta1[key] = np.mean(trace[key], axis=0)[0]
print('{}: {}'.format(key, theta1[key] - theta0[key]))
else:
theta0[key] = val['val']
theta1[key] = val['val']
range_v_prior = []
vel_v_prior = []
range_v = []
vel_v = []
range_v_true = []
vel_v_true = []
for rxi in range(len(rx_ecef)):
t_obs = t_obs_v[rxi]
print('Generating tracklet simulated data RX {}: {} points'.format(rxi, len(t_obs)))
states0 = orbit_determination.propagate_state(state0, t_obs, dpt.jd_to_mjd(prior_jd), prop, theta0)
states1 = orbit_determination.propagate_state(state1, t_obs, dpt.jd_to_mjd(prior_jd), prop, theta1)
states0_true = orbit_determination.propagate_state(state0_true, t_obs, dpt.jd_to_mjd(prior_jd), prop, theta1)
range_v_prior += [np.empty((len(t_obs), ), dtype=np.float64)]
vel_v_prior += [np.empty((len(t_obs), ), dtype=np.float64)]
range_v += [np.empty((len(t_obs), ), dtype=np.float64)]
vel_v += [np.empty((len(t_obs), ), dtype=np.float64)]
range_v_true += [np.empty((len(t_obs), ), dtype=np.float64)]
vel_v_true += [np.empty((len(t_obs), ), dtype=np.float64)]
for ind in range(len(t_obs)):
range_v_prior[rxi][ind], vel_v_prior[rxi][ind] = orbit_determination.generate_measurements(states0[:,ind], rx_ecef[rxi], tx_ecef)
range_v[rxi][ind], vel_v[rxi][ind] = orbit_determination.generate_measurements(states1[:,ind], rx_ecef[rxi], tx_ecef)
range_v_true[rxi][ind], vel_v_true[rxi][ind] = orbit_determination.generate_measurements(states0_true[:, ind], rx_ecef[rxi], tx_ecef)
prop_states = orbit_determination.propagate_state(
state0,
np.linspace(0, (np.max(jd_obs) - prior_jd)*(3600.0*24.0), num=1000),
dpt.jd_to_mjd(prior_jd),
prop,
theta1,
)
'''
pop = gen_pop()
obj = pop.get_object(0)
t_obs_pop = t_obs + (dpt.jd_to_mjd(prior_jd) - obj.mjd0)*3600.0*24.0
states0_true2 = obj.get_state(t_obs_pop)
print(states0_true2)
print(states0_true2 - states0_true)
'''
fig = plt.figure(figsize=(15,15))
ax = fig.add_subplot(111, projection='3d')
plothelp.draw_earth_grid(ax)
for ind, ecef in enumerate(rx_ecef):
if ind == 0:
ax.plot([ecef[0]], [ecef[1]], [ecef[2]], 'or', label='EISCAT 3D RX')
else:
ax.plot([ecef[0]], [ecef[1]], [ecef[2]], 'or')
ax.plot(states0[0,:], states0[1,:], states0[2,:], 'xb', label = 'Prior', alpha = 0.75)
ax.plot(states1[0,:], states1[1,:], states1[2,:], 'xr', label = 'Posterior', alpha = 0.75)
ax.plot(prop_states[0,:], prop_states[1,:], prop_states[2,:], '-k', label = 'Prior-propagation', alpha = 0.5)
ax.plot(true_prior[0,:], true_prior[1,:], true_prior[2,:], '-b', label = 'Prior-True', alpha = 0.75)
ax.plot(true_obs[0,:], true_obs[1,:], true_obs[2,:], '-r', label = 'Posterior-True', alpha = 0.75)
ax.legend()
for rxi in range(len(rx_ecef)):
fig = plt.figure(figsize=(15,15))
t_obs_h = t_obs_v[rxi]/3600.0
ax = fig.add_subplot(221)
lns = []
line1 = ax.plot(t_obs_h, (r_obs_v[rxi] - range_v[rxi])*1e-3, '-b', label='Maximum a posteriori: RX{}'.format(rxi))
line0 = ax.plot(t_obs_h, (r_obs_v[rxi] - range_v_true[rxi])*1e-3, '.b', label='True prior: RX{}'.format(rxi))
ax.set(
xlabel='Time [h]',
ylabel='2-way-Range residuals [km]',
)
ax2 = ax.twinx()
line2 = ax2.plot(t_obs_h, (r_obs_v[rxi] - range_v_prior[rxi])*1e-3, '-k', label='Maximum a priori: RX{}'.format(rxi))
ax.tick_params(axis='y', labelcolor='b')
ax2.tick_params(axis='y', labelcolor='k')
lns += line0+line1+line2
labs = [l.get_label() for l in lns]
ax.legend(lns, labs, loc=0)
ax = fig.add_subplot(222)
lns = []
line1 = ax.plot(t_obs_h, (v_obs_v[rxi] - vel_v[rxi])*1e-3, '-b', label='Maximum a posteriori: RX{}'.format(rxi))
line0 = ax.plot(t_obs_h, (v_obs_v[rxi] - vel_v_true[rxi])*1e-3, '.b', label='True prior: RX{}'.format(rxi))
ax.set(
xlabel='Time [h]',
ylabel='2-way-Velocity residuals [km/s]',
)
ax2 = ax.twinx()
line2 = ax2.plot(t_obs_h, (v_obs_v[rxi] - vel_v_prior[rxi])*1e-3, '-k', label='Maximum a priori: RX{}'.format(rxi))
ax.tick_params(axis='y', labelcolor='b')
ax2.tick_params(axis='y', labelcolor='k')
lns += line0+line1+line2
labs = [l.get_label() for l in lns]
ax.legend(lns, labs, loc=0)
ax = fig.add_subplot(223)
ax.errorbar(t_obs_h, r_obs_v[rxi]*1e-3, yerr=r_sig_v[rxi]*1e-3, label='Measurements: RX{}'.format(rxi))
ax.plot(t_obs_h, range_v[rxi]*1e-3, label='Maximum a posteriori: RX{}'.format(rxi))
ax.plot(t_obs_h, range_v_prior[rxi]*1e-3, label='Maximum a priori: RX{}'.format(rxi))
ax.set(
xlabel='Time [h]',
ylabel='2-way-Range [km]',
)
ax.legend()
ax = fig.add_subplot(224)
ax.errorbar(t_obs_h, v_obs_v[rxi]*1e-3, yerr=v_sig_v[rxi]*1e-3, label='Measurements: RX{}'.format(rxi))
ax.plot(t_obs_h, vel_v[rxi]*1e-3, label='Maximum a posteriori: RX{}'.format(rxi))
ax.plot(t_obs_h, vel_v_prior[rxi]*1e-3, label='Maximum a priori: RX{}'.format(rxi))
ax.set(
xlabel='Time [h]',
ylabel='2-way-Velocity [km/s]',
)
ax.legend()
#dpt.posterior(trace['state']*1e-3, var, show=False)
plt.show()