New Recursive updaters added

docs/examples/EnsembleFilterExample.py

@@ -0,0 +1,183 @@
+#!/usr/bin/env python
+# coding: utf-8
+
+"""
+=======================
+Ensemble Filter Example
+=======================
+"""
+
+# %%
+# The Ensemble Kalman Filter (EnKF) is a hybrid of the Kalman updating scheme and the Monte Carlo
+# approach of the particle filter. The EnKF provides an alternative to the Kalman Filter (and
+# extensions of) which is specifically designed for high-dimensional states.
+#
+# EnKF can be applied to both non-linear and non-Gaussian state-spaces due to being completely
+# based on sampling.
+#
+# Multiple versions of EnKF are implemented in Stone Soup - all make use of the same prediction
+# step, but implement different versions of the update step. Namely, the updaters are:
+#
+# - :class:`~.EnsembleUpdater`
+# - :class:`~.LinearisedEnsembleUpdater`
+# - :class:`~.RecursiveLinearisedEnsembleUpdater`
+# - :class:`~.RecursiveUpdater`
+#
+# The :class:`~.EnsembleUpdater` is deliberately structured to resemble the Vanilla Kalman Filter,
+# :meth:`update` first calls :meth:`predict_measurement` function which
+# proceeds by calculating the predicted measurement, innovation covariance
+# and measurement cross-covariance. Note however, these are not propagated
+# explicitly, they are derived from the sample covariance of the ensemble itself.
+#
+# The :class:`~.LinearisedEnsembleUpdater` is an implementation of 'The Linearized EnKF Update'
+# algorithm from "Ensemble Kalman Filter with Bayesian Recursive Update" by Kristen Michaelson,
+# Andrey A. Popov and Renato Zanetti. Similar to the EnsembleUpdater, but uses a different form
+# of Kalman gain. This alternative form of the EnKF calculates a separate kalman gain for each
+# ensemble member. This alternative Kalman gain calculation involves linearization of the
+# measurement. An additional step is introduced to perform inflation.
+#
+# The :class:`~.RecursiveLinearisedEnsembleUpdater` is an implementation of 'The Bayesian
+# Recursive Update Linearized EnKF' algorithm from "Ensemble Kalman Filter with Bayesian
+# Recursive Update" by Kristen Michaelson, Andrey A. Popov and Renato Zanetti. It is an
+# extended version of the LinearisedEnsembleUpdater that recursively iterates over the
+# update step for a given number of iterations. This recursive version is designed to
+# minimise the error caused by linearisation.
+#
+# The :class:`~.RecursiveEnsembleUpdater` is an extended version of the
+# :class:`~.EnsembleUpdater` which recursively iterates over the update step.
+
+# %%
+# Example using stonesoup
+# -----------------------
+# Package imports
+# ^^^^^^^^^^^^^^^
+
+
+import numpy as np
+from datetime import datetime, timedelta
+
+
+# %%
+# Start time of simulation
+# ^^^^^^^^^^^^^^^^^^^^^^^^
+
+
+start_time = datetime.now()
+np.random.seed(1991)
+
+
+# %%
+# Generate and plot ground truth
+# ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+
+from stonesoup.models.transition.linear import CombinedLinearGaussianTransitionModel, \
+    ConstantVelocity
+from stonesoup.types.groundtruth import GroundTruthPath, GroundTruthState
+from stonesoup.plotter import Plotterly
+
+transition_model = CombinedLinearGaussianTransitionModel([ConstantVelocity(0.05),
+                                                          ConstantVelocity(0.05)])
+truth = GroundTruthPath([GroundTruthState([0, 1, 0, 1], timestamp=start_time)])
+
+for k in range(1, 21):
+    truth.append(GroundTruthState(
+        transition_model.function(truth[k - 1], noise=True, time_interval=timedelta(seconds=1)),
+        timestamp=start_time + timedelta(seconds=k)))
+
+# %%
+
+plotter = Plotterly()
+plotter.plot_ground_truths(truth, [0, 2])
+plotter.fig
+
+# %%
+# Generate and plot detections
+# ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+
+from stonesoup.models.measurement.nonlinear import CartesianToBearingRange
+
+sensor_x = 50  # Placing the sensor off-centre
+sensor_y = 0
+
+measurement_model = CartesianToBearingRange(
+    ndim_state=4,
+    mapping=(0, 2),
+    noise_covar=np.diag([np.radians(0.2), 1]),  # Covariance matrix. 0.2 degree variance in
+    # bearing and 1 metre in range
+    translation_offset=np.array([[sensor_x], [sensor_y]])  # Offset measurements to location of
+    # sensor in cartesian.
+)
+
+# %%
+
+from stonesoup.types.detection import Detection
+
+measurements = []
+for state in truth:
+    measurement = measurement_model.function(state, noise=True)
+    measurements.append(Detection(measurement, timestamp=state.timestamp,
+                                  measurement_model=measurement_model))
+
+# %%
+
+plotter.plot_measurements(measurements, [0, 2])
+plotter.fig
+
+# %%
+# Set up predictor and updater
+# ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+# In this EnKF example we must use the :class:`~.EnsemblePredictor`, and choose to use the
+# standard :class:`~.EnsembleUpdater`. Note that we could instanciate any of the other (ensemble)
+# updaters mentioned in this example, in place of the :class:`~.EnsembleUpdater`.
+
+
+from stonesoup.predictor.ensemble import EnsemblePredictor
+from stonesoup.updater.ensemble import EnsembleUpdater
+
+predictor = EnsemblePredictor(transition_model)
+updater = EnsembleUpdater(measurement_model)
+
+
+# %%
+# Prior state
+# ^^^^^^^^^^^
+# For the simulation we must provide a prior state. The Ensemble Filter in stonesoup requires
+# this to be an :class:`~.EnsembleState`. We generate an :class:`~.EnsembleState` by calling
+# :meth:`~.generate_ensemble`. The prior state stores the prior state vector - see below that
+# for the :class:`~.EnsembleState`, the `state_vector` must be a :class:`~.StateVectors` object.
+
+
+from stonesoup.types.state import EnsembleState
+
+ensemble = EnsembleState.generate_ensemble(
+    mean=np.array([[0], [1], [0], [1]]),
+    covar=np.diag([[1.5, 0.5, 1.5, 0.5]]), num_vectors=100)
+prior = EnsembleState(state_vector=ensemble, timestamp=start_time)
+
+# %%
+
+type(prior.state_vector)
+
+
+# %%
+# Run the EnKF
+# ^^^^^^^^^^^^
+# Here we run the Ensemble Kalman Filter and plot the results. By marking flag `particle=True`,
+# we plot each member of the ensembles. As usual, the ellipses represent the uncertainty in the
+# tracks.
+
+from stonesoup.types.hypothesis import SingleHypothesis
+from stonesoup.types.track import Track
+
+track = Track()
+for measurement in measurements:
+    prediction = predictor.predict(prior, timestamp=measurement.timestamp)
+    hypothesis = SingleHypothesis(prediction, measurement)  # Group a prediction and measurement
+    post = updater.update(hypothesis)
+    track.append(post)
+    prior = track[-1]
+
+plotter.plot_tracks(track, [0, 2], uncertainty=True, particle=True)
+plotter.fig

