Source code for stonesoup.updater.information
from functools import lru_cache
import numpy as np
from ..base import Property
from ..types.prediction import GaussianMeasurementPrediction
from ..types.update import Update
from ..models.measurement.linear import LinearGaussian
from ..updater.kalman import KalmanUpdater
[docs]
class InformationKalmanUpdater(KalmanUpdater):
r"""A class which implements the update of information form of the Kalman filter. This is
conceptually very simple. The update proceeds as:
.. math::
Y_{k|k} = Y_{k|k-1} + H^{T}_k R^{-1}_k H_k
\mathbf{y}_{k|k} = \mathbf{y}_{k|k-1} + H^{T}_k R^{-1}_k \mathbf{z}_{k}
where :math:`\mathbf{y}_{k|k-1}` is the predicted information state and :math:`Y_{k|k-1}` the
predicted information matrix which form the :class:`~.InformationStatePrediction` object. The
measurement matrix :math:`H_k` and measurement covariance :math:`R_k` are those in the Kalman
filter (see tutorial 1). An :class:`~.InformationStateUpdate` object is returned.
Note
----
Analogously with the :class:`~.InformationKalmanPredictor`, the measurement model is queried
for the existence of an :meth:`inverse_covar()` property. If absent, the :meth:`covar()` is
inverted.
"""
measurement_model: LinearGaussian = Property(
default=None,
doc="A linear Gaussian measurement model. This need not be defined if "
"a measurement model is provided in the measurement. If no model "
"specified on construction, or in the measurement, then error "
"will be thrown.")
def _inverse_measurement_covar(self, measurement_model, **kwargs):
"""Return the inverse of the measurement covariance (or calculate it)
Parameters
----------
measurement_model
The measurement model to be queried
**kwargs : various, optional
These are passed to :meth:`~.LinearGaussian.covar()`
Returns
-------
: :class:`numpy.ndarray`
The inverse of the measurement covariance, :math:`R_k^{-1}`
"""
if hasattr(measurement_model, 'inverse_covar'):
inv_measurement_covar = measurement_model.inverse_covar(**kwargs)
else:
inv_measurement_covar = np.linalg.inv(measurement_model.covar(**kwargs))
return inv_measurement_covar
[docs]
@lru_cache()
def predict_measurement(self, predicted_state, measurement_model=None, measurement_noise=True,
**kwargs):
r"""There's no direct analogue of a predicted measurement in the information form. This
method is therefore provided to return the predicted measurement as would the standard
Kalman updater. This is mainly for compatibility as it's not anticipated that it would
be used in the usual operation of the information filter.
Parameters
----------
predicted_state : :class:`~.State`
The predicted state in information form :math:`\mathbf{y}_{k|k-1}`
measurement_model : :class:`~.MeasurementModel`
The measurement model. If omitted, the model in the updater object
is used
measurement_noise : bool
Whether to include measurement noise :math:`R` with innovation covariance.
Default `True`
**kwargs : various
These are passed to :meth:`~.MeasurementModel.matrix()`
Returns
-------
: :class:`~.GaussianMeasurementPrediction`
The measurement prediction, :math:`H \mathbf{x}_{k|k-1}`
"""
# If a measurement model is not specified then use the one that's
# native to the updater
measurement_model = self._check_measurement_model(measurement_model)
hh = self._measurement_matrix(predicted_state=predicted_state,
measurement_model=measurement_model,
**kwargs)
predicted_covariance = np.linalg.inv(predicted_state.precision)
predicted_state_mean = predicted_covariance @ predicted_state.state_vector
predicted_measurement = hh @ predicted_state_mean
innovation_covariance = hh @ predicted_covariance @ hh.T
if measurement_noise:
innovation_covariance += measurement_model.covar(**kwargs)
return GaussianMeasurementPrediction(predicted_measurement, innovation_covariance,
predicted_state.timestamp,
cross_covar=predicted_covariance @ hh.T)
[docs]
def update(self, hypothesis, **kwargs):
r"""The Information filter update (corrector) method. Given a hypothesised association
between a predicted information state and an actual measurement, calculate the posterior
information state.
Parameters
----------
hypothesis : :class:`~.SingleHypothesis`
the prediction-measurement association hypothesis. This hypothesis
carries a predicted information state.
**kwargs : various
These are passed to :meth:`predict_measurement`
Returns
-------
: :class:`~.InformationStateUpdate`
The posterior information state with information state :math:`\mathbf{y}_{k|k}` and
precision :math:`Y_{k|k}`
"""
measurement_model = hypothesis.measurement.measurement_model
measurement_model = self._check_measurement_model(measurement_model)
pred_info_mean = hypothesis.prediction.state_vector
hh = measurement_model.matrix()
invr = self._inverse_measurement_covar(measurement_model)
posterior_precision = hypothesis.prediction.precision + hh.T @ invr @ hh
posterior_information_mean = pred_info_mean + hh.T @ invr @ \
hypothesis.measurement.state_vector
if self.force_symmetric_covariance:
posterior_precision = (posterior_precision + posterior_precision.T)/2
return Update.from_state(hypothesis.prediction, posterior_information_mean,
posterior_precision,
timestamp=hypothesis.measurement.timestamp, hypothesis=hypothesis)