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_integrate():
# Test on the static bench.
dt = 1e-1
n = 100
Cnb = dcm.from_hpr(45, -30, 60)
gyro = np.array([0, 1 / np.sqrt(2), 1 / np.sqrt(2)]) * earth.RATE * dt
gyro = Cnb.T.dot(gyro)
gyro = np.resize(gyro, (n, 3))
accel = np.array([0, 0, earth.gravity(1 / np.sqrt(2))]) * dt
accel = Cnb.T.dot(accel)
accel = np.resize(accel, (n, 3))
theta, dv = coning_sculling(gyro, accel)
traj = integrate(dt, 45, 50, 0, 0, 45, -30, 60, theta, dv)
assert_allclose(traj.lat, 45, rtol=1e-12)
assert_allclose(traj.lon, 50, rtol=1e-12)
assert_allclose(traj.VE, 0, atol=1e-8)
assert_allclose(traj.VN, 0, atol=1e-8)
assert_allclose(traj.h, 45, rtol=1e-12)
assert_allclose(traj.p, -30, rtol=1e-12)
assert_allclose(traj.r, 60, rtol=1e-12)
Integrator.INITIAL_SIZE = 50
I = Integrator(dt, 45, 50, 0, 0, 45, -30, 60)
I.integrate(theta[:n // 2], dv[:n // 2])
I.integrate(theta[n // 2:], dv[n // 2:])
assert_allclose(I.traj.lat, 45, rtol=1e-12)
assert_allclose(I.traj.lon, 50, rtol=1e-12)
assert_allclose(I.traj.VE, 0, atol=1e-8)
assert_allclose(I.traj.VN, 0, atol=1e-8)
assert_allclose(I.traj.h, 45, rtol=1e-12)
assert_allclose(I.traj.p, -30, rtol=1e-12)
assert_allclose(I.traj.r, 60, rtol=1e-12)
align_samples = int(t_align / dt)
theta_align = theta[:align_samples]
theta_nav = theta[align_samples:]
dv_align = dv[:align_samples]
dv_nav = dv[align_samples:]
from pyins.align import align_wahba
(h0, p0, r0), P_align = align_wahba(dt, theta_align, dv_align, 12.915618)
VE0 = 0
VN0 = 0
lat0 = 12.915618
lon0 = 77.615240
traj_real = integrate(dt, lat0, lon0, VE0, VN0, h0, p0, r0, theta_nav,
dv_nav)
traj_error = traj_diff(traj_real, gps_data)
gps_data = pd.DataFrame(index=gps_data.index[::1])
gps_data['lat'] = df.latitude
gps_data['lon'] = df.longitude
gps_pos_sd = 10
from pyins.filt import LatLonObs
gps_obs = LatLonObs(gps_data, gps_pos_sd)
from pyins.filt import InertialSensor
gyro_model = InertialSensor(bias=gyro_bias_sd, noise=gyro_noise)
accel_model = InertialSensor(bias=accel_bias_sd, noise=accel_noise)