Add nonlinear constant turn models. Update transition model class hierarchy

docs/demos/OpenSky_Demo.py

@@ -74,7 +74,7 @@
 from stonesoup.platform.base import FixedPlatform, MultiTransitionMovingPlatform
 from stonesoup.simulator.platform import PlatformDetectionSimulator
 from stonesoup.models.transition.linear import CombinedLinearGaussianTransitionModel,\
-    ConstantVelocity, ConstantTurn
+    ConstantVelocity, KnownTurnRate
 
 # Create locations for reference later
 
@@ -89,10 +89,10 @@
                                                                   ConstantVelocity(0.01),
                                                                   ConstantVelocity(0.01)))
 
-transition_modelLeft = CombinedLinearGaussianTransitionModel((ConstantTurn((0.01, 0.01),
+transition_modelLeft = CombinedLinearGaussianTransitionModel((KnownTurnRate((0.01, 0.01),
                                                               np.radians(3)), ConstantVelocity(0.01)))
 
-transition_modelRight = CombinedLinearGaussianTransitionModel((ConstantTurn((0.01,0.01),
+transition_modelRight = CombinedLinearGaussianTransitionModel((KnownTurnRate((0.01,0.01),
                                                                np.radians(-3)), ConstantVelocity(0.01)))
 
 # Create specific transition model for example moving platform

docs/examples/Moving_Platform_Simulation.py

@@ -210,7 +210,7 @@
 # seconds, and it will turn through 45 degrees over the course of the turn manoeuvre.
 
 # Import a Constant Turn model to enable target to perform basic manoeuvre
-from stonesoup.models.transition.linear import ConstantTurn
+from stonesoup.models.transition.linear import KnownTurnRate
 
 straight_level = CombinedLinearGaussianTransitionModel(
     [ConstantVelocity(0.), ConstantVelocity(0.), ConstantVelocity(0.)])
@@ -220,7 +220,7 @@
 
 turn_rate = np.pi/32  # specified in radians per seconds...
 
-turn_model = ConstantTurn(turn_noise_diff_coeffs=turn_noise_diff_coeffs, turn_rate=turn_rate)
+turn_model = KnownTurnRate(turn_noise_diff_coeffs=turn_noise_diff_coeffs, turn_rate=turn_rate)
 
 # Configure turn model to maintain current altitude
 turning = CombinedLinearGaussianTransitionModel(

docs/examples/Smooth_Platform_Coordinate_Transitions.py

@@ -105,11 +105,11 @@
 
 # %%
 # This gives the transition times/models:
-from stonesoup.models.transition.linear import ConstantTurn
+from stonesoup.models.transition.linear import KnownTurnRate
 for transition_time, transition_model in zip(transition_times, transition_models):
     print('Duration: ', transition_time.total_seconds(), 's ',
           'Model: ', type(transition_model), end=' ')
-    if isinstance(transition_model, ConstantTurn):
+    if isinstance(transition_model, KnownTurnRate):
         print('turn-rate: ', transition_model.turn_rate)
     else:
         print('x-acceleration: ', transition_model.ax, end=', ')

stonesoup/initiator/simple.py

@@ -5,7 +5,7 @@
 from ..base import Property
 from ..dataassociator import DataAssociator
 from ..deleter import Deleter
-from ..models.base import NonLinearModel, ReversibleModel
+from ..models.base import LinearModel, ReversibleModel
 from ..models.measurement import MeasurementModel
 from ..types.hypothesis import SingleHypothesis
 from ..types.numeric import Probability
@@ -96,7 +96,11 @@ def initiate(self, detections, timestamp, **kwargs):
             else:
                 measurement_model = self.measurement_model
 
-            if isinstance(measurement_model, NonLinearModel):
+            if isinstance(measurement_model, LinearModel):
+                model_matrix = measurement_model.matrix()
+                inv_model_matrix = np.linalg.pinv(model_matrix)
+                state_vector = inv_model_matrix @ detection.state_vector
+            else:
                 if isinstance(measurement_model, ReversibleModel):
                     try:
                         state_vector = measurement_model.inverse_function(
@@ -114,10 +118,6 @@ def initiate(self, detections, timestamp, **kwargs):
                 else:
                     raise Exception("Invalid measurement model used.\
                                     Must be instance of linear or reversible.")
-            else:
-                model_matrix = measurement_model.matrix()
-                inv_model_matrix = np.linalg.pinv(model_matrix)
-                state_vector = inv_model_matrix @ detection.state_vector
 
             model_covar = measurement_model.covar()
 

stonesoup/initiator/tests/test_simple.py

@@ -164,19 +164,27 @@ def test_nonlinear_measurement(meas_model, skip_non_linear):
 
 
 def test_linear_measurement_non_direct():
-    class _LinearMeasurementModel:
+    class _LinearMeasurementModel(LinearModel):
         ndim_state = 2
         ndmim_meas = 2
         mapping = (0, 1)
 
-        @staticmethod
-        def matrix():
+        def matrix(self):
             return np.array([[0, 1], [2, 0]])
 
         @staticmethod
         def covar():
             return np.diag([10, 50])
 
+        def ndim(self):
+            pass
+
+        def pdf(self):
+            pass
+
+        def rvs(slef):
+            pass
+
     measurement_model = _LinearMeasurementModel()
     measurement_initiator = SimpleMeasurementInitiator(
         GaussianState(np.array([[0], [0]]), np.diag([100, 10])),
@@ -211,20 +219,28 @@ def covar():
 
 
 def test_linear_measurement_extra_state_dim():
-    class _LinearMeasurementModel:
+    class _LinearMeasurementModel(LinearModel):
         ndim_state = 3
         ndmim_meas = 2
 
         mapping = (0, 2)
 
-        @staticmethod
-        def matrix():
+        def matrix(self):
             return np.array([[1, 0, 0], [0, 0, 1]])
 
         @staticmethod
         def covar():
             return np.diag([10, 50])
 
+        def ndim(self):
+            pass
+
+        def pdf(self):
+            pass
+
+        def rvs(self):
+            pass
+
     measurement_model = _LinearMeasurementModel()
     measurement_initiator = SimpleMeasurementInitiator(
         GaussianState(np.array([[0], [0], [0]]), np.diag([100, 10, 500])),

stonesoup/models/base.py

@@ -47,6 +47,23 @@ def function(self, state: State, noise: Union[bool, np.ndarray] = False,
         """
         raise NotImplementedError
 
+    def jacobian(self, state, **kwargs):
+        """Model jacobian matrix :math:`H_{jac}`
+
+        Parameters
+        ----------
+        state : :class:`~.State`
+            An input state
+
+        Returns
+        -------
+        :class:`numpy.ndarray` of shape (:py:attr:`~ndim_meas`, \
+        :py:attr:`~ndim_state`)
+            The model jacobian matrix evaluated around the given state vector.
+        """
+
+        return compute_jac(self.function, state, **kwargs)
+
     @abstractmethod
     def rvs(self, num_samples: int = 1, **kwargs) -> Union[StateVector, StateVectors]:
         r"""Model noise/sample generation function
@@ -140,32 +157,7 @@ def jacobian(self, state: State, **kwargs) -> np.ndarray:
         return self.matrix(**kwargs)
 
 
-class NonLinearModel(Model):
-    """NonLinearModel class
-
-    Base/Abstract class for all non-linear models"""
-
-    def jacobian(self, state: State, **kwargs) -> np.ndarray:
-        """Model Jacobian matrix :math:`H_{jac}`
-
-        Parameters
-        ----------
-        state : :class:`~.State`
-            An input state
-
-        Returns
-        -------
-        :class:`numpy.ndarray` of shape (attr:`~ndim_meas`, :attr:`~ndim_state`)
-            The model Jacobian matrix evaluated around the given state vector.
-        """
-
-        def fun(x):
-            return self.function(x, noise=False, **kwargs)
-
-        return compute_jac(fun, state)
-
-
-class ReversibleModel(NonLinearModel):
+class ReversibleModel(Model):
     """Non-linear model containing sufficient co-ordinate
     information such that the linear co-ordinate conversions
     can be calculated from the non-linear counterparts.

stonesoup/models/measurement/nonlinear.py

@@ -15,7 +15,7 @@
     rotx, roty, rotz
 from ...types.array import StateVector, CovarianceMatrix, StateVectors
 from ...types.angle import Bearing, Elevation
-from ..base import LinearModel, NonLinearModel, GaussianModel, ReversibleModel
+from ..base import LinearModel, GaussianModel, ReversibleModel
 from .base import MeasurementModel
 
 
@@ -95,7 +95,7 @@ def rvs(self, num_samples=1, **kwargs) -> Union[StateVector, StateVectors]:
             return rvs_vectors.view(StateVectors)
 
 
-class NonLinearGaussianMeasurement(MeasurementModel, NonLinearModel, GaussianModel, ABC):
+class NonLinearGaussianMeasurement(MeasurementModel, GaussianModel, ABC):
     r"""This class combines the MeasurementModel, NonLinearModel and \
     GaussianModel classes. It is not meant to be instantiated directly \
     but subclasses should be derived from this class.

stonesoup/models/transition/base.py

@@ -1,11 +1,14 @@
 # -*- coding: utf-8 -*-
 from abc import abstractmethod
+import copy
 from typing import Sequence
 
 from scipy.linalg import block_diag
+import numpy as np
 
 from ..base import Model, GaussianModel
 from ...base import Property
+from ...types.state import StateVector
 
 
 class TransitionModel(Model):
@@ -22,9 +25,80 @@ def ndim_state(self) -> int:
         pass
 
 
-class _CombinedGaussianTransitionModel(TransitionModel, GaussianModel):
+class CombinedGaussianTransitionModel(TransitionModel, GaussianModel):
+    r"""Combine multiple models into a single model by stacking them.
+
+    The assumption is that all models are Gaussian.
+    Time Variant, and Time Invariant models can be combined together.
+    If any of the models are time variant the keyword argument "time_interval"
+    must be supplied to all methods
+    """
     model_list: Sequence[GaussianModel] = Property(doc="List of Transition Models.")
 
+    def function(self, state, noise=False, **kwargs) -> StateVector:
+        """Applies each transition model in :py:attr:`~model_list` in turn to the state's
+        corresponding state vector components.
+        For example, in a 3D state space, with :py:attr:`~model_list` = [modelA(ndim_state=2),
+        modelB(ndim_state=1)], this would apply modelA to the state vector's 1st and 2nd elements,
+        then modelB to the remaining 3rd element.
+
+        Parameters
+        ----------
+        state : :class:`stonesoup.state.State`
+            The state to be transitioned according to the models in :py:attr:`~model_list`.
+
+        Returns
+        -------
+        state_vector: :class:`stonesoup.types.array.StateVector`
+            of shape (:py:attr:`~ndim_state, 1`). The resultant state vector of the transition.
+        """
+        temp_state = copy.copy(state)
+        ndim_count = 0
+        state_vector = np.zeros(state.state_vector.shape).view(StateVector)
+        # To handle explicit noise vector(s) passed in we set the noise for the individual models
+        # to False and add the noise later. When noise is Boolean, we just pass in that value.
+        if noise is None:
+            noise = False
+        if isinstance(noise, bool):
+            noise_loop = noise
+        else:
+            noise_loop = False
+        for model in self.model_list:
+            temp_state.state_vector =\
+                state.state_vector[ndim_count:model.ndim_state + ndim_count, :]
+            state_vector[ndim_count:model.ndim_state + ndim_count, :] += \
+                model.function(temp_state, noise=noise_loop, **kwargs)
+            ndim_count += model.ndim_state
+        if isinstance(noise, bool):
+            noise = 0
+        return state_vector + noise
+
+    def jacobian(self, state, **kwargs):
+        """Model jacobian matrix :math:`H_{jac}`
+
+        Parameters
+        ----------
+        state : :class:`~.State`
+            An input state
+
+        Returns
+        -------
+        :class:`numpy.ndarray` of shape (:py:attr:`~ndim_meas`, \
+        :py:attr:`~ndim_state`)
+            The model jacobian matrix evaluated around the given state vector.
+        """
+        temp_state = copy.copy(state)
+        ndim_count = 0
+        J_list = []
+        for model in self.model_list:
+            temp_state.state_vector =\
+                state.state_vector[ndim_count:model.ndim_state + ndim_count, :]
+            J_list.append(model.jacobian(temp_state, **kwargs))
+
+            ndim_count += model.ndim_state
+        out = block_diag(*J_list)
+        return out
+
     @property
     def ndim_state(self):
         """ndim_state getter method

stonesoup/models/transition/linear.py

@@ -7,7 +7,7 @@
 from scipy.integrate import quad
 from scipy.linalg import block_diag
 
-from .base import TransitionModel, _CombinedGaussianTransitionModel
+from .base import TransitionModel, CombinedGaussianTransitionModel
 from ..base import (LinearModel, GaussianModel, TimeVariantModel,
                     TimeInvariantModel)
 from ...base import Property
@@ -30,7 +30,7 @@ def ndim_state(self):
         return self.matrix().shape[0]
 
 
-class CombinedLinearGaussianTransitionModel(LinearModel, _CombinedGaussianTransitionModel):
+class CombinedLinearGaussianTransitionModel(LinearModel, CombinedGaussianTransitionModel):
     r"""Combine multiple models into a single model by stacking them.
 
     The assumption is that all models are Linear and Gaussian.
@@ -577,9 +577,9 @@ def covar(self, time_interval, **kwargs):
         return CovarianceMatrix(covar)
 
 
-class ConstantTurnSandwich(LinearGaussianTransitionModel, TimeVariantModel):
+class KnownTurnRateSandwich(LinearGaussianTransitionModel, TimeVariantModel):
     r"""This is a class implementation of a time-variant 2D Constant Turn
-    Model. This model is used, as opposed to the normal :class:`~.ConstantTurn`
+    Model. This model is used, as opposed to the normal :class:`~.KnownTurnRate`
     model, when the turn occurs in 2 dimensions that are not adjacent in the
     state vector, eg if the turn occurs in the x-z plane but the state vector
     is of the form :math:`(x,y,z)`. The list of transition models are to be
@@ -654,7 +654,7 @@ def covar(self, time_interval, **kwargs):
         return CovarianceMatrix(block_diag(ctc1, *covar_list, ctc2))
 
 
-class ConstantTurn(ConstantTurnSandwich):
+class KnownTurnRate(KnownTurnRateSandwich):
     r"""This is a class implementation of a discrete, time-variant 2D Constant
     Turn Model.