From 7122310f85e6bffb8441f2c598d010738dc204c9 Mon Sep 17 00:00:00 2001
From: rjgreen <rjgreen@dstl.gov.uk>
Date: Thu, 16 Apr 2020 09:57:22 +0100
Subject: [PATCH 01/21] range rate measurement functions added

---
 stonesoup/models/measurement/nonlinear.py | 270 ++++++++++++++++++++++
 1 file changed, 270 insertions(+)

diff --git a/stonesoup/models/measurement/nonlinear.py b/stonesoup/models/measurement/nonlinear.py
index d449022d7..25b0b4df4 100644
--- a/stonesoup/models/measurement/nonlinear.py
+++ b/stonesoup/models/measurement/nonlinear.py
@@ -521,3 +521,273 @@ def rvs(self, num_samples=1, **kwargs):
         out = super().rvs(num_samples, **kwargs)
         out = np.array([[Elevation(0.)], [Bearing(0.)]]) + out
         return out
+
+
+class CartesianToBearingRangeRate(NonLinearGaussianMeasurement):
+    r"""This is a class implementation of a time-invariant measurement model, \
+    where measurements are assumed to be received in the form of bearing \
+    (:math:`\phi`), range (:math:`r`) and range-rate (:math:`\dot{r}`),
+    with Gaussian noise in each dimension.
+
+    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 \\
+                r \\
+                \dot{r}
+            \end{bmatrix}
+
+    * :math:`h` is a non-linear model function of the form:
+
+    .. math::
+
+      h(\vec{x}_t,\vec{v}_t) = \begin{bmatrix}
+                atan2(\mathcal{y},\mathcal{x}) \\
+                \sqrt{\mathcal{x}^2 + \mathcal{y}^2} \\
+                (x\dot{x} + y\dot{y})/\sqrt{x^2 + y^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_{r}^2 & 0 \\
+            0 & 0 & \sigma_{\dot{r}}^2
+            \end{bmatrix}
+
+    The :py:attr:`mapping` property of the model is a 2 element vector, \
+    whose first (i.e. :py:attr:`mapping[0]`) and second (i.e. \
+    :py:attr:`mapping[0]`) elements contain the state index of the \
+    :math:`x` and :math:`y` coordinates, respectively.
+
+    Note
+    ----
+    This class implementation assumes a 2D Cartesian plane.
+
+    The velocity states are assumed to be contained in the adjacent state \
+    the position - i.e. obtained by xy = state_vector[self.mapping+1]
+
+    """
+
+    translation_offset: StateVector = Property(default=StateVector(np.array([[0], [0]])),
+                                               doc="A 2x1 array specifying the origin \
+                                               offset in terms of :math:`x,y` coordinates.")
+
+    @property
+    def ndim_meas(self) -> int:
+        """ndim_meas getter method
+
+        Returns
+        -------
+        :class:`int`
+            The number of measurement dimensions
+        """
+
+        return 3
+
+    def function(self, target_pos, target_vel, sensor_pos,
+                 sensor_vel, noise=False, **kwargs) -> StateVector:
+        r"""Model function :math:`h(\vec{x}_t,\vec{v}_t)`
+
+        Parameters
+        ----------
+        target_pos: :class:`~.StateVector`
+            An input state vector target position
+        target_vel: :class:`~.StateVector`
+            An input state vector target velocity
+        sensor_pos: :class:`~.StateVector`
+            An input state vector for sensor position
+        sensor_vel: :class:`~.StateVector`
+            An input state vector for sensor velocity
+        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_meas`, 1)
+            The model function evaluated given the provided time interval.
+
+        Note
+        ----
+        The parameters will need to be modified in line with #177
+        """
+
+        if isinstance(noise, bool) or noise is None:
+            if noise:
+                noise = self.rvs()
+            else:
+                noise = 0
+
+        # Account for origin offset in position to enable range and angles to be determined
+        xy_pos = target_pos - sensor_pos
+
+        # Rotate coordinates based upon the sensor_velocity
+        xy_rot = self._rotation_matrix @ xy_pos
+
+        # Convert to Spherical
+        rho, phi, _ = cart2sphere(*xy_rot[:, 0])
+
+        # Determine the net velocity component in the engagement
+        xy_vel = target_vel - sensor_vel
+
+        # Use polar to calculate range rate
+        rr = -np.dot(xy_pos[:, 0], xy_vel[:, 0]) / np.linalg.norm(xy_pos)
+
+        return StateVector([[Bearing(phi)], [rho], [rr]]) + noise
+
+    def rvs(self, num_samples=1, **kwargs) -> np.array:
+        out = super().rvs(num_samples, **kwargs)
+        out = np.array([[Bearing(0)], [0.], [0.]]) + out
+        return out
+
+
+class CartesianToElevationBearingRangeRate(NonLinearGaussianMeasurement):
+    r"""This is a class implementation of a time-invariant measurement model, \
+    where measurements are assumed to be received in the form of elevation \
+    (:math:`\theta`),  bearing (:math:`\phi`), range (:math:`r`) and
+    range-rate (:math:'\dot{r}'), with Gaussian noise in each dimension.
+
+    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}
+                \theta \\
+                \phi \\
+                r \\
+                \dot{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{z}/\sqrt{\mathcal{x}^2 + \mathcal{y}^2 +\mathcal{z}^2}) \\
+                atan2(\mathcal{y},\mathcal{x}) \\
+                \sqrt{\mathcal{x}^2 + \mathcal{y}^2 + \mathcal{z}^2} \\
+                (x\dot{x} + y\dot{y} + z\dot{z})/\sqrt{x^2 + y^2 + 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_{\theta}^2 & 0 & 0 & 0\\
+            0 & \sigma_{\phi}^2 & 0 & 0\\
+            0 & 0 & \sigma_{r}^2 & 0\\
+            0 & 0 & 0 & \sigma_{\dot{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
+    ----
+    This class assumes a 3D Cartesian plane.
+
+    The velocity states are assumed to be contained in the adjacent state \
+    the position - i.e. obtained by \dot{x} = state_vector[self.mapping[0]+1]
+
+    """
+
+    translation_offset: StateVector = Property(default=StateVector(np.array([[0], [0], [0]])),
+                                               doc="A 3x1 array specifying the origin offset \
+                                               in terms of :math:`x,y`,z coordinates.")
+
+    @property
+    def ndim_meas(self) -> int:
+        """ndim_meas getter method
+
+        Returns
+        -------
+        :class:`int`
+            The number of measurement dimensions
+        """
+
+        return 4
+
+    def function(self, target_pos, target_vel, sensor_pos, sensor_vel,
+                 noise=False, **kwargs) -> StateVector:
+        r"""Model function :math:`h(\vec{x}_t,\vec{v}_t)`
+
+        Parameters
+        ----------
+        state_vector: :class:`~.StateVector`
+            An input state vector
+        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()
+            else:
+                noise = 0
+
+        # Account for origin offset in position to enable range and angles to be determined
+        xyz_pos = target_pos - sensor_pos
+
+        # Rotate coordinates based upon the sensor_velocity
+        xyz_rot = self._rotation_matrix @ xyz_pos
+
+        # Convert to Spherical
+        rho, phi, theta = cart2sphere(*xyz_rot[:, 0])
+
+        # Determine the net velocity component in the engagement
+        xyz_vel = target_vel - sensor_vel
+
+        # Use polar to calculate range rate
+        rr = -np.dot(xyz_pos[:, 0], xyz_vel[:, 0]) / np.linalg.norm(xyz_pos)
+
+        return StateVector([[Elevation(phi)],
+                            [Bearing(theta)],
+                            [rho],
+                            [rr]]) + noise
+
+    def rvs(self, num_samples=1, **kwargs) -> np.array:
+        out = super().rvs(num_samples, **kwargs)
+        out = np.array([[Elevation(0)], [Bearing(0)], [0.], [0.]]) + out
+        return out

From 750d46290fd8b87bbbded9371de16dc16c3a3227 Mon Sep 17 00:00:00 2001
From: rjgreen <rjgreen@dstl.gov.uk>
Date: Fri, 17 Apr 2020 10:02:11 +0100
Subject: [PATCH 02/21] Sensor base class modified to provide velocity link to
 platform

---
 stonesoup/sensor/base.py | 20 ++++++++++++++++++++
 1 file changed, 20 insertions(+)

diff --git a/stonesoup/sensor/base.py b/stonesoup/sensor/base.py
index 7d4328101..834e4a638 100644
--- a/stonesoup/sensor/base.py
+++ b/stonesoup/sensor/base.py
@@ -103,3 +103,23 @@ def orientation(self, value: StateVector):
     @property
     def _has_internal_platform(self) -> bool:
         return False
+
+    def velocity(self):
+        """The sensor velocity on a 3D Cartesian plane, expressed as a 3x1 :class:`StateVector`
+        of Cartesian coordinates in the order :math:`x,y,z`.
+
+        .. note::
+            This property delegates the actual calculation of velocity to the platform on which the
+            sensor is mounted.
+
+            It is settable if, and only if, the sensor holds its own internal platform."""
+        return self.platform_system().velocity(self)
+
+    @velocity.setter
+    def velocity(self, value):
+        if self._has_internal_platform:
+            self.platform.velocity = value
+        else:
+            raise AttributeError('Cannot set sensor velocity unless the sensor has its own '
+                                 'default platform')
+

From 0fa7f3b6123667ed70315af0d5f969d7be6f98e5 Mon Sep 17 00:00:00 2001
From: rjgreen <rjgreen@dstl.gov.uk>
Date: Fri, 17 Apr 2020 13:28:07 +0100
Subject: [PATCH 03/21] Velocity getter fixed

---
 stonesoup/sensor/base.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/stonesoup/sensor/base.py b/stonesoup/sensor/base.py
index 834e4a638..918e8f03e 100644
--- a/stonesoup/sensor/base.py
+++ b/stonesoup/sensor/base.py
@@ -113,13 +113,13 @@ def velocity(self):
             sensor is mounted.
 
             It is settable if, and only if, the sensor holds its own internal platform."""
-        return self.platform_system().velocity(self)
+        return self.platform_system().velocity
 
     @velocity.setter
     def velocity(self, value):
         if self._has_internal_platform:
             self.platform.velocity = value
         else:
-            raise AttributeError('Cannot set sensor velocity unless the sensor has its own '
+            raise AttributeError('Cannot set platform velocity unless the sensor has its own '
                                  'default platform')
 

From f1e0a6419a64f78e7e5116cccf6fb4346f6e80ac Mon Sep 17 00:00:00 2001
From: rjgreen <rjgreen@dstl.gov.uk>
Date: Fri, 17 Apr 2020 13:32:03 +0100
Subject: [PATCH 04/21] Added updated mapping to seperate pos/vel_mappings and
 relected test changes

---
 .../platform/tests/test_platform_simple.py    | 24 +++----
 stonesoup/sensor/radar/radar.py               | 72 ++++++++++++++++---
 stonesoup/sensor/radar/tests/test_radar.py    | 12 ++--
 3 files changed, 81 insertions(+), 27 deletions(-)

diff --git a/stonesoup/platform/tests/test_platform_simple.py b/stonesoup/platform/tests/test_platform_simple.py
index 73410d5de..4b0b66508 100644
--- a/stonesoup/platform/tests/test_platform_simple.py
+++ b/stonesoup/platform/tests/test_platform_simple.py
@@ -146,29 +146,29 @@ def radars_2d():
     # Create 5 simple radar sensor objects
     radar1 = RadarRangeBearing(
         ndim_state=4,
-        mapping=measurement_mapping,
+        pos_mapping=measurement_mapping,
         noise_covar=noise_covar,
     )
 
     radar2 = RadarRangeBearing(
         ndim_state=4,
-        mapping=measurement_mapping,
+        pos_mapping=measurement_mapping,
         noise_covar=noise_covar
     )
     radar3 = RadarRangeBearing(
         ndim_state=4,
-        mapping=measurement_mapping,
+        pos_mapping=measurement_mapping,
         noise_covar=noise_covar
     )
 
     radar4 = RadarRangeBearing(
         ndim_state=4,
-        mapping=measurement_mapping,
+        pos_mapping=measurement_mapping,
         noise_covar=noise_covar
     )
     radar5 = RadarRangeBearing(
         ndim_state=4,
-        mapping=measurement_mapping,
+        pos_mapping=measurement_mapping,
         noise_covar=noise_covar
     )
 
@@ -186,39 +186,39 @@ def radars_3d():
     # Create 5 simple radar sensor objects
     radar1 = RadarRangeBearing(
         ndim_state=6,
-        mapping=measurement_mapping,
+        pos_mapping=measurement_mapping,
         noise_covar=noise_covar
     )
 
     radar2 = RadarRangeBearing(
         ndim_state=6,
-        mapping=measurement_mapping,
+        pos_mapping=measurement_mapping,
         noise_covar=noise_covar
     )
     radar3 = RadarRangeBearing(
         ndim_state=6,
-        mapping=measurement_mapping,
+        pos_mapping=measurement_mapping,
         noise_covar=noise_covar
     )
 
     radar4 = RadarRangeBearing(
         ndim_state=6,
-        mapping=measurement_mapping,
+        pos_mapping=measurement_mapping,
         noise_covar=noise_covar
     )
     radar5 = RadarRangeBearing(
         ndim_state=6,
-        mapping=measurement_mapping,
+        pos_mapping=measurement_mapping,
         noise_covar=noise_covar
     )
     radar6 = RadarRangeBearing(
         ndim_state=6,
-        mapping=measurement_mapping,
+        pos_mapping=measurement_mapping,
         noise_covar=noise_covar
     )
     radar7 = RadarRangeBearing(
         ndim_state=6,
-        mapping=measurement_mapping,
+        pos_mapping=measurement_mapping,
         noise_covar=noise_covar
     )
     return [radar1, radar2, radar3, radar4, radar5, radar6, radar7]
diff --git a/stonesoup/sensor/radar/radar.py b/stonesoup/sensor/radar/radar.py
index fb4fff380..ea175e313 100644
--- a/stonesoup/sensor/radar/radar.py
+++ b/stonesoup/sensor/radar/radar.py
@@ -9,7 +9,8 @@
 from ...functions import cart2sphere, rotx, roty, rotz
 from ...base import Property
 
-from ...models.measurement.nonlinear import CartesianToBearingRange
+from ...models.measurement.nonlinear import (CartesianToBearingRange,
+                                             CartesianToBearingRangeRate)
 from ...types.array import CovarianceMatrix
 from ...types.detection import Detection
 from ...types.state import State, StateVector
@@ -32,10 +33,10 @@ class RadarRangeBearing(Sensor):
 
     ndim_state = Property(
         int,
-        doc="Number of state dimensions. This is utilised by (and follows in\
-            format) the underlying :class:`~.CartesianToBearingRange`\
-            model")
-    mapping = Property(
+        default=2,
+        doc="Number of state dimensions. This is utilised by (and follows in format) "
+            "the underlying :class:`~.CartesianToBearingRange` model")
+    pos_mapping = Property(
         [np.array],
         doc="Mapping between the targets state space and the sensors\
             measurement capability")
@@ -65,7 +66,7 @@ def measure(self, ground_truth, noise=True, **kwargs):
         """
         measurement_model = CartesianToBearingRange(
             ndim_state=self.ndim_state,
-            mapping=self.mapping,
+            mapping=self.pos_mapping,
             noise_covar=self.noise_covar,
             translation_offset=self.position,
             rotation_offset=self.orientation)
@@ -147,7 +148,7 @@ def measure(self, ground_truth, noise=True, **kwargs):
 
         measurement_model = CartesianToBearingRange(
             ndim_state=self.ndim_state,
-            mapping=self.mapping,
+            mapping=self.pos_mapping,
             noise_covar=self.noise_covar,
             translation_offset=self.position,
             rotation_offset=rot_offset)
@@ -201,6 +202,56 @@ def rotate(self, timestamp):
         )
 
 
+class RadarRangeRateBearing(RadarRangeBearing):
+    """ A radar sensor that generates measurements of targets, using a
+    :class:`~.CartesianToBearingRangeRate` model, relative to its position
+    and velocity.
+
+    Note
+    ----
+    The current implementation of this class assumes a 3D Cartesian plane.
+
+    """
+
+    vel_mapping = Property(
+        np.array,
+        doc="Mapping to the targets velocity information within its state space")
+
+    def measure(self, ground_truth, noise=True, **kwargs) -> Detection:
+        """Generate a measurement for a given state
+
+        Parameters
+        ----------
+        ground_truth : :class:`~.State`
+            A ground-truth state which includes position and velocity information
+        noise: :class:`numpy.ndarray` or bool
+            An externally generated random process noise sample (the default is
+            `True`, in which case :meth:`~.Model.rvs` is used
+            if 'False', no noise will be added)
+
+        Returns
+        -------
+        :class:`~.Detection`
+            A measurement generated from the given state. The timestamp of the\
+            measurement is set equal to that of the provided state.
+        """
+        measurement_model = CartesianToBearingRangeRate(
+            ndim_state=self.ndim_state,
+            mapping=self.pos_mapping,
+            vel_mapping=self.vel_mapping,
+            noise_covar=self.noise_covar,
+            translation_offset=self.position,
+            velocity=self.velocity,
+            rotation_offset=self.orientation)
+
+        measurement_vector = measurement_model.function(
+            ground_truth, noise=noise, **kwargs)
+
+        return Detection(measurement_vector,
+                         measurement_model=measurement_model,
+                         timestamp=ground_truth.timestamp)
+
+
 class RadarRasterScanRangeBearing(RadarRotatingRangeBearing):
     """A simple raster scan radar, with set field-of-regard (FoR) angle, \
      field-of-view (FoV) angle, range and rotations per minute (RPM), that \
@@ -314,7 +365,8 @@ class AESARadar(Sensor):
             "counter-clockwise rotation around the :math:`x,y,z` axis. i.e Roll, Pitch and Yaw. "
             "Default is ``StateVector([0, 0, 0])``")
 
