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_coning_sculling():
# Basically a smoke test, because the function is quite simple.
gyro = np.zeros((10, 3))
gyro[:, 0] = 0.01
gyro[:, 2] = -0.01
accel = np.zeros((10, 3))
accel[:, 2] = 0.1
dv_true = np.empty_like(accel)
dv_true[:, 0] = 0
dv_true[:, 1] = -0.5e-3
dv_true[:, 2] = 0.1
theta, dv = coning_sculling(gyro, accel)
assert_allclose(theta, gyro, rtol=1e-10)
assert_allclose(dv, dv_true, rtol=1e-10)
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)
def test_wahba():
lat = 45
Cnb = dcm.from_hpr(45, -30, 60)
dt = 1e-1
n = 1000
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))
np.random.seed(0)
gyro += 1e-6 * np.random.randn(*gyro.shape) * dt
accel += 1e-4 * np.random.randn(*accel.shape) * dt
phi, dv = coning_sculling(gyro, accel)
hpr, P = align.align_wahba(dt, phi, dv, lat)
assert_allclose(hpr, [45, -30, 60], rtol=1e-3)
gps_data = pd.DataFrame(index=df.index)
gps_data['lat'] = df.latitude
gps_data['lon'] = df.longitude
gyro = df[['Gyroscope_x', 'Gyroscope_y', 'Gyroscope_z']]
accel = df[['AcceleroMeter_x', 'AcceleroMeter_y', 'AcceleroMeter_z']]
dt = .1
lat = df.latitude
lon = df.longitude
from pyins.integrate import coning_sculling, integrate
from pyins.filt import traj_diff
theta, dv = coning_sculling(gyro, accel)
gyro_bias_sd = np.deg2rad(0.05) / 3600 # 0.05 d/h
accel_bias_sd = 5e-3
gyro_noise = 1e-6 # rad / s^0.5
accel_noise = 3e-4 # m / s^1.5
np.random.seed(1)
gyro_bias = gyro_bias_sd * np.random.uniform(-2, 2, 3)
accel_bias = accel_bias_sd * np.random.uniform(-2, 2, 3)
from pyins import earth
gyro_e = gyro + gyro_bias * dt + gyro_noise * np.random.randn(
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)))
