diff --git a/stonesoup/models/measurement/nonlinear.py b/stonesoup/models/measurement/nonlinear.py index 7431b4cad..c4b2871ae 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: @@ -97,14 +97,23 @@ 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 covar(self, **kwargs): + 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. Returns @@ -117,7 +126,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. @@ -135,8 +144,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 \ @@ -197,12 +205,21 @@ class CartesianToElevationBearingRange( """ # 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): + def ndim_meas(self) -> int: """ndim_meas getter method Returns @@ -213,7 +230,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 @@ -248,7 +265,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 xyz = StateVector(sphere2cart(rho, phi, theta)) @@ -261,7 +278,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 @@ -323,12 +340,21 @@ 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): + def ndim_meas(self) -> int: """ndim_meas getter method Returns @@ -339,7 +365,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) and (self.rotation_offset[1] == 0)): raise RuntimeError( @@ -359,7 +385,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 @@ -398,7 +424,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 @@ -462,12 +488,21 @@ 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_state) + @property - def ndim_meas(self): + def ndim_meas(self) -> int: """ndim_meas getter method Returns @@ -478,7 +513,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 @@ -513,7 +548,323 @@ 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 + + +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 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 implementation assuming at 3D cartesian space, it therefore\ + expects a 6D state space. + """ + + translation_offset = Property( + StateVector, default=None, + doc="A 3x1 array specifying the origin offset in terms of :math:`x,y` coordinates.") + velocity_mapping = Property( + np.array, default=(1, 3, 5), + 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,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 + + Returns + ------- + :class:`int` + The number of measurement dimensions + """ + + return 3 + + def function(self, state, noise=False, **kwargs) -> StateVector: + r"""Model function :math:`h(\vec{x}_t,\vec{v}_t)` + + Parameters + ---------- + state: :class:`~.State` + An input state + noise: :class:`numpy.ndarray` or bool + An externally generated random process noise sample (the default is + `False`, in which case no noise will be added + if 'True', the output of :meth:`~.Model.rvs` is added) + + Returns + ------- + :class:`numpy.ndarray` of shape (:py:attr:`~ndim_state`, 1) + The model function evaluated given the provided time interval. + + """ + + if isinstance(noise, bool) or noise is None: + if noise: + noise = self.rvs() + else: + 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 + + # Rotate coordinates based upon the sensor_velocity + xy_rot = self._rotation_matrix @ xy_pos + + # Convert to Spherical + rho, phi, _ = cart2sphere(*xy_rot) + + # Determine the net velocity component in the engagement + 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) + + return StateVector([[Bearing(phi)], [rho], [rr]]) + noise + + 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(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 + 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 implementation assuming at 3D cartesian space, it therefore\ + expects a 6D state space. + """ + + translation_offset = Property( + 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=(1, 3, 5), + 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,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 + + Returns + ------- + :class:`int` + The number of measurement dimensions + """ + + return 4 + + def function(self, state, noise=False, **kwargs) -> StateVector: + r"""Model function :math:`h(\vec{x}_t,\vec{v}_t)` + + Parameters + ---------- + 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 + 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 = state.state_vector[self.mapping, :] - self.translation_offset + + # Rotate coordinates based upon the sensor_velocity + xyz_rot = self._rotation_matrix @ xyz_pos + + # Convert to Spherical + rho, phi, theta = cart2sphere(*xyz_rot) + + # Determine the net velocity component in the engagement + 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) + + 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, 0] = x, y, z + out_vector[self.velocity_mapping, 0] = 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 + + 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 + return out diff --git a/stonesoup/models/measurement/tests/test_models.py b/stonesoup/models/measurement/tests/test_models.py index bda4cbab7..2066d7f43 100644 --- a/stonesoup/models/measurement/tests/test_models.py +++ b/stonesoup/models/measurement/tests/test_models.py @@ -4,130 +4,62 @@ import numpy as np from scipy.stats import multivariate_normal +from ....functions import rotz, rotx, roty, cart2sphere from ..nonlinear import ( CartesianToElevationBearingRange, CartesianToBearingRange, - CartesianToElevationBearing) + 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 from ....functions import pol2cart -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]], [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 + 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 - 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) - return np.array([[Bearing(phi)], [rho]]) + return StateVector([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 + 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 - 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) - return np.array([[Elevation(theta)], [Bearing(phi)], [rho]]) + return StateVector([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 + 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 - 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) - return np.array([[Elevation(theta)], [Bearing(phi)]]) + return StateVector([Elevation(theta), Bearing(phi)]) @pytest.mark.parametrize( @@ -137,45 +69,88 @@ def hbearing(state_vector, 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]), + 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]), - np.array([[1], [-1]]), - np.array([[0], [0], [1]]) + None, + None ), ( # 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]), + 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]), - np.array([[0], [0], [0]]), - np.array([[.2], [3], [-1]]) + None, + None ), ( # 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]]) + ), + ( # 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=["standard", "RBE", "BearingsOnly"] + ids=["BearingElevation1", "BearingElevation2", + "RangeBearingElevation1", "RangeBearingElevation1", + "BearingsOnly1", "BearingsOnly2"] ) 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) + # 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, @@ -186,7 +161,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 @@ -220,21 +195,24 @@ 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) prob = model.pdf(State(meas_pred_wo_noise), state) assert approx(prob) == multivariate_normal.pdf( (meas_pred_wo_noise - - np.array(h(state_vec, model.translation_offset, model.rotation_offset))).ravel(), + - np.array(h(state_vec, mapping, model.translation_offset, model.rotation_offset)) + ).ravel(), cov=R) # 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.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 @@ -242,7 +220,8 @@ def fun(x): prob = model.pdf(State(meas_pred_w_inoise), state) assert approx(prob) == multivariate_normal.pdf( (meas_pred_w_inoise - - np.array(h(state_vec, model.translation_offset, model.rotation_offset))).ravel(), + - np.array(h(state_vec, mapping, model.translation_offset, model.rotation_offset)) + ).ravel(), cov=R) # Propagate a state vector through the model @@ -251,14 +230,15 @@ 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) prob = model.pdf(State(meas_pred_w_enoise), state) assert approx(prob) == multivariate_normal.pdf( (meas_pred_w_enoise - - np.array(h(state_vec, model.translation_offset, model.rotation_offset))).ravel(), + - h(state_vec, model.mapping, model.translation_offset, model.rotation_offset) + ).ravel(), cov=R) @@ -284,3 +264,265 @@ def test_angle_pdf(): x, y = pol2cart(10, np.radians(179)) state = State(StateVector([[x], [y]])) assert approx(reference_probability) == model.pdf(measurement, state) + + +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_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) + + # 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 StateVector([Bearing(phi), rho, rr]) + + +def h3d_rr(state_vector, pos_map, vel_map, translation_offset, rotation_offset, velocity): + + xyz = state_vector[pos_map, :] - translation_offset + + # Get rotation matrix + 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) + + # 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 StateVector([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 + 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 + 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 + 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 + 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_1", "rrRB_2", "rrRBE_1", "rrRBE_2"] +) +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) + + # 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, + velocity_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.velocity_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.velocity_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 + - h(state_vec, model.mapping, model.velocity_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.velocity_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 + - h(state_vec, model.mapping, model.velocity_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.velocity_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 + - h(state_vec, model.mapping, model.velocity_mapping, model.translation_offset, + 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.02) == 9698.46 + assert approx(inv_measure_state[1], 0.02) == 96.98 + assert approx(inv_measure_state[2], 0.02) == 1710.1 + assert approx(inv_measure_state[3], 0.02) == 17.10 + assert approx(inv_measure_state[4], 0.02) == 1736.48 + assert approx(inv_measure_state[5], 0.02) == 17.36 diff --git a/stonesoup/platform/tests/test_platform_simple.py b/stonesoup/platform/tests/test_platform_simple.py index 73410d5de..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, - mapping=measurement_mapping, + position_mapping=measurement_mapping, noise_covar=noise_covar, ) radar2 = RadarRangeBearing( ndim_state=4, - mapping=measurement_mapping, + position_mapping=measurement_mapping, noise_covar=noise_covar ) radar3 = RadarRangeBearing( ndim_state=4, - mapping=measurement_mapping, + position_mapping=measurement_mapping, noise_covar=noise_covar ) radar4 = RadarRangeBearing( ndim_state=4, - mapping=measurement_mapping, + position_mapping=measurement_mapping, noise_covar=noise_covar ) radar5 = RadarRangeBearing( ndim_state=4, - 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, - mapping=measurement_mapping, + position_mapping=measurement_mapping, noise_covar=noise_covar ) radar2 = RadarRangeBearing( ndim_state=6, - mapping=measurement_mapping, + position_mapping=measurement_mapping, noise_covar=noise_covar ) radar3 = RadarRangeBearing( ndim_state=6, - mapping=measurement_mapping, + position_mapping=measurement_mapping, noise_covar=noise_covar ) radar4 = RadarRangeBearing( ndim_state=6, - mapping=measurement_mapping, + position_mapping=measurement_mapping, noise_covar=noise_covar ) radar5 = RadarRangeBearing( ndim_state=6, - mapping=measurement_mapping, + position_mapping=measurement_mapping, noise_covar=noise_covar ) radar6 = RadarRangeBearing( ndim_state=6, - mapping=measurement_mapping, + position_mapping=measurement_mapping, noise_covar=noise_covar ) radar7 = RadarRangeBearing( ndim_state=6, - 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 7d4328101..83e8ee658 100644 --- a/stonesoup/sensor/base.py +++ b/stonesoup/sensor/base.py @@ -103,3 +103,16 @@ def orientation(self, value: StateVector): @property def _has_internal_platform(self) -> bool: return False + + @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`. + + .. 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 which is + a MovingPlatfom.""" + return self.platform.velocity diff --git a/stonesoup/sensor/radar/radar.py b/stonesoup/sensor/radar/radar.py index c23131d6d..428793d8c 100644 --- a/stonesoup/sensor/radar/radar.py +++ b/stonesoup/sensor/radar/radar.py @@ -9,7 +9,9 @@ from ...functions import cart2sphere, rotx, roty, rotz from ...base import Property -from ...models.measurement.nonlinear import CartesianToBearingRange +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 @@ -31,11 +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( + int, default=2, + doc="Number of state dimensions. This is utilised by (and follows in format) " + "the underlying :class:`~.CartesianToBearingRange` model") + position_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.position_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.position_mapping, noise_covar=self.noise_covar, translation_offset=self.position, rotation_offset=rot_offset) @@ -201,6 +202,178 @@ 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.position_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 + and velocity. + + Note + ---- + This class implementation assuming at 3D cartesian space, it therefore\ + expects a 6D state space. + + """ + + velocity_mapping = Property( + np.array, default=(1, 3, 5), + doc="Mapping to the target's 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 + + 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.position_mapping, + velocity_mapping=self.velocity_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 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. + + """ + + velocity_mapping = Property( + np.array, default=(1, 3, 5), + doc="Mapping to the target's velocity information within its state space") + ndim_state = Property( + int, default=6, + 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.position_mapping, + velocity_mapping=self.velocity_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 \ @@ -215,7 +388,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. """ @@ -314,7 +488,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])``") - mapping = Property( + position_mapping = Property( np.array, default=(0, 1, 2), doc="Mapping between or positions and state " "dimensions. [x,y,z]") @@ -448,7 +622,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.position_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..89ed21406 100644 --- a/stonesoup/sensor/radar/tests/test_radar.py +++ b/stonesoup/sensor/radar/tests/test_radar.py @@ -1,24 +1,25 @@ # -*- 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 ....functions import rotz, rotx, roty, cart2sphere +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 +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, pos_map, 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[pos_map[0], 0] - translation_offset[0, 0]], + [state.state_vector[pos_map[1], 0] - translation_offset[1, 0]], [0]] # Get rotation matrix @@ -39,36 +40,176 @@ def h2d(state_vector, translation_offset, rotation_offset): return np.array([[Bearing(phi)], [rho]]) -def test_simple_radar(): +def h3d(state, pos_map, 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 = state.state_vector[pos_map, :] - translation_offset - # Create a radar object - radar = RadarRangeBearing(position=radar_position, - orientation=radar_orientation, - ndim_state=2, - mapping=measurement_mapping, - noise_covar=noise_covar) + # Get rotation matrix + theta_z = - rotation_offset[2, 0] + theta_y = - rotation_offset[1, 0] + theta_x = - rotation_offset[0, 0] - # Assert that the object has been correctly initialised - assert(np.equal(radar.position, radar_position).all()) + rotation_matrix = rotz(theta_z) @ roty(theta_y) @ rotx(theta_x) + xyz_rot = rotation_matrix @ xyz + + rho, phi, theta = cart2sphere(*xyz_rot) + + 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, + position_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, + pos_map=pos_mapping, + translation_offset=position, + rotation_offset=radar.orientation)).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] + theta_y = - rotation_offset[1, 0] + theta_x = - rotation_offset[0, 0] + + rotation_matrix = rotz(theta_z) @ roty(theta_y) @ rotx(theta_x) + xyz_rot = rotation_matrix @ xyz + + rho, phi, _ = cart2sphere(*xyz_rot) + + # 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 = state.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] + + rotation_matrix = rotz(theta_z)@roty(theta_y)@rotx(theta_x) + xyz_rot = rotation_matrix @ xyz + + rho, phi, theta = cart2sphere(*xyz_rot) + + # Calculate range rate extension + # Determine the net velocity component in the engagement + 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) + + return np.array([[theta], [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, + position_mapping=pos_mapping, + velocity_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) + 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(): @@ -100,7 +241,7 @@ def test_rotating_radar(): radar = RadarRotatingRangeBearing(position=radar_position, orientation=radar_orientation, ndim_state=2, - mapping=measurement_mapping, + position_mapping=measurement_mapping, noise_covar=noise_covar, dwell_center=dwell_center, rpm=rpm, @@ -122,7 +263,8 @@ 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, + measurement_mapping, radar.position, radar.orientation+[[0], [0], @@ -161,7 +303,7 @@ def test_raster_scan_radar(): radar = RadarRasterScanRangeBearing(position=radar_position, orientation=radar_orientation, ndim_state=2, - mapping=measurement_mapping, + position_mapping=measurement_mapping, noise_covar=noise_covar, dwell_center=dwell_center, rpm=rpm, @@ -193,7 +335,8 @@ 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, + [0, 1], radar.position, radar.orientation + [[0], [0], @@ -209,7 +352,7 @@ def test_aesaradar(): timestamp=datetime.datetime.now()) radar = AESARadar(antenna_gain=30, - mapping=[0, 2, 4], + position_mapping=[0, 2, 4], position=StateVector([0.0] * 3), orientation=StateVector([0.0] * 3), frequency=100e6, @@ -304,7 +447,7 @@ def test_failed_detect(): timestamp=datetime.datetime.now()) radar = AESARadar(antenna_gain=30, - mapping=[0, 2, 4], + position_mapping=[0, 2, 4], position=StateVector([0.0] * 3), orientation=StateVector([0.0] * 3), frequency=100e6, @@ -334,7 +477,7 @@ def test_target_rcs(): rcs_20.rcs = 20 radar = AESARadar(antenna_gain=36, - mapping=[0, 1, 2], + position_mapping=[0, 1, 2], position=StateVector([0.0]*3), orientation=StateVector([0.0] * 3), frequency=10e9, 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))