def test_NcvContinuous():
framerate = 100 # Hz
dt = 1.0 / framerate
d = 6
ncv = NcvContinuous(dimension=d, sa2=2.0)
assert ncv.dimension == d
assert ncv.dimension_pv == d
assert ncv.num_process_noise_parameters == 1
x = torch.rand(d)
y = ncv(x, dt)
assert_equal(y[0], x[0] + dt * x[d // 2])
dx = ncv.geodesic_difference(x, y)
assert_not_equal(dx, torch.zeros(d))
x_pv = ncv.mean2pv(x)
assert len(x_pv) == d
assert_equal(x, x_pv)
P = torch.eye(d)
P_pv = ncv.cov2pv(P)
assert P_pv.shape == (d, d)
assert_equal(P, P_pv)
Q = ncv.process_noise_cov(dt)
Q1 = ncv.process_noise_cov(dt) # Test caching.
assert_equal(Q, Q1)
assert Q1.shape == (d, d)
assert_cov_validity(Q1)
dx = ncv.process_noise_dist(dt).sample()
assert dx.shape == (ncv.dimension, )
def test_EKFState_with_NcvContinuous():
d = 6
ncv = NcvContinuous(dimension=d, sa2=2.0)
x = torch.rand(d)
P = torch.eye(d)
t = 0.0
dt = 2.0
ekf_state = EKFState(
dynamic_model=ncv, mean=x, cov=P, time=t)
assert ekf_state.dynamic_model.__class__ == NcvContinuous
assert ekf_state.dimension == d
assert ekf_state.dimension_pv == d
assert_equal(x, ekf_state.mean, prec=1e-5)
assert_equal(P, ekf_state.cov, prec=1e-5)
assert_equal(x, ekf_state.mean_pv, prec=1e-5)
assert_equal(P, ekf_state.cov_pv, prec=1e-5)
assert_equal(t, ekf_state.time, prec=1e-5)
ekf_state1 = EKFState(ncv, 2*x, 2*P, t)
ekf_state2 = ekf_state1.predict(dt)
assert ekf_state2.dynamic_model.__class__ == NcvContinuous
measurement = PositionMeasurement(
mean=torch.rand(d),
cov=torch.eye(d),
time=t + dt)
log_likelihood = ekf_state2.log_likelihood_of_update(measurement)
assert (log_likelihood < 0.).all()
ekf_state3, (dz, S) = ekf_state2.update(measurement)
assert dz.shape == (measurement.dimension,)
assert S.shape == (measurement.dimension, measurement.dimension)
assert_not_equal(ekf_state3.mean, ekf_state2.mean, prec=1e-5)