-    mapping = Property(
+
+    pos_mapping = Property(
         np.array, default=(0, 1, 2),
         doc="Mapping between or positions and state "
             "dimensions. [x,y,z]")
@@ -448,7 +500,9 @@ def gen_probability(self, sky_state):
         spoiled_gain = 10 ** (self.antenna_gain / 10) * np.cos(beam_az) * np.cos(beam_el)
         spoiled_width = self.beam_width / (np.cos(beam_az) * np.cos(beam_el))
         # state relative to radar (in cartesian space)
-        relative_vector = sky_state.state_vector[self.mapping, :] - self.position
+
+        relative_vector = sky_state.state_vector[self.pos_mapping, :] - self.position
+
         relative_vector = self._rotation_matrix @ relative_vector
 
         # calculate target position in spherical coordinates
diff --git a/stonesoup/sensor/radar/tests/test_radar.py b/stonesoup/sensor/radar/tests/test_radar.py
index 78d112be4..1ef24ebf1 100644
--- a/stonesoup/sensor/radar/tests/test_radar.py
+++ b/stonesoup/sensor/radar/tests/test_radar.py
@@ -54,7 +54,7 @@ def test_simple_radar():
     radar = RadarRangeBearing(position=radar_position,
                               orientation=radar_orientation,
                               ndim_state=2,
-                              mapping=measurement_mapping,
+                              pos_mapping=measurement_mapping,
                               noise_covar=noise_covar)
 
     # Assert that the object has been correctly initialised
@@ -100,7 +100,7 @@ def test_rotating_radar():
     radar = RadarRotatingRangeBearing(position=radar_position,
                                       orientation=radar_orientation,
                                       ndim_state=2,
-                                      mapping=measurement_mapping,
+                                      pos_mapping=measurement_mapping,
                                       noise_covar=noise_covar,
                                       dwell_center=dwell_center,
                                       rpm=rpm,
@@ -161,7 +161,7 @@ def test_raster_scan_radar():
     radar = RadarRasterScanRangeBearing(position=radar_position,
                                         orientation=radar_orientation,
                                         ndim_state=2,
-                                        mapping=measurement_mapping,
+                                        pos_mapping=measurement_mapping,
                                         noise_covar=noise_covar,
                                         dwell_center=dwell_center,
                                         rpm=rpm,
@@ -209,7 +209,7 @@ def test_aesaradar():
                    timestamp=datetime.datetime.now())
 
     radar = AESARadar(antenna_gain=30,
-                      mapping=[0, 2, 4],
+                      pos_mapping=[0, 2, 4],
                       position=StateVector([0.0] * 3),
                       orientation=StateVector([0.0] * 3),
                       frequency=100e6,
@@ -304,7 +304,7 @@ def test_failed_detect():
                    timestamp=datetime.datetime.now())
 
     radar = AESARadar(antenna_gain=30,
-                      mapping=[0, 2, 4],
+                      pos_mapping=[0, 2, 4],
                       position=StateVector([0.0] * 3),
                       orientation=StateVector([0.0] * 3),
                       frequency=100e6,
@@ -334,7 +334,7 @@ def test_target_rcs():
     rcs_20.rcs = 20
 
     radar = AESARadar(antenna_gain=36,
-                      mapping=[0, 1, 2],
+                      pos_mapping=[0, 1, 2],
                       position=StateVector([0.0]*3),
                       orientation=StateVector([0.0] * 3),
                       frequency=10e9,

From 4fdd391dc9a5da886e407c9d7ede1258b010ae90 Mon Sep 17 00:00:00 2001
From: rjgreen <rjgreen@dstl.gov.uk>
Date: Fri, 17 Apr 2020 13:33:13 +0100
Subject: [PATCH 05/21] range rate measurement models updated to use state not
 stateVect

---
 stonesoup/models/measurement/nonlinear.py | 63 +++++++++++++----------
 1 file changed, 35 insertions(+), 28 deletions(-)

diff --git a/stonesoup/models/measurement/nonlinear.py b/stonesoup/models/measurement/nonlinear.py
index 25b0b4df4..a73a6cec3 100644
--- a/stonesoup/models/measurement/nonlinear.py
+++ b/stonesoup/models/measurement/nonlinear.py
@@ -586,9 +586,17 @@ class CartesianToBearingRangeRate(NonLinearGaussianMeasurement):
 
     """
 
-    translation_offset: StateVector = Property(default=StateVector(np.array([[0], [0]])),
-                                               doc="A 2x1 array specifying the origin \
-                                               offset in terms of :math:`x,y` coordinates.")
+    translation_offset = Property(
+        StateVector,
+        default=StateVector(np.array([[0], [0]])),
+        doc="A 2x1 array specifying the origin offset in terms of :math:`x,y` coordinates.")
+    vel_mapping = Property(
+        np.array,
+        doc="Mapping to the targets velocity within its state space")
+    velocity = Property(
+        StateVector,
+        default=StateVector(np.array([[0], [0]])),
+        doc="A 2x1 array specifying the sensor velocity in terms of :math:`x,y` coordinates.")
 
     @property
     def ndim_meas(self) -> int:
@@ -602,20 +610,14 @@ def ndim_meas(self) -> int:
 
         return 3
 
-    def function(self, target_pos, target_vel, sensor_pos,
-                 sensor_vel, noise=False, **kwargs) -> StateVector:
+    def function(self, state, noise=False, **kwargs) -> StateVector:
         r"""Model function :math:`h(\vec{x}_t,\vec{v}_t)`
 
         Parameters
         ----------
-        target_pos: :class:`~.StateVector`
-            An input state vector target position
-        target_vel: :class:`~.StateVector`
-            An input state vector target velocity
-        sensor_pos: :class:`~.StateVector`
-            An input state vector for sensor position
-        sensor_vel: :class:`~.StateVector`
-            An input state vector for sensor velocity
+        state: :class:`~.StateVector`
+            An input state vector for the target
+
         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
@@ -626,9 +628,6 @@ def function(self, target_pos, target_vel, sensor_pos,
         :class:`numpy.ndarray` of shape (:py:attr:`~ndim_meas`, 1)
             The model function evaluated given the provided time interval.
 
-        Note
-        ----
-        The parameters will need to be modified in line with #177
         """
 
         if isinstance(noise, bool) or noise is None:
@@ -638,7 +637,7 @@ def function(self, target_pos, target_vel, sensor_pos,
                 noise = 0
 
         # Account for origin offset in position to enable range and angles to be determined
-        xy_pos = target_pos - sensor_pos
+        xy_pos = state.state_vector[self.mapping, :] - self.translation_offset
 
         # Rotate coordinates based upon the sensor_velocity
         xy_rot = self._rotation_matrix @ xy_pos
@@ -647,7 +646,7 @@ def function(self, target_pos, target_vel, sensor_pos,
         rho, phi, _ = cart2sphere(*xy_rot[:, 0])
 
         # Determine the net velocity component in the engagement
-        xy_vel = target_vel - sensor_vel
+        xy_vel = state.state_vector[self.vel_mapping, :] - self.velocity
 
         # Use polar to calculate range rate
         rr = -np.dot(xy_pos[:, 0], xy_vel[:, 0]) / np.linalg.norm(xy_pos)
@@ -660,7 +659,7 @@ def rvs(self, num_samples=1, **kwargs) -> np.array:
         return out
 
 
-class CartesianToElevationBearingRangeRate(NonLinearGaussianMeasurement):
+class CartesianToElevationBearingRangeRate(CartesianToBearingRangeRate):
     r"""This is a class implementation of a time-invariant measurement model, \
     where measurements are assumed to be received in the form of elevation \
     (:math:`\theta`),  bearing (:math:`\phi`), range (:math:`r`) and
@@ -726,9 +725,17 @@ class CartesianToElevationBearingRangeRate(NonLinearGaussianMeasurement):
 
     """
 
-    translation_offset: StateVector = Property(default=StateVector(np.array([[0], [0], [0]])),
-                                               doc="A 3x1 array specifying the origin offset \
-                                               in terms of :math:`x,y`,z coordinates.")
+    translation_offset = Property(
+        StateVector,
+        default=StateVector(np.array([[0], [0], [0]])),
+        doc="A 3x1 array specifying the origin offset in terms of :math:`x,y,z` coordinates.")
+    vel_mapping = Property(
+        np.array,
+        doc="Mapping to the targets velocity within its state space")
+    velocity = Property(
+        StateVector,
+        default=StateVector(np.array([[0], [0], [0]])),
+        doc="A 3x1 array specifying the sensor velocity in terms of :math:`x,y,z` coordinates.")
 
     @property
     def ndim_meas(self) -> int:
@@ -742,14 +749,14 @@ def ndim_meas(self) -> int:
 
         return 4
 
-    def function(self, target_pos, target_vel, sensor_pos, sensor_vel,
-                 noise=False, **kwargs) -> StateVector:
+    def function(self, state, noise=False, **kwargs) -> StateVector:
         r"""Model function :math:`h(\vec{x}_t,\vec{v}_t)`
 
         Parameters
         ----------
-        state_vector: :class:`~.StateVector`
-            An input state vector
+        state: :class:`~.StateVector`
+            An input state vector for the target
+
         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
@@ -768,7 +775,7 @@ def function(self, target_pos, target_vel, sensor_pos, sensor_vel,
                 noise = 0
 
         # Account for origin offset in position to enable range and angles to be determined
-        xyz_pos = target_pos - sensor_pos
+        xyz_pos = state.state_vector[self.mapping, :] - self.translation_offset
 
         # Rotate coordinates based upon the sensor_velocity
         xyz_rot = self._rotation_matrix @ xyz_pos
@@ -777,7 +784,7 @@ def function(self, target_pos, target_vel, sensor_pos, sensor_vel,
         rho, phi, theta = cart2sphere(*xyz_rot[:, 0])
 
         # Determine the net velocity component in the engagement
-        xyz_vel = target_vel - sensor_vel
+        xyz_vel = state.state_vector[self.vel_mapping, :] - self.velocity
 
         # Use polar to calculate range rate
         rr = -np.dot(xyz_pos[:, 0], xyz_vel[:, 0]) / np.linalg.norm(xyz_pos)

From df68e2b827e2760480de9a41713973589c192e07 Mon Sep 17 00:00:00 2001
From: rjgreen <rjgreen@dstl.gov.uk>
Date: Fri, 17 Apr 2020 14:19:19 +0100
Subject: [PATCH 06/21] Added RangeBearingElevation and
 RangeRateBearingElevation radars, doc changes to models

---
 stonesoup/models/measurement/nonlinear.py |  16 +--
 stonesoup/sensor/radar/radar.py           | 129 +++++++++++++++++++++-
 2 files changed, 132 insertions(+), 13 deletions(-)

diff --git a/stonesoup/models/measurement/nonlinear.py b/stonesoup/models/measurement/nonlinear.py
index a73a6cec3..9e6bf8e46 100644
--- a/stonesoup/models/measurement/nonlinear.py
+++ b/stonesoup/models/measurement/nonlinear.py
@@ -579,24 +579,22 @@ class CartesianToBearingRangeRate(NonLinearGaussianMeasurement):
 
     Note
     ----
-    This class implementation assumes a 2D Cartesian plane.
-
-    The velocity states are assumed to be contained in the adjacent state \
-    the position - i.e. obtained by xy = state_vector[self.mapping+1]
-
+    This class implementation assumes a 2D Cartesian plane by default, however\
+    it will operate in 3D so long as all upstream inputs are specified in 3D.
     """
 
     translation_offset = Property(
         StateVector,
         default=StateVector(np.array([[0], [0]])),
-        doc="A 2x1 array specifying the origin offset in terms of :math:`x,y` coordinates.")
+        doc="A 2x1 or 3x1 array specifying the origin offset in terms of :math:`x,y` coordinates.")
     vel_mapping = Property(
         np.array,
         doc="Mapping to the targets velocity within its state space")
     velocity = Property(
         StateVector,
         default=StateVector(np.array([[0], [0]])),
-        doc="A 2x1 array specifying the sensor velocity in terms of :math:`x,y` coordinates.")
+        doc="A 2x1 or 3x1 array specifying the sensor velocity in terms of :math:`x,y` \
+        coordinates.")
 
     @property
     def ndim_meas(self) -> int:
@@ -719,10 +717,6 @@ class CartesianToElevationBearingRangeRate(CartesianToBearingRangeRate):
     Note
     ----
     This class assumes a 3D Cartesian plane.
-
-    The velocity states are assumed to be contained in the adjacent state \
-    the position - i.e. obtained by \dot{x} = state_vector[self.mapping[0]+1]
-
     """
 
     translation_offset = Property(
diff --git a/stonesoup/sensor/radar/radar.py b/stonesoup/sensor/radar/radar.py
index ea175e313..0dd01b49b 100644
--- a/stonesoup/sensor/radar/radar.py
+++ b/stonesoup/sensor/radar/radar.py
@@ -9,8 +9,9 @@
 from ...functions import cart2sphere, rotx, roty, rotz
 from ...base import Property
 
-from ...models.measurement.nonlinear import (CartesianToBearingRange,
-                                             CartesianToBearingRangeRate)
+from ...models.measurement.nonlinear import \
+    (CartesianToBearingRange, CartesianToElevationBearingRange,
+     CartesianToBearingRangeRate, CartesianToElevationBearingRangeRate)
 from ...types.array import CovarianceMatrix
 from ...types.detection import Detection
 from ...types.state import State, StateVector
@@ -202,6 +203,60 @@ def rotate(self, timestamp):
         )
 
 
+class RadarRangeBearingElevation(RadarRangeBearing):
+    """A  radar sensor that generates measurements of targets, using a
+    :class:`~.CartesianToBearingElevationRange` model, relative to its position.
+
+    Note
+    ----
+    This implementation of this class assumes a 3D Cartesian space.
+
+    """
+
+    ndim_state = Property(
+        int,
+        default=3,
+        doc="Number of state dimensions. This is utilised by (and follows in format) "
+            "the underlying :class:`~.CartesianToBearingRange` model")
+    noise_covar = Property(
+        CovarianceMatrix,
+        doc="The sensor noise covariance matrix. This is utilised by\
+                (and follow in format) the underlying \
+                :class:`~.CartesianToElevationBearingRange` model")
+
+    def measure(self, ground_truth, noise=True, **kwargs):
+        """Generate a measurement for a given state
+
+        Parameters
+        ----------
+        ground_truth : :class:`~.State`
+            A ground-truth state
+        noise: :class:`numpy.ndarray` or bool
+            An externally generated random process noise sample (the default is
+            `True`, in which case :meth:`~.Model.rvs` is used
+            if 'False', no noise will be added)
+
+        Returns
+        -------
+        :class:`~.Detection`
+            A measurement generated from the given state. The timestamp of the\
+            measurement is set equal to that of the provided state.
+        """
+        measurement_model = CartesianToElevationBearingRange(
+            ndim_state=self.ndim_state,
+            mapping=self.pos_mapping,
+            noise_covar=self.noise_covar,
+            translation_offset=self.position,
+            rotation_offset=self.orientation)
+
+        measurement_vector = measurement_model.function(
+            ground_truth, noise=noise, **kwargs)
+
+        return Detection(measurement_vector,
+                         measurement_model=measurement_model,
+                         timestamp=ground_truth.timestamp)
+
+
 class RadarRangeRateBearing(RadarRangeBearing):
     """ A radar sensor that generates measurements of targets, using a
     :class:`~.CartesianToBearingRangeRate` model, relative to its position
@@ -216,6 +271,16 @@ class RadarRangeRateBearing(RadarRangeBearing):
     vel_mapping = Property(
         np.array,
         doc="Mapping to the targets velocity information within its state space")
+    ndim_state = Property(
+        int,
+        default=3,
+        doc="Number of state dimensions. This is utilised by (and follows in format) "
+            "the underlying :class:`~.CartesianToBearingRangeRate` model")
+    noise_covar = Property(
+        CovarianceMatrix,
+        doc="The sensor noise covariance matrix. This is utilised by\
+                    (and follow in format) the underlying \
+                    :class:`~.CartesianToBearingRangeRate` model")
 
     def measure(self, ground_truth, noise=True, **kwargs) -> Detection:
         """Generate a measurement for a given state
@@ -252,6 +317,66 @@ def measure(self, ground_truth, noise=True, **kwargs) -> Detection:
                          timestamp=ground_truth.timestamp)
 
 
+class RadarRangeRateBearingElevation(RadarRangeRateBearing):
+    """ A radar sensor that generates measurements of targets, using a
+    :class:`~.CartesianToElevationBearingRangeRate` model, relative to its position
+    and velocity.
+
+    Note
+    ----
+    The current implementation of this class assumes a 3D Cartesian plane.
+
+    """
+
+    vel_mapping = Property(
+        np.array,
+        doc="Mapping to the targets velocity information within its state space")
+    ndim_state = Property(
+        int,
+        default=4,
+        doc="Number of state dimensions. This is utilised by (and follows in format) "
+            "the underlying :class:`~.CartesianToElevationBearingRangeRate` model")
+    noise_covar = Property(
+        CovarianceMatrix,
+        doc="The sensor noise covariance matrix. This is utilised by\
+                        (and follow in format) the underlying \
+                        :class:`~.CartesianToElevationBearingRangeRate` model")
+
+    def measure(self, ground_truth, noise=True, **kwargs) -> Detection:
+        """Generate a measurement for a given state
+
+        Parameters
+        ----------
+        ground_truth : :class:`~.State`
+            A ground-truth state which includes position and velocity information
+        noise: :class:`numpy.ndarray` or bool
+            An externally generated random process noise sample (the default is
+            `True`, in which case :meth:`~.Model.rvs` is used
+            if 'False', no noise will be added)
+
+        Returns
+        -------
+        :class:`~.Detection`
+            A measurement generated from the given state. The timestamp of the\
+            measurement is set equal to that of the provided state.
+        """
+        measurement_model = CartesianToElevationBearingRangeRate(
+            ndim_state=self.ndim_state,
+            mapping=self.pos_mapping,
+            vel_mapping=self.vel_mapping,
+            noise_covar=self.noise_covar,
+            translation_offset=self.position,
+            velocity=self.velocity,
+            rotation_offset=self.orientation)
+
+        measurement_vector = measurement_model.function(
+            ground_truth, noise=noise, **kwargs)
+
+        return Detection(measurement_vector,
+                         measurement_model=measurement_model,
+                         timestamp=ground_truth.timestamp)
+
+
 class RadarRasterScanRangeBearing(RadarRotatingRangeBearing):
     """A simple raster scan radar, with set field-of-regard (FoR) angle, \
      field-of-view (FoV) angle, range and rotations per minute (RPM), that \

From dbb207ca948602a71408bb39cc2cf307e3841231 Mon Sep 17 00:00:00 2001
From: rjgreen <rjgreen@dstl.gov.uk>
Date: Fri, 17 Apr 2020 14:59:33 +0100
Subject: [PATCH 07/21] add prior to rebase

---
 stonesoup/models/measurement/nonlinear.py | 14 ++++++--------
 1 file changed, 6 insertions(+), 8 deletions(-)

diff --git a/stonesoup/models/measurement/nonlinear.py b/stonesoup/models/measurement/nonlinear.py
index 9e6bf8e46..0a7f54a0a 100644
--- a/stonesoup/models/measurement/nonlinear.py
+++ b/stonesoup/models/measurement/nonlinear.py
@@ -135,8 +135,7 @@ def _rotation_matrix(self):
         return rotz(theta_z)@roty(theta_y)@rotx(theta_x)
 
 
-class CartesianToElevationBearingRange(
-        NonLinearGaussianMeasurement, ReversibleModel):
+class CartesianToElevationBearingRange(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 bearing \
     (:math:`\phi`), elevation (:math:`\theta`) and range (:math:`r`), with \
@@ -613,9 +612,8 @@ def function(self, state, noise=False, **kwargs) -> StateVector:
 
         Parameters
         ----------
-        state: :class:`~.StateVector`
-            An input state vector for the target
-
+        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
@@ -623,7 +621,7 @@ def function(self, state, noise=False, **kwargs) -> StateVector:
 
         Returns
         -------
-        :class:`numpy.ndarray` of shape (:py:attr:`~ndim_meas`, 1)
+        :class:`numpy.ndarray` of shape (:py:attr:`~ndim_state`, 1)
             The model function evaluated given the provided time interval.
 
         """
@@ -635,7 +633,7 @@ def function(self, state, noise=False, **kwargs) -> StateVector:
                 noise = 0
 
         # Account for origin offset in position to enable range and angles to be determined
-        xy_pos = state.state_vector[self.mapping, :] - self.translation_offset
+        xy_pos = state.state_vector[self.mapping] - self.translation_offset
 
         # Rotate coordinates based upon the sensor_velocity
         xy_rot = self._rotation_matrix @ xy_pos
@@ -769,7 +767,7 @@ def function(self, state, noise=False, **kwargs) -> StateVector:
                 noise = 0
 
         # Account for origin offset in position to enable range and angles to be determined
-        xyz_pos = state.state_vector[self.mapping, :] - self.translation_offset
+        xyz_pos = state.state_vector[self.mapping] - self.translation_offset
 
         # Rotate coordinates based upon the sensor_velocity
         xyz_rot = self._rotation_matrix @ xyz_pos

From f2913ba1e76087a500276594effffb06712bb2bb Mon Sep 17 00:00:00 2001
From: rjgreen <rjgreen@dstl.gov.uk>
Date: Mon, 27 Apr 2020 16:06:34 +0100
Subject: [PATCH 08/21] Initial tests for range rate radar extension

---
 stonesoup/sensor/radar/tests/test_radar.py | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/stonesoup/sensor/radar/tests/test_radar.py b/stonesoup/sensor/radar/tests/test_radar.py
index 1ef24ebf1..5539b6df2 100644
--- a/stonesoup/sensor/radar/tests/test_radar.py
+++ b/stonesoup/sensor/radar/tests/test_radar.py
@@ -67,8 +67,7 @@ def test_simple_radar():
 
     # Assert correction of generated measurement
     assert(measurement.timestamp == target_state.timestamp)
-    assert(np.equal(measurement.state_vector,
-                    StateVector([phi, rho])).all())
+    assert(np.equal(measurement.state_vector, StateVector([phi, rho])).all())
 
 
 def test_rotating_radar():

From e9f6608de66d57703a7b0413439e30806c37129f Mon Sep 17 00:00:00 2001
From: rjgreen <rjgreen@dstl.gov.uk>
Date: Wed, 29 Apr 2020 16:12:15 +0100
Subject: [PATCH 09/21] rangerate nonlinear measurement models and tests
 completed

---
 stonesoup/models/measurement/nonlinear.py     |  56 ++--
 .../models/measurement/tests/test_models.py   | 264 +++++++++++++++++-
 2 files changed, 290 insertions(+), 30 deletions(-)

diff --git a/stonesoup/models/measurement/nonlinear.py b/stonesoup/models/measurement/nonlinear.py
index 0a7f54a0a..6adab8465 100644
--- a/stonesoup/models/measurement/nonlinear.py
+++ b/stonesoup/models/measurement/nonlinear.py
@@ -40,15 +40,15 @@ def __init__(self, *args, **kwargs):
                 raise ValueError("Models must all have the same `ndim_state`")
 
     @property
-    def ndim_state(self):
+    def ndim_state(self) -> int:
         """Number of state dimensions"""
         return self.model_list[0].ndim_state
 
     @property
-    def ndim_meas(self):
+    def ndim_meas(self) -> int:
         return sum(model.ndim_meas for model in self.model_list)
 
-    def function(self, state, **kwargs):
+    def function(self, state, **kwargs) -> StateVector:
         return np.vstack([model.function(state, **kwargs)
                           for model in self.model_list]).view(StateVector)
 
@@ -59,7 +59,7 @@ def _linear_inverse_function(model, state, **kwargs):
 
         return inv_model_matrix@state.state_vector
 
-    def inverse_function(self, detection, **kwargs):
+    def inverse_function(self, detection, **kwargs) -> StateVector:
         state = copy.copy(detection)
         ndim_count = 0
         state_vector = np.zeros((self.ndim_state, 1)).view(StateVector)
@@ -76,12 +76,12 @@ def inverse_function(self, detection, **kwargs):
 
         return state_vector
 
-    def covar(self, **kwargs):
+    def covar(self, **kwargs) -> CovarianceMatrix:
         return block_diag(
             *(model.covar(**kwargs) for model in self.model_list)
             ).view(CovarianceMatrix)
 
-    def rvs(self, num_samples=1, **kwargs):
+    def rvs(self, num_samples=1, **kwargs) -> np.ndarray:
         rvs_vectors = np.vstack([model.rvs(num_samples, **kwargs)
                                  for model in self.model_list])
         if num_samples == 1:
@@ -104,7 +104,7 @@ class NonLinearGaussianMeasurement(MeasurementModel, NonLinearModel, GaussianMod
             counter-clockwise direction when viewed by an observer looking\
             along the respective rotation axis, towards the origin.")
 
-    def covar(self, **kwargs):
+    def covar(self, **kwargs) -> CovarianceMatrix:
         """Returns the measurement model noise covariance matrix.
 
         Returns
@@ -117,7 +117,7 @@ def covar(self, **kwargs):
         return self.noise_covar
 
     @property
-    def _rotation_matrix(self):
+    def _rotation_matrix(self) -> np.ndarray:
         """_rotation_matrix getter method
 
         Calculates and returns the (3D) axis rotation matrix.
@@ -201,7 +201,7 @@ class CartesianToElevationBearingRange(NonLinearGaussianMeasurement, ReversibleM
             coordinates.")
 
     @property
-    def ndim_meas(self):
+    def ndim_meas(self) -> int:
         """ndim_meas getter method
 
         Returns
@@ -212,7 +212,7 @@ def ndim_meas(self):
 
         return 3
 
-    def function(self, state, noise=False, **kwargs):
+    def function(self, state, noise=False, **kwargs) -> StateVector:
         r"""Model function :math:`h(\vec{x}_t,\vec{v}_t)`
 
         Parameters
@@ -247,7 +247,7 @@ def function(self, state, noise=False, **kwargs):
 
         return StateVector([[Elevation(theta)], [Bearing(phi)], [rho]]) + noise
 
-    def inverse_function(self, detection, **kwargs):
+    def inverse_function(self, detection, **kwargs) -> StateVector:
 
         theta, phi, rho = detection.state_vector[:, 0]
         x, y, z = sphere2cart(rho, phi, theta)
@@ -263,7 +263,7 @@ def inverse_function(self, detection, **kwargs):
 
         return res
 
-    def rvs(self, num_samples=1, **kwargs):
+    def rvs(self, num_samples=1, **kwargs) -> np.ndarray:
         out = super().rvs(num_samples, **kwargs)
         out = np.array([[Elevation(0.)], [Bearing(0.)], [0.]]) + out
         return out
@@ -330,7 +330,7 @@ class CartesianToBearingRange(NonLinearGaussianMeasurement, ReversibleModel):
             coordinates.")
 
     @property
-    def ndim_meas(self):
+    def ndim_meas(self) -> int:
         """ndim_meas getter method
 
         Returns
@@ -341,7 +341,7 @@ def ndim_meas(self):
 
         return 2
 
-    def inverse_function(self, detection, **kwargs):
+    def inverse_function(self, detection, **kwargs) -> StateVector:
         if not ((self.rotation_offset[0][0] == 0)
                 and (self.rotation_offset[1][0] == 0)):
             raise RuntimeError(
@@ -362,7 +362,7 @@ def inverse_function(self, detection, **kwargs):
 
         return res
 
-    def function(self, state, noise=False, **kwargs):
+    def function(self, state, noise=False, **kwargs) -> StateVector:
         r"""Model function :math:`h(\vec{x}_t,\vec{v}_t)`
 
         Parameters
@@ -401,7 +401,7 @@ def function(self, state, noise=False, **kwargs):
 
         return StateVector([[Bearing(phi)], [rho]]) + noise
 
-    def rvs(self, num_samples=1, **kwargs):
+    def rvs(self, num_samples=1, **kwargs) -> np.ndarray:
         out = super().rvs(num_samples, **kwargs)
         out = np.array([[Bearing(0)], [0.]]) + out
         return out
@@ -470,7 +470,7 @@ class CartesianToElevationBearing(NonLinearGaussianMeasurement):
             coordinates.")
 
     @property
-    def ndim_meas(self):
+    def ndim_meas(self) -> int:
         """ndim_meas getter method
 
         Returns
@@ -481,7 +481,7 @@ def ndim_meas(self):
 
         return 2
 
-    def function(self, state, noise=False, **kwargs):
+    def function(self, state, noise=False, **kwargs) -> StateVector:
         r"""Model function :math:`h(\vec{x}_t,\vec{v}_t)`
 
         Parameters
@@ -516,7 +516,7 @@ def function(self, state, noise=False, **kwargs):
 
         return StateVector([[Elevation(theta)], [Bearing(phi)]]) + noise
 
-    def rvs(self, num_samples=1, **kwargs):
+    def rvs(self, num_samples=1, **kwargs) -> np.ndarray:
         out = super().rvs(num_samples, **kwargs)
         out = np.array([[Elevation(0.)], [Bearing(0.)]]) + out
         return out
@@ -578,21 +578,21 @@ class CartesianToBearingRangeRate(NonLinearGaussianMeasurement):
 
     Note
     ----
-    This class implementation assumes a 2D Cartesian plane by default, however\
-    it will operate in 3D so long as all upstream inputs are specified in 3D.
+    This class implementation assuming at 3D cartesian space, it therefore\
+     expects a 6D state space.
     """
 
     translation_offset = Property(
         StateVector,
-        default=StateVector(np.array([[0], [0]])),
-        doc="A 2x1 or 3x1 array specifying the origin offset in terms of :math:`x,y` coordinates.")
+        default=StateVector(np.array([[0], [0], [0]])),
+        doc="A 3x1 array specifying the origin offset in terms of :math:`x,y` coordinates.")
     vel_mapping = Property(
         np.array,
         doc="Mapping to the targets velocity within its state space")
     velocity = Property(
         StateVector,
-        default=StateVector(np.array([[0], [0]])),
-        doc="A 2x1 or 3x1 array specifying the sensor velocity in terms of :math:`x,y` \
+        default=StateVector(np.array([[0], [0], [0]])),
+        doc="A 3x1 array specifying the sensor velocity in terms of :math:`x,y` \
         coordinates.")
 
     @property
@@ -649,13 +649,13 @@ def function(self, state, noise=False, **kwargs) -> StateVector:
 
         return StateVector([[Bearing(phi)], [rho], [rr]]) + noise
 
-    def rvs(self, num_samples=1, **kwargs) -> np.array:
+    def rvs(self, num_samples=1, **kwargs) -> np.ndarray:
         out = super().rvs(num_samples, **kwargs)
         out = np.array([[Bearing(0)], [0.], [0.]]) + out
         return out
 
 
-class CartesianToElevationBearingRangeRate(CartesianToBearingRangeRate):
+class CartesianToElevationBearingRangeRate(NonLinearGaussianMeasurement):
     r"""This is a class implementation of a time-invariant measurement model, \
     where measurements are assumed to be received in the form of elevation \
     (:math:`\theta`),  bearing (:math:`\phi`), range (:math:`r`) and
@@ -786,7 +786,7 @@ def function(self, state, noise=False, **kwargs) -> StateVector:
                             [rho],
                             [rr]]) + noise
 
-    def rvs(self, num_samples=1, **kwargs) -> np.array:
+    def rvs(self, num_samples=1, **kwargs) -> np.ndarray:
         out = super().rvs(num_samples, **kwargs)
         out = np.array([[Elevation(0)], [Bearing(0)], [0.], [0.]]) + out
         return out
diff --git a/stonesoup/models/measurement/tests/test_models.py b/stonesoup/models/measurement/tests/test_models.py
index f476c55f1..b2310e9de 100644
--- a/stonesoup/models/measurement/tests/test_models.py
+++ b/stonesoup/models/measurement/tests/test_models.py
@@ -6,7 +6,8 @@
 
 from ..nonlinear import (
     CartesianToElevationBearingRange, CartesianToBearingRange,
-    CartesianToElevationBearing)
+    CartesianToElevationBearing, CartesianToBearingRangeRate,
+    CartesianToElevationBearingRangeRate)
 from ...base import ReversibleModel
 from ....types.state import State
 from ....functions import jacobian as compute_jac
@@ -170,7 +171,8 @@ def hbearing(state_vector, translation_offset, rotation_offset):
 )
 def test_models(h, ModelClass, state_vec, R,
                 mapping, translation_offset, rotation_offset):
-    """ CartesianToBearingRange Measurement Model test """
+    """ Test for the CartesianToBearingRange, CartesianToElevationBearingRange,
+     and CartesianToElevationBearing Measurement Models """
 
     ndim_state = state_vec.size
     state = State(state_vec)
@@ -266,3 +268,261 @@ def fun(x):
                         model.translation_offset,
                         model.rotation_offset)).ravel(),
         cov=R)
+
+
+def h2d_rr(state_vector, pos_map, vel_map, translation_offset, rotation_offset, velocity):
+
+    xyz = np.array([[state_vector[pos_map[0], 0] - translation_offset[0, 0]],
+                    [state_vector[pos_map[1], 0] - translation_offset[1, 0]],
+                    [0]])
+
+    # Get rotation matrix
+    theta_z = - rotation_offset[2, 0]
+    cos_z, sin_z = np.cos(theta_z), np.sin(theta_z)
+    rot_z = np.array([[cos_z, -sin_z, 0],
+                      [sin_z, cos_z, 0],
+                      [0, 0, 1]])
+
+    theta_y = - rotation_offset[1, 0]
+    cos_y, sin_y = np.cos(theta_y), np.sin(theta_y)
+    rot_y = np.array([[cos_y, 0, sin_y],
+                      [0, 1, 0],
+                      [-sin_y, 0, cos_y]])
+
+    theta_x = - rotation_offset[0, 0]
+    cos_x, sin_x = np.cos(theta_x), np.sin(theta_x)
+    rot_x = np.array([[1, 0, 0],
+                      [0, cos_x, -sin_x],
+                      [0, sin_x, cos_x]])
+
+    rotation_matrix = rot_z@rot_y@rot_x
+
+    xyz_rot = rotation_matrix @ xyz
+    x = xyz_rot[0, 0]
+    y = xyz_rot[1, 0]
+    z = xyz_rot[2, 0]
+
+    rho = np.sqrt(x**2 + y**2 + z**2)
+    phi = np.arctan2(y, x)
+    # theta = np.arcsin(z/rho)
+
+    # Calculate range rate extension
+    # Determine the net velocity component in the engagement
+    xyz_vel = np.array([[state_vector[vel_map[0], 0] - velocity[0, 0]],
+                        [state_vector[vel_map[1], 0] - velocity[1, 0]],
+                        [0]])
+
+    # Use polar to calculate range rate
+    rr = -np.dot(xyz[:, 0], xyz_vel[:, 0]) / np.linalg.norm(xyz)
+
+    return np.array([[Bearing(phi)], [rho], [rr]])
+
+
+def h3d_rr(state_vector, pos_map, vel_map, translation_offset, rotation_offset, velocity):
+
+    xyz = np.array([[state_vector[pos_map[0], 0] - translation_offset[0, 0]],
+                    [state_vector[pos_map[1], 0] - translation_offset[1, 0]],
+                    [state_vector[pos_map[2], 0] - translation_offset[2, 0]]])
+
+    # Get rotation matrix
+    theta_z = - rotation_offset[2, 0]
+    cos_z, sin_z = np.cos(theta_z), np.sin(theta_z)
+    rot_z = np.array([[cos_z, -sin_z, 0],
+                      [sin_z, cos_z, 0],
+                      [0, 0, 1]])
+
+    theta_y = - rotation_offset[1, 0]
+    cos_y, sin_y = np.cos(theta_y), np.sin(theta_y)
+    rot_y = np.array([[cos_y, 0, sin_y],
+                      [0, 1, 0],
+                      [-sin_y, 0, cos_y]])
+
+    theta_x = - rotation_offset[0, 0]
+    cos_x, sin_x = np.cos(theta_x), np.sin(theta_x)
+    rot_x = np.array([[1, 0, 0],
+                      [0, cos_x, -sin_x],
+                      [0, sin_x, cos_x]])
+
+    rotation_matrix = rot_z@rot_y@rot_x
+
+    xyz_rot = rotation_matrix @ xyz
+    x = xyz_rot[0, 0]
+    y = xyz_rot[1, 0]
+    z = xyz_rot[2, 0]
+
+    rho = np.sqrt(x**2 + y**2 + z**2)
+    phi = np.arctan2(y, x)
+    theta = np.arcsin(z/rho)
+
+    # Calculate range rate extension
+    # Determine the net velocity component in the engagement
+    xyz_vel = np.array([[state_vector[vel_map[0], 0] - velocity[0, 0]],
+                        [state_vector[vel_map[1], 0] - velocity[1, 0]],
+                        [state_vector[vel_map[2], 0] - velocity[2, 0]]])
+
+    # Use polar to calculate range rate
+    rr = -np.dot(xyz[:, 0], xyz_vel[:, 0]) / np.linalg.norm(xyz)
+
+    return np.array([[Elevation(theta)], [Bearing(phi)], [rho], [rr]])
+
+
+@pytest.mark.parametrize(
+    "h, ModelClass, state_vec, ndim_state, pos_mapping, vel_mapping,\
+    noise_covar, position, orientation",
+    [
+        (   # 3D meas, 6D state
+            h2d_rr,  # h
+            CartesianToBearingRangeRate,  # ModelClass
+            np.array([[200], [10], [0], [0], [0], [0]]),  # state_vec
+            6,  # ndim_state
+            np.array([0, 2, 4]),  # pos_mapping
+            np.array([1, 3, 5]),  # vel_mapping
+            np.array([[0.05, 0, 0],
+                      [0, 0.015, 0],
+                      [0, 0, 10]]),  # noise_covar
+            np.array([[1], [-1], [0]]),  # position (translation offset)
+            np.array([[0], [0], [1]])  # orientation (rotation offset)
+        ),
+        (   # 4D meas, 6D state
+            h3d_rr,  # h
+            CartesianToElevationBearingRangeRate,  # ModelClass
+            np.array([[200], [10], [0], [0], [0], [0]]),  # state_vec
+            6,  # ndim_state
+            np.array([0, 2, 4]),  # pos_mapping
+            np.array([1, 3, 5]),  # vel_mapping
+            np.array([[0.05, 0, 0, 0],
+                      [0, 0.05, 0, 0],
+                      [0, 0, 0.015, 0],
+                      [0, 0, 0, 10]]),  # noise_covar
+            np.array([[100], [0], [0]]),  # position (translation offset)
+            np.array([[0], [0], [0]])  # orientation (rotation offset)
+        )
+    ],
+    ids=["rrRB", "rrRBE"]
+)
+def test_rangeratemodels(h, ModelClass, state_vec, ndim_state, pos_mapping, vel_mapping,
+                         noise_covar, position, orientation):
+    """ Test for the CartesianToBearingRangeRate and
+    CartesianToElevationBearingRangeRate Measurement Models """
+
+    state = State(state_vec)
+
+    # Create and a measurement model object
+    model = ModelClass(ndim_state=ndim_state,
+                       mapping=pos_mapping,
+                       vel_mapping=vel_mapping,
+                       noise_covar=noise_covar,
+                       translation_offset=position,
+                       rotation_offset=orientation)
+
+    # Project a state through the model
+    # (without noise)
+    meas_pred_wo_noise = model.function(state)
+    eval_m = h(state_vec,
+               model.mapping,
+               model.vel_mapping,
+               model.translation_offset,
+               model.rotation_offset,
+               model.velocity)
+    assert np.array_equal(meas_pred_wo_noise, eval_m)
+
+    # Ensure ```lg.transfer_function()``` returns H
+    def fun(x):
+        return model.function(x)
+
+    H = compute_jac(fun, state)
+    assert np.array_equal(H, model.jacobian(state))
+
+    # Check Jacobian has proper dimensions
+    assert H.shape == (model.ndim_meas, ndim_state)
+
+    # Ensure inverse function returns original
+    if isinstance(model, ReversibleModel):
+        J = model.inverse_function(State(meas_pred_wo_noise))
+        assert np.allclose(J, state_vec)
+
+    # Ensure ```lg.covar()``` returns R
+    assert np.array_equal(noise_covar, model.covar())
+
+    # Ensure model creates noise
+    rvs = model.rvs()
+    assert rvs.shape == (model.ndim_meas, 1)
+    assert isinstance(rvs, StateVector)
+    rvs = model.rvs(10)
+    assert rvs.shape == (model.ndim_meas, 10)
+    assert isinstance(rvs, Matrix)
+    # StateVector is subclass of Matrix, so need to check explicitly.
+    assert not isinstance(rvs, StateVector)
+
+    # Project a state throught the model
+    # Project a state through the model
+    # (without noise)
+    meas_pred_wo_noise = model.function(state)
+    assert np.array_equal(meas_pred_wo_noise, h(state_vec,
+                                                model.mapping,
+                                                model.vel_mapping,
+                                                model.translation_offset,
+                                                model.rotation_offset,
+                                                model.velocity))
+
+    # Evaluate the likelihood of the predicted measurement, given the state
+    # (without noise)
+    prob = model.pdf(State(meas_pred_wo_noise), state)
+    assert approx(prob) == multivariate_normal.pdf(
+        meas_pred_wo_noise.T,
+        mean=np.array(h(state_vec,
+                        model.mapping,
+                        model.vel_mapping,
+                        model.translation_offset,
+                        model.rotation_offset,
+                        model.velocity)).ravel(),
+        cov=noise_covar)
+
+    # Propagate a state vector through the model
+    # (with internal noise)
+    meas_pred_w_inoise = model.function(state, noise=True)
+    assert not np.array_equal(
+        meas_pred_w_inoise, h(state_vec,
+                              model.mapping,
+                              model.vel_mapping,
+                              model.translation_offset,
+                              model.rotation_offset,
+                              model.velocity))
+
+    # Evaluate the likelihood of the predicted state, given the prior
+    # (with noise)
+    prob = model.pdf(State(meas_pred_w_inoise), state)
+    assert approx(prob) == multivariate_normal.pdf(
+        meas_pred_w_inoise.T,
+        mean=np.array(h(state_vec,
+                        model.mapping,
+                        model.vel_mapping,
+                        model.translation_offset,
+                        model.rotation_offset,
+                        model.velocity)).ravel(),
+        cov=noise_covar)
+
+    # Propagate a state vector throught the model
+    # (with external noise)
+    noise = model.rvs()
+    meas_pred_w_enoise = model.function(state,
+                                        noise=noise)
+    assert np.array_equal(meas_pred_w_enoise, h(state_vec,
+                                                model.mapping,
+                                                model.vel_mapping,
+                                                model.translation_offset,
+                                                model.rotation_offset,
+                                                model.velocity) + noise)
+
+    # Evaluate the likelihood of the predicted state, given the prior
+    # (with noise)
+    prob = model.pdf(State(meas_pred_w_enoise), state)
+    assert approx(prob) == multivariate_normal.pdf(
+        meas_pred_w_enoise.T,
+        mean=np.array(h(state_vec,
+                        model.mapping,
+                        model.vel_mapping,
+                        model.translation_offset,
+                        model.rotation_offset,
+                        model.velocity)).ravel(),
+        cov=noise_covar)

From c935b662fd41445a56ad2544cf40289e91d98894 Mon Sep 17 00:00:00 2001
From: rjgreen <rjgreen@dstl.gov.uk>
Date: Wed, 29 Apr 2020 16:14:55 +0100
Subject: [PATCH 10/21] minor change to velocity property

---
 stonesoup/sensor/base.py | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/stonesoup/sensor/base.py b/stonesoup/sensor/base.py
index 918e8f03e..67965928d 100644
--- a/stonesoup/sensor/base.py
+++ b/stonesoup/sensor/base.py
@@ -104,7 +104,8 @@ def orientation(self, value: StateVector):
     def _has_internal_platform(self) -> bool:
         return False
 
-    def velocity(self):
+    @property
+    def velocity(self) -> Optional[StateVector]:
         """The sensor velocity on a 3D Cartesian plane, expressed as a 3x1 :class:`StateVector`
         of Cartesian coordinates in the order :math:`x,y,z`.
 
@@ -122,4 +123,3 @@ def velocity(self, value):
         else:
             raise AttributeError('Cannot set platform velocity unless the sensor has its own '
                                  'default platform')
-

From 07d32ad5f53f34ef10fc707c0b31c10151399628 Mon Sep 17 00:00:00 2001
From: rjgreen <rjgreen@dstl.gov.uk>
Date: Wed, 29 Apr 2020 16:55:54 +0100
Subject: [PATCH 11/21] rangerate radar completed and tests, one assertation
 error

---
 stonesoup/sensor/radar/radar.py            |   3 +-
 stonesoup/sensor/radar/tests/test_radar.py | 157 ++++++++++++++++++++-
 2 files changed, 156 insertions(+), 4 deletions(-)

diff --git a/stonesoup/sensor/radar/radar.py b/stonesoup/sensor/radar/radar.py
index 0dd01b49b..988f39686 100644
--- a/stonesoup/sensor/radar/radar.py
+++ b/stonesoup/sensor/radar/radar.py
@@ -333,7 +333,7 @@ class RadarRangeRateBearingElevation(RadarRangeRateBearing):
         doc="Mapping to the targets velocity information within its state space")
     ndim_state = Property(
         int,
-        default=4,
+        default=6,
         doc="Number of state dimensions. This is utilised by (and follows in format) "
             "the underlying :class:`~.CartesianToElevationBearingRangeRate` model")
     noise_covar = Property(
@@ -490,7 +490,6 @@ class AESARadar(Sensor):
             "counter-clockwise rotation around the :math:`x,y,z` axis. i.e Roll, Pitch and Yaw. "
             "Default is ``StateVector([0, 0, 0])``")
 
-
     pos_mapping = Property(
         np.array, default=(0, 1, 2),
         doc="Mapping between or positions and state "
diff --git a/stonesoup/sensor/radar/tests/test_radar.py b/stonesoup/sensor/radar/tests/test_radar.py
index 5539b6df2..a20bda905 100644
--- a/stonesoup/sensor/radar/tests/test_radar.py
+++ b/stonesoup/sensor/radar/tests/test_radar.py
@@ -1,15 +1,16 @@
 # -*- coding: utf-8 -*-
 import datetime
+import pytest
 from pytest import approx
 import numpy as np
 
 from ....functions import cart2pol, rotz, rotx, roty
-from ....types.angle import Bearing
+from ....types.angle import Bearing, Elevation
 from ....types.array import StateVector, CovarianceMatrix
 from ....types.state import State
 from ....types.groundtruth import GroundTruthState
 from ..radar import RadarRangeBearing, RadarRotatingRangeBearing, AESARadar, \
-    RadarRasterScanRangeBearing
+    RadarRasterScanRangeBearing, RadarRangeRateBearing, RadarRangeRateBearingElevation
 from ..beam_pattern import StationaryBeam
 from ..beam_shape import Beam2DGaussian
 from ....models.measurement.linear import LinearGaussian
@@ -70,6 +71,158 @@ def test_simple_radar():
     assert(np.equal(measurement.state_vector, StateVector([phi, rho])).all())
 
 
+def h2d_rr(state, pos_map, vel_map, translation_offset, rotation_offset, velocity):
+
+    xyz = np.array([[state.state_vector[pos_map[0], 0] - translation_offset[0, 0]],
+                    [state.state_vector[pos_map[1], 0] - translation_offset[1, 0]],
+                    [0]])
+
+    # Get rotation matrix
+    theta_z = - rotation_offset[2, 0]
+    cos_z, sin_z = np.cos(theta_z), np.sin(theta_z)
+    rot_z = np.array([[cos_z, -sin_z, 0],
+                      [sin_z, cos_z, 0],
+                      [0, 0, 1]])
+
+    theta_y = - rotation_offset[1, 0]
+    cos_y, sin_y = np.cos(theta_y), np.sin(theta_y)
+    rot_y = np.array([[cos_y, 0, sin_y],
+                      [0, 1, 0],
+                      [-sin_y, 0, cos_y]])
+
+    theta_x = - rotation_offset[0, 0]
+    cos_x, sin_x = np.cos(theta_x), np.sin(theta_x)
+    rot_x = np.array([[1, 0, 0],
+                      [0, cos_x, -sin_x],
+                      [0, sin_x, cos_x]])
+
+    rotation_matrix = rot_z@rot_y@rot_x
+
+    xyz_rot = rotation_matrix @ xyz
+    x = xyz_rot[0, 0]
+    y = xyz_rot[1, 0]
+    z = xyz_rot[2, 0]
+
+    rho = np.sqrt(x**2 + y**2 + z**2)
+    phi = np.arctan2(y, x)
+    # theta = np.arcsin(z/rho)
+
+    # Calculate range rate extension
+    # Determine the net velocity component in the engagement
+    xyz_vel = np.array([[state.state_vector[vel_map[0], 0] - velocity[0, 0]],
+                        [state.state_vector[vel_map[1], 0] - velocity[1, 0]],
+                        [0]])
+
+    # Use polar to calculate range rate
+    rr = -np.dot(xyz[:, 0], xyz_vel[:, 0]) / np.linalg.norm(xyz)
+
+    return np.array([[Bearing(phi)], [rho], [rr]])
+
+
+def h3d_rr(state, pos_map, vel_map, translation_offset, rotation_offset, velocity):
+
+    xyz = np.array([[state.state_vector[pos_map[0], 0] - translation_offset[0, 0]],
+                    [state.state_vector[pos_map[1], 0] - translation_offset[1, 0]],
+                    [state.state_vector[pos_map[2], 0] - translation_offset[2, 0]]])
+
+    # Get rotation matrix
+    theta_z = - rotation_offset[2, 0]
+    cos_z, sin_z = np.cos(theta_z), np.sin(theta_z)
+    rot_z = np.array([[cos_z, -sin_z, 0],
+                      [sin_z, cos_z, 0],
+                      [0, 0, 1]])
+
+    theta_y = - rotation_offset[1, 0]
+    cos_y, sin_y = np.cos(theta_y), np.sin(theta_y)
+    rot_y = np.array([[cos_y, 0, sin_y],
+                      [0, 1, 0],
+                      [-sin_y, 0, cos_y]])
+
+    theta_x = - rotation_offset[0, 0]
+    cos_x, sin_x = np.cos(theta_x), np.sin(theta_x)
+    rot_x = np.array([[1, 0, 0],
+                      [0, cos_x, -sin_x],
+                      [0, sin_x, cos_x]])
+
+    rotation_matrix = rot_z@rot_y@rot_x
+
+    xyz_rot = rotation_matrix @ xyz
+    x = xyz_rot[0, 0]
+    y = xyz_rot[1, 0]
+    z = xyz_rot[2, 0]
+
+    rho = np.sqrt(x**2 + y**2 + z**2)
+    phi = np.arctan2(y, x)
+    theta = np.arcsin(z/rho)
+
+    # Calculate range rate extension
+    # Determine the net velocity component in the engagement
+    xyz_vel = np.array([[state.state_vector[vel_map[0], 0] - velocity[0, 0]],
+                        [state.state_vector[vel_map[1], 0] - velocity[1, 0]],
+                        [state.state_vector[vel_map[2], 0] - velocity[2, 0]]])
+
+    # Use polar to calculate range rate
+    rr = -np.dot(xyz[:, 0], xyz_vel[:, 0]) / np.linalg.norm(xyz)
+
+    return np.array([[Elevation(theta)], [Bearing(phi)], [rho], [rr]])
+
+
+@pytest.mark.parametrize(
+    "h, sensorclass, pos_mapping, vel_mapping, noise_covar, position",
+    [
+        (
+            h2d_rr,  # h
+            RadarRangeRateBearing,  # sensorclass
+            np.array([0, 2, 4]),  # pos_mapping
+            np.array([1, 3, 5]),  # vel_mapping
+            np.array([[0.05, 0, 0],
+                      [0, 0.015, 0],
+                      [0, 0, 10]]),  # noise_covar
+            StateVector([[100], [0], [0]])  # position
+         ),
+        (
+            h3d_rr,
+            RadarRangeRateBearingElevation,
+            np.array([0, 2, 4]),  # pos_mapping
+            np.array([1, 3, 5]),  # vel_mapping
+            np.array([[0.05, 0, 0, 0],
+                      [0, 0.05, 0, 0],
+                      [0, 0, 0.015, 0],
+                      [0, 0, 0, 10]]),  # noise_covar
+            StateVector([[100], [0], [0]])  # position
+        )
+    ],
+    ids=["RadarRangeRateBearing", "RadarRangeRateBearingElevation"]
+)
+def test_range_rate_radar(h, sensorclass, pos_mapping, vel_mapping, noise_covar, position):
+
+    # Instantiate the rotating radar
+    radar = sensorclass(ndim_state=6,
+                        pos_mapping=pos_mapping,
+                        vel_mapping=vel_mapping,
+                        noise_covar=noise_covar,
+                        position=position)
+
+    assert (np.equal(radar.position, position).all())
+
+    target_state = State(np.array([[200], [10], [0], [0], [0], [0]]),
+                         timestamp=datetime.datetime.now())
+
+    # Generate a noiseless measurement for the given target
+    measurement = radar.measure(target_state, noise=False)
+
+    # Assert correction of generated measurement
+    assert (measurement.timestamp == target_state.timestamp)
+    print(radar.orientation)
+    print(radar.velocity)
+    assert (np.equal(measurement.state_vector, h(target_state,
+                                                 pos_map=pos_mapping,
+                                                 vel_map=vel_mapping,
+                                                 translation_offset=position,
+                                                 rotation_offset=radar.orientation,
+                                                 velocity=radar.velocity)).all())
+
+
 def test_rotating_radar():
 
     # Input arguments

From fa19cb68f4cd39cf08fc997c279fb8e308512a0e Mon Sep 17 00:00:00 2001
From: rjgreen <rjgreen@dstl.gov.uk>
Date: Thu, 30 Apr 2020 11:27:16 +0100
Subject: [PATCH 12/21] rangerate measurements and sensor tests completed

---
 .../models/measurement/tests/test_models.py   |   6 +-
 stonesoup/sensor/radar/tests/test_radar.py    | 116 +++++++++++++-----
 2 files changed, 88 insertions(+), 34 deletions(-)

diff --git a/stonesoup/models/measurement/tests/test_models.py b/stonesoup/models/measurement/tests/test_models.py
index b2310e9de..73b78ea2f 100644
--- a/stonesoup/models/measurement/tests/test_models.py
+++ b/stonesoup/models/measurement/tests/test_models.py
@@ -367,7 +367,7 @@ def h3d_rr(state_vector, pos_map, vel_map, translation_offset, rotation_offset,
 
 
 @pytest.mark.parametrize(
-    "h, ModelClass, state_vec, ndim_state, pos_mapping, vel_mapping,\
+    "h, modelclass, state_vec, ndim_state, pos_mapping, vel_mapping,\
     noise_covar, position, orientation",
     [
         (   # 3D meas, 6D state
@@ -400,7 +400,7 @@ def h3d_rr(state_vector, pos_map, vel_map, translation_offset, rotation_offset,
     ],
     ids=["rrRB", "rrRBE"]
 )
-def test_rangeratemodels(h, ModelClass, state_vec, ndim_state, pos_mapping, vel_mapping,
+def test_rangeratemodels(h, modelclass, state_vec, ndim_state, pos_mapping, vel_mapping,
                          noise_covar, position, orientation):
     """ Test for the CartesianToBearingRangeRate and
     CartesianToElevationBearingRangeRate Measurement Models """
@@ -408,7 +408,7 @@ def test_rangeratemodels(h, ModelClass, state_vec, ndim_state, pos_mapping, vel_
     state = State(state_vec)
 
     # Create and a measurement model object
-    model = ModelClass(ndim_state=ndim_state,
+    model = modelclass(ndim_state=ndim_state,
                        mapping=pos_mapping,
                        vel_mapping=vel_mapping,
                        noise_covar=noise_covar,
diff --git a/stonesoup/sensor/radar/tests/test_radar.py b/stonesoup/sensor/radar/tests/test_radar.py
index a20bda905..83f951b08 100644
--- a/stonesoup/sensor/radar/tests/test_radar.py
+++ b/stonesoup/sensor/radar/tests/test_radar.py
@@ -4,22 +4,22 @@
 from pytest import approx
 import numpy as np
 
-from ....functions import cart2pol, rotz, rotx, roty
+from ....functions import rotz, rotx, roty
 from ....types.angle import Bearing, Elevation
 from ....types.array import StateVector, CovarianceMatrix
 from ....types.state import State
 from ....types.groundtruth import GroundTruthState
-from ..radar import RadarRangeBearing, RadarRotatingRangeBearing, AESARadar, \
-    RadarRasterScanRangeBearing, RadarRangeRateBearing, RadarRangeRateBearingElevation
+from ..radar import RadarRangeBearing, RadarRangeBearingElevation, RadarRotatingRangeBearing, \
+    AESARadar, RadarRasterScanRangeBearing, RadarRangeRateBearing, RadarRangeRateBearingElevation
 from ..beam_pattern import StationaryBeam
 from ..beam_shape import Beam2DGaussian
 from ....models.measurement.linear import LinearGaussian
 
 
-def h2d(state_vector, translation_offset, rotation_offset):
+def h2d(state, translation_offset, rotation_offset):
 
-    xyz = [[state_vector[0, 0] - translation_offset[0, 0]],
-           [state_vector[1, 0] - translation_offset[1, 0]],
+    xyz = [[state.state_vector[0, 0] - translation_offset[0, 0]],
+           [state.state_vector[1, 0] - translation_offset[1, 0]],
            [0]]
 
     # Get rotation matrix
@@ -40,35 +40,91 @@ def h2d(state_vector, translation_offset, rotation_offset):
     return np.array([[Bearing(phi)], [rho]])
 
 
-def test_simple_radar():
+def h3d(state, translation_offset, rotation_offset):
 
-    # Input arguments
-    # TODO: pytest parametarization
-    noise_covar = CovarianceMatrix([[0.015, 0],
-                                    [0, 0.1]])
-    radar_position = StateVector([1, 1])
-    radar_orientation = StateVector([0, 0, 0])
-    target_state = State(radar_position + np.array([[1], [1]]), timestamp=datetime.datetime.now())
-    measurement_mapping = np.array([0, 1])
+    xyz = np.array([[state.state_vector[0, 0] - translation_offset[0, 0]],
+                    [state.state_vector[1, 0] - translation_offset[1, 0]],
+                    [state.state_vector[2, 0] - translation_offset[2, 0]]])
 
-    # Create a radar object
-    radar = RadarRangeBearing(position=radar_position,
-                              orientation=radar_orientation,
-                              ndim_state=2,
-                              pos_mapping=measurement_mapping,
-                              noise_covar=noise_covar)
+    # Get rotation matrix
+    theta_z = - rotation_offset[2, 0]
+    cos_z, sin_z = np.cos(theta_z), np.sin(theta_z)
+    rot_z = np.array([[cos_z, -sin_z, 0],
+                      [sin_z, cos_z, 0],
+                      [0, 0, 1]])
 
-    # Assert that the object has been correctly initialised
-    assert(np.equal(radar.position, radar_position).all())
+    theta_y = - rotation_offset[1, 0]
+    cos_y, sin_y = np.cos(theta_y), np.sin(theta_y)
+    rot_y = np.array([[cos_y, 0, sin_y],
+                      [0, 1, 0],
+                      [-sin_y, 0, cos_y]])
+
+    theta_x = - rotation_offset[0, 0]
+    cos_x, sin_x = np.cos(theta_x), np.sin(theta_x)
+    rot_x = np.array([[1, 0, 0],
+                      [0, cos_x, -sin_x],
+                      [0, sin_x, cos_x]])
+
+    rotation_matrix = rot_z@rot_y@rot_x
+
+    xyz_rot = rotation_matrix @ xyz
+    x = xyz_rot[0, 0]
+    y = xyz_rot[1, 0]
+    z = xyz_rot[2, 0]
+
+    rho = np.sqrt(x**2 + y**2 + z**2)
+    phi = np.arctan2(y, x)
+    theta = np.arcsin(z/rho)
+
+    return np.array([[Elevation(theta)], [Bearing(phi)], [rho]])
+
+
+@pytest.mark.parametrize(
+    "h, sensorclass, ndim_state, pos_mapping, noise_covar, position, target",
+    [
+        (
+            h2d,  # h
+            RadarRangeBearing,  # sensorclass
+            2,
+            np.array([0, 1]),  # pos_mapping
+            np.array([[0.015, 0],
+                      [0, 0.1]]),  # noise_covar
+            StateVector([[1], [1]]),  # position
+            np.array([[200], [10]])  # target
+         ),
+        (
+            h3d,  # h
+            RadarRangeBearingElevation,  # sensorclass
+            3,
+            np.array([0, 1, 2]),  # pos_mapping
+            np.array([[0.015, 0, 0],
+                      [0, 0.015, 0],
+                      [0, 0, 0.1]]),  # noise_covar
+            StateVector([[1], [1], [0]]),  # position
+            np.array([[200], [10], [10]])  # target
+        )
+    ],
+    ids=["RadarRangeBearing", "RadarRangeBearingElevation"]
+)
+def test_simple_radar(h, sensorclass, ndim_state, pos_mapping, noise_covar, position, target):
+    # Instantiate the rotating radar
+    radar = sensorclass(ndim_state=ndim_state,
+                        pos_mapping=pos_mapping,
+                        noise_covar=noise_covar,
+                        position=position)
+
+    assert (np.equal(radar.position, position).all())
+
+    target_state = State(target, timestamp=datetime.datetime.now())
 
     # Generate a noiseless measurement for the given target
     measurement = radar.measure(target_state, noise=False)
-    rho, phi = cart2pol(target_state.state_vector[0, 0] - radar_position[0, 0],
-                        target_state.state_vector[1, 0] - radar_position[1, 0])
 
     # Assert correction of generated measurement
-    assert(measurement.timestamp == target_state.timestamp)
-    assert(np.equal(measurement.state_vector, StateVector([phi, rho])).all())
+    assert (measurement.timestamp == target_state.timestamp)
+    assert (np.equal(measurement.state_vector, h(target_state,
+                                                 translation_offset=position,
+                                                 rotation_offset=radar.orientation)).all())
 
 
 def h2d_rr(state, pos_map, vel_map, translation_offset, rotation_offset, velocity):
@@ -213,8 +269,6 @@ def test_range_rate_radar(h, sensorclass, pos_mapping, vel_mapping, noise_covar,
 
     # Assert correction of generated measurement
     assert (measurement.timestamp == target_state.timestamp)
-    print(radar.orientation)
-    print(radar.velocity)
     assert (np.equal(measurement.state_vector, h(target_state,
                                                  pos_map=pos_mapping,
                                                  vel_map=vel_mapping,
@@ -274,7 +328,7 @@ def test_rotating_radar():
                          np.array([[5], [5]]),
                          timestamp=timestamp)
     measurement = radar.measure(target_state, noise=False)
-    eval_m = h2d(target_state.state_vector,
+    eval_m = h2d(target_state,
                  radar.position,
                  radar.orientation+[[0],
                                     [0],
@@ -345,7 +399,7 @@ def test_raster_scan_radar():
                          np.array([[-5], [5]]),
                          timestamp=timestamp)
     measurement = radar.measure(target_state, noise=False)
-    eval_m = h2d(target_state.state_vector,
+    eval_m = h2d(target_state,
                  radar.position,
                  radar.orientation + [[0],
                                       [0],

From 8d2356473353b67c2886e07e68a72e4f9abc8731 Mon Sep 17 00:00:00 2001
From: rjgreen <rjgreen@dstl.gov.uk>
Date: Fri, 1 May 2020 10:33:26 +0100
Subject: [PATCH 13/21] Comments except for maths detail in test cases
 completed

---
 stonesoup/models/measurement/nonlinear.py     | 22 +++++++-----
 .../models/measurement/tests/test_models.py   | 16 ++++-----
 .../platform/tests/test_platform_simple.py    | 24 ++++++-------
 stonesoup/sensor/base.py                      |  2 +-
 stonesoup/sensor/radar/radar.py               | 36 ++++++++++---------
 stonesoup/sensor/radar/tests/test_radar.py    | 16 ++++-----
 6 files changed, 62 insertions(+), 54 deletions(-)

diff --git a/stonesoup/models/measurement/nonlinear.py b/stonesoup/models/measurement/nonlinear.py
index 6adab8465..297f8f6cf 100644
--- a/stonesoup/models/measurement/nonlinear.py
+++ b/stonesoup/models/measurement/nonlinear.py
@@ -571,10 +571,11 @@ class CartesianToBearingRangeRate(NonLinearGaussianMeasurement):
             0 & 0 & \sigma_{\dot{r}}^2
             \end{bmatrix}
 
-    The :py:attr:`mapping` property of the model is a 2 element vector, \
-    whose first (i.e. :py:attr:`mapping[0]`) and second (i.e. \
-    :py:attr:`mapping[0]`) elements contain the state index of the \
-    :math:`x` and :math:`y` coordinates, respectively.
+    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
     ----
@@ -586,8 +587,9 @@ class CartesianToBearingRangeRate(NonLinearGaussianMeasurement):
         StateVector,
         default=StateVector(np.array([[0], [0], [0]])),
         doc="A 3x1 array specifying the origin offset in terms of :math:`x,y` coordinates.")
-    vel_mapping = Property(
+    velocity_mapping = Property(
         np.array,
+        default=np.array([[1], [3], [5]]),
         doc="Mapping to the targets velocity within its state space")
     velocity = Property(
         StateVector,
@@ -642,7 +644,7 @@ def function(self, state, noise=False, **kwargs) -> StateVector:
         rho, phi, _ = cart2sphere(*xy_rot[:, 0])
 
         # Determine the net velocity component in the engagement
-        xy_vel = state.state_vector[self.vel_mapping, :] - self.velocity
+        xy_vel = state.state_vector[self.velocity_mapping, :] - self.velocity
 
         # Use polar to calculate range rate
         rr = -np.dot(xy_pos[:, 0], xy_vel[:, 0]) / np.linalg.norm(xy_pos)
@@ -714,15 +716,17 @@ class CartesianToElevationBearingRangeRate(NonLinearGaussianMeasurement):
 
     Note
     ----
-    This class assumes a 3D Cartesian plane.
+    This class implementation assuming at 3D cartesian space, , it therefore\
+    expects a 6D state space.
     """
 
     translation_offset = Property(
         StateVector,
         default=StateVector(np.array([[0], [0], [0]])),
         doc="A 3x1 array specifying the origin offset in terms of :math:`x,y,z` coordinates.")
-    vel_mapping = Property(
+    velocity_mapping = Property(
         np.array,
+        default=np.array([[1], [3], [5]]),
         doc="Mapping to the targets velocity within its state space")
     velocity = Property(
         StateVector,
@@ -776,7 +780,7 @@ def function(self, state, noise=False, **kwargs) -> StateVector:
         rho, phi, theta = cart2sphere(*xyz_rot[:, 0])
 
         # Determine the net velocity component in the engagement
-        xyz_vel = state.state_vector[self.vel_mapping, :] - self.velocity
+        xyz_vel = state.state_vector[self.velocity_mapping, :] - self.velocity
 
         # Use polar to calculate range rate
         rr = -np.dot(xyz_pos[:, 0], xyz_vel[:, 0]) / np.linalg.norm(xyz_pos)
diff --git a/stonesoup/models/measurement/tests/test_models.py b/stonesoup/models/measurement/tests/test_models.py
index 73b78ea2f..fc230873f 100644
--- a/stonesoup/models/measurement/tests/test_models.py
+++ b/stonesoup/models/measurement/tests/test_models.py
@@ -410,7 +410,7 @@ def test_rangeratemodels(h, modelclass, state_vec, ndim_state, pos_mapping, vel_
     # Create and a measurement model object
     model = modelclass(ndim_state=ndim_state,
                        mapping=pos_mapping,
-                       vel_mapping=vel_mapping,
+                       velocity_mapping=vel_mapping,
                        noise_covar=noise_covar,
                        translation_offset=position,
                        rotation_offset=orientation)
@@ -420,7 +420,7 @@ def test_rangeratemodels(h, modelclass, state_vec, ndim_state, pos_mapping, vel_
     meas_pred_wo_noise = model.function(state)
     eval_m = h(state_vec,
                model.mapping,
-               model.vel_mapping,
+               model.velocity_mapping,
                model.translation_offset,
                model.rotation_offset,
                model.velocity)
@@ -460,7 +460,7 @@ def fun(x):
     meas_pred_wo_noise = model.function(state)
     assert np.array_equal(meas_pred_wo_noise, h(state_vec,
                                                 model.mapping,
-                                                model.vel_mapping,
+                                                model.velocity_mapping,
                                                 model.translation_offset,
                                                 model.rotation_offset,
                                                 model.velocity))
@@ -472,7 +472,7 @@ def fun(x):
         meas_pred_wo_noise.T,
         mean=np.array(h(state_vec,
                         model.mapping,
-                        model.vel_mapping,
+                        model.velocity_mapping,
                         model.translation_offset,
                         model.rotation_offset,
                         model.velocity)).ravel(),
@@ -484,7 +484,7 @@ def fun(x):
     assert not np.array_equal(
         meas_pred_w_inoise, h(state_vec,
                               model.mapping,
-                              model.vel_mapping,
+                              model.velocity_mapping,
                               model.translation_offset,
                               model.rotation_offset,
                               model.velocity))
@@ -496,7 +496,7 @@ def fun(x):
         meas_pred_w_inoise.T,
         mean=np.array(h(state_vec,
                         model.mapping,
-                        model.vel_mapping,
+                        model.velocity_mapping,
                         model.translation_offset,
                         model.rotation_offset,
                         model.velocity)).ravel(),
@@ -509,7 +509,7 @@ def fun(x):
                                         noise=noise)
     assert np.array_equal(meas_pred_w_enoise, h(state_vec,
                                                 model.mapping,
-                                                model.vel_mapping,
+                                                model.velocity_mapping,
                                                 model.translation_offset,
                                                 model.rotation_offset,
                                                 model.velocity) + noise)
@@ -521,7 +521,7 @@ def fun(x):
         meas_pred_w_enoise.T,
         mean=np.array(h(state_vec,
                         model.mapping,
-                        model.vel_mapping,
+                        model.velocity_mapping,
                         model.translation_offset,
                         model.rotation_offset,
                         model.velocity)).ravel(),
diff --git a/stonesoup/platform/tests/test_platform_simple.py b/stonesoup/platform/tests/test_platform_simple.py
index 4b0b66508..3e487cc6e 100644
--- a/stonesoup/platform/tests/test_platform_simple.py
+++ b/stonesoup/platform/tests/test_platform_simple.py
@@ -146,29 +146,29 @@ def radars_2d():
     # Create 5 simple radar sensor objects
     radar1 = RadarRangeBearing(
         ndim_state=4,
-        pos_mapping=measurement_mapping,
+        position_mapping=measurement_mapping,
         noise_covar=noise_covar,
     )
 
     radar2 = RadarRangeBearing(
         ndim_state=4,
-        pos_mapping=measurement_mapping,
+        position_mapping=measurement_mapping,
         noise_covar=noise_covar
     )
     radar3 = RadarRangeBearing(
         ndim_state=4,
-        pos_mapping=measurement_mapping,
+        position_mapping=measurement_mapping,
         noise_covar=noise_covar
     )
 
     radar4 = RadarRangeBearing(
         ndim_state=4,
-        pos_mapping=measurement_mapping,
+        position_mapping=measurement_mapping,
         noise_covar=noise_covar
     )
     radar5 = RadarRangeBearing(
         ndim_state=4,
-        pos_mapping=measurement_mapping,
+        position_mapping=measurement_mapping,
         noise_covar=noise_covar
     )
 
@@ -186,39 +186,39 @@ def radars_3d():
     # Create 5 simple radar sensor objects
     radar1 = RadarRangeBearing(
         ndim_state=6,
-        pos_mapping=measurement_mapping,
+        position_mapping=measurement_mapping,
         noise_covar=noise_covar
     )
 
     radar2 = RadarRangeBearing(
         ndim_state=6,
-        pos_mapping=measurement_mapping,
+        position_mapping=measurement_mapping,
         noise_covar=noise_covar
     )
     radar3 = RadarRangeBearing(
         ndim_state=6,
-        pos_mapping=measurement_mapping,
+        position_mapping=measurement_mapping,
         noise_covar=noise_covar
     )
 
     radar4 = RadarRangeBearing(
         ndim_state=6,
-        pos_mapping=measurement_mapping,
+        position_mapping=measurement_mapping,
         noise_covar=noise_covar
     )
     radar5 = RadarRangeBearing(
         ndim_state=6,
-        pos_mapping=measurement_mapping,
+        position_mapping=measurement_mapping,
         noise_covar=noise_covar
     )
     radar6 = RadarRangeBearing(
         ndim_state=6,
-        pos_mapping=measurement_mapping,
+        position_mapping=measurement_mapping,
         noise_covar=noise_covar
     )
     radar7 = RadarRangeBearing(
         ndim_state=6,
-        pos_mapping=measurement_mapping,
+        position_mapping=measurement_mapping,
         noise_covar=noise_covar
     )
     return [radar1, radar2, radar3, radar4, radar5, radar6, radar7]
diff --git a/stonesoup/sensor/base.py b/stonesoup/sensor/base.py
index 67965928d..ccc9464dc 100644
--- a/stonesoup/sensor/base.py
+++ b/stonesoup/sensor/base.py
@@ -114,7 +114,7 @@ def velocity(self) -> Optional[StateVector]:
             sensor is mounted.
 
             It is settable if, and only if, the sensor holds its own internal platform."""
-        return self.platform_system().velocity
+        return self.platform.velocity
 
     @velocity.setter
     def velocity(self, value):
diff --git a/stonesoup/sensor/radar/radar.py b/stonesoup/sensor/radar/radar.py
index 988f39686..5fb4adc8b 100644
--- a/stonesoup/sensor/radar/radar.py
+++ b/stonesoup/sensor/radar/radar.py
@@ -37,7 +37,7 @@ class RadarRangeBearing(Sensor):
         default=2,
         doc="Number of state dimensions. This is utilised by (and follows in format) "
             "the underlying :class:`~.CartesianToBearingRange` model")
-    pos_mapping = Property(
+    position_mapping = Property(
         [np.array],
         doc="Mapping between the targets state space and the sensors\
             measurement capability")
@@ -67,7 +67,7 @@ def measure(self, ground_truth, noise=True, **kwargs):
         """
         measurement_model = CartesianToBearingRange(
             ndim_state=self.ndim_state,
-            mapping=self.pos_mapping,
+            mapping=self.position_mapping,
             noise_covar=self.noise_covar,
             translation_offset=self.position,
             rotation_offset=self.orientation)
@@ -149,7 +149,7 @@ def measure(self, ground_truth, noise=True, **kwargs):
 
         measurement_model = CartesianToBearingRange(
             ndim_state=self.ndim_state,
-            mapping=self.pos_mapping,
+            mapping=self.position_mapping,
             noise_covar=self.noise_covar,
             translation_offset=self.position,
             rotation_offset=rot_offset)
@@ -244,7 +244,7 @@ def measure(self, ground_truth, noise=True, **kwargs):
         """
         measurement_model = CartesianToElevationBearingRange(
             ndim_state=self.ndim_state,
-            mapping=self.pos_mapping,
+            mapping=self.position_mapping,
             noise_covar=self.noise_covar,
             translation_offset=self.position,
             rotation_offset=self.orientation)
@@ -264,13 +264,15 @@ class RadarRangeRateBearing(RadarRangeBearing):
 
     Note
     ----
-    The current implementation of this class assumes a 3D Cartesian plane.
+    This class implementation assuming at 3D cartesian space, it therefore\
+     expects a 6D state space.
 
     """
 
-    vel_mapping = Property(
+    velocity_mapping = Property(
         np.array,
-        doc="Mapping to the targets velocity information within its state space")
+        default=np.array([[1], [3], [5]]),
+        doc="Mapping to the target's velocity information within its state space")
     ndim_state = Property(
         int,
         default=3,
@@ -302,8 +304,8 @@ def measure(self, ground_truth, noise=True, **kwargs) -> Detection:
         """
         measurement_model = CartesianToBearingRangeRate(
             ndim_state=self.ndim_state,
-            mapping=self.pos_mapping,
-            vel_mapping=self.vel_mapping,
+            mapping=self.position_mapping,
+            velocity_mapping=self.velocity_mapping,
             noise_covar=self.noise_covar,
             translation_offset=self.position,
             velocity=self.velocity,
@@ -328,9 +330,10 @@ class RadarRangeRateBearingElevation(RadarRangeRateBearing):
 
     """
 
-    vel_mapping = Property(
+    velocity_mapping = Property(
         np.array,
-        doc="Mapping to the targets velocity information within its state space")
+        default=np.array([[1], [3], [5]]),
+        doc="Mapping to the target's velocity information within its state space")
     ndim_state = Property(
         int,
         default=6,
@@ -362,8 +365,8 @@ def measure(self, ground_truth, noise=True, **kwargs) -> Detection:
         """
         measurement_model = CartesianToElevationBearingRangeRate(
             ndim_state=self.ndim_state,
-            mapping=self.pos_mapping,
-            vel_mapping=self.vel_mapping,
+            mapping=self.position_mapping,
+            velocity_mapping=self.velocity_mapping,
             noise_covar=self.noise_covar,
             translation_offset=self.position,
             velocity=self.velocity,
@@ -391,7 +394,8 @@ class RadarRasterScanRangeBearing(RadarRotatingRangeBearing):
 
     Note
     ----
-    * The current implementation of this class assumes a 3D Cartesian plane.
+    This class implementation assuming at 3D cartesian space, it therefore\
+     expects a 6D state space.
 
     """
 
@@ -490,7 +494,7 @@ class AESARadar(Sensor):
             "counter-clockwise rotation around the :math:`x,y,z` axis. i.e Roll, Pitch and Yaw. "
             "Default is ``StateVector([0, 0, 0])``")
 
-    pos_mapping = Property(
+    position_mapping = Property(
         np.array, default=(0, 1, 2),
         doc="Mapping between or positions and state "
             "dimensions. [x,y,z]")
@@ -625,7 +629,7 @@ def gen_probability(self, sky_state):
         spoiled_width = self.beam_width / (np.cos(beam_az) * np.cos(beam_el))
         # state relative to radar (in cartesian space)
 
-        relative_vector = sky_state.state_vector[self.pos_mapping, :] - self.position
+        relative_vector = sky_state.state_vector[self.position_mapping, :] - self.position
 
         relative_vector = self._rotation_matrix @ relative_vector
 
diff --git a/stonesoup/sensor/radar/tests/test_radar.py b/stonesoup/sensor/radar/tests/test_radar.py
index 83f951b08..7727088d8 100644
--- a/stonesoup/sensor/radar/tests/test_radar.py
+++ b/stonesoup/sensor/radar/tests/test_radar.py
@@ -109,7 +109,7 @@ def h3d(state, translation_offset, rotation_offset):
 def test_simple_radar(h, sensorclass, ndim_state, pos_mapping, noise_covar, position, target):
     # Instantiate the rotating radar
     radar = sensorclass(ndim_state=ndim_state,
-                        pos_mapping=pos_mapping,
+                        position_mapping=pos_mapping,
                         noise_covar=noise_covar,
                         position=position)
 
@@ -254,8 +254,8 @@ def test_range_rate_radar(h, sensorclass, pos_mapping, vel_mapping, noise_covar,
 
     # Instantiate the rotating radar
     radar = sensorclass(ndim_state=6,
-                        pos_mapping=pos_mapping,
-                        vel_mapping=vel_mapping,
+                        position_mapping=pos_mapping,
+                        velocity_mapping=vel_mapping,
                         noise_covar=noise_covar,
                         position=position)
 
@@ -306,7 +306,7 @@ def test_rotating_radar():
     radar = RadarRotatingRangeBearing(position=radar_position,
                                       orientation=radar_orientation,
                                       ndim_state=2,
-                                      pos_mapping=measurement_mapping,
+                                      position_mapping=measurement_mapping,
                                       noise_covar=noise_covar,
                                       dwell_center=dwell_center,
                                       rpm=rpm,
@@ -367,7 +367,7 @@ def test_raster_scan_radar():
     radar = RadarRasterScanRangeBearing(position=radar_position,
                                         orientation=radar_orientation,
                                         ndim_state=2,
-                                        pos_mapping=measurement_mapping,
+                                        position_mapping=measurement_mapping,
                                         noise_covar=noise_covar,
                                         dwell_center=dwell_center,
                                         rpm=rpm,
@@ -415,7 +415,7 @@ def test_aesaradar():
                    timestamp=datetime.datetime.now())
 
     radar = AESARadar(antenna_gain=30,
-                      pos_mapping=[0, 2, 4],
+                      position_mapping=[0, 2, 4],
                       position=StateVector([0.0] * 3),
                       orientation=StateVector([0.0] * 3),
                       frequency=100e6,
@@ -510,7 +510,7 @@ def test_failed_detect():
                    timestamp=datetime.datetime.now())
 
     radar = AESARadar(antenna_gain=30,
-                      pos_mapping=[0, 2, 4],
+                      position_mapping=[0, 2, 4],
                       position=StateVector([0.0] * 3),
                       orientation=StateVector([0.0] * 3),
                       frequency=100e6,
@@ -540,7 +540,7 @@ def test_target_rcs():
     rcs_20.rcs = 20
 
     radar = AESARadar(antenna_gain=36,
-                      pos_mapping=[0, 1, 2],
+                      position_mapping=[0, 1, 2],
                       position=StateVector([0.0]*3),
                       orientation=StateVector([0.0] * 3),
                       frequency=10e9,

From 452b95469ba639a5d94934739566dd43c1018538 Mon Sep 17 00:00:00 2001
From: rjgreen <rjgreen@dstl.gov.uk>
Date: Mon, 4 May 2020 16:36:41 +0100
Subject: [PATCH 14/21] Range-rate inverted, condenced radar tests to remove
 unnessairy code

---
 stonesoup/models/measurement/nonlinear.py     | 29 ++++++++-
 .../models/measurement/tests/test_models.py   | 33 ++++++++--
 stonesoup/sensor/radar/tests/test_radar.py    | 62 +++----------------
 3 files changed, 63 insertions(+), 61 deletions(-)

diff --git a/stonesoup/models/measurement/nonlinear.py b/stonesoup/models/measurement/nonlinear.py
index 297f8f6cf..e84de34d7 100644
--- a/stonesoup/models/measurement/nonlinear.py
+++ b/stonesoup/models/measurement/nonlinear.py
@@ -647,7 +647,7 @@ def function(self, state, noise=False, **kwargs) -> StateVector:
         xy_vel = state.state_vector[self.velocity_mapping, :] - self.velocity
 
         # Use polar to calculate range rate
-        rr = -np.dot(xy_pos[:, 0], xy_vel[:, 0]) / np.linalg.norm(xy_pos)
+        rr = np.dot(xy_pos[:, 0], xy_vel[:, 0]) / np.linalg.norm(xy_pos)
 
         return StateVector([[Bearing(phi)], [rho], [rr]]) + noise
 
@@ -657,7 +657,7 @@ def rvs(self, num_samples=1, **kwargs) -> np.ndarray:
         return out
 
 
-class CartesianToElevationBearingRangeRate(NonLinearGaussianMeasurement):
+class CartesianToElevationBearingRangeRate(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 elevation \
     (:math:`\theta`),  bearing (:math:`\phi`), range (:math:`r`) and
@@ -783,13 +783,36 @@ def function(self, state, noise=False, **kwargs) -> StateVector:
         xyz_vel = state.state_vector[self.velocity_mapping, :] - self.velocity
 
         # Use polar to calculate range rate
-        rr = -np.dot(xyz_pos[:, 0], xyz_vel[:, 0]) / np.linalg.norm(xyz_pos)
+        rr = np.dot(xyz_pos[:, 0], xyz_vel[:, 0]) / np.linalg.norm(xyz_pos)
 
         return StateVector([[Elevation(phi)],
                             [Bearing(theta)],
                             [rho],
                             [rr]]) + noise
 
+    def inverse_function(self, detection, **kwargs) -> StateVector:
+        phi, theta, rho, rho_rate = detection.state_vector
+
+        [x, y, z] = sphere2cart(rho, theta, phi)
+        # because only rho_rate is known, only the components in
+        # x,y and z of the range rate can be found.
+        x_rate = np.cos(phi) * np.cos(theta) * rho_rate
+        y_rate = np.cos(phi) * np.sin(theta) * rho_rate
+        z_rate = np.sin(phi) * rho_rate
+
+        inv_rotation_matrix = inv(self._rotation_matrix)
+
+        out_vector = StateVector([[0.], [0.], [0.], [0.], [0.], [0.]])
+        out_vector[self.mapping] = x, y, z
+        out_vector[self.velocity_mapping] = x_rate, y_rate, z_rate
+
+        out_vector[self.mapping] = inv_rotation_matrix @ out_vector[self.mapping]
+        out_vector[self.velocity_mapping] = inv_rotation_matrix @ out_vector[self.velocity_mapping]
+
+        out_vector[self.mapping] = out_vector[self.mapping] + self.translation_offset
+        print(out_vector)
+        return out_vector
+
     def rvs(self, num_samples=1, **kwargs) -> np.ndarray:
         out = super().rvs(num_samples, **kwargs)
         out = np.array([[Elevation(0)], [Bearing(0)], [0.], [0.]]) + out
diff --git a/stonesoup/models/measurement/tests/test_models.py b/stonesoup/models/measurement/tests/test_models.py
index fc230873f..1e7b6b354 100644
--- a/stonesoup/models/measurement/tests/test_models.py
+++ b/stonesoup/models/measurement/tests/test_models.py
@@ -313,7 +313,7 @@ def h2d_rr(state_vector, pos_map, vel_map, translation_offset, rotation_offset,
                         [0]])
 
     # Use polar to calculate range rate
-    rr = -np.dot(xyz[:, 0], xyz_vel[:, 0]) / np.linalg.norm(xyz)
+    rr = np.dot(xyz[:, 0], xyz_vel[:, 0]) / np.linalg.norm(xyz)
 
     return np.array([[Bearing(phi)], [rho], [rr]])
 
@@ -361,7 +361,7 @@ def h3d_rr(state_vector, pos_map, vel_map, translation_offset, rotation_offset,
                         [state_vector[vel_map[2], 0] - velocity[2, 0]]])
 
     # Use polar to calculate range rate
-    rr = -np.dot(xyz[:, 0], xyz_vel[:, 0]) / np.linalg.norm(xyz)
+    rr = np.dot(xyz[:, 0], xyz_vel[:, 0]) / np.linalg.norm(xyz)
 
     return np.array([[Elevation(theta)], [Bearing(phi)], [rho], [rr]])
 
@@ -373,7 +373,7 @@ def h3d_rr(state_vector, pos_map, vel_map, translation_offset, rotation_offset,
         (   # 3D meas, 6D state
             h2d_rr,  # h
             CartesianToBearingRangeRate,  # ModelClass
-            np.array([[200], [10], [0], [0], [0], [0]]),  # state_vec
+            np.array([[200.], [10.], [0.], [0.], [0.], [0.]]),  # state_vec
             6,  # ndim_state
             np.array([0, 2, 4]),  # pos_mapping
             np.array([1, 3, 5]),  # vel_mapping
@@ -386,7 +386,7 @@ def h3d_rr(state_vector, pos_map, vel_map, translation_offset, rotation_offset,
         (   # 4D meas, 6D state
             h3d_rr,  # h
             CartesianToElevationBearingRangeRate,  # ModelClass
-            np.array([[200], [10], [0], [0], [0], [0]]),  # state_vec
+            np.array([[200.], [10.], [0.], [0.], [0.], [0.]]),  # state_vec
             6,  # ndim_state
             np.array([0, 2, 4]),  # pos_mapping
             np.array([1, 3, 5]),  # vel_mapping
@@ -414,7 +414,7 @@ def test_rangeratemodels(h, modelclass, state_vec, ndim_state, pos_mapping, vel_
                        noise_covar=noise_covar,
                        translation_offset=position,
                        rotation_offset=orientation)
-
+    print(model)
     # Project a state through the model
     # (without noise)
     meas_pred_wo_noise = model.function(state)
@@ -436,6 +436,7 @@ def fun(x):
     # Check Jacobian has proper dimensions
     assert H.shape == (model.ndim_meas, ndim_state)
 
+    print(meas_pred_wo_noise)
     # Ensure inverse function returns original
     if isinstance(model, ReversibleModel):
         J = model.inverse_function(State(meas_pred_wo_noise))
@@ -526,3 +527,25 @@ def fun(x):
                         model.rotation_offset,
                         model.velocity)).ravel(),
         cov=noise_covar)
+
+
+def test_inverse_function():
+    measure_model = CartesianToElevationBearingRangeRate(
+        ndim_state=6,
+        mapping=np.array([0, 2, 4]),
+        velocity_mapping=np.array([1, 3, 5]),
+        noise_covar=np.array([[0, 0, 0, 0],
+                              [0, 0, 0, 0],
+                              [0, 0, 0, 0],
+                              [0, 0, 0, 0]]))
+
+    measured_state = State(StateVector([np.pi / 18, np.pi / 18, 10e3, 100.0]))
+
+    inv_measure_state = measure_model.inverse_function(measured_state)
+
+    assert approx(inv_measure_state[0, 0], 0.02) == 9698.46
+    assert approx(inv_measure_state[1, 0], 0.02) == 96.98
+    assert approx(inv_measure_state[2, 0], 0.02) == 1710.1
+    assert approx(inv_measure_state[3, 0], 0.02) == 17.10
+    assert approx(inv_measure_state[4, 0], 0.02) == 1736.48
+    assert approx(inv_measure_state[5, 0], 0.02) == 17.36
diff --git a/stonesoup/sensor/radar/tests/test_radar.py b/stonesoup/sensor/radar/tests/test_radar.py
index 7727088d8..f18d82987 100644
--- a/stonesoup/sensor/radar/tests/test_radar.py
+++ b/stonesoup/sensor/radar/tests/test_radar.py
@@ -4,7 +4,7 @@
 from pytest import approx
 import numpy as np
 
-from ....functions import rotz, rotx, roty
+from ....functions import rotz, rotx, roty, cart2sphere
 from ....types.angle import Bearing, Elevation
 from ....types.array import StateVector, CovarianceMatrix
 from ....types.state import State
@@ -135,33 +135,13 @@ def h2d_rr(state, pos_map, vel_map, translation_offset, rotation_offset, velocit
 
     # Get rotation matrix
     theta_z = - rotation_offset[2, 0]
-    cos_z, sin_z = np.cos(theta_z), np.sin(theta_z)
-    rot_z = np.array([[cos_z, -sin_z, 0],
-                      [sin_z, cos_z, 0],
-                      [0, 0, 1]])
-
     theta_y = - rotation_offset[1, 0]
-    cos_y, sin_y = np.cos(theta_y), np.sin(theta_y)
-    rot_y = np.array([[cos_y, 0, sin_y],
-                      [0, 1, 0],
-                      [-sin_y, 0, cos_y]])
-
     theta_x = - rotation_offset[0, 0]
-    cos_x, sin_x = np.cos(theta_x), np.sin(theta_x)
-    rot_x = np.array([[1, 0, 0],
-                      [0, cos_x, -sin_x],
-                      [0, sin_x, cos_x]])
-
-    rotation_matrix = rot_z@rot_y@rot_x
 
+    rotation_matrix = rotz(theta_z) @ roty(theta_y) @ rotx(theta_x)
     xyz_rot = rotation_matrix @ xyz
-    x = xyz_rot[0, 0]
-    y = xyz_rot[1, 0]
-    z = xyz_rot[2, 0]
 
-    rho = np.sqrt(x**2 + y**2 + z**2)
-    phi = np.arctan2(y, x)
-    # theta = np.arcsin(z/rho)
+    rho, phi, _ = cart2sphere(*xyz_rot[:, 0])
 
     # Calculate range rate extension
     # Determine the net velocity component in the engagement
@@ -170,55 +150,31 @@ def h2d_rr(state, pos_map, vel_map, translation_offset, rotation_offset, velocit
                         [0]])
 
     # Use polar to calculate range rate
-    rr = -np.dot(xyz[:, 0], xyz_vel[:, 0]) / np.linalg.norm(xyz)
+    rr = np.dot(xyz[:, 0], xyz_vel[:, 0]) / np.linalg.norm(xyz)
 
     return np.array([[Bearing(phi)], [rho], [rr]])
 
 
 def h3d_rr(state, pos_map, vel_map, translation_offset, rotation_offset, velocity):
 
-    xyz = np.array([[state.state_vector[pos_map[0], 0] - translation_offset[0, 0]],
-                    [state.state_vector[pos_map[1], 0] - translation_offset[1, 0]],
-                    [state.state_vector[pos_map[2], 0] - translation_offset[2, 0]]])
+    xyz = state.state_vector[pos_map, :] - translation_offset
 
     # Get rotation matrix
     theta_z = - rotation_offset[2, 0]
-    cos_z, sin_z = np.cos(theta_z), np.sin(theta_z)
-    rot_z = np.array([[cos_z, -sin_z, 0],
-                      [sin_z, cos_z, 0],
-                      [0, 0, 1]])
-
     theta_y = - rotation_offset[1, 0]
-    cos_y, sin_y = np.cos(theta_y), np.sin(theta_y)
-    rot_y = np.array([[cos_y, 0, sin_y],
-                      [0, 1, 0],
-                      [-sin_y, 0, cos_y]])
-
     theta_x = - rotation_offset[0, 0]
-    cos_x, sin_x = np.cos(theta_x), np.sin(theta_x)
-    rot_x = np.array([[1, 0, 0],
-                      [0, cos_x, -sin_x],
-                      [0, sin_x, cos_x]])
-
-    rotation_matrix = rot_z@rot_y@rot_x
 
+    rotation_matrix = rotz(theta_z)@roty(theta_y)@rotx(theta_x)
     xyz_rot = rotation_matrix @ xyz
-    x = xyz_rot[0, 0]
-    y = xyz_rot[1, 0]
-    z = xyz_rot[2, 0]
 
-    rho = np.sqrt(x**2 + y**2 + z**2)
-    phi = np.arctan2(y, x)
-    theta = np.arcsin(z/rho)
+    rho, phi, theta = cart2sphere(*xyz_rot[:, 0])
 
     # Calculate range rate extension
     # Determine the net velocity component in the engagement
-    xyz_vel = np.array([[state.state_vector[vel_map[0], 0] - velocity[0, 0]],
-                        [state.state_vector[vel_map[1], 0] - velocity[1, 0]],
-                        [state.state_vector[vel_map[2], 0] - velocity[2, 0]]])
+    xyz_vel = state.state_vector[vel_map, :] - velocity
 
     # Use polar to calculate range rate
-    rr = -np.dot(xyz[:, 0], xyz_vel[:, 0]) / np.linalg.norm(xyz)
+    rr = np.dot(xyz[:, 0], xyz_vel[:, 0]) / np.linalg.norm(xyz)
 
     return np.array([[Elevation(theta)], [Bearing(phi)], [rho], [rr]])
 

From ca7f86e5b681c92c1e4be6bf8f5a046ac843dcf9 Mon Sep 17 00:00:00 2001
From: rjgreen <rjgreen@dstl.gov.uk>
Date: Tue, 5 May 2020 09:07:52 +0100
Subject: [PATCH 15/21] updated tests for functions, radar and models

---
 .../models/measurement/tests/test_models.py   | 157 ++++--------------
 stonesoup/sensor/radar/tests/test_radar.py    |  41 ++---
 stonesoup/tests/test_functions.py             |  64 ++++++-
 3 files changed, 102 insertions(+), 160 deletions(-)

diff --git a/stonesoup/models/measurement/tests/test_models.py b/stonesoup/models/measurement/tests/test_models.py
index 1e7b6b354..d9e0e42fb 100644
--- a/stonesoup/models/measurement/tests/test_models.py
+++ b/stonesoup/models/measurement/tests/test_models.py
@@ -4,6 +4,7 @@
 import numpy as np
 from scipy.stats import multivariate_normal
 
+from ....functions import rotz, rotx, roty, cart2sphere
 from ..nonlinear import (
     CartesianToElevationBearingRange, CartesianToBearingRange,
     CartesianToElevationBearing, CartesianToBearingRangeRate,
@@ -15,7 +16,7 @@
 from ....types.array import StateVector, Matrix
 
 
-def h2d(state_vector, translation_offset, rotation_offset):
+def h2d(state_vector, pos_map, translation_offset, rotation_offset):
 
     xyz = [[state_vector[0, 0] - translation_offset[0, 0]],
            [state_vector[1, 0] - translation_offset[1, 0]],
@@ -23,109 +24,45 @@ def h2d(state_vector, translation_offset, rotation_offset):
 
     # Get rotation matrix
     theta_z = - rotation_offset[2, 0]
-    cos_z, sin_z = np.cos(theta_z), np.sin(theta_z)
-    rot_z = np.array([[cos_z, -sin_z, 0],
-                      [sin_z, cos_z, 0],
-                      [0, 0, 1]])
-
     theta_y = - rotation_offset[1, 0]
-    cos_y, sin_y = np.cos(theta_y), np.sin(theta_y)
-    rot_y = np.array([[cos_y, 0, sin_y],
-                      [0, 1, 0],
-                      [-sin_y, 0, cos_y]])
-
     theta_x = - rotation_offset[0, 0]
-    cos_x, sin_x = np.cos(theta_x), np.sin(theta_x)
-    rot_x = np.array([[1, 0, 0],
-                      [0, cos_x, -sin_x],
-                      [0, sin_x, cos_x]])
-
-    rotation_matrix = rot_z@rot_y@rot_x
 
+    rotation_matrix = rotz(theta_z) @ roty(theta_y) @ rotx(theta_x)
     xyz_rot = rotation_matrix @ xyz
-    x = xyz_rot[0, 0]
-    y = xyz_rot[1, 0]
-    # z = 0  # xyz_rot[2, 0]
 
-    rho = np.sqrt(x**2 + y**2)
-    phi = np.arctan2(y, x)
+    rho, phi, _ = cart2sphere(*xyz_rot[:, 0])
 
     return np.array([[Bearing(phi)], [rho]])
 
 
-def h3d(state_vector,  translation_offset, rotation_offset):
-
-    xyz = [[state_vector[0, 0] - translation_offset[0, 0]],
-           [state_vector[1, 0] - translation_offset[1, 0]],
-           [state_vector[2, 0] - translation_offset[2, 0]]]
+def h3d(state_vector, pos_map,  translation_offset, rotation_offset):
+    xyz = state_vector[pos_map, :] - translation_offset
 
     # Get rotation matrix
     theta_z = - rotation_offset[2, 0]
-    cos_z, sin_z = np.cos(theta_z), np.sin(theta_z)
-    rot_z = np.array([[cos_z, -sin_z, 0],
-                      [sin_z, cos_z, 0],
-                      [0, 0, 1]])
-
     theta_y = - rotation_offset[1, 0]
-    cos_y, sin_y = np.cos(theta_y), np.sin(theta_y)
-    rot_y = np.array([[cos_y, 0, sin_y],
-                      [0, 1, 0],
-                      [-sin_y, 0, cos_y]])
-
     theta_x = - rotation_offset[0, 0]
-    cos_x, sin_x = np.cos(theta_x), np.sin(theta_x)
-    rot_x = np.array([[1, 0, 0],
-                      [0, cos_x, -sin_x],
-                      [0, sin_x, cos_x]])
-
-    rotation_matrix = rot_z@rot_y@rot_x
 
+    rotation_matrix = rotz(theta_z) @ roty(theta_y) @ rotx(theta_x)
     xyz_rot = rotation_matrix @ xyz
-    x = xyz_rot[0, 0]
-    y = xyz_rot[1, 0]
-    z = xyz_rot[2, 0]
 
-    rho = np.sqrt(x**2 + y**2 + z**2)
-    phi = np.arctan2(y, x)
-    theta = np.arcsin(z/rho)
+    rho, phi, theta = cart2sphere(*xyz_rot[:, 0])
 
     return np.array([[Elevation(theta)], [Bearing(phi)], [rho]])
 
 
-def hbearing(state_vector, translation_offset, rotation_offset):
-    xyz = [[state_vector[0, 0] - translation_offset[0, 0]],
-           [state_vector[1, 0] - translation_offset[1, 0]],
-           [state_vector[2, 0] - translation_offset[2, 0]]]
+def hbearing(state_vector, pos_map, translation_offset, rotation_offset):
+    xyz = state_vector[pos_map, :] - translation_offset
 
     # Get rotation matrix
     theta_z = - rotation_offset[2, 0]
-    cos_z, sin_z = np.cos(theta_z), np.sin(theta_z)
-    rot_z = np.array([[cos_z, -sin_z, 0],
-                      [sin_z, cos_z, 0],
-                      [0, 0, 1]])
-
     theta_y = - rotation_offset[1, 0]
-    cos_y, sin_y = np.cos(theta_y), np.sin(theta_y)
-    rot_y = np.array([[cos_y, 0, sin_y],
-                      [0, 1, 0],
-                      [-sin_y, 0, cos_y]])
-
     theta_x = - rotation_offset[0, 0]
-    cos_x, sin_x = np.cos(theta_x), np.sin(theta_x)
-    rot_x = np.array([[1, 0, 0],
-                      [0, cos_x, -sin_x],
-                      [0, sin_x, cos_x]])
-
-    rotation_matrix = rot_z@rot_y@rot_x
 
+    rotation_matrix = rotz(theta_z) @ roty(theta_y) @ rotx(theta_x)
     xyz_rot = rotation_matrix @ xyz
-    x = xyz_rot[0, 0]
-    y = xyz_rot[1, 0]
-    z = xyz_rot[2, 0]
 
-    rho = np.sqrt(x**2 + y**2 + z**2)
-    phi = np.arctan2(y, x)
-    theta = np.arcsin(z/rho)
+    _, phi, theta = cart2sphere(*xyz_rot[:, 0])
 
     return np.array([[Elevation(theta)], [Bearing(phi)]])
 
@@ -167,7 +104,7 @@ def hbearing(state_vector, translation_offset, rotation_offset):
             np.array([[-3], [0], [np.pi/3]])
         )
     ],
-    ids=["standard", "RBE", "BearingsOnly"]
+    ids=["BearingElevation", "RangeBearingElevation", "BearingsOnly"]
 )
 def test_models(h, ModelClass, state_vec, R,
                 mapping, translation_offset, rotation_offset):
@@ -187,7 +124,7 @@ def test_models(h, ModelClass, state_vec, R,
     # Project a state through the model
     # (without noise)
     meas_pred_wo_noise = model.function(state)
-    eval_m = h(state_vec, model.translation_offset, model.rotation_offset)
+    eval_m = h(state_vec, mapping, model.translation_offset, model.rotation_offset)
     assert np.array_equal(meas_pred_wo_noise, eval_m)
 
     # Ensure ```lg.transfer_function()``` returns H
@@ -222,7 +159,7 @@ def fun(x):
     # (without noise)
     meas_pred_wo_noise = model.function(state)
     assert np.array_equal(meas_pred_wo_noise,  h(
-        state_vec, model.translation_offset, model.rotation_offset))
+        state_vec, mapping, model.translation_offset, model.rotation_offset))
 
     # Evaluate the likelihood of the predicted measurement, given the state
     # (without noise)
@@ -230,6 +167,7 @@ def fun(x):
     assert approx(prob) == multivariate_normal.pdf(
         meas_pred_wo_noise.T,
         mean=np.array(h(state_vec,
+                        mapping,
                         model.translation_offset,
                         model.rotation_offset)).ravel(),
         cov=R)
@@ -238,7 +176,9 @@ def fun(x):
     # (with internal noise)
     meas_pred_w_inoise = model.function(state, noise=True)
     assert not np.array_equal(
-        meas_pred_w_inoise,  h(state_vec, model.translation_offset,
+        meas_pred_w_inoise,  h(state_vec,
+                               mapping,
+                               model.translation_offset,
                                model.rotation_offset))
 
     # Evaluate the likelihood of the predicted state, given the prior
@@ -247,6 +187,7 @@ def fun(x):
     assert approx(prob) == multivariate_normal.pdf(
         meas_pred_w_inoise.T,
         mean=np.array(h(state_vec,
+                        mapping,
                         model.translation_offset,
                         model.rotation_offset)).ravel(),
         cov=R)
@@ -257,7 +198,7 @@ def fun(x):
     meas_pred_w_enoise = model.function(state,
                                         noise=noise)
     assert np.array_equal(meas_pred_w_enoise,  h(
-        state_vec, model.translation_offset, model.rotation_offset)+noise)
+        state_vec, mapping, model.translation_offset, model.rotation_offset)+noise)
 
     # Evaluate the likelihood of the predicted state, given the prior
     # (with noise)
@@ -265,6 +206,7 @@ def fun(x):
     assert approx(prob) == multivariate_normal.pdf(
         meas_pred_w_enoise.T,
         mean=np.array(h(state_vec,
+                        mapping,
                         model.translation_offset,
                         model.rotation_offset)).ravel(),
         cov=R)
@@ -278,33 +220,13 @@ def h2d_rr(state_vector, pos_map, vel_map, translation_offset, rotation_offset,
 
     # Get rotation matrix
     theta_z = - rotation_offset[2, 0]
-    cos_z, sin_z = np.cos(theta_z), np.sin(theta_z)
-    rot_z = np.array([[cos_z, -sin_z, 0],
-                      [sin_z, cos_z, 0],
-                      [0, 0, 1]])
-
     theta_y = - rotation_offset[1, 0]
-    cos_y, sin_y = np.cos(theta_y), np.sin(theta_y)
-    rot_y = np.array([[cos_y, 0, sin_y],
-                      [0, 1, 0],
-                      [-sin_y, 0, cos_y]])
-
     theta_x = - rotation_offset[0, 0]
-    cos_x, sin_x = np.cos(theta_x), np.sin(theta_x)
-    rot_x = np.array([[1, 0, 0],
-                      [0, cos_x, -sin_x],
-                      [0, sin_x, cos_x]])
-
-    rotation_matrix = rot_z@rot_y@rot_x
 
+    rotation_matrix = rotz(theta_z) @ roty(theta_y) @ rotx(theta_x)
     xyz_rot = rotation_matrix @ xyz
-    x = xyz_rot[0, 0]
-    y = xyz_rot[1, 0]
-    z = xyz_rot[2, 0]
 
-    rho = np.sqrt(x**2 + y**2 + z**2)
-    phi = np.arctan2(y, x)
-    # theta = np.arcsin(z/rho)
+    rho, phi, _ = cart2sphere(*xyz_rot[:, 0])
 
     # Calculate range rate extension
     # Determine the net velocity component in the engagement
@@ -320,39 +242,17 @@ def h2d_rr(state_vector, pos_map, vel_map, translation_offset, rotation_offset,
 
 def h3d_rr(state_vector, pos_map, vel_map, translation_offset, rotation_offset, velocity):
 
-    xyz = np.array([[state_vector[pos_map[0], 0] - translation_offset[0, 0]],
-                    [state_vector[pos_map[1], 0] - translation_offset[1, 0]],
-                    [state_vector[pos_map[2], 0] - translation_offset[2, 0]]])
+    xyz = state_vector[pos_map, :] - translation_offset
 
     # Get rotation matrix
     theta_z = - rotation_offset[2, 0]
-    cos_z, sin_z = np.cos(theta_z), np.sin(theta_z)
-    rot_z = np.array([[cos_z, -sin_z, 0],
-                      [sin_z, cos_z, 0],
-                      [0, 0, 1]])
-
     theta_y = - rotation_offset[1, 0]
-    cos_y, sin_y = np.cos(theta_y), np.sin(theta_y)
-    rot_y = np.array([[cos_y, 0, sin_y],
-                      [0, 1, 0],
-                      [-sin_y, 0, cos_y]])
-
     theta_x = - rotation_offset[0, 0]
-    cos_x, sin_x = np.cos(theta_x), np.sin(theta_x)
-    rot_x = np.array([[1, 0, 0],
-                      [0, cos_x, -sin_x],
-                      [0, sin_x, cos_x]])
-
-    rotation_matrix = rot_z@rot_y@rot_x
 
+    rotation_matrix = rotz(theta_z) @ roty(theta_y) @ rotx(theta_x)
     xyz_rot = rotation_matrix @ xyz
-    x = xyz_rot[0, 0]
-    y = xyz_rot[1, 0]
-    z = xyz_rot[2, 0]
 
-    rho = np.sqrt(x**2 + y**2 + z**2)
-    phi = np.arctan2(y, x)
-    theta = np.arcsin(z/rho)
+    rho, phi, theta = cart2sphere(*xyz_rot[:, 0])
 
     # Calculate range rate extension
     # Determine the net velocity component in the engagement
@@ -414,7 +314,7 @@ def test_rangeratemodels(h, modelclass, state_vec, ndim_state, pos_mapping, vel_
                        noise_covar=noise_covar,
                        translation_offset=position,
                        rotation_offset=orientation)
-    print(model)
+
     # Project a state through the model
     # (without noise)
     meas_pred_wo_noise = model.function(state)
@@ -436,7 +336,6 @@ def fun(x):
     # Check Jacobian has proper dimensions
     assert H.shape == (model.ndim_meas, ndim_state)
 
-    print(meas_pred_wo_noise)
     # Ensure inverse function returns original
     if isinstance(model, ReversibleModel):
         J = model.inverse_function(State(meas_pred_wo_noise))
diff --git a/stonesoup/sensor/radar/tests/test_radar.py b/stonesoup/sensor/radar/tests/test_radar.py
index f18d82987..a929c42f1 100644
--- a/stonesoup/sensor/radar/tests/test_radar.py
+++ b/stonesoup/sensor/radar/tests/test_radar.py
@@ -16,10 +16,10 @@
 from ....models.measurement.linear import LinearGaussian
 
 
-def h2d(state, translation_offset, rotation_offset):
+def h2d(state, pos_map, translation_offset, rotation_offset):
 
-    xyz = [[state.state_vector[0, 0] - translation_offset[0, 0]],
-           [state.state_vector[1, 0] - translation_offset[1, 0]],
+    xyz = [[state.state_vector[pos_map[0], 0] - translation_offset[0, 0]],
+           [state.state_vector[pos_map[1], 0] - translation_offset[1, 0]],
            [0]]
 
     # Get rotation matrix
@@ -40,41 +40,19 @@ def h2d(state, translation_offset, rotation_offset):
     return np.array([[Bearing(phi)], [rho]])
 
 
-def h3d(state, translation_offset, rotation_offset):
+def h3d(state, pos_map, translation_offset, rotation_offset):
 
-    xyz = np.array([[state.state_vector[0, 0] - translation_offset[0, 0]],
-                    [state.state_vector[1, 0] - translation_offset[1, 0]],
-                    [state.state_vector[2, 0] - translation_offset[2, 0]]])
+    xyz = state.state_vector[pos_map, :] - translation_offset
 
     # Get rotation matrix
     theta_z = - rotation_offset[2, 0]
-    cos_z, sin_z = np.cos(theta_z), np.sin(theta_z)
-    rot_z = np.array([[cos_z, -sin_z, 0],
-                      [sin_z, cos_z, 0],
-                      [0, 0, 1]])
-
     theta_y = - rotation_offset[1, 0]
-    cos_y, sin_y = np.cos(theta_y), np.sin(theta_y)
-    rot_y = np.array([[cos_y, 0, sin_y],
-                      [0, 1, 0],
-                      [-sin_y, 0, cos_y]])
-
     theta_x = - rotation_offset[0, 0]
-    cos_x, sin_x = np.cos(theta_x), np.sin(theta_x)
-    rot_x = np.array([[1, 0, 0],
-                      [0, cos_x, -sin_x],
-                      [0, sin_x, cos_x]])
-
-    rotation_matrix = rot_z@rot_y@rot_x
 
+    rotation_matrix = rotz(theta_z) @ roty(theta_y) @ rotx(theta_x)
     xyz_rot = rotation_matrix @ xyz
-    x = xyz_rot[0, 0]
-    y = xyz_rot[1, 0]
-    z = xyz_rot[2, 0]
 
-    rho = np.sqrt(x**2 + y**2 + z**2)
-    phi = np.arctan2(y, x)
-    theta = np.arcsin(z/rho)
+    rho, phi, theta = cart2sphere(*xyz_rot[:, 0])
 
     return np.array([[Elevation(theta)], [Bearing(phi)], [rho]])
 
@@ -123,6 +101,7 @@ def test_simple_radar(h, sensorclass, ndim_state, pos_mapping, noise_covar, posi
     # Assert correction of generated measurement
     assert (measurement.timestamp == target_state.timestamp)
     assert (np.equal(measurement.state_vector, h(target_state,
+                                                 pos_map=pos_mapping,
                                                  translation_offset=position,
                                                  rotation_offset=radar.orientation)).all())
 
@@ -176,7 +155,7 @@ def h3d_rr(state, pos_map, vel_map, translation_offset, rotation_offset, velocit
     # Use polar to calculate range rate
     rr = np.dot(xyz[:, 0], xyz_vel[:, 0]) / np.linalg.norm(xyz)
 
-    return np.array([[Elevation(theta)], [Bearing(phi)], [rho], [rr]])
+    return np.array([[theta], [phi], [rho], [rr]])
 
 
 @pytest.mark.parametrize(
@@ -285,6 +264,7 @@ def test_rotating_radar():
                          timestamp=timestamp)
     measurement = radar.measure(target_state, noise=False)
     eval_m = h2d(target_state,
+                 measurement_mapping,
                  radar.position,
                  radar.orientation+[[0],
                                     [0],
@@ -356,6 +336,7 @@ def test_raster_scan_radar():
                          timestamp=timestamp)
     measurement = radar.measure(target_state, noise=False)
     eval_m = h2d(target_state,
+                 [0, 1],
                  radar.position,
                  radar.orientation + [[0],
                                       [0],
diff --git a/stonesoup/tests/test_functions.py b/stonesoup/tests/test_functions.py
index e82fe1d75..69243912f 100644
--- a/stonesoup/tests/test_functions.py
+++ b/stonesoup/tests/test_functions.py
@@ -1,9 +1,11 @@
+import pytest
 import numpy as np
 from numpy import deg2rad
 from pytest import approx
 
 from ..functions import (
-    jacobian, gm_reduce_single, mod_bearing, mod_elevation, gauss2sigma)
+    jacobian, gm_reduce_single, mod_bearing, mod_elevation, gauss2sigma,
+    rotx, roty, rotz, cart2sphere, cart2angles, pol2cart, sphere2cart)
 from ..types.state import State, GaussianState
 
 
@@ -133,3 +135,63 @@ def test_gauss2sigma_int():
         # Resultant sigma points are still ints
         assert sigma_point_state.state_vector[0, 0] == int(mean + n*covar**0.5)
         assert isinstance(sigma_point_state.state_vector[0, 0], np.integer)
+
+
+@pytest.mark.parametrize(
+    "angle",
+    [
+        (
+            np.array([np.pi]),  # angle
+            np.array([np.pi / 2]),
+            np.array([-np.pi]),
+            np.array([-np.pi / 2]),
+            np.array([np.pi / 4]),
+            np.array([-np.pi / 4]),
+            np.array([np.pi / 8]),
+            np.array([-np.pi / 8]),
+        )
+    ]
+)
+def test_rotations(angle):
+
+    c, s = np.cos(angle), np.sin(angle)
+    zero = np.zeros_like(angle)
+    one = np.ones_like(angle)
+
+    assert np.array_equal(rotx(angle), np.array([[one, zero, zero],
+                                                 [zero, c, -s],
+                                                 [zero, s, c]]))
+    assert np.array_equal(roty(angle), np.array([[c, zero, s],
+                                                 [zero, one, zero],
+                                                 [-s, zero, c]]))
+    assert np.array_equal(rotz(angle), np.array([[c, -s, zero],
+                                                 [s, c, zero],
+                                                 [zero, zero, one]]))
+
+
+@pytest.mark.parametrize(
+    "x, y, z",
+    [  # Cartesian values
+        (1., 0., 0.),
+        (0., 1., 0.),
+        (0., 0., 1.),
+        (1., 1., 0.),
+        (1., 0., 1.),
+        (0., 1., 1.),
+        (1., 1., 1.)
+    ]
+)
+def test_cart_sphere_inversions(x, y, z):
+
+    rho, phi, theta = cart2sphere(x, y, z)
+
+    # Check sphere2cart(cart2sphere(cart)) == cart
+    assert np.allclose(np.array([x, y, z]), sphere2cart(rho, phi, theta))
+
+    # Check cart2angle == cart2sphere for angles
+    assert np.allclose(np.array([phi, theta]), cart2angles(x, y, z))
+
+    # Check that pol2cart(cart2angle(cart)) == cart
+    #   note, this only works correctly when z==0
+    if z == 0:
+        assert np.allclose(np.array([x, y]), pol2cart(rho, phi))

From 78c5a9802447383e4b51d40145dbf60bba71ef8d Mon Sep 17 00:00:00 2001
From: rjgreen <rjgreen@dstl.gov.uk>
Date: Tue, 5 May 2020 11:20:29 +0100
Subject: [PATCH 16/21] reviewer comments addressed

---
 stonesoup/models/measurement/nonlinear.py     |  2 +-
 .../models/measurement/tests/test_models.py   | 86 ++++++++-----------
 2 files changed, 39 insertions(+), 49 deletions(-)

diff --git a/stonesoup/models/measurement/nonlinear.py b/stonesoup/models/measurement/nonlinear.py
index e84de34d7..c92e3a4d2 100644
--- a/stonesoup/models/measurement/nonlinear.py
+++ b/stonesoup/models/measurement/nonlinear.py
@@ -716,7 +716,7 @@ class CartesianToElevationBearingRangeRate(NonLinearGaussianMeasurement, Reversi
 
     Note
     ----
-    This class implementation assuming at 3D cartesian space, , it therefore\
+    This class implementation assuming at 3D cartesian space, it therefore\
     expects a 6D state space.
     """
 
diff --git a/stonesoup/models/measurement/tests/test_models.py b/stonesoup/models/measurement/tests/test_models.py
index d9e0e42fb..b2214dbf6 100644
--- a/stonesoup/models/measurement/tests/test_models.py
+++ b/stonesoup/models/measurement/tests/test_models.py
@@ -10,7 +10,7 @@
     CartesianToElevationBearing, CartesianToBearingRangeRate,
     CartesianToElevationBearingRangeRate)
 from ...base import ReversibleModel
-from ....types.state import State
+from ....types.state import State, CovarianceMatrix
 from ....functions import jacobian as compute_jac
 from ....types.angle import Bearing, Elevation
 from ....types.array import StateVector, Matrix
@@ -23,48 +23,42 @@ def h2d(state_vector, pos_map, translation_offset, rotation_offset):
            [0]]
 
     # Get rotation matrix
-    theta_z = - rotation_offset[2, 0]
-    theta_y = - rotation_offset[1, 0]
-    theta_x = - rotation_offset[0, 0]
+    theta_x, theta_y, theta_z = - rotation_offset[:, 0]
 
     rotation_matrix = rotz(theta_z) @ roty(theta_y) @ rotx(theta_x)
     xyz_rot = rotation_matrix @ xyz
 
     rho, phi, _ = cart2sphere(*xyz_rot[:, 0])
 
-    return np.array([[Bearing(phi)], [rho]])
+    return StateVector([Bearing(phi), rho])
 
 
 def h3d(state_vector, pos_map,  translation_offset, rotation_offset):
     xyz = state_vector[pos_map, :] - translation_offset
 
     # Get rotation matrix
-    theta_z = - rotation_offset[2, 0]
-    theta_y = - rotation_offset[1, 0]
-    theta_x = - rotation_offset[0, 0]
+    theta_x, theta_y, theta_z = - rotation_offset[:, 0]
 
     rotation_matrix = rotz(theta_z) @ roty(theta_y) @ rotx(theta_x)
     xyz_rot = rotation_matrix @ xyz
 
     rho, phi, theta = cart2sphere(*xyz_rot[:, 0])
 
-    return np.array([[Elevation(theta)], [Bearing(phi)], [rho]])
+    return StateVector([Elevation(theta), Bearing(phi), rho])
 
 
 def hbearing(state_vector, pos_map, translation_offset, rotation_offset):
     xyz = state_vector[pos_map, :] - translation_offset
 
     # Get rotation matrix
-    theta_z = - rotation_offset[2, 0]
-    theta_y = - rotation_offset[1, 0]
-    theta_x = - rotation_offset[0, 0]
+    theta_x, theta_y, theta_z = - rotation_offset[:, 0]
 
     rotation_matrix = rotz(theta_z) @ roty(theta_y) @ rotx(theta_x)
     xyz_rot = rotation_matrix @ xyz
 
     _, phi, theta = cart2sphere(*xyz_rot[:, 0])
 
-    return np.array([[Elevation(theta)], [Bearing(phi)]])
+    return StateVector([Elevation(theta), Bearing(phi)])
 
 
 @pytest.mark.parametrize(
@@ -74,34 +68,34 @@ def hbearing(state_vector, pos_map, translation_offset, rotation_offset):
         (   # 2D meas, 2D state
             h2d,
             CartesianToBearingRange,
-            np.array([[0], [1]]),
-            np.array([[0.015, 0],
-                      [0, 0.1]]),
+            StateVector([[0], [1]]),
+            CovarianceMatrix([[0.015, 0],
+                              [0, 0.1]]),
             np.array([0, 1]),
-            np.array([[1], [-1]]),
-            np.array([[0], [0], [1]])
+            StateVector([[1], [-1]]),
+            StateVector([[0], [0], [1]])
 
         ),
         (   # 3D meas, 3D state
             h3d,
             CartesianToElevationBearingRange,
-            np.array([[1], [2], [2]]),
-            np.array([[0.05, 0, 0],
-                      [0, 0.015, 0],
-                      [0, 0, 0.1]]),
+            StateVector([[1], [2], [2]]),
+            CovarianceMatrix([[0.05, 0, 0],
+                              [0, 0.015, 0],
+                              [0, 0, 0.1]]),
             np.array([0, 1, 2]),
-            np.array([[0], [0], [0]]),
-            np.array([[.2], [3], [-1]])
+            StateVector([[0], [0], [0]]),
+            StateVector([[.2], [3], [-1]])
         ),
         (   # 2D meas, 3D state
             hbearing,
             CartesianToElevationBearing,
-            np.array([[1], [2], [3]]),
+            StateVector([[1], [2], [3]]),
             np.array([[0.05, 0],
                       [0, 0.015]]),
             np.array([0, 1, 2]),
-            np.array([[0], [0], [0]]),
-            np.array([[-3], [0], [np.pi/3]])
+            StateVector([[0], [0], [0]]),
+            StateVector([[-3], [0], [np.pi/3]])
         )
     ],
     ids=["BearingElevation", "RangeBearingElevation", "BearingsOnly"]
@@ -219,9 +213,7 @@ def h2d_rr(state_vector, pos_map, vel_map, translation_offset, rotation_offset,
                     [0]])
 
     # Get rotation matrix
-    theta_z = - rotation_offset[2, 0]
-    theta_y = - rotation_offset[1, 0]
-    theta_x = - rotation_offset[0, 0]
+    theta_x, theta_y, theta_z = - rotation_offset[:, 0]
 
     rotation_matrix = rotz(theta_z) @ roty(theta_y) @ rotx(theta_x)
     xyz_rot = rotation_matrix @ xyz
@@ -237,7 +229,7 @@ def h2d_rr(state_vector, pos_map, vel_map, translation_offset, rotation_offset,
     # Use polar to calculate range rate
     rr = np.dot(xyz[:, 0], xyz_vel[:, 0]) / np.linalg.norm(xyz)
 
-    return np.array([[Bearing(phi)], [rho], [rr]])
+    return StateVector([Bearing(phi), rho, rr])
 
 
 def h3d_rr(state_vector, pos_map, vel_map, translation_offset, rotation_offset, velocity):
@@ -245,9 +237,7 @@ def h3d_rr(state_vector, pos_map, vel_map, translation_offset, rotation_offset,
     xyz = state_vector[pos_map, :] - translation_offset
 
     # Get rotation matrix
-    theta_z = - rotation_offset[2, 0]
-    theta_y = - rotation_offset[1, 0]
-    theta_x = - rotation_offset[0, 0]
+    theta_x, theta_y, theta_z = - rotation_offset[:, 0]
 
     rotation_matrix = rotz(theta_z) @ roty(theta_y) @ rotx(theta_x)
     xyz_rot = rotation_matrix @ xyz
@@ -263,7 +253,7 @@ def h3d_rr(state_vector, pos_map, vel_map, translation_offset, rotation_offset,
     # Use polar to calculate range rate
     rr = np.dot(xyz[:, 0], xyz_vel[:, 0]) / np.linalg.norm(xyz)
 
-    return np.array([[Elevation(theta)], [Bearing(phi)], [rho], [rr]])
+    return StateVector([Elevation(theta), Bearing(phi), rho, rr])
 
 
 @pytest.mark.parametrize(
@@ -273,29 +263,29 @@ def h3d_rr(state_vector, pos_map, vel_map, translation_offset, rotation_offset,
         (   # 3D meas, 6D state
             h2d_rr,  # h
             CartesianToBearingRangeRate,  # ModelClass
-            np.array([[200.], [10.], [0.], [0.], [0.], [0.]]),  # state_vec
+            StateVector([[200.], [10.], [0.], [0.], [0.], [0.]]),  # state_vec
             6,  # ndim_state
             np.array([0, 2, 4]),  # pos_mapping
             np.array([1, 3, 5]),  # vel_mapping
-            np.array([[0.05, 0, 0],
-                      [0, 0.015, 0],
-                      [0, 0, 10]]),  # noise_covar
-            np.array([[1], [-1], [0]]),  # position (translation offset)
-            np.array([[0], [0], [1]])  # orientation (rotation offset)
+            CovarianceMatrix([[0.05, 0, 0],
+                              [0, 0.015, 0],
+                              [0, 0, 10]]),  # noise_covar
+            StateVector([[1], [-1], [0]]),  # position (translation offset)
+            StateVector([[0], [0], [1]])  # orientation (rotation offset)
         ),
         (   # 4D meas, 6D state
             h3d_rr,  # h
             CartesianToElevationBearingRangeRate,  # ModelClass
-            np.array([[200.], [10.], [0.], [0.], [0.], [0.]]),  # state_vec
+            StateVector([[200.], [10.], [0.], [0.], [0.], [0.]]),  # state_vec
             6,  # ndim_state
             np.array([0, 2, 4]),  # pos_mapping
             np.array([1, 3, 5]),  # vel_mapping
-            np.array([[0.05, 0, 0, 0],
-                      [0, 0.05, 0, 0],
-                      [0, 0, 0.015, 0],
-                      [0, 0, 0, 10]]),  # noise_covar
-            np.array([[100], [0], [0]]),  # position (translation offset)
-            np.array([[0], [0], [0]])  # orientation (rotation offset)
+            CovarianceMatrix([[0.05, 0, 0, 0],
+                              [0, 0.05, 0, 0],
+                              [0, 0, 0.015, 0],
+                              [0, 0, 0, 10]]),  # noise_covar
+            StateVector([[100], [0], [0]]),  # position (translation offset)
+            StateVector([[0], [0], [0]])  # orientation (rotation offset)
         )
     ],
     ids=["rrRB", "rrRBE"]

From cd1e25d08626428619f54a91a42966ffa23bcdb8 Mon Sep 17 00:00:00 2001
From: rjgreen <rjgreen@dstl.gov.uk>
Date: Tue, 5 May 2020 14:43:55 +0100
Subject: [PATCH 17/21] removed mutable defaults in nonlinear measurements

---
 stonesoup/models/measurement/nonlinear.py | 74 ++++++++++++++++++++---
 1 file changed, 67 insertions(+), 7 deletions(-)

diff --git a/stonesoup/models/measurement/nonlinear.py b/stonesoup/models/measurement/nonlinear.py
index c92e3a4d2..089594971 100644
--- a/stonesoup/models/measurement/nonlinear.py
+++ b/stonesoup/models/measurement/nonlinear.py
@@ -97,13 +97,22 @@ class NonLinearGaussianMeasurement(MeasurementModel, NonLinearModel, GaussianMod
     """
     noise_covar = Property(CovarianceMatrix, doc="Noise covariance")
     rotation_offset = Property(
-        StateVector, default=StateVector(np.array([[0], [0], [0]])),
+        StateVector, default=None,
         doc="A 3x1 array of angles (rad), specifying the clockwise rotation\
             around each Cartesian axis in the order :math:`x,y,z`.\
             The rotation angles are positive if the rotation is in the \
             counter-clockwise direction when viewed by an observer looking\
             along the respective rotation axis, towards the origin.")
 
+    def __init__(self, *args, **kwargs):
+        """
+        Ensure that the rotation offset is initiated
+        """
+        super().__init__(*args, **kwargs)
+        # Set values to defaults if not provided
+        if self.rotation_offset is None:
+            self.rotation_offset = StateVector([[0], [0], [0]])
+
     def covar(self, **kwargs) -> CovarianceMatrix:
         """Returns the measurement model noise covariance matrix.
 
@@ -196,10 +205,19 @@ class CartesianToElevationBearingRange(NonLinearGaussianMeasurement, ReversibleM
     """  # noqa:E501
 
     translation_offset = Property(
-        StateVector, default=StateVector(np.array([[0], [0], [0]])),
+        StateVector, 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] * self.ndim)
+
     @property
     def ndim_meas(self) -> int:
         """ndim_meas getter method
@@ -325,10 +343,19 @@ class CartesianToBearingRange(NonLinearGaussianMeasurement, ReversibleModel):
     """  # noqa:E501
 
     translation_offset = Property(
-        StateVector, default=StateVector(np.array([[0], [0]])),
+        StateVector, default=None,
         doc="A 2x1 array specifying the origin offset in terms of :math:`x,y`\
             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] * self.ndim)
+
     @property
     def ndim_meas(self) -> int:
         """ndim_meas getter method
@@ -465,10 +492,19 @@ class CartesianToElevationBearing(NonLinearGaussianMeasurement):
     """  # noqa:E501
 
     translation_offset = Property(
-        StateVector, default=StateVector(np.array([[0], [0], [0]])),
+        StateVector, default=None,
         doc="A 3x1 array specifying the 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] * self.ndim)
+
     @property
     def ndim_meas(self) -> int:
         """ndim_meas getter method
@@ -585,7 +621,7 @@ class CartesianToBearingRangeRate(NonLinearGaussianMeasurement):
 
     translation_offset = Property(
         StateVector,
-        default=StateVector(np.array([[0], [0], [0]])),
+        default=None,
         doc="A 3x1 array specifying the origin offset in terms of :math:`x,y` coordinates.")
     velocity_mapping = Property(
         np.array,
@@ -593,10 +629,22 @@ class CartesianToBearingRangeRate(NonLinearGaussianMeasurement):
         doc="Mapping to the targets velocity within its state space")
     velocity = Property(
         StateVector,
-        default=StateVector(np.array([[0], [0], [0]])),
+        default=None,
         doc="A 3x1 array specifying the sensor velocity in terms of :math:`x,y` \
         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)
+
+        if self.velocity is None:
+            self.velocity = StateVector([0] * 3)
+
     @property
     def ndim_meas(self) -> int:
         """ndim_meas getter method
@@ -722,7 +770,7 @@ class CartesianToElevationBearingRangeRate(NonLinearGaussianMeasurement, Reversi
 
     translation_offset = Property(
         StateVector,
-        default=StateVector(np.array([[0], [0], [0]])),
+        default=None,
         doc="A 3x1 array specifying the origin offset in terms of :math:`x,y,z` coordinates.")
     velocity_mapping = Property(
         np.array,
@@ -733,6 +781,18 @@ class CartesianToElevationBearingRangeRate(NonLinearGaussianMeasurement, Reversi
         default=StateVector(np.array([[0], [0], [0]])),
         doc="A 3x1 array specifying the sensor velocity 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)
+
+        if self.velocity is None:
+            self.velocity = StateVector([0] * 3)
+
     @property
     def ndim_meas(self) -> int:
         """ndim_meas getter method

From ff9861072bc35f88c00b6f5be425ee81a0fe3aef Mon Sep 17 00:00:00 2001
From: rjgreen <rjgreen@dstl.gov.uk>
Date: Tue, 5 May 2020 15:23:38 +0100
Subject: [PATCH 18/21] Updated models to test non-mutable modification

---
 stonesoup/models/measurement/nonlinear.py      |  4 ++--
 .../models/measurement/tests/test_models.py    | 18 ++++++++++++++++++
 2 files changed, 20 insertions(+), 2 deletions(-)

diff --git a/stonesoup/models/measurement/nonlinear.py b/stonesoup/models/measurement/nonlinear.py
index 089594971..08bffbb89 100644
--- a/stonesoup/models/measurement/nonlinear.py
+++ b/stonesoup/models/measurement/nonlinear.py
@@ -503,7 +503,7 @@ def __init__(self, *args, **kwargs):
         super().__init__(*args, **kwargs)
         # Set values to defaults if not provided
         if self.translation_offset is None:
-            self.translation_offset = StateVector([0] * self.ndim)
+            self.translation_offset = StateVector([0] * self.ndim_state)
 
     @property
     def ndim_meas(self) -> int:
@@ -778,7 +778,7 @@ class CartesianToElevationBearingRangeRate(NonLinearGaussianMeasurement, Reversi
         doc="Mapping to the targets velocity within its state space")
     velocity = Property(
         StateVector,
-        default=StateVector(np.array([[0], [0], [0]])),
+        default=None,
         doc="A 3x1 array specifying the sensor velocity in terms of :math:`x,y,z` coordinates.")
 
     def __init__(self, *args, **kwargs):
diff --git a/stonesoup/models/measurement/tests/test_models.py b/stonesoup/models/measurement/tests/test_models.py
index b2214dbf6..64846bfbd 100644
--- a/stonesoup/models/measurement/tests/test_models.py
+++ b/stonesoup/models/measurement/tests/test_models.py
@@ -108,6 +108,14 @@ def test_models(h, ModelClass, state_vec, R,
     ndim_state = state_vec.size
     state = State(state_vec)
 
+    # Check default translation_offset, rotation_offset and velocity is applied
+    model_test = ModelClass(ndim_state=ndim_state,
+                            mapping=mapping,
+                            noise_covar=R)
+
+    assert len(model_test.translation_offset) == ndim_state
+    assert len(model_test.rotation_offset) == 3
+
     # Create and a measurement model object
     model = ModelClass(ndim_state=ndim_state,
                        mapping=mapping,
@@ -297,6 +305,16 @@ def test_rangeratemodels(h, modelclass, state_vec, ndim_state, pos_mapping, vel_
 
     state = State(state_vec)
 
+    # Check default translation_offset, rotation_offset and velocity is applied
+    model_test = modelclass(ndim_state=ndim_state,
+                            mapping=pos_mapping,
+                            velocity_mapping=vel_mapping,
+                            noise_covar=noise_covar)
+
+    assert len(model_test.translation_offset) == 3
+    assert len(model_test.rotation_offset) == 3
+    assert len(model_test.velocity) == 3
+
     # Create and a measurement model object
     model = modelclass(ndim_state=ndim_state,
                        mapping=pos_mapping,

From 9223e54d175535365fb1d6321f59b67df78012e8 Mon Sep 17 00:00:00 2001
From: rjgreen <rjgreen@dstl.gov.uk>
Date: Wed, 6 May 2020 11:06:49 +0100
Subject: [PATCH 19/21] Mapping defualts converted to non-mutable tuples

---
 stonesoup/models/measurement/nonlinear.py | 18 ++++++------------
 stonesoup/sensor/radar/radar.py           | 18 ++++++------------
 2 files changed, 12 insertions(+), 24 deletions(-)

diff --git a/stonesoup/models/measurement/nonlinear.py b/stonesoup/models/measurement/nonlinear.py
index 08bffbb89..cb73a7e96 100644
--- a/stonesoup/models/measurement/nonlinear.py
+++ b/stonesoup/models/measurement/nonlinear.py
@@ -620,16 +620,13 @@ class CartesianToBearingRangeRate(NonLinearGaussianMeasurement):
     """
 
     translation_offset = Property(
-        StateVector,
-        default=None,
+        StateVector, default=None,
         doc="A 3x1 array specifying the origin offset in terms of :math:`x,y` coordinates.")
     velocity_mapping = Property(
-        np.array,
-        default=np.array([[1], [3], [5]]),
+        np.array, default=(1, 3, 5),
         doc="Mapping to the targets velocity within its state space")
     velocity = Property(
-        StateVector,
-        default=None,
+        StateVector, default=None,
         doc="A 3x1 array specifying the sensor velocity in terms of :math:`x,y` \
         coordinates.")
 
@@ -769,16 +766,13 @@ class CartesianToElevationBearingRangeRate(NonLinearGaussianMeasurement, Reversi
     """
 
     translation_offset = Property(
-        StateVector,
-        default=None,
+        StateVector, default=None,
         doc="A 3x1 array specifying the origin offset in terms of :math:`x,y,z` coordinates.")
     velocity_mapping = Property(
-        np.array,
-        default=np.array([[1], [3], [5]]),
+        np.array, default=(1, 3, 5),
         doc="Mapping to the targets velocity within its state space")
     velocity = Property(
-        StateVector,
-        default=None,
+        StateVector, default=None,
         doc="A 3x1 array specifying the sensor velocity in terms of :math:`x,y,z` coordinates.")
 
     def __init__(self, *args, **kwargs):
diff --git a/stonesoup/sensor/radar/radar.py b/stonesoup/sensor/radar/radar.py
index 5fb4adc8b..05dc61bbb 100644
--- a/stonesoup/sensor/radar/radar.py
+++ b/stonesoup/sensor/radar/radar.py
@@ -33,8 +33,7 @@ class RadarRangeBearing(Sensor):
     """
 
     ndim_state = Property(
-        int,
-        default=2,
+        int, default=2,
         doc="Number of state dimensions. This is utilised by (and follows in format) "
             "the underlying :class:`~.CartesianToBearingRange` model")
     position_mapping = Property(
@@ -214,8 +213,7 @@ class RadarRangeBearingElevation(RadarRangeBearing):
     """
 
     ndim_state = Property(
-        int,
-        default=3,
+        int, default=3,
         doc="Number of state dimensions. This is utilised by (and follows in format) "
             "the underlying :class:`~.CartesianToBearingRange` model")
     noise_covar = Property(
@@ -270,12 +268,10 @@ class RadarRangeRateBearing(RadarRangeBearing):
     """
 
     velocity_mapping = Property(
-        np.array,
-        default=np.array([[1], [3], [5]]),
+        np.array, default=(1, 3, 5),
         doc="Mapping to the target's velocity information within its state space")
     ndim_state = Property(
-        int,
-        default=3,
+        int, default=3,
         doc="Number of state dimensions. This is utilised by (and follows in format) "
             "the underlying :class:`~.CartesianToBearingRangeRate` model")
     noise_covar = Property(
@@ -331,12 +327,10 @@ class RadarRangeRateBearingElevation(RadarRangeRateBearing):
     """
 
     velocity_mapping = Property(
-        np.array,
-        default=np.array([[1], [3], [5]]),
+        np.array, default=(1, 3, 5),
         doc="Mapping to the target's velocity information within its state space")
     ndim_state = Property(
-        int,
-        default=6,
+        int, default=6,
         doc="Number of state dimensions. This is utilised by (and follows in format) "
             "the underlying :class:`~.CartesianToElevationBearingRangeRate` model")
     noise_covar = Property(

From 9087bceda8cf30e7c076c9e19a93f5be2d81d359 Mon Sep 17 00:00:00 2001
From: rjgreen <rjgreen@dstl.gov.uk>
Date: Wed, 6 May 2020 16:05:55 +0100
Subject: [PATCH 20/21] tests added for non-mutable default changes

---
 stonesoup/models/measurement/nonlinear.py     |  2 +-
 .../models/measurement/tests/test_models.py   | 65 ++++++++++++++++++-
 stonesoup/sensor/base.py                      | 11 +---
 3 files changed, 66 insertions(+), 12 deletions(-)

diff --git a/stonesoup/models/measurement/nonlinear.py b/stonesoup/models/measurement/nonlinear.py
index cb73a7e96..7e9ca7994 100644
--- a/stonesoup/models/measurement/nonlinear.py
+++ b/stonesoup/models/measurement/nonlinear.py
@@ -627,7 +627,7 @@ class CartesianToBearingRangeRate(NonLinearGaussianMeasurement):
         doc="Mapping to the targets velocity within its state space")
     velocity = Property(
         StateVector, default=None,
-        doc="A 3x1 array specifying the sensor velocity in terms of :math:`x,y` \
+        doc="A 3x1 array specifying the sensor velocity in terms of :math:`x,y,z` \
         coordinates.")
 
     def __init__(self, *args, **kwargs):
diff --git a/stonesoup/models/measurement/tests/test_models.py b/stonesoup/models/measurement/tests/test_models.py
index 64846bfbd..b23b7a849 100644
--- a/stonesoup/models/measurement/tests/test_models.py
+++ b/stonesoup/models/measurement/tests/test_models.py
@@ -75,6 +75,17 @@ def hbearing(state_vector, pos_map, translation_offset, rotation_offset):
             StateVector([[1], [-1]]),
             StateVector([[0], [0], [1]])
 
+        ),
+        (   # 2D meas, 2D state
+            h2d,
+            CartesianToBearingRange,
+            StateVector([[0], [1]]),
+            CovarianceMatrix([[0.015, 0],
+                              [0, 0.1]]),
+            np.array([0, 1]),
+            None,
+            None
+
         ),
         (   # 3D meas, 3D state
             h3d,
@@ -87,6 +98,17 @@ def hbearing(state_vector, pos_map, translation_offset, rotation_offset):
             StateVector([[0], [0], [0]]),
             StateVector([[.2], [3], [-1]])
         ),
+        (   # 3D meas, 3D state
+            h3d,
+            CartesianToElevationBearingRange,
+            StateVector([[1], [2], [2]]),
+            CovarianceMatrix([[0.05, 0, 0],
+                              [0, 0.015, 0],
+                              [0, 0, 0.1]]),
+            np.array([0, 1, 2]),
+            None,
+            None
+        ),
         (   # 2D meas, 3D state
             hbearing,
             CartesianToElevationBearing,
@@ -96,9 +118,21 @@ def hbearing(state_vector, pos_map, translation_offset, rotation_offset):
             np.array([0, 1, 2]),
             StateVector([[0], [0], [0]]),
             StateVector([[-3], [0], [np.pi/3]])
+        ),
+        (   # 2D meas, 3D state
+            hbearing,
+            CartesianToElevationBearing,
+            StateVector([[1], [2], [3]]),
+            np.array([[0.05, 0],
+                      [0, 0.015]]),
+            np.array([0, 1, 2]),
+            None,
+            None
         )
     ],
-    ids=["BearingElevation", "RangeBearingElevation", "BearingsOnly"]
+    ids=["BearingElevation1", "BearingElevation2",
+         "RangeBearingElevation1", "RangeBearingElevation1",
+         "BearingsOnly1", "BearingsOnly2"]
 )
 def test_models(h, ModelClass, state_vec, R,
                 mapping, translation_offset, rotation_offset):
@@ -281,6 +315,19 @@ def h3d_rr(state_vector, pos_map, vel_map, translation_offset, rotation_offset,
             StateVector([[1], [-1], [0]]),  # position (translation offset)
             StateVector([[0], [0], [1]])  # orientation (rotation offset)
         ),
+        (   # 3D meas, 6D state
+            h2d_rr,  # h
+            CartesianToBearingRangeRate,  # ModelClass
+            StateVector([[200.], [10.], [0.], [0.], [0.], [0.]]),  # state_vec
+            6,  # ndim_state
+            np.array([0, 2, 4]),  # pos_mapping
+            np.array([1, 3, 5]),  # vel_mapping
+            CovarianceMatrix([[0.05, 0, 0],
+                              [0, 0.015, 0],
+                              [0, 0, 10]]),  # noise_covar
+            None,  # position (translation offset)
+            None  # orientation (rotation offset)
+        ),
         (   # 4D meas, 6D state
             h3d_rr,  # h
             CartesianToElevationBearingRangeRate,  # ModelClass
@@ -294,9 +341,23 @@ def h3d_rr(state_vector, pos_map, vel_map, translation_offset, rotation_offset,
                               [0, 0, 0, 10]]),  # noise_covar
             StateVector([[100], [0], [0]]),  # position (translation offset)
             StateVector([[0], [0], [0]])  # orientation (rotation offset)
+        ),
+        (   # 4D meas, 6D state
+            h3d_rr,  # h
+            CartesianToElevationBearingRangeRate,  # ModelClass
+            StateVector([[200.], [10.], [0.], [0.], [0.], [0.]]),  # state_vec
+            6,  # ndim_state
+            np.array([0, 2, 4]),  # pos_mapping
+            np.array([1, 3, 5]),  # vel_mapping
+            CovarianceMatrix([[0.05, 0, 0, 0],
+                              [0, 0.05, 0, 0],
+                              [0, 0, 0.015, 0],
+                              [0, 0, 0, 10]]),  # noise_covar
+            None,  # position (translation offset)
+            None  # orientation (rotation offset)
         )
     ],
-    ids=["rrRB", "rrRBE"]
+    ids=["rrRB_1", "rrRB_2", "rrRBE_1", "rrRBE_2"]
 )
 def test_rangeratemodels(h, modelclass, state_vec, ndim_state, pos_mapping, vel_mapping,
                          noise_covar, position, orientation):
diff --git a/stonesoup/sensor/base.py b/stonesoup/sensor/base.py
index ccc9464dc..83e8ee658 100644
--- a/stonesoup/sensor/base.py
+++ b/stonesoup/sensor/base.py
@@ -113,13 +113,6 @@ def velocity(self) -> Optional[StateVector]:
             This property delegates the actual calculation of velocity to the platform on which the
             sensor is mounted.
 
-            It is settable if, and only if, the sensor holds its own internal platform."""
+            It is settable if, and only if, the sensor holds its own internal platform which is
+            a MovingPlatfom."""
         return self.platform.velocity
-
-    @velocity.setter
-    def velocity(self, value):
-        if self._has_internal_platform:
-            self.platform.velocity = value
-        else:
-            raise AttributeError('Cannot set platform velocity unless the sensor has its own '
-                                 'default platform')

From e175f0ec65bd692b5307d172efeb70cff06c9656 Mon Sep 17 00:00:00 2001
From: rjgreen <rjgreen@dstl.gov.uk>
Date: Wed, 6 May 2020 16:12:58 +0100
Subject: [PATCH 21/21] index modification as reviewer requested

---
 stonesoup/models/measurement/nonlinear.py | 17 +++++++++--------
 1 file changed, 9 insertions(+), 8 deletions(-)

diff --git a/stonesoup/models/measurement/nonlinear.py b/stonesoup/models/measurement/nonlinear.py
index 7e9ca7994..3f04e0b88 100644
--- a/stonesoup/models/measurement/nonlinear.py
+++ b/stonesoup/models/measurement/nonlinear.py
@@ -680,7 +680,7 @@ def function(self, state, noise=False, **kwargs) -> StateVector:
                 noise = 0
 
         # Account for origin offset in position to enable range and angles to be determined
-        xy_pos = state.state_vector[self.mapping] - self.translation_offset
+        xy_pos = state.state_vector[self.mapping, :] - self.translation_offset
 
         # Rotate coordinates based upon the sensor_velocity
         xy_rot = self._rotation_matrix @ xy_pos
@@ -825,7 +825,7 @@ def function(self, state, noise=False, **kwargs) -> StateVector:
                 noise = 0
 
         # Account for origin offset in position to enable range and angles to be determined
-        xyz_pos = state.state_vector[self.mapping] - self.translation_offset
+        xyz_pos = state.state_vector[self.mapping, :] - self.translation_offset
 
         # Rotate coordinates based upon the sensor_velocity
         xyz_rot = self._rotation_matrix @ xyz_pos
@@ -857,14 +857,15 @@ def inverse_function(self, detection, **kwargs) -> StateVector:
         inv_rotation_matrix = inv(self._rotation_matrix)
 
         out_vector = StateVector([[0.], [0.], [0.], [0.], [0.], [0.]])
-        out_vector[self.mapping] = x, y, z
-        out_vector[self.velocity_mapping] = x_rate, y_rate, z_rate
+        out_vector[self.mapping, :] = x, y, z
+        out_vector[self.velocity_mapping, :] = x_rate, y_rate, z_rate
 
-        out_vector[self.mapping] = inv_rotation_matrix @ out_vector[self.mapping]
-        out_vector[self.velocity_mapping] = inv_rotation_matrix @ out_vector[self.velocity_mapping]
+        out_vector[self.mapping, :] = inv_rotation_matrix @ out_vector[self.mapping, :]
+        out_vector[self.velocity_mapping, :] = \
+            inv_rotation_matrix @ out_vector[self.velocity_mapping, :]
+
+        out_vector[self.mapping, :] = out_vector[self.mapping, :] + self.translation_offset
 
-        out_vector[self.mapping] = out_vector[self.mapping] + self.translation_offset
-        print(out_vector)
         return out_vector
 
     def rvs(self, num_samples=1, **kwargs) -> np.ndarray: