Add nonlinear constant turn models. Update transition model class hie…

…rachy.

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,26 @@ 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.
+        """
+
+        def fun(x, **kwargs):
+            return self.function(x, noise=False, **kwargs)
+
+        return compute_jac(fun, state, **kwargs)
+
     @abstractmethod
     def rvs(self, num_samples: int = 1, **kwargs) -> Union[StateVector, StateVectors]:
         r"""Model noise/sample generation function
@@ -140,32 +160,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,78 @@ 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((self.ndim_state, 1)).view(StateVector)
+        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.