def test_propagate_errors():
# This test is complex and hardly a unit test, but it is strong.
# I believe it's better than a formal test.
dt = 0.5
t = 6 * 3600
n_samples = int(t / dt)
lat = np.full(n_samples, 50.0)
lon = np.full(n_samples, 60.0)
alt = np.zeros_like(lat)
h = np.full(n_samples, 10.0)
r = np.full(n_samples, -5.0)
p = np.full(n_samples, 3.0)
traj, gyro, accel = sim.from_position(dt, lat, lon, alt, h, p, r)
gyro_bias = np.array([1e-8, -2e-8, 3e-8])
accel_bias = np.array([3e-3, -4e-3, 2e-3])
gyro += gyro_bias * dt
accel += accel_bias * dt
theta, dv = coning_sculling(gyro, accel)
d_lat = 100
d_lon = -200
d_VE = 1
d_VN = -2
d_h = 0.01
d_p = -0.02
d_r = 0.03
lat0, lon0 = perturb_ll(traj.lat[0], traj.lon[0], d_lat, d_lon)
VE0 = traj.VE[0] + d_VE
VN0 = traj.VN[0] + d_VN
h0 = traj.h[0] + d_h
p0 = traj.p[0] + d_p
r0 = traj.r[0] + d_r
traj_c = integrate(dt, lat0, lon0, VE0, VN0, h0, p0, r0, theta, dv)
error_true = traj_diff(traj_c, traj)
error_linear = propagate_errors(dt, traj, d_lat, d_lon, d_VE, d_VN, d_h,
d_p, d_r, gyro_bias, accel_bias)
error_scale = np.mean(np.abs(error_true))
rel_diff = (error_linear - error_true) / error_scale
assert_allclose(rel_diff.lat, 0, atol=0.1)
assert_allclose(rel_diff.lon, 0, atol=0.1)
assert_allclose(rel_diff.VE, 0, atol=0.1)
assert_allclose(rel_diff.VN, 0, atol=0.1)
assert_allclose(rel_diff.h, 0, atol=0.1)
assert_allclose(rel_diff.p, 0, atol=0.1)
assert_allclose(rel_diff.r, 0, atol=0.1)
def test_from_position():
dt = 1e-1
n_points = 1000
lat = np.full(n_points, 50.0)
lon = np.full(n_points, 45.0)
alt = np.zeros(n_points)
h = np.zeros(n_points)
p = np.zeros(n_points)
r = np.zeros(n_points)
VE = np.zeros(n_points)
VN = np.zeros(n_points)
VU = np.zeros(n_points)
slat = np.sin(np.deg2rad(50))
clat = (1 - slat**2)**0.5
gyro = earth.RATE * np.array([0, clat, slat]) * dt
accel = np.array([0, 0, earth.gravity(slat)]) * dt
traj, gyro_g, accel_g = sim.from_position(dt, lat, lon, alt, h, p, r)
assert_allclose(traj.lat, 50, rtol=1e-12)
assert_allclose(traj.lon, 45, rtol=1e-12)
assert_allclose(traj.VE, 0, atol=1e-7)
assert_allclose(traj.VN, 0, atol=1e-7)
assert_allclose(traj.h, 0, atol=1e-8)
assert_allclose(traj.p, 0, atol=1e-8)
assert_allclose(traj.r, 0, atol=1e-8)
for i in range(3):
assert_allclose(gyro_g[:, i], gyro[i], atol=1e-16)
assert_allclose(accel_g[:, i], accel[i], atol=1e-7)
traj, gyro_g, accel_g = sim.from_velocity(dt, 50, 45, 0, VE, VN, VU, h, p,
r)
assert_allclose(traj.lat, 50, rtol=1e-12)
assert_allclose(traj.lon, 45, rtol=1e-12)
assert_allclose(traj.VE, 0, atol=1e-7)
assert_allclose(traj.VN, 0, atol=1e-7)
assert_allclose(traj.h, 0, atol=1e-8)
assert_allclose(traj.p, 0, atol=1e-8)
assert_allclose(traj.r, 0, atol=1e-8)
for i in range(3):
assert_allclose(gyro_g[:, i], gyro[i], atol=1e-16)
assert_allclose(accel_g[:, i], accel[i], atol=1e-7)
def test_FeedbackFilter():
dt = 0.9
traj = pd.DataFrame(index=np.arange(1 * 3600))
traj['lat'] = 50
traj['lon'] = 60
traj['VE'] = 0
traj['VN'] = 0
traj['h'] = 0
traj['p'] = 0
traj['r'] = 0
_, gyro, accel = sim.from_position(dt, traj.lat, traj.lon,
np.zeros_like(traj.lat),
h=traj.h, p=traj.p, r=traj.r)
theta, dv = coning_sculling(gyro, accel)
np.random.seed(0)
obs_data = pd.DataFrame(
index=traj.index[::10],
data={
'lat': traj.lat[::10],
'lon': traj.lon[::10]
}
)
obs_data['lat'], obs_data['lon'] = perturb_ll(
obs_data.lat, obs_data.lon,
10 * np.random.randn(obs_data.shape[0]),
10 * np.random.randn(obs_data.shape[0]))
obs = LatLonObs(obs_data, 10)
f = FeedbackFilter(dt, 5, 1, 0.2, 0.05)
d_lat = 5
d_lon = -3
d_VE = 1
d_VN = -1
d_h = 0.1
d_p = 0.03
d_r = -0.02
lat0, lon0 = perturb_ll(50, 60, d_lat, d_lon)
integrator = Integrator(dt, lat0, lon0, d_VE, d_VN, d_h, d_p, d_r)
res = f.run(integrator, theta, dv, observations=[obs])
error = traj_diff(res.traj, traj)
error = error.iloc[3000:]
assert_allclose(error.lat, 0, rtol=0, atol=10)
assert_allclose(error.lon, 0, rtol=0, atol=10)
assert_allclose(error.VE, 0, rtol=0, atol=1e-2)
assert_allclose(error.VN, 0, rtol=0, atol=1e-2)
assert_allclose(error.h, 0, rtol=0, atol=1.5e-3)
assert_allclose(error.p, 0, rtol=0, atol=1e-4)
assert_allclose(error.r, 0, rtol=0, atol=1e-4)
assert_(np.all(np.abs(res.residuals[0] < 4)))
res = f.run_smoother(integrator, theta, dv, [obs])
error = traj_diff(res.traj, traj)
assert_allclose(error.lat, 0, rtol=0, atol=10)
assert_allclose(error.lon, 0, rtol=0, atol=10)
assert_allclose(error.VE, 0, rtol=0, atol=1e-2)
assert_allclose(error.VN, 0, rtol=0, atol=1e-2)
assert_allclose(error.h, 0, rtol=0, atol=1.5e-3)
assert_allclose(error.p, 0, rtol=0, atol=1e-4)
assert_allclose(error.r, 0, rtol=0, atol=1e-4)
assert_(np.all(np.abs(res.residuals[0] < 4)))