From e25d9055310784fa0a680a37f240e32d331fdd6d Mon Sep 17 00:00:00 2001 From: David Kirkland Date: Sun, 26 Sep 2021 14:01:23 -0400 Subject: [PATCH 1/3] Add nonlinear constant turn models. Update transition model class hierachy. --- stonesoup/initiator/simple.py | 12 +- stonesoup/initiator/tests/test_simple.py | 28 ++- stonesoup/models/base.py | 44 ++-- stonesoup/models/measurement/nonlinear.py | 4 +- stonesoup/models/transition/base.py | 76 +++++- stonesoup/models/transition/linear.py | 4 +- stonesoup/models/transition/nonlinear.py | 233 +++++++++++++++--- stonesoup/models/transition/tests/conftest.py | 48 ++++ stonesoup/models/transition/tests/test_ca.py | 10 +- .../models/transition/tests/test_combined.py | 56 ++++- stonesoup/models/transition/tests/test_ct.py | 4 +- .../models/transition/tests/test_nlct.py | 68 +++++ .../models/transition/tests/test_nlcts.py | 88 +++++++ .../models/transition/tests/test_singer.py | 11 +- .../tests/test_singer_approximate.py | 11 +- stonesoup/plotter.py | 4 +- stonesoup/types/tests/test_array.py | 18 +- 17 files changed, 612 insertions(+), 107 deletions(-) create mode 100644 stonesoup/models/transition/tests/conftest.py create mode 100644 stonesoup/models/transition/tests/test_nlct.py create mode 100644 stonesoup/models/transition/tests/test_nlcts.py diff --git a/stonesoup/initiator/simple.py b/stonesoup/initiator/simple.py index 5d7936bd6..9b02306ce 100644 --- a/stonesoup/initiator/simple.py +++ b/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() diff --git a/stonesoup/initiator/tests/test_simple.py b/stonesoup/initiator/tests/test_simple.py index 079a71539..12bd71a0c 100644 --- a/stonesoup/initiator/tests/test_simple.py +++ b/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])), diff --git a/stonesoup/models/base.py b/stonesoup/models/base.py index 87a5a4468..b4b1bd8c1 100644 --- a/stonesoup/models/base.py +++ b/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. diff --git a/stonesoup/models/measurement/nonlinear.py b/stonesoup/models/measurement/nonlinear.py index 4eb7b6c7a..6fd26b409 100644 --- a/stonesoup/models/measurement/nonlinear.py +++ b/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. diff --git a/stonesoup/models/transition/base.py b/stonesoup/models/transition/base.py index eec77a9c0..0eefc8a6a 100644 --- a/stonesoup/models/transition/base.py +++ b/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 diff --git a/stonesoup/models/transition/linear.py b/stonesoup/models/transition/linear.py index 718fcff74..3735c4e15 100644 --- a/stonesoup/models/transition/linear.py +++ b/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. diff --git a/stonesoup/models/transition/nonlinear.py b/stonesoup/models/transition/nonlinear.py index 3f1c2aec1..b294238fa 100644 --- a/stonesoup/models/transition/nonlinear.py +++ b/stonesoup/models/transition/nonlinear.py @@ -1,46 +1,215 @@ - import copy - +from typing import Sequence import numpy as np +from scipy.linalg import block_diag + +from ...types.array import StateVector, StateVectors +from .base import TransitionModel +from ..base import GaussianModel, TimeVariantModel +from ...base import Property +from ...types.array import CovarianceMatrix + + +class GaussianTransitionModel(TransitionModel, GaussianModel): + pass + + +class ConstantTurn(GaussianTransitionModel, TimeVariantModel): + r"""This is a class implementation of a discrete, time-variant 2D Constant + Turn Model. + + The target is assumed to move with (nearly) constant velocity and also + an unknown (nearly) constant turn rate. + + The model is described by the following SDEs: + + .. math:: + :nowrap: -from ...types.state import StateVector -from .base import _CombinedGaussianTransitionModel -from ..base import NonLinearModel + \begin{align} + dx_{pos} & = x_{vel} d \quad | {Position \ on \ + X-axis (m)} \\ + dx_{vel} & = -\omega y_{pos} d \quad | {Speed \ + on\ X-axis (m/s)} &\\ + dy_{pos} & = y_{vel} d \quad | {Position \ on \ + Y-axis (m)} \\ + dy_{vel} & = \omega x_{pos} d \quad | {Speed \ + on\ Y-axis (m/s)} \\ + d\omega & = q_\omega dt \quad | {Position \ on \ X,Y-axes (rad/sec)} + \end{align} + Or equivalently: -class CombinedNonlinearGaussianTransitionModel(_CombinedGaussianTransitionModel, NonLinearModel): - r"""Combine multiple models into a single model by stacking them. + .. math:: + x_t = F_t x_{t-1} + w_t,\ w_t \sim \mathcal{N}(0,Q_t) - 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 + where: + + .. math:: + x & = & \begin{bmatrix} + x_{pos} \\ + x_{vel} \\ + y_{pos} \\ + y_{vel} \\ + \omega + \end{bmatrix} + + .. math:: + F(x) & = & \begin{bmatrix} + x+ \frac{x_{vel}}{\omega}\sin\omega dt - + \frac{y_{vel}}{\omega}(1-\cos\omega dt) \\ + x_{vel}\cos\omega dt - y_{vel}\sin\omega dt \\ + y+ \frac{v_{vel}}{\omega}\sin\omega dt + + \frac{x_{vel}}{\omega}(1-\cos\omega dt) \\ + x_{vel}\sin\omega dt + y_{vel}\sin\omega dt \\ + \omega + \end{bmatrix} + + .. math:: + Q_t & = & \begin{bmatrix} + \frac{dt^4q_x^2}{4} & \frac{dt^3q_x^2}{2} & \frac{dt^4q_xq_y}{4} & + \frac{dt^3q_xq_y}{2} & \frac{dt^2q_xq_\omega}{2} \\ + \frac{dt^3q_x^2}{2} & dt^2q_x^2 & \frac{dt^3q_xq_y}{2} & dt^2q_xq_y & + dt q_x q_\omega \\ + \frac{dt^4q_xq_y}{4} & \frac{dt^3q_xq_y}{2} & \frac{dt^4q_y^2}{4} & + \frac{dt^3q_y^2}{2} & \frac{dt^2q_y q_\omega}{2} \\ + \frac{dt^3q_x q_y}{2} & dt^2q_xq_y & \frac{dt^3q_y^2}{2} & + dt^2q_y^2 & dt q_y q_\omega \\ + \frac{dt^2q_xq_\omega}{2} & dtq_xq_\omega & \frac{dt^2q_yq_\omega}{2} & + dt q_y q_\omega & q_\omega^2 + \end{bmatrix} """ + linear_noise_coeffs: np.ndarray = Property( + doc=r"The acceleration noise diffusion coefficients :math:`[q_x, \: q_y]^T`") + turn_noise_coeff: float = Property( + doc=r"The turn rate noise coefficient :math:`q_\omega`") + + @property + def ndim_state(self): + """ndim_state getter method + + Returns + ------- + : :class:`int` + The number of combined model state dimensions. + """ + return 5 + + def function(self, state, noise=False, **kwargs) -> StateVector: + time_interval_sec = kwargs['time_interval'].total_seconds() + sv1 = state.state_vector + turn_rate = sv1[4, :] + # Avoid divide by zero in the function evaluation + turn_rate[turn_rate == 0.] = np.finfo(float).eps + dAngle = turn_rate * time_interval_sec + cos_dAngle = np.cos(dAngle) + sin_dAngle = np.sin(dAngle) + sv2 = StateVectors( + [sv1[0, :] + sin_dAngle/turn_rate * sv1[1, :] - sv1[3, :] / turn_rate * + (1. - cos_dAngle), + sv1[1, :] * cos_dAngle - sv1[3, :] * sin_dAngle, + sv1[1, :] / turn_rate * (1. - cos_dAngle) + sv1[2, :] + sv1[3, :] * sin_dAngle + / turn_rate, + sv1[1, :] * sin_dAngle + sv1[3, :] * cos_dAngle, + turn_rate]) + if isinstance(noise, bool) or noise is None: + if noise: + noise = self.rvs(**kwargs) + else: + noise = 0 + return sv2 + noise - def function(self, state, **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. + def covar(self, time_interval, **kwargs): + """Returns the transition model noise covariance matrix. - Parameters - ---------- - state : :class:`stonesoup.state.State` - The state to be transitioned according to the models in :py:attr:`~model_list`. + Returns + ------- + : :class:`stonesoup.types.state.CovarianceMatrix` of shape\ + (:py:attr:`~ndim_state`, :py:attr:`~ndim_state`) + The process noise covariance. + """ + q_x, q_y = self.linear_noise_coeffs + q = self.turn_noise_coeff + dt = time_interval.total_seconds() + + Q = np.array([[dt**3 / 3., dt**2 / 2.], + [dt**2 / 2., dt]]) + C = block_diag(Q*q_x**2, Q*q_y**2, q**2/dt) + + return CovarianceMatrix(C) + + +class ConstantTurnSandwich(ConstantTurn): + 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, 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 + applied to any state variables that lie in between, eg if for the above + example you wanted the y component to move with constant velocity, you + would put a :class:`~.ConstantVelocity` model in the list. + + The target is assumed to move with (nearly) constant velocity and also + unknown (nearly) constant turn rate. + """ + model_list: Sequence[GaussianTransitionModel] = Property( + doc="List of Transition Models.") + + @property + def ndim_state(self): + """ndim_state getter method Returns ------- - state_vector: :class:`stonesoup.types.array.StateVector` - of shape (:py:attr:`~ndim_state, 1`). The resultant state vector of the transition. + : :class:`int` + The number of combined model state dimensions. """ - temp_state = copy.copy(state) - ndim_count = 0 - state_vector = np.zeros((self.ndim_state, 1)).view(StateVector) + return sum(model.ndim_state for model in self.model_list) + 5 + + def function(self, state, noise=False, **kwargs) -> StateVector: + state_tmp = copy.copy(state) + sv_in = state.state_vector + sv1 = np.concatenate((sv_in[0:2, 0:], sv_in[-3:, 0:])) + state_tmp.state_vector = sv1 + # Calculate state vector for CT model + sv_ct = super().function(state_tmp, noise=False, **kwargs) + + # Calculate state vector for model list + idx1 = 2 + sv_list = [sv_ct[0:2, 0:]] 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, - **kwargs) - ndim_count += model.ndim_state - return state_vector + idx2 = idx1 + model.ndim + state_tmp.state_vector = sv_in[idx1:idx2, 0:] + sv_list.append(model.function(state_tmp, noise=False, **kwargs)) + idx1 = idx2 + sv_list.append(sv_ct[-3:, 0:]) + sv_out = StateVectors(np.concatenate(sv_list)) + if isinstance(noise, bool) or noise is None: + if noise: + noise = self.rvs(**kwargs) + else: + noise = 0 + return sv_out + noise + + def covar(self, time_interval, **kwargs): + """Returns the transition model noise covariance matrix. + + Returns + ------- + : :class:`stonesoup.types.state.CovarianceMatrix` of shape\ + (:py:attr:`~ndim_state`, :py:attr:`~ndim_state`) + The process noise covariance. + """ + C_t = np.zeros([self.ndim, self.ndim]) + C_ct = super().covar(time_interval, **kwargs) + covar_list = [model.covar(time_interval) for model in self.model_list] + + # Assemble diag block components + C_t[2:-3, 2:-3] = block_diag(*covar_list) + C_t[0:2, 0:2] = C_ct[0:2, 0:2] + C_t[-3:, -3:] = C_ct[-3:, -3:] + # Reorder offdiagonal elements + C_t[0:2:, -3:] = C_ct[0:2, -3:] + C_t[-3:, 0:2] = C_ct[-3:, 0:2] + + return CovarianceMatrix(C_t) diff --git a/stonesoup/models/transition/tests/conftest.py b/stonesoup/models/transition/tests/conftest.py new file mode 100644 index 000000000..709eb366f --- /dev/null +++ b/stonesoup/models/transition/tests/conftest.py @@ -0,0 +1,48 @@ +# -*- coding: utf-8 -*- +import pytest +import numpy as np +from scipy.linalg import block_diag + + +class CT_helper: + @staticmethod + def function(state, **kwargs): + time_interval_sec = kwargs['time_interval'].total_seconds() + sv1 = state.state_vector + turn_rate = sv1[4, :] + # Avoid divide by zero in the function evaluation + turn_rate[turn_rate == 0.] = np.finfo(float).eps + dAngle = turn_rate * time_interval_sec + sv2 = np.array( + [sv1[0] + np.sin(dAngle)/turn_rate * sv1[1] - sv1[3]/turn_rate*(1. - np.cos(dAngle)), + sv1[1] * np.cos(dAngle) - sv1[3] * np.sin(dAngle), + sv1[1]/turn_rate * (1. - np.cos(dAngle)) + sv1[2] + sv1[3]*np.sin(dAngle)/turn_rate, + sv1[1] * np.sin(dAngle) + sv1[3] * np.cos(dAngle), + turn_rate]) + + return sv2 + + @staticmethod + def covar(linear_noise_coeffs, turn_noise_coeff, time_interval): + """Returns the transition model noise covariance matrix. + + Returns + ------- + : :class:`stonesoup.types.state.CovarianceMatrix` of shape\ + (:py:attr:`~ndim_state`, :py:attr:`~ndim_state`) + The process noise covariance. + """ + q_x, q_y = linear_noise_coeffs + q = turn_noise_coeff + dt = time_interval.total_seconds() + + Q = np.array([[dt**3 / 3., dt**2 / 2.], + [dt**2 / 2., dt]]) + C = block_diag(Q*q_x**2, Q*q_y**2, q**2/dt) + + return C + + +@pytest.fixture +def CT_model(): + return CT_helper diff --git a/stonesoup/models/transition/tests/test_ca.py b/stonesoup/models/transition/tests/test_ca.py index 1a1f77bb4..de5cf809f 100644 --- a/stonesoup/models/transition/tests/test_ca.py +++ b/stonesoup/models/transition/tests/test_ca.py @@ -8,8 +8,8 @@ from scipy.stats import multivariate_normal from ....types.state import State -from ..linear import ( - ConstantAcceleration, CombinedLinearGaussianTransitionModel) +from ..linear import ConstantAcceleration +from ..base import CombinedGaussianTransitionModel def test_ca1dmodel(): @@ -47,7 +47,7 @@ def base(state_vec, noise_diff_coeffs): else: model_list = [ConstantAcceleration( noise_diff_coeff=noise_diff_coeffs[i]) for i in range(0, dim)] - model_obj = CombinedLinearGaussianTransitionModel(model_list) + model_obj = CombinedGaussianTransitionModel(model_list) # State related variables state_vec = state_vec @@ -78,8 +78,8 @@ def base(state_vec, noise_diff_coeffs): Q = sp.linalg.block_diag(*covar_list) # Ensure ```model_obj.transfer_function(time_interval)``` returns F - assert np.array_equal(F, model_obj.matrix( - timestamp=new_timestamp, time_interval=time_interval)) + assert F == approx(model_obj.jacobian(State(state_vec), + time_interval=time_interval)) # Ensure ```model_obj.covar(time_interval)``` returns Q assert np.array_equal(Q, model_obj.covar( diff --git a/stonesoup/models/transition/tests/test_combined.py b/stonesoup/models/transition/tests/test_combined.py index 4d1199131..0cb7e8a26 100644 --- a/stonesoup/models/transition/tests/test_combined.py +++ b/stonesoup/models/transition/tests/test_combined.py @@ -1,16 +1,22 @@ # coding: utf-8 +import pytest import datetime from numbers import Real import numpy as np -from ..linear import (LinearGaussianTimeInvariantTransitionModel, ConstantVelocity, - CombinedLinearGaussianTransitionModel) -from ..nonlinear import CombinedNonlinearGaussianTransitionModel +from ..linear import (LinearGaussianTimeInvariantTransitionModel, + CombinedLinearGaussianTransitionModel, + ConstantVelocity) +from ..nonlinear import ConstantTurn +from ..base import CombinedGaussianTransitionModel from ....types.state import State +from ....types.array import StateVectors -def test__linear_combined(): +@pytest.mark.parametrize("comb_model", [CombinedGaussianTransitionModel, + CombinedLinearGaussianTransitionModel]) +def test__linear_combined(comb_model): F = 3*np.eye(3) Q = 3*np.eye(3) model_1 = LinearGaussianTimeInvariantTransitionModel( @@ -21,19 +27,28 @@ def test__linear_combined(): DIM = 9 - combined_model = CombinedLinearGaussianTransitionModel( + combined_model = CombinedGaussianTransitionModel( [model_1, model_2, model_3, model_4]) t_delta = datetime.timedelta(0, 3) x_prior = np.ones([DIM, 1]) x_post = np.ones([DIM, 1]) + state = State(x_prior) + sv = state.state_vector + state.state_vector = StateVectors([sv, sv, sv]) + assert DIM == combined_model.ndim_state - assert (DIM, DIM) == combined_model.matrix(time_interval=t_delta).shape + assert (DIM, DIM) == combined_model.jacobian(State(x_prior), + time_interval=t_delta).shape assert (DIM, DIM) == combined_model.covar(time_interval=t_delta).shape assert (DIM, 1) == combined_model.function( State(x_prior), noise=np.random.randn(DIM, 1), time_interval=t_delta).shape + # Test vectorized handling i.e. multiple state vector inputs + assert state.state_vector.shape == combined_model.function( + state, + time_interval=t_delta).shape assert (DIM, 1) == combined_model.rvs(time_interval=t_delta).shape assert isinstance( combined_model.pdf(State(x_post), State(x_prior), @@ -41,30 +56,47 @@ def test__linear_combined(): # TODO: Should use non-linear transition models when these are implemented in Stone Soup. -def test_nonlinear_combined(): +@pytest.mark.parametrize( + "model_4", + [ConstantVelocity(noise_diff_coeff=1), + ConstantTurn(linear_noise_coeffs=[1.0, 1.0], turn_noise_coeff=0.1)]) +def test_nonlinear_combined(model_4): F = 3*np.eye(3) Q = 3*np.eye(3) model_1 = LinearGaussianTimeInvariantTransitionModel( transition_matrix=F, covariance_matrix=Q) model_2 = ConstantVelocity(noise_diff_coeff=3) model_3 = ConstantVelocity(noise_diff_coeff=1) - model_4 = ConstantVelocity(noise_diff_coeff=1) - - DIM = 9 + # model_4 = ConstantVelocity(noise_diff_coeff=1) + model_list = [model_1, model_2, model_3, model_4] + DIM = 0 + for model in model_list: + DIM += model.ndim_state - combined_model = CombinedNonlinearGaussianTransitionModel( - [model_1, model_2, model_3, model_4]) + combined_model = CombinedGaussianTransitionModel(model_list) t_delta = datetime.timedelta(0, 3) x_prior = np.ones([DIM, 1]) x_post = np.ones([DIM, 1]) + state = State(x_prior) + sv = state.state_vector + state.state_vector = StateVectors([sv, sv, sv]) + assert DIM == combined_model.ndim_state assert (DIM, DIM) == combined_model.covar(time_interval=t_delta).shape assert (DIM, 1) == combined_model.function( State(x_prior), noise=True, time_interval=t_delta).shape + # Test vectorized handling i.e. multiple state vector inputs + assert state.state_vector.shape == combined_model.function( + state, + time_interval=t_delta).shape assert (DIM, 1) == combined_model.rvs(time_interval=t_delta).shape + + # TODO: Figure out handling of pdf for non-linear models and singular convariance matrix + # e.g. See Non-Linear Constant Turn model + # if isinstance(model_4,LinearModel): assert isinstance( combined_model.pdf(State(x_post), State(x_prior), time_interval=t_delta), Real) diff --git a/stonesoup/models/transition/tests/test_ct.py b/stonesoup/models/transition/tests/test_ct.py index 15c3a95e2..c93d4920a 100644 --- a/stonesoup/models/transition/tests/test_ct.py +++ b/stonesoup/models/transition/tests/test_ct.py @@ -66,8 +66,8 @@ def base(model, state, turn_noise_diff_coeffs, turn_rate): qy * timediff]]) # Ensure ```model_obj.transfer_function(time_interval)``` returns F - assert np.array_equal(F, model_obj.matrix( - timestamp=new_timestamp, time_interval=time_interval)) + assert np.array_equal(F, model_obj.jacobian(State(state_vec), + time_interval=time_interval)) # Ensure ```model_obj.covar(time_interval)``` returns Q assert np.array_equal(Q, model_obj.covar( diff --git a/stonesoup/models/transition/tests/test_nlct.py b/stonesoup/models/transition/tests/test_nlct.py new file mode 100644 index 000000000..f47c705e2 --- /dev/null +++ b/stonesoup/models/transition/tests/test_nlct.py @@ -0,0 +1,68 @@ +# coding: utf-8 +import datetime + +import numpy as np +from ..nonlinear import ConstantTurn +from ....types.state import State + + +def test_ctmodel(CT_model): + """ ConstantTurn Transition Model test """ + state = State(np.array([[3.0], [1.0], [2.0], [1.0], [-0.05]])) + linear_noise_coeffs = np.array([0.1, 0.1]) + turn_noise_coeff = 0.01 + base(CT_model, ConstantTurn, state, linear_noise_coeffs, turn_noise_coeff) + + +def base(CT_model, model, state, linear_noise_coeffs, turn_noise_coeff): + """ Base test for n-dimensional ConstantAcceleration Transition Models """ + + # Create an ConstantTurn model object + model = model + model_obj = model(linear_noise_coeffs=linear_noise_coeffs, + turn_noise_coeff=turn_noise_coeff) + + # State related variables + old_timestamp = datetime.datetime.now() + timediff = 1 # 1sec + new_timestamp = old_timestamp + datetime.timedelta(seconds=timediff) + time_interval = datetime.timedelta(seconds=timediff) + + # Model-related components + F = CT_model.function(state, time_interval=time_interval) + + Q = CT_model.covar(linear_noise_coeffs, turn_noise_coeff, time_interval) + + # Ensure ```model_obj.covar(time_interval)``` returns Q + assert np.array_equal(Q, model_obj.covar( + timestamp=new_timestamp, time_interval=time_interval)) + + # Propagate a state vector through the mode (without noise) + new_state_vec_wo_noise = model_obj.function( + state, + timestamp=new_timestamp, + time_interval=time_interval) + + assert np.array_equal(new_state_vec_wo_noise, F) + + # Eliminated the pdf based tests since for nonlinear models these will no + # longer be Gaussian + + # Propagate a state vector throughout the model + # (with internal noise) + new_state_vec_w_inoise = model_obj.function( + state, + noise=True, + timestamp=new_timestamp, + time_interval=time_interval) + assert not np.array_equal(new_state_vec_w_inoise, F) + + # Propagate a state vector through the model + # (with external noise) + noise = model_obj.rvs(timestamp=new_timestamp, time_interval=time_interval) + new_state_vec_w_enoise = model_obj.function( + state, + timestamp=new_timestamp, + time_interval=time_interval, + noise=noise) + assert np.array_equal(new_state_vec_w_enoise, F + noise) diff --git a/stonesoup/models/transition/tests/test_nlcts.py b/stonesoup/models/transition/tests/test_nlcts.py new file mode 100644 index 000000000..a82c3f102 --- /dev/null +++ b/stonesoup/models/transition/tests/test_nlcts.py @@ -0,0 +1,88 @@ +# coding: utf-8 +import datetime +import copy + +import numpy as np + +from stonesoup.models.transition.nonlinear import ConstantTurnSandwich +from stonesoup.models.transition.base import CombinedGaussianTransitionModel +from stonesoup.models.transition.linear import ConstantVelocity, ConstantAcceleration +from stonesoup.types.state import State + + +def test_ctsmodel(CT_model): + """ ConstantTurn Transition Model test """ + state = State(np.array([[3.0], [1.0], [10.], [-1.], [-8.], [-1.], [0.5], [2.0], [1.0], + [-0.05]])) + linear_noise_coeffs = np.array([0.1, 0.1]) + turn_noise_coeff = 0.01 + base(CT_model, ConstantTurnSandwich, state, linear_noise_coeffs, turn_noise_coeff) + + +def base(ct_model, model, state, linear_noise_coeffs, turn_noise_coeff): + """ Base test for n-dimensional ConstantAcceleration Transition Models """ + cv_coeff = 1.1 + ca_coeff = 0.4 + + cv_model = ConstantVelocity(cv_coeff) + ca_model = ConstantAcceleration(ca_coeff) + + model_list = [cv_model, ca_model] + comb_model = CombinedGaussianTransitionModel(model_list=model_list) + # Create an ConstantTurnSandwich model object + model = model + model_obj = model(linear_noise_coeffs=linear_noise_coeffs, + turn_noise_coeff=turn_noise_coeff, model_list=model_list) + + # State related variables + old_timestamp = datetime.datetime.now() + timediff = 1 # 1sec + new_timestamp = old_timestamp + datetime.timedelta(seconds=timediff) + time_interval = datetime.timedelta(seconds=timediff) + state2 = copy.deepcopy(state) + + # Model-related components + # Calculate indices for CT component - needed to extract ct elements from the sandwich + idx = [0, 1] + tmp = state.state_vector.shape[0] - 3 + idx.extend(list(range(tmp, tmp + 3))) + + state2.state_vector = state.state_vector[idx, :] + state_out1 = ct_model.function(state2, time_interval=time_interval) + state2.state_vector = state.state_vector[2:-3, :] + state_out2 = comb_model.function(state2, time_interval=time_interval) + + # Propagate a state vector through the model (without noise) + model_out = model_obj.function(state, time_interval=time_interval) + assert np.array_equal(model_out[idx, :], state_out1) + assert np.array_equal(model_out[2:-3, :], state_out2) + + # Ensure ```model_obj.covar(time_interval)``` returns Q + # Only checking block diagonal components + Q1 = ct_model.covar(linear_noise_coeffs, turn_noise_coeff, time_interval) + Q2 = comb_model.covar(timestamp=new_timestamp, time_interval=time_interval) + model_Q = model_obj.covar(timestamp=new_timestamp, time_interval=time_interval) + assert np.array_equal(model_Q[0:2, 0:2], Q1[0:2, 0:2]) + assert np.array_equal(model_Q[-3:, -3:], Q1[-3:, -3:]) + assert np.array_equal(model_Q[2:-3, 2:-3], Q2) + + # Eliminated the pdf based tests since for nonlinear models these will no + # longer be Gaussian + + # Propagate a state vector throughout the model + # (with internal noise) + new_state_vec_w_inoise = model_obj.function( + state, + noise=True, + timestamp=new_timestamp, + time_interval=time_interval) + assert not np.array_equal(new_state_vec_w_inoise, model_out) + + # Propagate a state vector through the model (with external noise) + noise = model_obj.rvs(timestamp=new_timestamp, time_interval=time_interval) + new_state_vec_w_enoise = model_obj.function( + state, + timestamp=new_timestamp, + time_interval=time_interval, + noise=noise) + assert np.array_equal(new_state_vec_w_enoise, model_out + noise) diff --git a/stonesoup/models/transition/tests/test_singer.py b/stonesoup/models/transition/tests/test_singer.py index fa5b29473..c5e9cfb79 100644 --- a/stonesoup/models/transition/tests/test_singer.py +++ b/stonesoup/models/transition/tests/test_singer.py @@ -6,7 +6,8 @@ import scipy as sp from scipy.stats import multivariate_normal -from ..linear import Singer, CombinedLinearGaussianTransitionModel +from ..linear import Singer +from ..base import CombinedGaussianTransitionModel from ....types.state import State @@ -42,7 +43,7 @@ def base(state, noise_diff_coeffs, damping_coeffs, timediff=1.0): state_vec = state.state_vector # Create a 1D Singer or an n-dimensional - # CombinedLinearGaussianTransitionModel object + # CombinedGaussianTransitionModel object dim = len(state_vec) // 3 # pos, vel, acc for each dimension if dim == 1: model_obj = Singer(noise_diff_coeff=noise_diff_coeffs[0], @@ -51,7 +52,7 @@ def base(state, noise_diff_coeffs, damping_coeffs, timediff=1.0): model_list = [Singer(noise_diff_coeff=noise_diff_coeffs[i], damping_coeff=damping_coeffs[i]) for i in range(0, dim)] - model_obj = CombinedLinearGaussianTransitionModel(model_list) + model_obj = CombinedGaussianTransitionModel(model_list) # State related variables state_vec = state_vec @@ -112,8 +113,8 @@ def base(state, noise_diff_coeffs, damping_coeffs, timediff=1.0): Q = sp.linalg.block_diag(*covar_list) # Ensure ```model_obj.transfer_function(time_interval)``` returns F - assert np.allclose(F, model_obj.matrix( - timestamp=new_timestamp, time_interval=time_interval), rtol=1e-6) + assert np.allclose(F, model_obj.jacobian( + State(state_vec), time_interval=time_interval), rtol=1e-6) # Ensure ```model_obj.covar(time_interval)``` returns Q assert np.allclose(Q, model_obj.covar( diff --git a/stonesoup/models/transition/tests/test_singer_approximate.py b/stonesoup/models/transition/tests/test_singer_approximate.py index b02be3c13..91935188c 100644 --- a/stonesoup/models/transition/tests/test_singer_approximate.py +++ b/stonesoup/models/transition/tests/test_singer_approximate.py @@ -6,7 +6,8 @@ import scipy as sp from scipy.stats import multivariate_normal -from ..linear import SingerApproximate, CombinedLinearGaussianTransitionModel +from ..linear import SingerApproximate +from ..base import CombinedGaussianTransitionModel from ....types.state import State @@ -42,7 +43,7 @@ def base(state, noise_diff_coeffs, damping_coeffs, timediff=1.0): state_vec = state.state_vector # Create a 1D Singer or an n-dimensional - # CombinedLinearGaussianTransitionModel object + # CombinedGaussianTransitionModel object dim = len(state_vec) // 3 # pos, vel, acc for each dimension if dim == 1: model_obj = SingerApproximate(noise_diff_coeff=noise_diff_coeffs[0], @@ -54,7 +55,7 @@ def base(state, noise_diff_coeffs, damping_coeffs, timediff=1.0): damping_coeff=damping_coeffs[i]) for i in range(0, dim) ] - model_obj = CombinedLinearGaussianTransitionModel(model_list) + model_obj = CombinedGaussianTransitionModel(model_list) # State related variables state_vec = state_vec @@ -99,8 +100,8 @@ def base(state, noise_diff_coeffs, damping_coeffs, timediff=1.0): Q = sp.linalg.block_diag(*covar_list) # Ensure ```model_obj.transfer_function(time_interval)``` returns F - assert np.allclose(F, model_obj.matrix( - timestamp=new_timestamp, time_interval=time_interval), rtol=1e-10) + assert np.allclose(F, model_obj.jacobian( + State(state_vec), time_interval=time_interval), rtol=1e-6) # Ensure ```model_obj.covar(time_interval)``` returns Q assert np.allclose(Q, model_obj.covar( diff --git a/stonesoup/plotter.py b/stonesoup/plotter.py index dbfb6eb66..0b6e4d645 100644 --- a/stonesoup/plotter.py +++ b/stonesoup/plotter.py @@ -8,7 +8,7 @@ from matplotlib.legend_handler import HandlerPatch from .types import detection -from .models.base import LinearModel, NonLinearModel +from .models.base import LinearModel, Model class Plotter: @@ -127,7 +127,7 @@ def plot_measurements(self, measurements, mapping, measurement_model=None, inv_model_matrix = np.linalg.pinv(model_matrix) state_vec = inv_model_matrix @ state.state_vector - elif isinstance(meas_model, NonLinearModel): + elif isinstance(meas_model, Model): try: state_vec = meas_model.inverse_function(state) except (NotImplementedError, AttributeError): diff --git a/stonesoup/types/tests/test_array.py b/stonesoup/types/tests/test_array.py index 276c08cc2..73689b622 100644 --- a/stonesoup/types/tests/test_array.py +++ b/stonesoup/types/tests/test_array.py @@ -2,7 +2,7 @@ import numpy as np import pytest -from ..array import Matrix, StateVector, CovarianceMatrix, PrecisionMatrix +from ..array import Matrix, StateVector, StateVectors, CovarianceMatrix, PrecisionMatrix def test_statevector(): @@ -21,6 +21,22 @@ def test_statevector(): assert np.array_equal(StateVector(state_vector_array), state_vector) +def test_statevectors(): + vec1 = np.array([[1.], [2.], [3.]]) + vec2 = np.array([[2.], [3.], [4.]]) + + sv1 = StateVector(vec1) + sv2 = StateVector(vec2) + + vecs1 = np.concatenate((vec1, vec2), axis=1) + svs1 = StateVectors([sv1, sv2]) + svs2 = StateVectors(vecs1) + svs3 = StateVectors([vec1, vec2]) # Creates 3dim array + assert np.array_equal(svs1, vecs1) + assert np.array_equal(svs2, vecs1) + assert svs3.shape != vecs1.shape + + def test_standard_statevector_indexing(): state_vector_array = np.array([[1], [2], [3], [4]]) state_vector = StateVector(state_vector_array) From c8077bbe2457b7b78518208816c28dd0ae509f93 Mon Sep 17 00:00:00 2001 From: David Kirkland Date: Tue, 1 Feb 2022 13:24:20 -0500 Subject: [PATCH 2/3] Rename Linear.ConstantTurn model to KnownTurnRate model. --- stonesoup/models/transition/linear.py | 6 +++--- stonesoup/models/transition/tests/test_ct.py | 8 ++++---- stonesoup/models/transition/tests/test_cts.py | 7 ++++--- stonesoup/movable/tests/test_movable.py | 6 +++--- stonesoup/platform/tests/test_platform_base.py | 6 +++--- stonesoup/simulator/transition.py | 8 ++++---- 6 files changed, 21 insertions(+), 20 deletions(-) diff --git a/stonesoup/models/transition/linear.py b/stonesoup/models/transition/linear.py index 3735c4e15..4904ec749 100644 --- a/stonesoup/models/transition/linear.py +++ b/stonesoup/models/transition/linear.py @@ -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. diff --git a/stonesoup/models/transition/tests/test_ct.py b/stonesoup/models/transition/tests/test_ct.py index c93d4920a..4cb966e23 100644 --- a/stonesoup/models/transition/tests/test_ct.py +++ b/stonesoup/models/transition/tests/test_ct.py @@ -5,22 +5,22 @@ import numpy as np from scipy.stats import multivariate_normal -from ..linear import ConstantTurn +from ..linear import KnownTurnRate from ....types.state import State def test_ctmodel(): - """ ConstantTurn Transition Model test """ + """ KnownTurnRate Transition Model test """ state = State(np.array([[3.0], [1.0], [2.0], [1.0]])) turn_noise_diff_coeffs = np.array([0.01, 0.01]) turn_rate = 0.1 - base(ConstantTurn, state, turn_noise_diff_coeffs, turn_rate) + base(KnownTurnRate, state, turn_noise_diff_coeffs, turn_rate) def base(model, state, turn_noise_diff_coeffs, turn_rate): """ Base test for n-dimensional ConstantAcceleration Transition Models """ - # Create an ConstantTurn model object + # Create an KnownTurnRate model object model = model model_obj = model(turn_noise_diff_coeffs=turn_noise_diff_coeffs, turn_rate=turn_rate) diff --git a/stonesoup/models/transition/tests/test_cts.py b/stonesoup/models/transition/tests/test_cts.py index f1fe35539..15f7b6162 100644 --- a/stonesoup/models/transition/tests/test_cts.py +++ b/stonesoup/models/transition/tests/test_cts.py @@ -5,18 +5,19 @@ import numpy as np from scipy.stats import multivariate_normal -from ..linear import ConstantTurnSandwich, ConstantVelocity +from ..linear import KnownTurnRateSandwich +from ..linear import ConstantVelocity from ....types.state import State def test_ctmodel(): - """ ConstantTurnSandwich Transition Model test """ + """ KnownTurnRateSandwich Transition Model test """ state = State(np.array([[3.0], [1.0], [2.0], [1.0], [1.0], [1.0]])) turn_noise_diff_coeffs = np.array([0.01, 0.01]) turn_rate = 0.1 noise_diff_cv = 0.1 model_list = [ConstantVelocity(noise_diff_cv)] - base(ConstantTurnSandwich, state, turn_noise_diff_coeffs, turn_rate, + base(KnownTurnRateSandwich, state, turn_noise_diff_coeffs, turn_rate, noise_diff_cv, model_list) diff --git a/stonesoup/movable/tests/test_movable.py b/stonesoup/movable/tests/test_movable.py index 3bd43388c..f09e19dbd 100644 --- a/stonesoup/movable/tests/test_movable.py +++ b/stonesoup/movable/tests/test_movable.py @@ -3,7 +3,7 @@ import numpy as np import pytest -from stonesoup.models.transition.linear import ConstantVelocity, ConstantTurn, \ +from stonesoup.models.transition.linear import ConstantVelocity, KnownTurnRate, \ CombinedLinearGaussianTransitionModel from stonesoup.movable import MovingMovable, FixedMovable, MultiTransitionMovable from stonesoup.types.array import StateVector @@ -47,7 +47,7 @@ def test_empty_state_error(): def test_multi_transition_movable_errors(): # First check no error - models = [ConstantVelocity(0), ConstantTurn(0, np.pi / 2)] + models = [ConstantVelocity(0), KnownTurnRate(0, np.pi / 2)] now = datetime.now() times = [timedelta(seconds=10), timedelta(seconds=10)] _ = MultiTransitionMovable(states=State(StateVector([0, 0, 0, 0, 0, 0])), @@ -80,7 +80,7 @@ def test_multi_transition_movable_move(): input_state_vector = StateVector([0, 1, 2.2, 78.6]) pre_state = State(input_state_vector, timestamp=None) models = [CombinedLinearGaussianTransitionModel((ConstantVelocity(0), ConstantVelocity(0))), - ConstantTurn([0, 0], turn_rate=np.pi / 2)] + KnownTurnRate([0, 0], turn_rate=np.pi / 2)] times = [timedelta(seconds=10), timedelta(seconds=10)] movable = MultiTransitionMovable(states=pre_state, diff --git a/stonesoup/platform/tests/test_platform_base.py b/stonesoup/platform/tests/test_platform_base.py index 436e46b64..2aebb7e45 100644 --- a/stonesoup/platform/tests/test_platform_base.py +++ b/stonesoup/platform/tests/test_platform_base.py @@ -6,7 +6,7 @@ from stonesoup.models.measurement.tests.test_models import position_measurement_sets from stonesoup.models.transition.linear import CombinedLinearGaussianTransitionModel, \ - ConstantVelocity, ConstantTurn + ConstantVelocity, KnownTurnRate from stonesoup.movable import FixedMovable, MovingMovable from stonesoup.sensor.sensor import Sensor from stonesoup.types.array import StateVector @@ -522,7 +522,7 @@ def test_mapping_types(mapping_type): def test_multi_transition(): transition_model1 = CombinedLinearGaussianTransitionModel( (ConstantVelocity(0), ConstantVelocity(0))) - transition_model2 = ConstantTurn((0, 0), np.radians(4.5)) + transition_model2 = KnownTurnRate((0, 0), np.radians(4.5)) transition_models = [transition_model1, transition_model2] transition_times = [datetime.timedelta(seconds=10), datetime.timedelta(seconds=20)] @@ -597,7 +597,7 @@ def test_multi_transition(): assert platform.transition_index == 1 # Add new transition model (right-turn) to list - transition_model3 = ConstantTurn((0, 0), np.radians(-9)) + transition_model3 = KnownTurnRate((0, 0), np.radians(-9)) platform.transition_models.append(transition_model3) platform.transition_times.append(datetime.timedelta(seconds=10)) diff --git a/stonesoup/simulator/transition.py b/stonesoup/simulator/transition.py index 9c1fcdc13..5dd54db43 100644 --- a/stonesoup/simulator/transition.py +++ b/stonesoup/simulator/transition.py @@ -8,7 +8,7 @@ from ..base import Property from ..models.transition.base import TransitionModel -from ..models.transition.linear import ConstantTurn, ConstantVelocity, \ +from ..models.transition.linear import KnownTurnRate, ConstantVelocity, \ CombinedLinearGaussianTransitionModel from ..types.array import StateVector from ..types.state import State @@ -38,8 +38,8 @@ def create_smooth_transition_models(initial_state, x_coords, y_coords, times, tu Returns ------- transition_models: - A list of :class:`~.ConstantTurn` and :class:`~.Point2PointConstantAcceleration` transition - models. + A list of :class:`~.KnownTurnRate` and :class:`~.Point2PointConstantAcceleration` + transition models. transition_times: A list of :class:`~.datetime.timedelta` dictating the transition time for each corresponding transition model in transition_models. @@ -117,7 +117,7 @@ def create_smooth_transition_models(initial_state, x_coords, y_coords, times, tu if t1 > 0: # make turn model and add to list - turn_model = ConstantTurn(turn_noise_diff_coeffs=(0, 0), turn_rate=w) + turn_model = KnownTurnRate(turn_noise_diff_coeffs=(0, 0), turn_rate=w) state.state_vector = turn_model.function(state=state, time_interval=timedelta( seconds=t1)) # move platform through turn state.timestamp += timedelta(seconds=t1) From 16b988b3bbd2bf4d4fdcf2478405ca14ddd6690e Mon Sep 17 00:00:00 2001 From: Steven Hiscocks Date: Wed, 2 Feb 2022 10:23:47 +0000 Subject: [PATCH 3/3] Change ConstantTurn to KnownTurnRate models in demos/examples --- docs/demos/OpenSky_Demo.py | 6 +++--- docs/examples/Moving_Platform_Simulation.py | 4 ++-- docs/examples/Smooth_Platform_Coordinate_Transitions.py | 4 ++-- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/docs/demos/OpenSky_Demo.py b/docs/demos/OpenSky_Demo.py index e23558aa2..f8cc3a6af 100644 --- a/docs/demos/OpenSky_Demo.py +++ b/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 diff --git a/docs/examples/Moving_Platform_Simulation.py b/docs/examples/Moving_Platform_Simulation.py index 79085a6c0..1ab03caa8 100644 --- a/docs/examples/Moving_Platform_Simulation.py +++ b/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( diff --git a/docs/examples/Smooth_Platform_Coordinate_Transitions.py b/docs/examples/Smooth_Platform_Coordinate_Transitions.py index c67955f9e..fb9b958be 100644 --- a/docs/examples/Smooth_Platform_Coordinate_Transitions.py +++ b/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=', ')