def test_EKFState_with_NcpContinuous():
d = 3
ncp = NcpContinuous(dimension=d, sv2=2.0)
x = torch.rand(d)
P = torch.eye(d)
t = 0.0
dt = 2.0
ekf_state = EKFState(dynamic_model=ncp, mean=x, cov=P, time=t)
assert ekf_state.dynamic_model.__class__ == NcpContinuous
assert ekf_state.dimension == d
assert ekf_state.dimension_pv == 2*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[:d], prec=1e-5)
assert_equal(P, ekf_state.cov_pv[:d, :d], prec=1e-5)
assert_equal(t, ekf_state.time, prec=1e-5)
ekf_state1 = EKFState(ncp, 2*x, 2*P, t)
ekf_state2 = ekf_state1.predict(dt)
assert ekf_state2.dynamic_model.__class__ == NcpContinuous
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)
def test_NcpContinuous():
framerate = 100 # Hz
dt = 1.0 / framerate
d = 3
ncp = NcpContinuous(dimension=d, sv2=2.0)
assert ncp.dimension == d
assert ncp.dimension_pv == 2 * d
assert ncp.num_process_noise_parameters == 1
x = torch.rand(d)
y = ncp(x, dt)
assert_equal(y, x)
dx = ncp.geodesic_difference(x, y)
assert_equal(dx, torch.zeros(d))
x_pv = ncp.mean2pv(x)
assert len(x_pv) == 6
assert_equal(x, x_pv[:d])
assert_equal(torch.zeros(d), x_pv[d:])
P = torch.eye(d)
P_pv = ncp.cov2pv(P)
assert P_pv.shape == (2 * d, 2 * d)
P_pv_ref = torch.zeros((2 * d, 2 * d))
P_pv_ref[:d, :d] = P
assert_equal(P_pv_ref, P_pv)
Q = ncp.process_noise_cov(dt)
Q1 = ncp.process_noise_cov(dt) # Test caching.
assert_equal(Q, Q1)
assert Q1.shape == (d, d)
assert_cov_validity(Q1)
dx = ncp.process_noise_dist(dt).sample()
assert dx.shape == (ncp.dimension, )