Added tutorial for Information Filter

docs/tutorials/12_InformationFilterTutorial.py

@@ -0,0 +1,215 @@
+#!/usr/bin/env python
+# coding: utf-8
+
+"""
+================================================================
+12 - An introduction to Stone Soup: using the Information filter
+================================================================
+"""
+
+# %%
+# This notebook is designed to introduce the Information Filter using a single target scenario as an example.
+#
+# Background and notation:
+# ------------------------
+#
+# The information filter can be used when there is large, or infinite uncertainty about the initial
+# state of an object. To compute the predicting and updating steps, the infinite covariance is therefore
+# converted to its inverse, using both the Information matrix and the Information state.
+# 
+# We begin by creating a constant velocity model with q = 0.05
+# A ‘truth path’ is created starting at (20,20) moving to the NE at one distance unit per (time) step
+# in each dimension. We propagate this with the transition model to generate a ground truth path
+#
+# Firstly, we run the general imports, create the start time and build the Ground Truth constant velocity model.
+# This follows the same procedure as shown in Tutorial 1 - Kalman Filter.
+#
+#
+
+
+# %%
+import numpy as np
+
+from datetime import datetime, timedelta
+start_time = datetime.now()
+
+np.random.seed(1991)
+
+
+# setting up ground truth
+
+from stonesoup.types.groundtruth import GroundTruthPath, GroundTruthState
+from stonesoup.models.transition.linear import (CombinedLinearGaussianTransitionModel,
+                                               ConstantVelocity)
+
+transition_model = CombinedLinearGaussianTransitionModel([ConstantVelocity(0.05),ConstantVelocity(0.05)])
+
+# Creating the initial truth state.
+
+truth = GroundTruthPath([GroundTruthState([20,1,20,1],timestamp=start_time)])
+
+# Generating the Ground truth path using a transition model.
+
+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)))
+
+# %%
+# Importing the :class:`~.Plotter` class from Stone Soup, we can plot the results.
+# Note that the mapping argument is [0, 2] because those are the :math:`x` and :math:`y` position
+# indices from our state vector.
+
+
+# %%
+# Plotting:
+# ^^^^^^^^^
+
+from stonesoup.plotter import Plotter
+plotter = Plotter()
+plotter.plot_ground_truths(truth,[0,2])
+
+# %%
+# Taking Measurements:
+# ^^^^^^^^^^^^^^^^^^^^
+#
+# As per the original Kalman tutorial, we’ll use one of Stone Soup’s measurement models in order to
+# generate measurements from the ground truth. We shall assume a ‘linear’ sensor which detects the
+# position only (not the velocity) of a target.
+# 
+# Omega is set to 5.
+# 
+# The linear Gaussian measurement model is set up by indicating the number of dimensions in the state
+# vector and the dimensions that are measured (specifying :math:`{H}_{k}`) and the noise covariance matrix :math:`R`.
+#
+
+
+# measurements
+from stonesoup.types.detection import Detection
+from stonesoup.models.measurement.linear import LinearGaussian
+
+measurement_model = LinearGaussian(ndim_state = 4, # Number of state dimensions (position and velocity in 2D)
+                                  mapping=(0,2), # Mapping measurement vector index to state index
+                                  noise_covar=np.array([[5,0], # Covariance matrix for Gaussian PDF
+                                                      [0,5]])
+                                  )
+
+# taking measurements
+
+measurements = []
+
+for state in truth:
+    measurement = measurement_model.function(state,noise=True)
+    measurements.append(Detection(measurement,timestamp=state.timestamp,measurement_model = measurement_model))
+
+
+# %%
+# We plot the measurements using the Plotter class in Stone Soup. Again specifying the :math:`x`, :math:`y` position
+# indicies from the state vector.
+
+
+plotter.plot_measurements(measurements,[0,2])
+plotter.fig
+
+# %%
+# Running the Information Filter:
+# ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+#
+# We must first import the :class:`~.InformationKalmanPredictor`, and :class:`~.InformationKalmanUpdater` from the
+# corresponding libraries.
+#
+# As before, the Predictor must be passed a Transition model, and the updater a Measurement model.
+# 
+# It is important to note that, unlike the Kalman Filter in Tutorial 1, the Information Filter requires
+# the prior estimate to be in the form of an :class:`~.InformationState` (not a :class:`~.GaussianState`).
+# 
+# The InformationState can be imported from stonesoup.types.state, and takes arguments: Information
+# state, Information Matrix and a timestamp.
+# 
+# The information matrix is defined as : :math:`{Y}_{k-1}` = :math:`[{P}_{k-1}]^{-1}`.
+# That is, the inverse of the covariance matrix.
+# The information state is defined as : :math:`{y}_{k-1}` = :math:`[{P}_{k-1}]^{-1}` :math:`{x}_{k-1}`.
+#
+# That is the matrix multiplication of the information matrix and the prior state in :math:`{x}_{k-1}`.
+# 
+# Using the same prior state as the original Kalman filtering example, we must firstly convert the
+# covariance to be the Information matrix, :math:`{Y}_{k-1}`, and calculate
+# the :class:`~.InformationState`, :math:`{y}_{k-1}`.
+#
+
+
+# predicting and updating
+from stonesoup.predictor.information import InformationKalmanPredictor
+from stonesoup.updater.information import InformationKalmanUpdater
+
+
+# Creating Information predictor and updater objects
+predictor = InformationKalmanPredictor(transition_model)
+updater = InformationKalmanUpdater(measurement_model)
+
+# %%
+# As before, we use the :class:`~.SingleHypothesis` class. The explicitly associates a single
+# predicted state to a single detection.
+
+from stonesoup.types.state import InformationState
+
+# Precision matrix - the inverse of the covariance matrix
+kalman_covar = np.diag([1.5, 0.5, 1.5, 0.5])
+inverse_kal_covar = np.linalg.inv(kalman_covar) # information matrix
+
+
+# yk_1 = Pk_1^-1 @ x_k-1
+kalman_state = [20,1,20,1]
+information_state = inverse_kal_covar @ kalman_state
+
+
+# Must use information state with precision matrix instead of Gaussian state
+prior = InformationState(information_state, inverse_kal_covar, timestamp=start_time)
+
+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)
+    post = updater.update(hypothesis)
+    track.append(post)
+    prior = track[-1]
+
+# %%
+# Plotting:
+# ^^^^^^^^^
+# Plotting the resulting track, including uncertainty ellipses.
+
+plotter.plot_tracks(track,[0,2],uncertainty=True)
+plotter.fig
+
+# %%
+# State Vector Comparison:
+# ^^^^^^^^^^^^^^^^^^^^^^^^
+#
+# The state vector of the first position as an :class:`~.InformationState`.
+
+
+print(track[0].state_vector)
+
+
+# %%
+# The state vector of the first position as a :class:`~.GaussianState`. We use the .gaussian_state property to
+# convert to Gaussian form.
+#
+# We can obtain the covariance by taking the inverse of the Information Matrix.
+# We can also calculate the Gaussian state mean by multiplying by the covariance. In order to derive
+# :math:`{x}_{k-1}`, we multiply by the covariance matrix.
+#
+# :math:`{y}_{k-1}` = :math:`[{P}_{k-1}]^{-1}` :math:`{x}_{k-1}`.
+#
+
+print(track[0].gaussian_state.state_vector)
+
+# %%
+
+
+