Merge pull request #773 from mattbrown11/jacobian

Implement analytic Jacobian for CartesianToElevationBearingRangeRate.

stonesoup/models/measurement/nonlinear.py

@@ -2,6 +2,7 @@
 import copy
 from typing import Sequence, Tuple, Union
 
+from math import sqrt
 import numpy as np
 from scipy.linalg import inv, pinv, block_diag
 from scipy.stats import multivariate_normal
@@ -962,6 +963,93 @@ def rvs(self, num_samples=1, **kwargs) -> Union[StateVector, StateVectors]:
         out = np.array([[Elevation(0)], [Bearing(0)], [0.], [0.]]) + out
         return out
 
+    def jacobian(self, state, **kwargs):
+        """Model jacobian matrix :math:`H_{jac}`
+
+        Parameters
+        ----------
+        state : :class:`~.State`
+            An input state
+
+        Returns
+        -------
+        :class:`numpy.ndarray` of shape (:py:attr:`~ndim_meas`, \
+        :py:attr:`~ndim_state`)
+            The model jacobian matrix evaluated around the given state vector.
+        """
+        # Account for origin offset in position to enable range and angles to be determined
+        xyz_pos = state.state_vector[self.mapping, :] - self.translation_offset
+
+        # Determine the net velocity component in the engagement
+        xyz_vel = state.state_vector[self.velocity_mapping, :] - self.velocity
+
+        # Rotate into RADAR coordinate system to linearize around the correct
+        # state
+        xyz_pos = self.rotation_matrix @ xyz_pos
+        xyz_vel = self.rotation_matrix @ xyz_vel
+
+        jac = np.zeros((4, 6), dtype=np.float_)
+
+        x, y, z = xyz_pos
+        vx, vy, vz = xyz_vel
+        x2, y2, z2 = x**2, y**2, z**2
+        x2y2 = x2 + y2
+        r2 = x2y2 + z2
+        r = sqrt(r2)
+        sqrt_x2_y2 = sqrt(x2y2)
+        r32 = r2*r
+
+        # Jacobian encodes partial derivatives of measurement vector components
+        # Y = <theta, phi, r, rdot> against state vector
+        # X = <x, vx, y, vy, z, vz>.
+
+        # dtheta/dx
+        sqrt_x2_y2r2 = sqrt_x2_y2*r2
+        jac[0, 0] = -(x*z)/(sqrt_x2_y2r2)
+
+        # dtheta/dy
+        jac[0, 2] = -(y*z)/(sqrt_x2_y2r2)
+
+        # dthtea/dz
+        jac[0, 4] = sqrt_x2_y2/r2
+
+        # dphi/dx
+        jac[1, 0] = - y/(x2y2)
+
+        # dphi/dy
+        jac[1, 2] = x/(x2y2)
+
+        # dphi/dz = 0
+
+        # dr/dx and drdot/dvx
+        jac[2, 0] = jac[3, 1] = x/r
+
+        # dr/dx and drdot/dvy
+        jac[2, 2] = jac[3, 3] = y/r
+
+        # dr/dx and drdot/dvz
+        jac[2, 4] = jac[3, 5] = z/r
+
+        vx_x, vy_y, vz_z = vx*x, vy*y, vz*z
+
+        # drdot/dx
+        jac[3, 0] = (-x*(vy_y + vz_z) + vx*(y2 + z2))/r32
+
+        # drdot/dy
+        jac[3, 2] = (vy*(x2 + z2) - y*(vx_x + vz_z))/r32
+
+        # drdot/dz
+        jac[3, 4] = (vz*(x2y2) - (vx_x + vy_y)*z)/r32
+
+        # Up to this point, the Jacobian has been with respect to the state
+        # vector after rotating into the RADAR coordinate system. However, we
+        # want the Jacobian with respect to world state vector, so we must post
+        # multiply Jacobian by the RADAR rotation matrix.
+        jac[:, self.mapping] = jac[:, self.mapping] @ self.rotation_matrix
+        jac[:, self.velocity_mapping] = jac[:, self.velocity_mapping] @ self.rotation_matrix
+
+        return jac
+
 
 class RangeRangeRateBinning(CartesianToElevationBearingRangeRate):
     r"""This is a class implementation of a time-invariant measurement model, \

stonesoup/models/measurement/tests/test_models.py

@@ -622,7 +622,7 @@ def fun(x):
         return model.function(x)
 
     H = compute_jac(fun, state)
-    assert np.array_equal(H, model.jacobian(state))
+    assert np.allclose(H, model.jacobian(state), atol=5e-4, rtol=1e-5)
 
     # Check Jacobian has proper dimensions
     assert H.shape == (model.ndim_meas, ndim_state)
@@ -927,6 +927,46 @@ def test_rangeratemodels_with_particles(h, modelclass, state_vec, ndim_state, po
             cov=noise_covar)
 
 
+def test_rangeratemodel_analytic_jacobian():
+    """Test the analytic Jacobian of CartesianToElevationBearingRangeRate.
+
+    """
+    noise_covar = np.zeros((4, 4))
+    mapping = np.array([0, 2, 4])
+    velocity_mapping = np.array([1, 3, 5])
+    measure_model1 = CartesianToElevationBearingRangeRate(
+        ndim_state=6, mapping=mapping, velocity_mapping=velocity_mapping,
+        noise_covar=noise_covar)
+
+    measure_model2 = CartesianToElevationBearingRangeRate(
+        ndim_state=6, mapping=mapping, velocity_mapping=velocity_mapping,
+        noise_covar=noise_covar,
+        translation_offset=[[-30], [50], [14]],
+        rotation_offset=np.array([[-0.4], [-0.5], [-0.2]]))
+
+    measure_model3 = CartesianToElevationBearingRangeRate(
+        ndim_state=6, mapping=mapping, velocity_mapping=velocity_mapping,
+        noise_covar=noise_covar,
+        translation_offset=[[-330], [-350], [-104]],
+        rotation_offset=np.array([[0.1], [0.25], [0.75]]))
+
+    for state in [State(StateVectors([[1], [2], [3], [4], [5], [6]])),
+                  State(StateVectors([[-20], [2], [3.46], [4], [-28], [22]])),
+                  State(StateVectors([[100], [46], [-3.5], [-184], [45], [11]])),
+                  State(StateVectors([[31.02], [2.156], [-13], [4], [-5], [6]])),
+                  State(StateVectors([[142], [-23], [43], [-1.4], [33.5], [2.6]]))]:
+
+        for measure_model in [measure_model1, measure_model2, measure_model3]:
+            # Calculate numerically
+            jac0 = compute_jac(measure_model.function, state)
+
+            # Calculate using the analytic expression
+            jac = measure_model.jacobian(state)
+
+            # Not going to be exact since jac0 is an approximation
+            assert np.allclose(jac, jac0, atol=5e-4, rtol=1e-5)
+
+
 def test_inverse_function():
     measure_model = CartesianToElevationBearingRangeRate(
         ndim_state=6,