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)
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)
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)))