Source code for stonesoup.predictor.kalman

# -*- coding: utf-8 -*-

import numpy as np
import scipy.linalg as la
from functools import partial

from .base import Predictor
from ._utils import predict_lru_cache
from ..base import Property
from ..types.prediction import Prediction, SqrtGaussianStatePrediction
from ..models.base import LinearModel
from ..models.transition import TransitionModel
from ..models.transition.linear import LinearGaussianTransitionModel
from ..models.control import ControlModel
from ..models.control.linear import LinearControlModel
from ..functions import gauss2sigma, unscented_transform


[docs]class KalmanPredictor(Predictor): r"""A predictor class which forms the basis for the family of Kalman predictors. This class also serves as the (specific) Kalman Filter :class:`~.Predictor` class. Here .. math:: f_k( \mathbf{x}_{k-1}) = F_k \mathbf{x}_{k-1}, \ b_k( \mathbf{u}_k) = B_k \mathbf{u}_k \ \mathrm{and} \ \mathbf{\nu}_k \sim \mathcal{N}(0,Q_k) Notes ----- In the Kalman filter, transition and control models must be linear. Raises ------ ValueError If no :class:`~.TransitionModel` is specified. """ transition_model: LinearGaussianTransitionModel = Property( doc="The transition model to be used.") control_model: LinearControlModel = Property( default=None, doc="The control model to be used. Default `None` where the predictor " "will create a zero-effect linear :class:`~.ControlModel`.") def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) # If no control model insert a linear zero-effect one # TODO: Think about whether it's more efficient to leave this out if self.control_model is None: ndims = self.transition_model.ndim_state self.control_model = LinearControlModel(ndims, [], np.zeros([ndims, 1]), np.zeros([ndims, ndims]), np.zeros([ndims, ndims])) def _transition_matrix(self, **kwargs): """Return the transition matrix Parameters ---------- **kwargs : various, optional These are passed to :meth:`~.LinearGaussianTransitionModel.matrix` Returns ------- : :class:`numpy.ndarray` The transition matrix, :math:`F_k` """ return self.transition_model.matrix(**kwargs) def _transition_function(self, prior, **kwargs): r"""Applies the linear transition function to a single vector in the absence of a control input, returns a single predicted state. Parameters ---------- prior : :class:`~.GaussianState` The prior state, :math:`\mathbf{x}_{k-1}` **kwargs : various, optional These are passed to :meth:`~.LinearGaussianTransitionModel.matrix` Returns ------- : :class:`~.State` The predicted state """ return self.transition_model.matrix(**kwargs) @ prior.state_vector @property def _control_matrix(self): r"""Convenience function which returns the control matrix Returns ------- : :class:`numpy.ndarray` control matrix, :math:`B_k` """ return self.control_model.matrix() def _predict_over_interval(self, prior, timestamp): """Private function to get the prediction interval (or None) Parameters ---------- prior : :class:`~.State` The prior state timestamp : :class:`datetime.datetime`, optional The (current) timestamp Returns ------- : :class:`datetime.timedelta` time interval to predict over """ # Deal with undefined timestamps if timestamp is None or prior.timestamp is None: predict_over_interval = None else: predict_over_interval = timestamp - prior.timestamp return predict_over_interval def _predicted_covariance(self, prior, predict_over_interval, **kwargs): """Private function to return the predicted covariance. Useful in that it can be overwritten in children. Parameters ---------- prior : :class:`~.GaussianState` The prior class predict_over_interval : :class`~.timedelta` Returns ------- : :class:`~.CovarianceMatrix` The predicted covariance matrix """ prior_cov = prior.covar trans_m = self._transition_matrix(prior=prior, time_interval=predict_over_interval, **kwargs) trans_cov = self.transition_model.covar(time_interval=predict_over_interval, **kwargs) # As this is Kalman-like, the control model must be capable of # returning a control matrix (B) ctrl_mat = self._control_matrix ctrl_noi = self.control_model.control_noise return trans_m @ prior_cov @ trans_m.T + trans_cov + ctrl_mat @ ctrl_noi @ ctrl_mat.T
[docs] @predict_lru_cache() def predict(self, prior, timestamp=None, **kwargs): r"""The predict function Parameters ---------- prior : :class:`~.State` :math:`\mathbf{x}_{k-1}` timestamp : :class:`datetime.datetime`, optional :math:`k` **kwargs : These are passed, via :meth:`~.KalmanFilter.transition_function` to :meth:`~.LinearGaussianTransitionModel.matrix` Returns ------- : :class:`~.GaussianStatePrediction` :math:`\mathbf{x}_{k|k-1}`, the predicted state and the predicted state covariance :math:`P_{k|k-1}` """ # Get the prediction interval predict_over_interval = self._predict_over_interval(prior, timestamp) # Prediction of the mean x_pred = self._transition_function( prior, time_interval=predict_over_interval, **kwargs) \ + self.control_model.control_input() # Prediction of the covariance p_pred = self._predicted_covariance(prior, predict_over_interval) # And return the state in the correct form return Prediction.from_state(prior, x_pred, p_pred, timestamp=timestamp, transition_model=self.transition_model)
[docs]class ExtendedKalmanPredictor(KalmanPredictor): """ExtendedKalmanPredictor class An implementation of the Extended Kalman Filter predictor. Here the transition and control functions may be non-linear, their transition and control matrices are approximated via Jacobian matrices. To this end the transition and control models, if non-linear, must be able to return the :attr:`jacobian()` function. """ # In this version the models can be non-linear, but must have access to the # :attr:`jacobian()` function # TODO: Enforce the presence of :attr:`jacobian()` transition_model: TransitionModel = Property(doc="The transition model to be used.") control_model: ControlModel = Property( default=None, doc="The control model to be used. Default `None` where the predictor " "will create a zero-effect linear :class:`~.ControlModel`.") def _transition_matrix(self, prior, **kwargs): r"""Returns the transition matrix, a matrix if the model is linear, or approximated as Jacobian otherwise. Parameters ---------- prior : :class:`~.State` :math:`\mathbf{x}_{k-1}` **kwargs : various, optional These are passed to :meth:`~.TransitionModel.matrix` or :meth:`~.TransitionModel.jacobian` Returns ------- : :class:`numpy.ndarray` The transition matrix, :math:`F_k`, if linear (i.e. :meth:`TransitionModel.matrix` exists, or :meth:`~.TransitionModel.jacobian` if not) """ if isinstance(self.transition_model, LinearModel): return self.transition_model.matrix(**kwargs) else: return self.transition_model.jacobian(prior, **kwargs) def _transition_function(self, prior, **kwargs): r"""This is the application of :math:`f_k(\mathbf{x}_{k-1})`, the transition function, non-linear in general, in the absence of a control input Parameters ---------- prior : :class:`~.State` The prior state, :math:`\mathbf{x}_{k-1}` **kwargs : various, optional These are passed to :meth:`~.TransitionModel.function` Returns ------- : :class:`~.State` The predicted state """ return self.transition_model.function(prior, **kwargs) @property def _control_matrix(self): r"""Returns the control input model matrix, :math:`B_k`, or its linear approximation via a Jacobian. The :class:`~.ControlModel`, if non-linear must therefore be capable of returning a :meth:`~.ControlModel.jacobian`, Returns ------- : :class:`numpy.ndarray` The control model matrix, or its linear approximation """ if isinstance(self.control_model, LinearModel): return self.control_model.matrix() else: return self.control_model.jacobian( self.control_model.control_vector)
[docs]class UnscentedKalmanPredictor(KalmanPredictor): """UnscentedKalmanFilter class The predict is accomplished by calculating the sigma points from the Gaussian mean and covariance, then putting these through the (in general non-linear) transition function, then reconstructing the Gaussian. """ transition_model: TransitionModel = Property(doc="The transition model to be used.") control_model: ControlModel = Property( default=None, doc="The control model to be used. Default `None` where the predictor " "will create a zero-effect linear :class:`~.ControlModel`.") alpha: float = Property( default=0.5, doc="Primary sigma point spread scaling parameter. Default is 0.5.") beta: float = Property( default=2, doc="Used to incorporate prior knowledge of the distribution. If the " "true distribution is Gaussian, the value of 2 is optimal. " "Default is 2") kappa: float = Property( default=0, doc="Secondary spread scaling parameter. Default is calculated as " "3-Ns") def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) self._time_interval = None def _transition_and_control_function(self, prior_state, **kwargs): r"""Returns the result of applying the transition and control functions for the unscented transform Parameters ---------- prior_state_vector : :class:`~.State` Prior state vector **kwargs : various, optional These are passed to :class:`~.TransitionModel.function` Returns ------- : :class:`numpy.ndarray` The combined, noiseless, effect of applying the transition and control """ return (self.transition_model.function(prior_state, **kwargs) + self.control_model.control_input())
[docs] @predict_lru_cache() def predict(self, prior, timestamp=None, **kwargs): r"""The unscented version of the predict step Parameters ---------- prior : :class:`~.State` Prior state, :math:`\mathbf{x}_{k-1}` timestamp : :class:`datetime.datetime` Time to transit to (:math:`k`) **kwargs : various, optional These are passed to :meth:`~.TransitionModel.covar` Returns ------- : :class:`~.GaussianStatePrediction` The predicted state :math:`\mathbf{x}_{k|k-1}` and the predicted state covariance :math:`P_{k|k-1}` """ # Get the prediction interval predict_over_interval = self._predict_over_interval(prior, timestamp) # The covariance on the transition model + the control model # TODO: Note that I'm not sure you can actually do this with the # TODO: covariances, i.e. sum them before calculating # TODO: the sigma points and then just sticking them into the # TODO: unscented transform, and I haven't checked the statistics. total_noise_covar = \ self.transition_model.covar( time_interval=predict_over_interval, **kwargs) \ + self.control_model.control_noise # Get the sigma points from the prior mean and covariance. sigma_point_states, mean_weights, covar_weights = gauss2sigma( prior, self.alpha, self.beta, self.kappa) # This ensures that function passed to unscented transform has the # correct time interval transition_and_control_function = partial( self._transition_and_control_function, time_interval=predict_over_interval) # Put these through the unscented transform, together with the total # covariance to get the parameters of the Gaussian x_pred, p_pred, _, _, _, _ = unscented_transform( sigma_point_states, mean_weights, covar_weights, transition_and_control_function, covar_noise=total_noise_covar ) # and return a Gaussian state based on these parameters return Prediction.from_state(prior, x_pred, p_pred, timestamp=timestamp, transition_model=self.transition_model)
[docs]class SqrtKalmanPredictor(KalmanPredictor): r"""The version of the Kalman predictor that operates on the square root parameterisation of the Gaussian state, :class:`~.SqrtGaussianState`. The prediction is undertaken in one of two ways. The default is to work in exactly the same way as the parent class, with the exception that the predicted covariance is subject to a Cholesky factorisation prior to initialisation of the :class:`~.SqrtGaussianState` output. The alternative, accessible via the :attr:`qr_method = True` flag, is to predict via a modified Gram-Schmidt process. See [1] for details. If transition and control models are possessed of the square root form of the covariance (as :attr:`sqrt_covar` in the case of the transition model and :attr:`sqrt_control_noise` for control models), then these are used directly. If not then they are created from the full matrices using the scipy.linalg :meth:`sqrtm()` method. (Unlike the Cholesky decomposition this works on positive semi-definite matrices, as well as positive definite ones. References ---------- 1. Maybeck, P.S. 1994, Stochastic Models, Estimation, and Control, Vol. 1, NavtechGPS, Springfield, VA. """ qr_method: bool = Property( default=False, doc="A switch to do the prediction via a QR decomposition, rather than using a Cholesky " "decomposition.") # This predictor returns a square root form of the Gaussian state prediction _prediction_class = SqrtGaussianStatePrediction def _predicted_covariance(self, prior, predict_over_interval, **kwargs): """Private function to return the predicted covariance. Parameters ---------- prior : :class:`~.SqrtGaussianState` The prior class (which carries the covariance in square root form) predict_over_interval : :class`~.timedelta` The interval over which the model is applied Returns ------- : :class:`~.CovarianceMatrix` The predicted covariance matrix """ sqrt_prior_cov = prior.sqrt_covar trans_m = self._transition_matrix(prior=prior, time_interval=predict_over_interval, **kwargs) try: sqrt_trans_cov = self.transition_model.sqrt_covar(time_interval=predict_over_interval, **kwargs) except AttributeError: sqrt_trans_cov = la.sqrtm(self.transition_model.covar( time_interval=predict_over_interval, **kwargs)) # As this is Kalman-like, the control model must be capable of returning a control matrix # (B) ctrl_mat = self._control_matrix try: sqrt_ctrl_noi = self.control_model.sqrt_control_noise except AttributeError: sqrt_ctrl_noi = la.sqrtm(self.control_model.control_noise) if self.qr_method: # Note that the control matrix aspect of this hasn't been tested m_sq_trans_cov = np.block([[trans_m @ sqrt_prior_cov, sqrt_trans_cov, ctrl_mat@sqrt_ctrl_noi]]) _, pred_sqrt_cov = np.linalg.qr(m_sq_trans_cov.T) return pred_sqrt_cov.T else: return np.linalg.cholesky(trans_m@sqrt_prior_cov@sqrt_prior_cov.T@trans_m.T + sqrt_trans_cov@sqrt_trans_cov.T + ctrl_mat@sqrt_ctrl_noi@sqrt_ctrl_noi.T@ctrl_mat.T)