def setUp(self):
gs.random.seed(123)
self.linear_model = LocalizationLinear()
self.nonlinear_model = Localization()
self.kalman = KalmanFilter(self.linear_model)
self.prior_cov = gs.eye(2)
self.process_cov = gs.eye(1)
self.obs_cov = 2.0 * gs.eye(1)
class TestKalmanFilter(geomstats.tests.TestCase):
_multiprocess_can_split_ = True
def setUp(self):
gs.random.seed(123)
self.linear_model = LocalizationLinear()
self.nonlinear_model = Localization()
self.kalman = KalmanFilter(self.linear_model)
self.prior_cov = gs.eye(2)
self.process_cov = gs.eye(1)
self.obs_cov = 2.0 * gs.eye(1)
def test_LocalizationLinear_propagate(self):
initial_state = gs.array([0.5, 1.0])
time_step = 0.5
acc = 2.0
increment = gs.array([time_step, acc])
expected = gs.array([1.0, 2.0])
result = self.linear_model.propagate(initial_state, increment)
self.assertAllClose(expected, result)
def test_LocalizationLinear_propagation_jacobian(self):
time_step = 0.5
acc = 2.0
increment = gs.array([time_step, acc])
expected = gs.array([[1.0, 0.5], [0.0, 1.0]])
result = self.linear_model.propagation_jacobian(None, increment)
self.assertAllClose(expected, result)
def test_LocalizationLinear_observation_model(self):
initial_state = gs.array([0.5, 1.0])
expected = gs.array([0.5])
result = self.linear_model.observation_model(initial_state)
self.assertAllClose(expected, result)
def test_LocalizationLinear_observation_jacobian(self):
expected = gs.array([[1.0, 0.0]])
result = self.linear_model.observation_jacobian(None, None)
self.assertAllClose(expected, result)
def test_LocalizationLinear_innovation(self):
initial_state = gs.array([0.5, 1.0])
measurement = gs.array([0.7])
expected = gs.array([0.2])
result = self.linear_model.innovation(initial_state, measurement)
self.assertAllClose(expected, result)
def test_Localization_preprocess_input(self):
time_step = gs.array([0.5])
linear_vel = gs.array([1.0, 0.5])
angular_vel = gs.array([0.0])
increment = gs.concatenate((time_step, linear_vel, angular_vel),
axis=0)
expected = time_step[0], linear_vel, angular_vel
result = self.nonlinear_model.preprocess_input(increment)
for i in range(3):
self.assertAllClose(expected[i], result[i])
def test_Localization_rotation_matrix(self):
initial_state = gs.array([0.5, 1.0, 2.0])
angle = initial_state[0]
rotation = gs.array([[gs.cos(angle), -gs.sin(angle)],
[gs.sin(angle), gs.cos(angle)]])
expected = rotation
result = self.nonlinear_model.rotation_matrix(angle)
self.assertAllClose(expected, result)
def test_Localization_adjoint_map(self):
initial_state = gs.array([0.5, 1.0, 2.0])
angle = initial_state[0]
rotation = gs.array([[gs.cos(angle), -gs.sin(angle)],
[gs.sin(angle), gs.cos(angle)]])
first_line = gs.eye(1, 3)
last_lines = gs.hstack((gs.array([[2.0], [-1.0]]), rotation))
expected = gs.vstack((first_line, last_lines))
result = self.nonlinear_model.adjoint_map(initial_state)
self.assertAllClose(expected, result)
def test_Localization_propagate(self):
initial_state = gs.array([0.5, 1.0, 2.0])
time_step = gs.array([0.5])
linear_vel = gs.array([1.0, 0.5])
angular_vel = gs.array([0.0])
increment = gs.concatenate((time_step, linear_vel, angular_vel),
axis=0)
angle = initial_state[0]
rotation = gs.array([[gs.cos(angle), -gs.sin(angle)],
[gs.sin(angle), gs.cos(angle)]])
next_position = initial_state[1:] + time_step * gs.matmul(
rotation, linear_vel)
expected = gs.concatenate((gs.array([angle]), next_position), axis=0)
result = self.nonlinear_model.propagate(initial_state, increment)
self.assertAllClose(expected, result)
def test_Localization_propagation_jacobian(self):
time_step = gs.array([0.5])
linear_vel = gs.array([1.0, 0.5])
angular_vel = gs.array([0.0])
increment = gs.concatenate((time_step, linear_vel, angular_vel),
axis=0)
first_line = gs.eye(1, 3)
last_lines = gs.hstack((gs.array([[-0.25], [0.5]]), gs.eye(2)))
expected = gs.vstack((first_line, last_lines))
result = self.nonlinear_model.propagation_jacobian(None, increment)
self.assertAllClose(expected, result)
def test_Localization_observation_model(self):
initial_state = gs.array([0.5, 1.0, 2.0])
expected = gs.array([1.0, 2.0])
result = self.nonlinear_model.observation_model(initial_state)
self.assertAllClose(expected, result)
def test_Localization_observation_jacobian(self):
expected = gs.array([[0.0, 1.0, 0.0], [0.0, 0.0, 1.0]])
result = self.nonlinear_model.observation_jacobian(None, None)
self.assertAllClose(expected, result)
def test_Localization_innovation(self):
initial_state = gs.array([0.5, 1.0, 2.0])
measurement = gs.array([0.7, 2.1])
angle = initial_state[0]
rotation = gs.array([[gs.cos(angle), -gs.sin(angle)],
[gs.sin(angle), gs.cos(angle)]])
expected = gs.matmul(gs.transpose(rotation), gs.array([-0.3, 0.1]))
result = self.nonlinear_model.innovation(initial_state, measurement)
self.assertAllClose(expected, result)
def test_initialize_covariances(self):
self.kalman.initialize_covariances(self.prior_cov, self.process_cov,
self.obs_cov)
self.assertAllClose(self.kalman.covariance, self.prior_cov)
self.assertAllClose(self.kalman.process_noise, self.process_cov)
self.assertAllClose(self.kalman.measurement_noise, self.obs_cov)
def test_propagate(self):
self.kalman.initialize_covariances(self.prior_cov, self.process_cov,
self.obs_cov)
time_step = 0.5
acc = 2.0
increment = gs.array([time_step, acc])
state_jacobian = self.linear_model.propagation_jacobian(
self.kalman.state, increment)
noise_jacobian = self.linear_model.noise_jacobian(
self.kalman.state, increment)
expected_covariance = Matrices.mul(
state_jacobian, self.kalman.covariance,
gs.transpose(state_jacobian)) + Matrices.mul(
noise_jacobian, self.kalman.process_noise,
gs.transpose(noise_jacobian))
expected_state = self.linear_model.propagate(self.kalman.state,
increment)
self.kalman.propagate(increment)
self.assertAllClose(self.kalman.state, expected_state)
self.assertAllClose(self.kalman.covariance, expected_covariance)
def test_compute_gain(self):
self.kalman.initialize_covariances(self.prior_cov, self.process_cov,
self.obs_cov)
innovation_cov = 3 * gs.eye(1)
expected = gs.vstack(
(1.0 / innovation_cov, gs.zeros_like(innovation_cov)))
result = self.kalman.compute_gain(None)
self.assertAllClose(expected, result)
def test_update(self):
self.kalman.state = gs.zeros(2)
self.kalman.initialize_covariances(self.prior_cov, self.process_cov,
self.obs_cov)
measurement = gs.array([0.6])
expected_cov = from_vector_to_diagonal_matrix(
gs.array([2.0 / 3.0, 1.0]))
expected_state = gs.array([0.2, 0.0])
self.kalman.update(measurement)
self.assertAllClose(expected_state, self.kalman.state)
self.assertAllClose(expected_cov, self.kalman.covariance)
def main():
"""Carry out two examples of state estimation on groups.
Both examples are localization problems, where only a part of the system
is observed. The first one is a linear system, while the second one is
non-linear.
"""
np.random.seed(12345)
model = LocalizationLinear()
kalman = KalmanFilter(model)
n_traj = 1000
obs_freq = 50
dt = 0.1
init_cov = gs.array([10.0, 1.0])
init_cov = algebra_utils.from_vector_to_diagonal_matrix(init_cov)
prop_cov = 0.001 * gs.eye(model.dim_noise)
obs_cov = 10 * gs.eye(model.dim_obs)
initial_covs = (init_cov, prop_cov, obs_cov)
kalman.initialize_covariances(*initial_covs)
true_state = gs.array([0.0, 0.0])
true_acc = gs.random.uniform(-1, 1, (n_traj, 1))
dt_vectorized = dt * gs.ones((n_traj, 1))
true_inputs = gs.hstack((dt_vectorized, true_acc))
true_traj, inputs, observations = create_data(
kalman, true_state, true_inputs, obs_freq
)
initial_state = np.random.multivariate_normal(true_state, init_cov)
estimate, uncertainty = estimation(
kalman, initial_state, inputs, observations, obs_freq
)
plt.figure()
plt.plot(true_traj[:, 0], label="Ground Truth")
plt.plot(estimate[:, 0], label="Kalman")
plt.plot(estimate[:, 0] + uncertainty[:, 0], color="k", linestyle=":")
plt.plot(
estimate[:, 0] - uncertainty[:, 0],
color="k",
linestyle=":",
label="3_sigma envelope",
)
plt.plot(
range(obs_freq, n_traj + 1, obs_freq),
observations,
marker="*",
linestyle="",
label="Observation",
)
plt.legend()
plt.title("1D Localization - Position")
plt.figure()
plt.plot(true_traj[:, 1], label="Ground Truth")
plt.plot(estimate[:, 1], label="Kalman")
plt.plot(estimate[:, 1] + uncertainty[:, 1], color="k", linestyle=":")
plt.plot(
estimate[:, 1] - uncertainty[:, 1],
color="k",
linestyle=":",
label="3_sigma envelope",
)
plt.legend()
plt.title("1D Localization - Speed")
model = Localization()
kalman = KalmanFilter(model)
init_cov = gs.array([1.0, 10.0, 10.0])
init_cov = algebra_utils.from_vector_to_diagonal_matrix(init_cov)
prop_cov = 0.001 * gs.eye(model.dim_noise)
obs_cov = 0.1 * gs.eye(model.dim_obs)
initial_covs = (init_cov, prop_cov, obs_cov)
kalman.initialize_covariances(*initial_covs)
true_state = gs.zeros(model.dim)
true_inputs = [gs.array([dt, 0.5, 0.0, 0.05]) for _ in range(n_traj)]
true_traj, inputs, observations = create_data(
kalman, true_state, true_inputs, obs_freq
)
initial_state = gs.array(np.random.multivariate_normal(true_state, init_cov))
initial_state = gs.cast(initial_state, true_state.dtype)
estimate, uncertainty = estimation(
kalman, initial_state, inputs, observations, obs_freq
)
plt.figure()
plt.plot(true_traj[:, 1], true_traj[:, 2], label="Ground Truth")
plt.plot(estimate[:, 1], estimate[:, 2], label="Kalman")
plt.scatter(observations[:, 0], observations[:, 1], s=2, c="k", label="Observation")
plt.legend()
plt.axis("equal")
plt.title("2D Localization")
plt.show()