Merge pull request #880 from Carlson-J/AzimuthElevationRangeModel

Azimuth elevation range model

stonesoup/functions/__init__.py

@@ -402,6 +402,52 @@ def sphere2cart(rho, phi, theta):
     return (x, y, z)
 
 
+def cart2az_el_rg(x, y, z):
+    """Convert Cartesian to azimuth (phi), elevation(theta), and range(rho)
+
+    Parameters
+    ----------
+    x: float
+        The x coordinate
+    y: float
+        the y coordinate
+    z: float
+        the z coordinate
+
+    Returns
+    -------
+    (float, float, float)
+        A tuple of the form `(phi, theta, rho)`
+    """
+    rho = np.sqrt(x**2 + y**2 + z**2)
+    phi = np.arcsin(x / rho)
+    theta = np.arcsin(y / rho)
+    return phi, theta, rho
+
+
+def az_el_rg2cart(phi, theta, rho):
+    """Convert azimuth (phi), elevation(theta), and range(rho) to Cartesian
+
+    Parameters
+    ----------
+    phi: float
+        azimuth, expressed in radians
+    theta: float
+        Elevation expressed in radians, measured from x, y plane
+    rho: float
+        Range(a.k.a. radial distance)
+
+    Returns
+    -------
+    (float, float, float)
+        A tuple of the form `(phi, theta, rho)`
+    """
+    x = rho * np.sin(phi)
+    y = rho * np.sin(theta)
+    z = rho * np.sqrt(1.0 - np.sin(theta) ** 2 - np.sin(phi) ** 2)
+    return x, y, z
+
+
 def rotx(theta):
     r"""Rotation matrix for rotations around x-axis
 

stonesoup/models/measurement/nonlinear.py

@@ -12,9 +12,9 @@
 
 from ...functions import cart2pol, pol2cart, \
     cart2sphere, sphere2cart, cart2angles, \
-    build_rotation_matrix
+    build_rotation_matrix, cart2az_el_rg, az_el_rg2cart
 from ...types.array import StateVector, CovarianceMatrix, StateVectors
-from ...types.angle import Bearing, Elevation
+from ...types.angle import Bearing, Elevation, Azimuth
 from ..base import LinearModel, GaussianModel, ReversibleModel
 from .base import MeasurementModel
 
@@ -1248,3 +1248,155 @@ def pdf(self, state1, state2, **kwargs):
     def logpdf(self, *args, **kwargs):
         # As pdf replaced, need to go to first non GaussianModel parent
         return super(ReversibleModel, self).logpdf(*args, **kwargs)
+
+
+class CartesianToAzimuthElevationRange(NonLinearGaussianMeasurement, ReversibleModel):
+    r"""This is a class implementation of a time-invariant measurement model, \
+    where measurements are assumed to be received in the form of azimuth \
+    (:math:`\phi`), elevation (:math:`\theta`), and range (:math:`r`), with \
+    Gaussian noise in each dimension.
+    For this model, the Azimuth is defined as the angle of the measurement from \
+    the YZ plan to the YX plane and the Elevation is the angle from the XY plan \
+    to the XZ plane. The z axis is the direction the radar is pointing (broadside) \
+    and is only defined in the positive z. The x axis is generally the direction of travel \
+    for an airborne radar and the y axis is orthogonal to both the x and z.
+    Measurements are only correctly defined for +z (measurements must be in front \
+    of the sensor.
+
+    The model is described by the following equations:
+
+    .. math::
+
+      \vec{y}_t = h(\vec{x}_t, \vec{v}_t)
+
+    where:
+
+    * :math:`\vec{y}_t` is a measurement vector of the form:
+
+    .. math::
+
+      \vec{y}_t = \begin{bmatrix}
+                \phi \\
+                \theta \\
+                r
+            \end{bmatrix}
+
+    * :math:`h` is a non-linear model function of the form:
+
+    .. math::
+
+      h(\vec{x}_t,\vec{v}_t) = \begin{bmatrix}
+                asin(\mathcal{x}/\sqrt{\mathcal{x}^2 + \mathcal{y}^2 +\mathcal{z}^2}) \\
+                asin(\mathcal{y}/\sqrt{\mathcal{x}^2 + \mathcal{y}^2 +\mathcal{z}^2}) \\
+                \sqrt{\mathcal{x}^2 + \mathcal{y}^2 + \mathcal{z}^2}
+                \end{bmatrix} + \vec{v}_t
+
+    * :math:`\vec{v}_t` is Gaussian distributed with covariance :math:`R`, i.e.:
+
+    .. math::
+
+      \vec{v}_t \sim \mathcal{N}(0,R)
+
+    .. math::
+
+      R = \begin{bmatrix}
+            \sigma_{\phi}^2 & 0 & 0 \\
+            0 & \sigma_{\theta}^2 & 0 \\
+            0 & 0 & \sigma_{r}^2
+            \end{bmatrix}
+
+    The :py:attr:`mapping` property of the model is a 3 element vector, \
+    whose first (i.e. :py:attr:`mapping[0]`), second (i.e. \
+    :py:attr:`mapping[1]`) and third (i.e. :py:attr:`mapping[2]`) elements \
+    contain the state index of the :math:`x`, :math:`y` and :math:`z`  \
+    coordinates, respectively.
+
+    Note
+    ----
+    The current implementation of this class assumes a 3D Cartesian plane.
+
+    """  # noqa:E501
+
+    translation_offset: StateVector = Property(
+        default=None,
+        doc="A 3x1 array specifying the Cartesian origin offset in terms of :math:`x,y,z` "
+            "coordinates.")
+
+    def __init__(self, *args, **kwargs):
+        """
+        Ensure that the translation offset is initiated
+        """
+        super().__init__(*args, **kwargs)
+        # Set values to defaults if not provided
+        if self.translation_offset is None:
+            self.translation_offset = StateVector([0] * 3)
+
+    @property
+    def ndim_meas(self) -> int:
+        """ndim_meas getter method
+
+        Returns
+        -------
+        :class:`int`
+            The number of measurement dimensions
+        """
+
+        return 3
+
+    def function(self, state, noise=False, **kwargs) -> StateVector:
+        r"""Model function :math:`h(\vec{x}_t,\vec{v}_t)`
+
+        Parameters
+        ----------
+        state: :class:`~.State`
+            An input state
+        noise: :class:`numpy.ndarray` or bool
+            An externally generated random process noise sample (the default is
+            `False`, in which case no noise will be added
+            if 'True', the output of :meth:`~.Model.rvs` is added)
+
+        Returns
+        -------
+        :class:`numpy.ndarray` of shape (:py:attr:`~ndim_state`, 1)
+            The model function evaluated given the provided time interval.
+        """
+
+        if isinstance(noise, bool) or noise is None:
+            if noise:
+                noise = self.rvs(num_samples=state.state_vector.shape[1], **kwargs)
+            else:
+                noise = 0
+
+        # Account for origin offset
+        xyz = state.state_vector[self.mapping, :] - self.translation_offset
+
+        # Rotate coordinates
+        xyz_rot = self.rotation_matrix @ xyz
+
+        # Convert to measurement space
+        phi, theta, rho = cart2az_el_rg(xyz_rot[0, :], xyz_rot[1, :], xyz_rot[2, :])
+        elevations = [Elevation(i) for i in theta]
+        azimuths = [Azimuth(i) for i in phi]
+
+        return StateVectors([azimuths, elevations, rho]) + noise
+
+    def inverse_function(self, detection, **kwargs) -> StateVector:
+
+        phi, theta, rho = detection.state_vector
+
+        # convert to cartesian
+        x, y, z = az_el_rg2cart(phi, theta, rho)
+        xyz = StateVector([x, y, z])
+
+        inv_rotation_matrix = inv(self.rotation_matrix)
+        xyz = inv_rotation_matrix @ xyz
+
+        res = np.zeros((self.ndim_state, 1)).view(StateVector)
+        res[self.mapping, :] = xyz + self.translation_offset
+
+        return res
+
+    def rvs(self, num_samples=1, **kwargs) -> Union[StateVector, StateVectors]:
+        out = super().rvs(num_samples, **kwargs)
+        out = np.array([[Azimuth(0.)], [Elevation(0.)], [0.]]) + out
+        return out

stonesoup/models/measurement/tests/test_models.py

@@ -2,18 +2,20 @@
 import pytest
 from pytest import approx
 from scipy.stats import multivariate_normal
+from scipy.linalg import inv
 
 from ..nonlinear import (
     CartesianToElevationBearingRange, CartesianToBearingRange,
     CartesianToElevationBearing, Cartesian2DToBearing, CartesianToBearingRangeRate,
-    CartesianToElevationBearingRangeRate, RangeRangeRateBinning)
+    CartesianToElevationBearingRangeRate, RangeRangeRateBinning,
+    CartesianToAzimuthElevationRange)
 
 from ...base import ReversibleModel
 from ...measurement.linear import LinearGaussian
 from ....functions import jacobian as compute_jac
 from ....functions import pol2cart
-from ....functions import rotz, rotx, roty, cart2sphere
-from ....types.angle import Bearing, Elevation
+from ....functions import rotz, rotx, roty, cart2sphere, cart2az_el_rg
+from ....types.angle import Bearing, Elevation, Azimuth
 from ....types.array import StateVector, StateVectors
 from ....types.state import State, CovarianceMatrix, ParticleState
 
@@ -80,6 +82,20 @@ def hbearing(state_vector, pos_map, translation_offset, rotation_offset):
     return StateVector([Elevation(theta), Bearing(phi)])
 
 
+def az_el_rng(state_vector, pos_map, translation_offset, rotation_offset):
+    xyz = state_vector[pos_map, :]
+
+    # Get rotation matrix
+    theta_x, theta_y, theta_z = rotation_offset[:, 0]
+
+    rotation_matrix = inv(rotx(theta_x) @ roty(theta_y) @ rotz(theta_z))
+    xyz_rot = rotation_matrix @ xyz - translation_offset[pos_map, :]
+
+    phi, theta, rho = cart2az_el_rg(*xyz_rot)
+
+    return StateVector([Azimuth(phi), Elevation(theta), rho])
+
+
 @pytest.mark.parametrize(
     "model_class",
     [LinearGaussian,
@@ -193,12 +209,23 @@ def test_none_covar(model_class):
             np.array([0, 1, 2]),
             None,
             None
+        ),
+        (  # 3D meas, 3D state
+            az_el_rng,
+            CartesianToAzimuthElevationRange,
+            StateVector([[10], [2], [3]]),
+            np.array([[0.05, 0, 0],
+                      [0, 0.015, 0],
+                      [0, 0, .8]]),
+            np.array([0, 1, 2]),
+            StateVector([[1.0], [-0.2], [-10]]),
+            StateVector([[0], [0], [0]])
         )
     ],
     ids=["Bearing1", "Bearing2",
          "BearingRange1", "BearingRange2", "BearingRange3",
          "RangeBearingElevation1", "RangeBearingElevation1",
-         "BearingsOnly1", "BearingsOnly2"]
+         "BearingsOnly1", "BearingsOnly2", "AzimuthElevationRange"]
 )
 def test_models(h, ModelClass, state_vec, R,
                 mapping, translation_offset, rotation_offset):

stonesoup/plotter.py

@@ -391,7 +391,11 @@ def plot_tracks(self, tracks, mapping, uncertainty=False, particle=False, track_
                 # Plot uncertainty ellipses
                 for track in tracks:
                     HH = np.eye(track.ndim)[mapping, :]  # Get position mapping matrix
+                    check = err_freq - 1    # plot the first one
                     for state in track:
+                        check += 1
+                        if check % err_freq:
+                            continue
                         w, v = np.linalg.eig(HH @ state.covar @ HH.T)
                         if np.iscomplexobj(w) or np.iscomplexobj(v):
                             warnings.warn("Can not plot uncertainty for all states due to complex "
@@ -2094,7 +2098,7 @@ class AnimatedPlotterly(_Plotter):
     """
 
     def __init__(self, timesteps, tail_length=0.3, equal_size=False,
-                 sim_duration=6, **kwargs):
+                 sim_duration=6, allow_unequal_timesteps=False, **kwargs):
         """
         Initialise the figure and checks that inputs are correctly formatted.
         Creates an empty frame for each timestep, and configures
@@ -2116,7 +2120,7 @@ def __init__(self, timesteps, tail_length=0.3, equal_size=False,
 
         # gives the unique values of time gaps between timesteps. If this contains more than
         # one value, then timesteps are not all evenly spaced which is an issue.
-        if len(time_spaces) != 1:
+        if not allow_unequal_timesteps and len(time_spaces) != 1:
             raise ValueError("Ensure timesteps are equally spaced.")
         self.timesteps = timesteps
 

stonesoup/types/angle.py

@@ -222,6 +222,18 @@ def mod_angle(value):
         return mod_elevation(value)
 
 
+class Azimuth(Angle):
+    """Azimuth angle class.
+
+    Azimuth handles modulo arithmetic for adding and subtracting angles. \
+    The return type for addition and subtraction is Azimuth.
+    Multiplication or division produces a float object rather than Azimuth.
+    """
+    @staticmethod
+    def mod_angle(value):
+        return mod_bearing(value)
+
+
 class Longitude(Bearing):
     """Longitude angle class.