def test_inner_product_and_sphere_inner_product(
self,
dim,
tangent_vec_a,
tangent_vec_b,
base_point,
):
"""Test consistency between sphere's inner-products.
The inner-product of the class Hypersphere is
defined in terms of extrinsic coordinates.
The inner-product of pullback_metric is defined in terms
of the spherical coordinates.
"""
pullback_metric = PullbackMetric(
dim=dim, embedding_dim=dim + 1, immersion=immersion
)
immersed_base_point = immersion(base_point)
jac_immersion = pullback_metric.jacobian_immersion(base_point)
immersed_tangent_vec_a = gs.matvec(jac_immersion, tangent_vec_a)
immersed_tangent_vec_b = gs.matvec(jac_immersion, tangent_vec_b)
result = pullback_metric.inner_product(
tangent_vec_a, tangent_vec_b, base_point=base_point
)
expected = Hypersphere(dim).metric.inner_product(
immersed_tangent_vec_a,
immersed_tangent_vec_b,
base_point=immersed_base_point,
)
self.assertAllClose(result, expected)
def test_parallel_transport_and_sphere_parallel_transport(
self, dim, tangent_vec_a, tangent_vec_b, base_point
):
"""Test consistency between sphere's parallel transports.
The parallel transport of the class Hypersphere is
defined in terms of extrinsic coordinates.
The parallel transport of pullback_metric is defined
in terms of the spherical coordinates.
"""
pullback_metric = PullbackMetric(
dim=dim, embedding_dim=dim + 1, immersion=immersion
)
immersed_base_point = immersion(base_point)
jac_immersion = pullback_metric.jacobian_immersion(base_point)
immersed_tangent_vec_a = gs.matvec(jac_immersion, tangent_vec_a)
immersed_tangent_vec_b = gs.matvec(jac_immersion, tangent_vec_b)
result_dict = pullback_metric.ladder_parallel_transport(
tangent_vec_a, base_point=base_point, direction=tangent_vec_b
)
result = result_dict["transported_tangent_vec"]
end_point = result_dict["end_point"]
result = pullback_metric.tangent_immersion(v=result, x=end_point)
expected = Hypersphere(dim).metric.parallel_transport(
immersed_tangent_vec_a,
base_point=immersed_base_point,
direction=immersed_tangent_vec_b,
)
self.assertAllClose(result, expected, atol=1e-5)
def pickable_inner_product(i, j):
immersed_basis_element_i = gs.matvec(jacobian_immersion,
basis_elements[i])
immersed_basis_element_j = gs.matvec(jacobian_immersion,
basis_elements[j])
return self.embedding_metric.inner_product(
immersed_basis_element_i,
immersed_basis_element_j,
base_point=immersed_base_point,
)
def propagate(state, sensor_input):
r"""Propagate state with constant velocity motion model on SE(2).
From a given state (orientation, position) pair :math:`(\theta, x)`,
a new one is obtained as :math:`(\theta + dt * \omega,
x + dt * R(\theta) u)`, where the time step, the linear and angular
velocities u and :math:\omega are given some sensor (e.g., odometers).
Parameters
----------
state : array-like, shape=[dim]
Vector representing a state (orientation, position).
sensor_input : array-like, shape=[4]
Vector representing the information from the sensor.
Returns
-------
new_state : array-like, shape=[dim]
Vector representing the propagated state.
"""
dt, linear_vel, angular_vel = Localization.preprocess_input(sensor_input)
theta, _, _ = state
local_vel = gs.matvec(Localization.rotation_matrix(theta), linear_vel)
new_pos = state[1:] + dt * local_vel
theta = theta + dt * angular_vel
theta = Localization.regularize_angle(theta)
return gs.concatenate((theta, new_pos), axis=0)
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.matvec(gs.transpose(rotation), gs.array([-0.3, 0.1]))
result = self.nonlinear_model.innovation(initial_state, measurement)
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.matvec(
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_exp_and_sphere_exp(self, dim, tangent_vec, base_point):
"""Test consistency between sphere's Riemannian exp.
The exp map of the class Hypersphere is
defined in terms of extrinsic coordinates.
The exp map of pullback_metric is defined
in terms of the spherical coordinates.
"""
pullback_metric = PullbackMetric(
dim=dim, embedding_dim=dim + 1, immersion=immersion
)
immersed_base_point = immersion(base_point)
jac_immersion = pullback_metric.jacobian_immersion(base_point)
immersed_tangent_vec_a = gs.matvec(jac_immersion, tangent_vec)
result = pullback_metric.exp(tangent_vec, base_point=base_point)
result = Hypersphere(dim).spherical_to_extrinsic(result)
expected = Hypersphere(dim).metric.exp(
immersed_tangent_vec_a, base_point=immersed_base_point
)
self.assertAllClose(result, expected, atol=1e-1)
def update(self, observation):
r"""Update the current estimate given an observation.
The state is updated by the matrix-vector product of the Kalman gain K
and the innovation. The possibly non-linear update function is provided
by the model.
Given the observation Jacobian H and covariance N, the current
covariance P is updated as (I - KH)P.
Parameters
----------
observation : array-like, shape=[dim_obs]
Obtained measurement.
"""
innovation = self.model.innovation(self.state, observation)
gain = self.compute_gain(observation)
obs_jac = self.model.observation_jacobian(self.state, observation)
cov_factor = gs.eye(self.model.dim) - Matrices.mul(gain, obs_jac)
self.covariance = Matrices.mul(cov_factor, self.covariance)
state_upd = gs.matvec(gain, innovation)
self.state = self.model.group.exp(state_upd, self.state)
def innovation(state, observation):
"""Discrepancy between the measurement and its expected value.
The linear error (observation - expected) is cast into the state's
frame by rotation, following [BB2017]
Parameters
----------
state : array-like, shape=[dim]
Vector representing the state.
observation : array-like, shape=[dim_obs]
Obtained measurement.
Returns
-------
innovation : array-like, shape=[dim_obs]
Error between the measurement and the expected value.
"""
theta, _, _ = state
rot = Localization.rotation_matrix(theta)
expected = Localization.observation_model(state)
return gs.matvec(Matrices.transpose(rot), observation - expected)
def _tangent_immersion(v, x):
return gs.matvec(jacobian_immersion(x), v)
def loss(x, use_gs=False):
if use_gs:
return gs.matvec(x, gs.matvec(A, x))
return np.matmul(x, np.matmul(A, x))
def grad(x):
return 2 * gs.matvec(A, x)
