def test_propagate_with_lunar_third_body():
x = [66666, 0, 0, 0, 2.451, 0]
dt = 8600
r = x[0:3] * u.km
v = x[3:6] * u.km / u.s
epoch = Time(2454283.0, format="jd", scale="tdb")
epoch_f = epoch + (dt * u.s)
prop_params = PropParams(epoch)
prop_params.add_perturbation(Perturbations.Moon,
build_lunar_third_body(epoch))
k_moon = Moon.k.to(u.km**3 / u.s**2).value
body_moon = build_ephem_interpolant(
Moon,
lunar_period, (epoch.value * u.day, epoch.value * u.day + 60 * u.day),
rtol=1e-2)
sat_i = Orbit.from_vectors(Earth, r, v, epoch=epoch)
sat_f = sat_i.cov_propagate(dt * u.s,
method=cowell,
ad=third_body,
k_third=k_moon,
third_body=body_moon)
x_poli = np.concatenate([sat_f.r.value, sat_f.v.value])
x_custom = state_propagate(np.array(x), epoch_f, prop_params)
assert np.array_equal(x_custom, x_poli)
def test_propagate_with_drag():
x = [66666, 0, 0, 0, 2.451, 0]
dt = 100 * u.s
r = x[0:3] * u.km
v = x[3:6] * u.km / u.s
epoch = Time(2454283.0, format="jd", scale="tdb")
epoch_obs = epoch + dt
C_D = 1
A = 10
m = 1000
Drag = build_basic_drag(C_D, A, m)
prop_params = PropParams(epoch)
prop_params.add_perturbation(Perturbations.Drag, Drag)
sat_i = Orbit.from_vectors(Earth, r, v, epoch=epoch)
sat_f = sat_i.cov_propagate(dt,
method=cowell,
ad=atmospheric_drag,
R=R,
C_D=C_D,
A=A,
m=m,
H0=H0_earth,
rho0=rho0_earth)
x_poli = np.concatenate([sat_f.r.value, sat_f.v.value])
x_custom = state_propagate(x, epoch_obs, prop_params)
assert np.array_equal(x_custom, x_poli)
def test_propagate_with_srp():
x = [66666, 0, 0, 0, 2.451, 0]
dt = 1 * u.day
r = x[0:3] * u.km
v = x[3:6] * u.km / u.s
epoch = Time(2454283.0, format="jd", scale="tdb")
epoch_obs = epoch + dt
C_R = 1
A = 10
m = 1000
srp = build_srp(C_R, A, m, epoch)
prop_params = PropParams(epoch)
prop_params.add_perturbation(Perturbations.SRP, srp)
body_sun = build_ephem_interpolant(
Sun,
solar_period, (epoch.value * u.day, epoch.value * u.day + 60 * u.day),
rtol=1e-2)
sat_i = Orbit.from_vectors(Earth, r, v, epoch=epoch)
sat_f = sat_i.cov_propagate(dt,
method=cowell,
ad=radiation_pressure,
R=R,
C_R=C_R,
A=A,
m=m,
Wdivc_s=Wdivc_sun.value,
star=body_sun)
x_poli = np.concatenate([sat_f.r.value, sat_f.v.value])
x_custom = state_propagate(x, epoch_obs, prop_params)
assert np.array_equal(x_custom, x_poli)
def get_satellite_position_over_time(x, epoch, tf, dt) -> np.matrix:
t = np.arange(0, tf, dt)
r = np.zeros((len(t), 3))
prop_params = PropParams(dt, epoch)
for i in range(0, len(t)):
r[i] = x[0:3]
x = state_propagate(x, prop_params)
prop_params.epoch = prop_params.epoch + prop_params.dt
return r
def dx_dx0(x: np.ndarray, epoch_t: Time, prop_params: PropParams, delta: np.ndarray) -> np.ndarray:
"""
Calculates partial derivatives of a state vector in the future based on a current state vector.
:param x: state vector at original epoch
:param epoch_t: Final epoch
:param prop_params: Parameters relevant to propagation
:param delta: Array holding step sizes for derivatives
"""
n = len(x)
a = np.zeros((n, n))
for j in range(0, n):
temp1 = state_propagate(x + direction_isolator(delta, j), epoch_t, prop_params)
temp2 = state_propagate(x - direction_isolator(delta, j), epoch_t, prop_params)
temp3 = temp1 - temp2
for i in range(0, n):
a[i][j] = temp3[i] / (2 * delta[i])
return a
def dy_dstate(x: np.ndarray, delta: np.ndarray, observation: Observation, prop_params: PropParams, n=2) -> np.ndarray:
"""
dy_dstate() calculates derivatives of the prediction function per state vector and returns a matrix where each
element is the column corresponds to an element of the prediction function output and the row corresponds
to an element being varied in the state vector. Uses a second-order centered difference equation.
:param x: State vector
:param delta: variation in position/velocity to be used in derivatives
:param observation: Observational parameters, passed directly to Ffun0(
:param prop_params: Propagation parameters, passed directly to propagate()
:param n: number of elements in prediction function output
:return: Matrix of derivatives of the prediction function in position/velocity space
"""
m = len(x)
a = np.zeros((n, m))
for j in range(0, m):
temp1 = state_propagate(x + direction_isolator(delta, j), observation.epoch, prop_params)
temp2 = state_propagate(x - direction_isolator(delta, j), observation.epoch, prop_params)
temp3 = (y(temp1, observation) - y(temp2, observation)) / (2 * delta[j])
for i in range(0, n):
a[i][j] = temp3[i]
return a
def test_propagate_with_no_perturbations():
x = [66666, 0, 0, 0, 2.451, 0]
r = x[0:3] * u.km
v = x[3:6] * u.km / u.s
dt = 100 * u.day
epoch = Time(2454283.0, format="jd", scale="tdb")
epoch_obs = epoch + dt
prop_params = PropParams(epoch)
sat_i = Orbit.from_vectors(Earth, r, v, epoch=epoch)
sat_f = sat_i.cov_propagate(dt, method=cowell)
x_poli = np.concatenate([sat_f.r.value, sat_f.v.value])
x_custom = state_propagate(x, epoch_obs, prop_params)
assert np.array_equal(x_custom, x_poli)
def milani(x: np.ndarray, observations: List[Observation], prop_params: PropParams,
l=np.zeros((6, 6)), dr=.1, dv=.005, max_iter=15) -> Tuple[np.ndarray, np.ndarray]:
"""
Scheme outlined in Adrea Milani's 1998 paper "Asteroid Idenitification Problem". It is a least-squared psuedo-newton
approach to improving a objects's orbit description based on differences in object's measurement in the sky versus
where it was predicted to be.
:param x: State vector of the satellite at a time separate from the observation
:param observations: List of observational objects that capture location, time, and direct observational parameters.
:param prop_params: Propagation parameters, passed directly to propagate()
:param l: Information matrix. Inverse of covariance matrix. Represents uncertainty in initial state. Default value
of zero matrix will leave the code unaffected. l_kk ~ 1/sigma_k^s.
:param dr: Spatial resolution to be used in derivative function.
:param dv: Resolution used for velocity in derivative function
:param max_iter: Maximum number of iterations for Least Squares filter
:return: A more accurate state vector at the same time as original description, not observation
"""
n = len(x)
delta = np. array([dr, dr, dr, dv, dv, dv])
delta_x = np.ones(n) # Must break the stopping criteria
i = 0
while not stopping_criteria(delta_x) and i < max_iter:
c = np.zeros((n, n))
d = np.zeros((n, 1))
for observation in observations:
ypred = y(state_propagate(x, observation.epoch, prop_params), observation)
yobs = observation.obs_values
xi = yobs - ypred
w = np.diag(1/np.multiply(observation.obs_sigmas, observation.obs_sigmas))
b = -dy_dstate(x, delta, observation, prop_params)
c += b.T @ w @ b
d += -b.T @ w @ xi
delta_x = get_delta_x(l + c, d)
xnew = x + delta_x
x = xnew - np.zeros(n)
i = i+1
p = np.linalg.inv(l + c)
# covariance_residual = p @ (l + c) - np.eye(n)
return xnew, p
def test_propagate_with_j2j3():
x = [66666, 0, 0, 0, 2.451, 0]
dt = 100 * u.day
r = x[0:3] * u.km
v = x[3:6] * u.km / u.s
epoch = Time(2454283.0, format="jd", scale="tdb")
epoch_obs = epoch + dt
prop_params = PropParams(epoch)
prop_params.add_perturbation(Perturbations.J2, build_j2())
prop_params.add_perturbation(Perturbations.J3, build_j3())
sat_i = Orbit.from_vectors(Earth, r, v, epoch=epoch)
sat_f = sat_i.cov_propagate(dt,
method=cowell,
ad=a_d_j2j3,
J2=Earth.J2.value,
R=R,
J3=Earth.J3.value)
x_poli = np.concatenate([sat_f.r.value, sat_f.v.value])
x_custom = state_propagate(x, epoch_obs, prop_params)
assert np.array_equal(x_custom, x_poli)
for i in range(0, len(M)):
E = M[i]
for j in range(0, 8):
E = E + (M[i] - E+e*np.sin(E))/(1-e*np.cos(E))
F[i] = 1-(a/r0)*(1-np.cos(E))
G[i] = t[i] + math.sqrt(a*a*a/mu)*(np.sin(E)-E)
r_lg[i] = F[i]*rr0 + G[i]*vv0
# Poliastro construction
r_poli = np.zeros((len(t), 3))
epoch = Time(2454283.0, format="jd", scale="tdb")
prop_params = PropParams(dt, epoch)
for i in range(0, len(t)):
r_poli[i] = x[0:3]
x = state_propagate(x, prop_params)
prop_params.epoch = prop_params.epoch + prop_params.dt
# Difference calculation
r_diff = r_lg - r_poli
diff = np.zeros((len(t), 1))
for i in range(0, len(t)):
diff[i] = la.norm(r_diff[i])
# Plots orbits on top of oen another
x, y, z = generate_earth_surface()
fig = plt.figure()
ax = fig.gca(projection='3d')
ax.plot3D(r_lg[:, 0], r_lg[:, 1], r_lg[:, 2], 'grey')
ax.plot3D(r_poli[:, 0], r_poli[:, 1], r_poli[:, 2], 'red')
ax.plot_surface(x, y, z, color='b')
r = [66666, 0, 0]
v = [0, -2.144, 1]
x = np.array([r[0], r[1], r[2], v[0], v[1], v[2]])
period = get_period(x)
dt = period / 100
tf = period / 2
epoch_obs = Time(2454283.0, format="jd", scale="tdb")
epoch_i = epoch_obs - tf * u.s
x_offset = np.array([100, 50, 10, .01, .01, .03])
obs_pos = [29.2108, 81.0228,
3.9624] #Daytona Beach, except 13 feet above sea level
obs_params = Observation(obs_pos, Frames.LLA, epoch_obs)
obs_params = convert_obs_params_from_lla_to_eci(obs_params)
prop_params = PropParams(tf, epoch_i)
yobs = y(state_propagate(x + x_offset, prop_params), obs_params)
x_alg = milani(x, yobs, LsqParams(np.array([1 / 100, 1 / 100])), obs_params,
prop_params)
r_init = get_satellite_position_over_time(x, epoch_obs, tf, dt)
r_offset = get_satellite_position_over_time(x + x_offset, epoch_obs, tf, dt)
r_alg = get_satellite_position_over_time(x_alg, epoch_obs, tf, dt)
n = r_alg.shape[0] - 1
print(r_offset[n, 0])
fig = plt.figure()
ax = fig.gca(projection='3d')
x, y, z = generate_earth_surface()
ax.plot3D(r_init[:, 0],
from astropy import units as u
from src.observation_function import y
from src.state_propagator import state_propagate
x = np.array([66666, 0, 0, 0, -2.6551, 1])
xoffset = np.array([100, 50, 10, .01, .01, .03])
epoch_i = Time("2018-08-17 12:05:50", scale="tdb")
epoch_i.format = "jd"
epoch_f = epoch_i + 112 * u.day
dt = (epoch_f - epoch_i).value
obs_position = [29.2108 * u.deg, 81.0228 * u.deg, 3.9624 * u.m
] #Daytona Beach, except 13 feet above sea level (6378 km)
obs_params = Observation(obs_position, Frames.LLA, epoch_f)
obs_params = verify_locational_units(obs_params)
obs_params = convert_obs_params_from_lla_to_eci(obs_params)
prop_params = PropParams(dt, epoch_f)
prop_params.add_perturbation(Perturbations.J2, build_j2())
xobs = state_propagate(x + xoffset, prop_params)
yobs = y(xobs, obs_params)
xout = milani(x, yobs, LsqParams(), obs_params, prop_params)
print("The outcome of our algorithm is \nposition: ", xout[0:3],
"\nvelocity: ", xout[3:6])
print("\nCompared to the original \nposition: ", x[0:3], "\nvelocity:", x[3:6])
print("\nDifference in observational values of x and xout")
print(yobs - y(state_propagate(xout, prop_params), obs_params))
