/
InformationFilterTutorial.py
264 lines (192 loc) · 8.68 KB
/
InformationFilterTutorial.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
#!/usr/bin/env python
"""
===========================
Information filter tutorial
===========================
"""
# %%
# 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
# converted to its inverse, using both the Precision 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:`~.Plotterly` 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 Ground Truths:
# ^^^^^^^^^^^^^^^^^^^^^^^
from stonesoup.plotter import Plotterly
plotter = Plotterly()
plotter.plot_ground_truths(truth,[0,2])
plotter.fig
# %%
# 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`.
#
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]])
)
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 Plotterly 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, Precision Matrix and a timestamp.
#
# The Precision 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 Precision 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 Precision matrix, :math:`{Y}_{k-1}`, and calculate
# the :class:`~.InformationState`, :math:`{y}_{k-1}`.
#
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
covar = np.diag([1.5, 0.5, 1.5, 0.5])
precision_matrix = np.linalg.inv(covar)
# yk_1 = Pk_1^-1 @ x_k-1
state = [20,1,20,1]
information_state = precision_matrix @ state
# Must use information state with precision matrix instead of Gaussian state
prior = InformationState(information_state, precision_matrix, 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.
# sphinx_gallery_thumbnail_number = 3
plotter.plot_tracks(track,[0,2],uncertainty=True)
plotter.fig
# %%
# Comparison to Kalman Filter:
# ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
# We can show that the track created by the information filter and that of the Kalman filter are equivalent.
# Below we re-run the predicting and updating process with the :class:`~.KalmanPredictor` and :class:`~.KalmanUpdater`
from stonesoup.types.state import GaussianState
from stonesoup.predictor.kalman import KalmanPredictor
from stonesoup.updater.kalman import KalmanUpdater
# Prior estimate:
prior = GaussianState([[20], [1], [20], [1]], np.diag([1.5, 0.5, 1.5, 0.5]), timestamp=start_time)
# Creating kalman predictor and updater
predictor = KalmanPredictor(transition_model)
updater = KalmanUpdater(measurement_model)
# Creating the kalman track
Kalman_track = Track()
for measurement in measurements:
prediction = predictor.predict(prior,timestamp=measurement.timestamp)
hypothesis = SingleHypothesis(prediction, measurement)
post = updater.update(hypothesis)
Kalman_track.append(post)
prior = Kalman_track[-1]
# %%
# Comparison of Kalman Information Filter Tracks:
# ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
#
# We can use the Gaussian Hellinger measure to return the Hellinger distance between a
# pair of :class:`~.GaussianState` multivariate objects. The distance is bounded between 0-1, and we can therefore show
# that the total distance between the two tracks is close to zero.
#
#
from stonesoup.measures import GaussianHellinger
gh_measure = GaussianHellinger()
total_distance = 0
for information_state, kalman_state in zip(track, Kalman_track):
distance = gh_measure(information_state.gaussian_state, kalman_state)
total_distance += distance
print(total_distance)
# %%
# 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 Precision 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:`{x}_{k-1}` = :math:`[{P}_{k-1}]` :math:`{y}_{k-1}`.
#
print(track[0].gaussian_state.state_vector)
# %%