docs/source/stonesoup.updater.rst

@@ -25,6 +25,12 @@ Ensemble
 .. automodule:: stonesoup.updater.ensemble
     :show-inheritance:
 
+Recursive
+---------
+
+.. automodule:: stonesoup.updater.recursive
+    :show-inheritance:
+
 Information
 -----------
 

stonesoup/updater/ensemble.py

@@ -6,6 +6,7 @@
 from .kalman import KalmanUpdater
 from ..base import Property
 from ..types.state import State, EnsembleState
+from ..types.array import StateVectors
 from ..types.prediction import MeasurementPrediction
 from ..types.update import Update
 from ..models.measurement import MeasurementModel
@@ -14,7 +15,7 @@
 class EnsembleUpdater(KalmanUpdater):
     r"""Ensemble Kalman Filter Updater class
     The EnKF is a hybrid of the Kalman updating scheme and the
-    Monte Carlo aproach of the the particle filter.
+    Monte Carlo approach of the particle filter.
 
     Deliberately structured to resemble the Vanilla Kalman Filter,
     :meth:`update` first calls :meth:`predict_measurement` function which
@@ -24,7 +25,7 @@ class EnsembleUpdater(KalmanUpdater):
     itself.
 
     Note that the EnKF equations are simpler when written in the following
-    formalism. Note that h is not neccisarily a matrix, but could be a
+    formalism. Note that h is not necessarily a matrix, but could be a
     nonlinear measurement function.
 
     .. math::
@@ -113,7 +114,7 @@ def predict_measurement(self, predicted_state, measurement_model=None,
 
         Parameters
         ----------
-        pred_state : :class:`~.State`
+        predicted_state : :class:`~.State`
             The predicted state :math:`\mathbf{x}_{k|k-1}`
         measurement_model : :class:`~.MeasurementModel`
             The measurement model. If omitted, the model in the updater object
@@ -293,3 +294,94 @@ def update(self, hypothesis, **kwargs):
         return Update.from_state(hypothesis.prediction,
                                  posterior_ensemble, timestamp=hypothesis.measurement.timestamp,
                                  hypothesis=hypothesis)
+
+
+class LinearisedEnsembleUpdater(EnsembleUpdater):
+    """
+    Implementation of 'The Linearized EnKF Update' algorithm from "Ensemble Kalman Filter with
+    Bayesian Recursive Update" by Kristen Michaelson, Andrey A. Popov and Renato Zanetti.
+    Similar to the EnsembleUpdater, but uses a different form of Kalman gain. This alternative form
+    of the EnKF calculates a separate kalman gain for each ensemble member. This alternative
+    Kalman gain calculation involves linearization of the measurement. An additional step is
+    introduced to perform inflation.
+
+    References
+    ----------
+
+    1. K. Michaelson, A. A. Popov and R. Zanetti,
+    "Ensemble Kalman Filter with Bayesian Recursive Update"
+    """
+    inflation_factor: float = Property(
+        default=1.,
+        doc="Parameter to control inflation")
+
+    def update(self, hypothesis, **kwargs):
+        r"""The LinearisedEnsembleUpdater update method. This method includes an additional step
+        over the EnsembleUpdater update step to perform inflation.
+
+        Parameters
+        ----------
+        hypothesis : :class:`~.SingleHypothesis`
+            the prediction-measurement association hypothesis. This hypothesis
+            may carry a predicted measurement, or a predicted state. In the
+            latter case a predicted measurement will be calculated.
+
+
+        Returns
+        -------
+        : :class:`~.EnsembleStateUpdate`
+            The posterior state which contains an ensemble of state vectors
+            and a timestamp.
+        """
+        # Extract the number of vectors from the prediction
+        num_vectors = hypothesis.prediction.num_vectors
+
+        # Assign measurement prediction if prediction is missing
+        hypothesis = self._check_measurement_prediction(hypothesis)
+
+        # Prior state vector
+        X0 = hypothesis.prediction.state_vector
+
+        # Measurement covariance
+        R = self.measurement_model.covar()
+
+        # Line 1: Compute mean
+        m = hypothesis.prediction.mean
+
+        # Line 2: Compute inflation
+        X = StateVectors(m + self.inflation_factor * (X0 - m))
+
+        # Line 3: Recompute prior covariance
+        P = 1/(num_vectors-1) * (X0 - m) @ (X0 - m).T
+
+        states = list()
+
+        # Line 5: Y_hat
+        Y_hat = hypothesis.measurement_prediction.state_vector
+
+        # Line 4
+        for x, y_hat in zip(X, Y_hat):
+
+            # Line 6: Compute Jacobian
+            H = self.measurement_model.jacobian(State(state_vector=x,
+                                                      timestamp=hypothesis.prediction.timestamp))
+
+            # Line 7: Calculate Innovation
+            S = H @ P @ H.T + R
+
+            # Line 8: Calculate Kalman gain
+            K = P @ H.T @ scipy.linalg.inv(S)
+
+            # Line 9: Recalculate X
+            x = x + K @ (hypothesis.measurement.state_vector - y_hat)
+
+            # Collect state vectors
+            states.append(x)
+
+        # Convert list of state vectors into a StateVectors class
+        X = StateVectors(np.hstack(states))
+
+        return Update.from_state(hypothesis.prediction,
+                                 X,
+                                 timestamp=hypothesis.measurement.timestamp,
+                                 hypothesis=hypothesis)