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)