Source code for stonesoup.models.transition.linear

# -*- coding: utf-8 -*-
import math
from typing import Sequence
from functools import lru_cache

import numpy as np
from scipy.integrate import quad
from scipy.linalg import block_diag

from .base import TransitionModel, _CombinedGaussianTransitionModel
from ..base import (LinearModel, GaussianModel, TimeVariantModel,
                    TimeInvariantModel)
from ...base import Property
from ...types.array import CovarianceMatrix


[docs]class LinearGaussianTransitionModel( TransitionModel, LinearModel, GaussianModel): @property def ndim_state(self): """ndim_state getter method Returns ------- : :class:`int` The number of model state dimensions. """ return self.matrix().shape[0]
[docs]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. 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 """
[docs] def matrix(self, **kwargs): """Model matrix :math:`F` Returns ------- : :class:`numpy.ndarray` of shape\ (:py:attr:`~ndim_state`, :py:attr:`~ndim_state`) """ transition_matrices = [ model.matrix(**kwargs) for model in self.model_list] return block_diag(*transition_matrices)
[docs]class LinearGaussianTimeInvariantTransitionModel(LinearGaussianTransitionModel, TimeInvariantModel): r"""Generic Linear Gaussian Time Invariant Transition Model.""" transition_matrix: np.ndarray = Property(doc="Transition matrix :math:`\\mathbf{F}`.") control_matrix: np.ndarray = Property( default=None, doc="Control matrix :math:`\\mathbf{B}`.") covariance_matrix: CovarianceMatrix = Property( doc="Transition noise covariance matrix :math:`\\mathbf{Q}`.")
[docs] def matrix(self, **kwargs): """Model matrix :math:`F` Returns ------- : :class:`numpy.ndarray` of shape\ (:py:attr:`~ndim_state`, :py:attr:`~ndim_state`) The model matrix evaluated given the provided time interval. """ return self.transition_matrix
[docs] def covar(self, **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. """ return self.covariance_matrix
[docs]class ConstantNthDerivative(LinearGaussianTransitionModel, TimeVariantModel): r"""Discrete model based on the Nth derivative with respect to time being constant, to set derivative use keyword argument :attr:`constant_derivative` The model is described by the following SDEs: .. math:: :nowrap: \begin{eqnarray} dx^{(N-1)} & = & x^{(N)} dt & | {(N-1)th \ derivative \ on \ X-axis (m)} \\ dx^{(N)} & = & q\cdot dW_t,\ W_t \sim \mathcal{N}(0,q^2) & | \ Nth\ derivative\ on\ X-axis (m/s^{N}) \end{eqnarray} It is hard to represent the matrix form of these due to the fact that they vary with N, examples for N=1 and N=2 can be found in the :class:`~.ConstantVelocity` and :class:`~.ConstantAcceleration` models respectively. To aid visualisation of :math:`F_t` the elements are calculated as the terms of the taylor expansion of each state variable. """ constant_derivative: int = Property( doc="The order of the derivative with respect to time to be kept constant, eg if 2 " "identical to constant acceleration") noise_diff_coeff: float = Property( doc="The Nth derivative noise diffusion coefficient (Variance) :math:`q`") @property def ndim_state(self): return self.constant_derivative + 1
[docs] def matrix(self, time_interval, **kwargs): time_interval_sec = time_interval.total_seconds() N = self.constant_derivative Fmat = np.zeros((N + 1, N + 1)) dt = time_interval_sec for i in range(0, N + 1): for j in range(i, N + 1): Fmat[i, j] = (dt ** (j - i)) / math.factorial(j - i) return Fmat
[docs] def covar(self, time_interval, **kwargs): time_interval_sec = time_interval.total_seconds() dt = time_interval_sec N = self.constant_derivative if N == 1: covar = np.array([[dt**3 / 3, dt**2 / 2], [dt**2 / 2, dt]]) else: Fmat = self.matrix(time_interval, **kwargs) Q = np.zeros((N + 1, N + 1)) Q[N, N] = 1 igrand = Fmat @ Q @ Fmat.T covar = np.zeros((N + 1, N + 1)) for l in range(0, N + 1): # noqa: E741 for k in range(0, N + 1): covar[l, k] = (igrand[l, k]*dt / (1 + N**2 - l - k)) covar *= self.noise_diff_coeff return CovarianceMatrix(covar)
[docs]class RandomWalk(ConstantNthDerivative): r"""This is a class implementation of a discrete, time-variant 1D Linear-Gaussian Random Walk Transition Model. The target is assumed to be (almost) stationary, where target velocity is modelled as white noise. """ noise_diff_coeff: float = Property(doc="The position noise diffusion coefficient :math:`q`") @property def constant_derivative(self): """For random walk, this is 0.""" return 0
[docs]class ConstantVelocity(ConstantNthDerivative): r"""This is a class implementation of a discrete, time-variant 1D Linear-Gaussian Constant Velocity Transition Model. The target is assumed to move with (nearly) constant velocity, where target acceleration is modelled as white noise. The model is described by the following SDEs: .. math:: :nowrap: \begin{eqnarray} dx_{pos} & = & x_{vel} d & | {Position \ on \ X-axis (m)} \\ dx_{vel} & = & q\cdot dW_t,\ W_t \sim \mathcal{N}(0,q^2) & | \ Speed on\ X-axis (m/s) \end{eqnarray} Or equivalently: .. math:: x_t = F_t x_{t-1} + w_t,\ w_t \sim \mathcal{N}(0,Q_t) where: .. math:: x & = & \begin{bmatrix} x_{pos} \\ x_{vel} \end{bmatrix} .. math:: F_t & = & \begin{bmatrix} 1 & dt\\ 0 & 1 \end{bmatrix} .. math:: Q_t & = & \begin{bmatrix} \frac{dt^3}{3} & \frac{dt^2}{2} \\ \frac{dt^2}{2} & dt \end{bmatrix} q """ noise_diff_coeff: float = Property(doc="The velocity noise diffusion coefficient :math:`q`") @property def constant_derivative(self): """For constant velocity, this is 1.""" return 1
[docs]class ConstantAcceleration(ConstantNthDerivative): r"""This is a class implementation of a discrete, time-variant 1D Constant Acceleration Transition Model. The target acceleration is modeled as a zero-mean white noise random process. The model is described by the following SDEs: .. math:: :nowrap: \begin{eqnarray} dx_{pos} & = & x_{vel} d & | {Position \ on \ X-axis (m)} \\ dx_{vel} & = & x_{acc} d & | {Speed \ on\ X-axis (m/s)} \\ dx_{acc} & = & q W_t,\ W_t \sim \mathcal{N}(0,q^2) & | {Acceleration \ on \ X-axis (m^2/s)} \end{eqnarray} Or equivalently: .. math:: x_t = F_t x_{t-1} + w_t,\ w_t \sim \mathcal{N}(0,Q_t) where: .. math:: x & = & \begin{bmatrix} x_{pos} \\ x_{vel} \\ x_{acc} \end{bmatrix} .. math:: F_t & = & \begin{bmatrix} 1 & dt & \frac{dt^2}{2} \\ 0 & 1 & dt \\ 0 & 0 & 1 \end{bmatrix} .. math:: Q_t & = & \begin{bmatrix} \frac{dt^5}{20} & \frac{dt^4}{8} & \frac{dt^3}{6} \\ \frac{dt^4}{8} & \frac{dt^3}{3} & \frac{dt^2}{2} \\ \frac{dt^3}{6} & \frac{dt^2}{2} & dt \end{bmatrix} q """ noise_diff_coeff: float = Property( doc="The acceleration noise diffusion coefficient :math:`q`") @property def constant_derivative(self): """For constant acceleration, this is 2.""" return 2
[docs]class NthDerivativeDecay(LinearGaussianTransitionModel, TimeVariantModel): r"""Discrete model based on the Nth derivative with respect to time decaying to 0 exponentially, to set derivative use keyword argument :attr:`decay_derivative` The model is described by the following SDEs: .. math:: :nowrap: \begin{eqnarray} dx^{(N-1)} & = & x^{(N)} dt & | {(N-1)th derivative \ on \ X-axis (m)} \\ dx^{(N)} & = & -K x^{N} dt + q\cdot dW_t,\ W_t \sim \mathcal{N}(0,q^2) & | \ Nth\ derivative\ on\ X-axis (m/s^{N}) \end{eqnarray} The transition and covariance matrices are very difficult to express simply, but examples for N=1 and N=2 are given in :class:`~.OrnsteinUhlenbeck` and :class:`~.Singer` respectively. """ decay_derivative: int = Property( doc="The derivative with respect to time to decay exponentially, eg if 2 identical to " "singer") noise_diff_coeff: float = Property(doc="The noise diffusion coefficient :math:`q`") damping_coeff: float = Property(doc="The Nth derivative damping coefficient :math:`K`") @property def ndim_state(self): return self.decay_derivative + 1 @staticmethod @lru_cache() def _continoustransitionmatrix(t, N, K): FCont = np.zeros((N + 1, N + 1)) for i in range(0, N + 1): FCont[i, N] = np.exp(-K * t) * (-1) ** (N - i) / K ** (N - i) for n in range(1, N - i + 1): FCont[i, N] -= (-1) ** n * t ** (N - i - n) /\ (math.factorial(N - i - n) * K ** n) for j in range(i, N): FCont[i, j] = (t ** (j - i)) / math.factorial(j - i) return FCont
[docs] def matrix(self, time_interval, **kwargs): dt = time_interval.total_seconds() N = self.decay_derivative K = self.damping_coeff return self._continoustransitionmatrix(dt, N, K)
@classmethod def _continouscovar(cls, t, N, K, k, l): # noqa: E741 FcCont = cls._continoustransitionmatrix(t, N, K) Q = np.zeros((N + 1, N + 1)) Q[N, N] = 1 CovarCont = FcCont @ Q @ FcCont.T return CovarCont[k, l] @classmethod @lru_cache() def _covardiscrete(cls, N, q, K, dt): covar = np.zeros((N + 1, N + 1)) for k in range(0, N + 1): for l in range(0, N + 1): # noqa: E741 covar[k, l] = quad(cls._continouscovar, 0, dt, args=(N, K, k, l))[0] return covar * q
[docs] def covar(self, time_interval, **kwargs): N = self.decay_derivative q = self.noise_diff_coeff K = self.damping_coeff dt = time_interval.total_seconds() return self._covardiscrete(N, q, K, dt)
[docs]class OrnsteinUhlenbeck(NthDerivativeDecay): r"""This is a class implementation of a discrete, time-variant 1D Linear-Gaussian Ornstein Uhlenbeck Transition Model. The target is assumed to move with (nearly) constant velocity, which exponentially decays to zero over time, and target acceleration is modeled as white noise. The model is described by the following SDEs: .. math:: :nowrap: \begin{eqnarray} dx_{pos} & = & x_{vel} dt & | {Position \ on \ X-axis (m)} \\ dx_{vel} & = & -K x_{vel} dt + q dW_t, W_t \sim \mathcal{N}(0,q) & | {Speed\ on \ X-axis (m/s)} \end{eqnarray} Or equivalently: .. math:: x_t = F_t x_{t-1} + w_t,\ w_t \sim \mathcal{N}(0,Q_t) where: .. math:: x & = & \begin{bmatrix} x_{pos} \\ x_{vel} \end{bmatrix} .. math:: F_t & = & \begin{bmatrix} 1 & \frac{1}{K}(1 - e^{-Kdt})\\ 0 & e^{-Kdt} \end{bmatrix} .. math:: Q_t & = & \begin{bmatrix} \frac{dt - \frac{2}{K}(1 - e^{-Kdt}) + \frac{1}{2K}(1 - e^{-2Kdt})}{K^2} & \frac{\frac{1}{K}(1 - e^{-Kdt}) - \frac{1}{2K}(1 - e^{-2Kdt})}{K} \\ \frac{\frac{1}{K}(1 - e^{-Kdt}) - \frac{1}{2K}(1 - e^{-2Kdt})}{K} & \frac{1 - e^{-2Kdt}}{2K} \end{bmatrix} q """ noise_diff_coeff: float = Property(doc="The velocity noise diffusion coefficient :math:`q`") damping_coeff: float = Property(doc="The velocity damping coefficient :math:`K`") @property def decay_derivative(self): return 1
[docs]class Singer(NthDerivativeDecay): r"""This is a class implementation of a discrete, time-variant 1D Singer Transition Model. The target acceleration is modeled as a zero-mean Gauss-Markov random process. The model is described by the following SDEs: .. math:: :nowrap: \begin{eqnarray} dx_{pos} & = & x_{vel} dt & | {Position \ on \ X-axis (m)} \\ dx_{vel} & = & x_{acc} dt & | {Speed \ on\ X-axis (m/s)} \\ dx_{acc} & = & -K x_{acc} dt + q W_t,\ W_t \sim \mathcal{N}(0,q^2) & | {Acceleration \ on \ X-axis (m^2/s)} \end{eqnarray} Or equivalently: .. math:: x_t = F_t x_{t-1} + w_t,\ w_t \sim \mathcal{N}(0,Q_t) where: .. math:: x & = & \begin{bmatrix} x_{pos} \\ x_{vel} \\ x_{acc} \end{bmatrix} .. math:: F_t & = & \begin{bmatrix} 1 & dt & (K dt-1+e^{-K dt})/K^2 \\ 0 & 1 & (1-e^{-K dt})/K \\ 0 & 0 & e^{-K t} \end{bmatrix} .. math:: Q_t & = & q \begin{bmatrix} \frac{[1-e^{-2K dt}] + 2K dt + \frac{2K^3 dt^3}{3}- 2K^2 dt^2 - 4K dt e^{-K dt} }{2K^5} & \frac{(K dt - [1-e^{-K dt}])^2}{2K^4} & \frac{[1-e^{-2K dt}]-2K dt e^{-K dt}} {2K^3} \\ \frac{(K dt - [1 - e^{-K dt}])^2}{2K^4} & \frac{2K dt - 4[1-e^{-K dt}] + [1-e^{-2K dt}]}{2K^3} & \frac{[1-e^{-K dt}]^2}{2K^2} \\ \frac{[1- e^{-2K dt}]-2K dt e^{-K dt}} {2K^3} & \frac{[1-e^{-K dt}]^2}{2K^2} & \frac{1-e^{-2K dt}}{2K} \end{bmatrix} """ noise_diff_coeff: float = Property( doc="The acceleration noise diffusion coefficient :math:`q`") damping_coeff: float = Property(doc=r"The reciprocal of the decorrelation time :math:`\alpha`") @property def decay_derivative(self): return 2
[docs]class SingerApproximate(Singer): @property def decay_derivative(self): return 2 r"""This is a class implementation of a discrete, time-variant 1D Singer Transition Model, with covariance approximation applicable for smaller time intervals. The target acceleration is modeled as a zero-mean Gauss-Markov random process. The model is described by the following SDEs: .. math:: :nowrap: \begin{eqnarray} dx_{pos} & = & x_{vel} d & | {Position \ on \ X-axis (m)} \\ dx_{vel} & = & x_{acc} d & | {Speed \ on\ X-axis (m/s)} \\ dx_{acc} & = & -\alpha x_{acc} d + q W_t,\ W_t \sim \mathcal{N}(0,q^2) & | {Acceleration \ on \ X-axis (m^2/s)} \end{eqnarray} Or equivalently: .. math:: x_t = F_t x_{t-1} + w_t,\ w_t \sim \mathcal{N}(0,Q_t) where: .. math:: x & = & \begin{bmatrix} x_{pos} \\ x_{vel} \\ x_{acc} \end{bmatrix} .. math:: F_t & = & \begin{bmatrix} 1 & dt & (\alpha dt-1+e^{-\alpha dt})/\alpha^2 \\ 0 & 1 & (1-e^{-\alpha dt})/\alpha \\ 0 & 0 & e^{-\alpha t} \end{bmatrix} For small dt: .. math:: Q_t & = & q \begin{bmatrix} \frac{dt^5}{20} & \frac{dt^4}{8} & \frac{dt^3}{6} \\ \frac{dt^4}{8} & \frac{dt^3}{3} & \frac{dt^2}{2} \\ \frac{dt^3}{6} & \frac{dt^2}{2} & dt \end{bmatrix} """
[docs] def covar(self, time_interval, **kwargs): """Returns the transition model noise covariance matrix. Parameters ---------- time_interval : :class:`datetime.timedelta` A time interval :math:`dt` Returns ------- :class:`stonesoup.types.state.CovarianceMatrix` of shape\ (:py:attr:`~ndim_state`, :py:attr:`~ndim_state`) The process noise covariance. """ # time_interval_threshold is currently set arbitrarily. time_interval_sec = time_interval.total_seconds() # Only leading terms get calculated for speed. covar = np.array( [[time_interval_sec**5 / 20, time_interval_sec**4 / 8, time_interval_sec**3 / 6], [time_interval_sec**4 / 8, time_interval_sec**3 / 3, time_interval_sec**2 / 2], [time_interval_sec**3 / 6, time_interval_sec**2 / 2, time_interval_sec]] ) * self.noise_diff_coeff return CovarianceMatrix(covar)
[docs]class ConstantTurnSandwich(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, 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 known (nearly) constant turn rate. """ turn_noise_diff_coeffs: np.ndarray = Property( doc="The acceleration noise diffusion coefficients :math:`q`") turn_rate: float = Property( doc=r"The turn rate :math:`\omega`") model_list: Sequence[LinearGaussianTransitionModel] = Property( doc="List of Transition Models.") @property def ndim_state(self): """ndim_state getter method Returns ------- : :class:`int` The number of combined model state dimensions. """ return sum(model.ndim_state for model in self.model_list)+4
[docs] def matrix(self, time_interval, **kwargs): """Model matrix :math:`F` Returns ------- : :class:`numpy.ndarray` of shape\ (:py:attr:`~ndim_state`, :py:attr:`~ndim_state`) """ time_interval_sec = time_interval.total_seconds() turn_ratedt = self.turn_rate * time_interval_sec z = np.zeros([2, 2]) transition_matrices = [ model.matrix(time_interval) for model in self.model_list] sandwich = block_diag(z, *transition_matrices, z) sandwich[0:2, 0:2] = np.array([[1, np.sin(turn_ratedt)/self.turn_rate], [0, np.cos(turn_ratedt)]]) sandwich[0:2, -2:] = np.array( [[0, (np.cos(turn_ratedt)-1)/self.turn_rate], [0, -np.sin(turn_ratedt)]]) sandwich[-2:, 0:2] = np.array( [[0, (1-np.cos(turn_ratedt))/self.turn_rate], [0, np.sin(turn_ratedt)]]) sandwich[-2:, -2:] = np.array([[1, np.sin(turn_ratedt)/self.turn_rate], [0, np.cos(turn_ratedt)]]) return sandwich
[docs] 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. """ q1, q2 = self.turn_noise_diff_coeffs dt = time_interval.total_seconds() covar_list = [model.covar(time_interval) for model in self.model_list] ctc1 = np.array([[q1*dt**3/3, q1*dt**2/2], [q1*dt**2/2, q1*dt]]) ctc2 = np.array([[q1*dt**3/3, q1*dt**2/2], [q1*dt**2/2, q1*dt]]) return CovarianceMatrix(block_diag(ctc1, *covar_list, ctc2))
[docs]class ConstantTurn(ConstantTurnSandwich): 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 known (nearly) constant turn rate. The model is described by the following SDEs: .. math:: :nowrap: \begin{eqnarray} dx_{pos} & = & x_{vel} d & | {Position \ on \ X-axis (m)} \\ dx_{vel} & = &-\omega y_{pos} d & | {Speed \ on\ X-axis (m/s)} \\ dy_{pos} & = & y_{vel} d & | {Position \ on \ Y-axis (m)} \\ dy_{vel} & = & \omega x_{pos} d & | {Speed \ on\ Y-axis (m/s)} \end{eqnarray} Or equivalently: .. math:: x_t = F_t x_{t-1} + w_t,\ w_t \sim \mathcal{N}(0,Q_t) where: .. math:: x & = & \begin{bmatrix} x_{pos} \\ x_{vel} \\ y_{pos} \\ y_{vel} \end{bmatrix} .. math:: F_t & = & \begin{bmatrix} 1 & \frac{\sin\omega dt}{\omega} & 0 &-\frac{1-\cos\omega dt}{\omega} \\ 0 & \cos\omega dt & 0 & -\sin\omega dt \\ 0 & \frac{1-\cos\omega dt}{\omega} & 1 & \frac{\sin\omega dt}{\omega}\\ 0 & \sin\omega dt & 0 & \cos\omega dt \end{bmatrix} .. math:: Q_t & = & \begin{bmatrix} q_x^2 \frac{dt^3}{3} & q_x^2 \frac{dt^2}{2} & 0 & 0 \\ q_x^2 \frac{dt^2}{2} & q_x^2 dt & 0 & 0 \\ 0 & 0 & q_y^2 \frac{dt^3}{3} & q_y^2 \frac{dt^2}{2}\\ 0 & 0 & q_y^2 \frac{dt^2}{2} & q_y^2 dt \end{bmatrix} """ @property def model_list(self): """For a turn in adjacent state vectors, no transition models go in between""" return []