Professional Documents
Culture Documents
Understanding The Basis of The Kalman Filter PDF
Understanding The Basis of The Kalman Filter PDF
Ramsey Faragher
understand the basis of the Kalman filter via a simple and intuitive derivation.
RELEVANCE
The Kalman filter [2] (and its variants
such as the extended Kalman filter [3]
and unscented Kalman filter [4]) is
one of the most celebrated and popular data fusion algorithms in the field
of information processing. The most
famous early use of the Kalman filter
was in the Apollo navigation computer
that took Neil Armstrong to the moon,
and (most importantly) brought him
back. Today, Kalman filters are at work
in every satellite navigation device,
every smart phone, and many computer games.
(1)
where
xt is the state vector containing
the terms of interest for the system
(e.g., position, velocity, heading) at
time t
u t is the vector containing any
control inputs (steering angle, throttle setting, braking force)
Ft is the state transition matrix
which applies the effect of each system state parameter at time t-1 on
the system state at time t (e.g., the
position and velocity at time t-1
both affect the position at time t)
B t is the control input matrix
which applies the effect of each
1053-5888/12/$31.002012IEEE
(2)
where
zt is the vector of measurements
H t is the transformation matrix
that maps the state vector parameters into the measurement domain
v t is the vector containing the
measurement noise terms for each
observation in the measurement vector. Like the process noise, the measurement noise is assumed to be zero
mean Gaussian white noise with
covariance R t.
In the derivation that follows, we will
consider a simple one-dimensional tracking problem, particularly that of a train
moving along a railway line (see
Figure 1). We can therefore consider
some example vectors and matrices in
this problem. The state vector x t contains
the position and velocity of the train
xt
xt = ; E.
xo t
The train driver may apply a braking or
accelerating input to the system, which
ft
.
m
2
1 Tt
E and B t = (Tt) .
Ft = ;
0 1
> 2 H
Tt
(4)
Measurement (Noisy)
(3)
Prediction (Estimate)
[lecture NOTES]
continued
[FIG2] The initial knowledge of the system at time t = 0. The red Gaussian distribution represents the pdf providing the initial
confidence in the estimate of the position of the train. The arrow pointing to the right represents the known initial velocity of
the train.
# F T + FE [(x t-1
- xt t-1 ; t-1) w tT]
+ E [w t x t - 1
- xt t-1 ; t-1T] F T
+ E [w t w tT] .
Noting that the state estimation errors
and process noise are uncorrelated
E [(x t-1 - xt t-1 ; t-1) w Tt ]
= E 6w t (x t-1 - xt t-1 ; t-1) T@ = 0
& Pt ; t-1 = FE [(x t-1 - xt t-1 ; t-1) (x t-1
- xt t-1 ; t-1) T] F T + E [w t w tT]
& Pt ; t-1 = FPt-1 ; t-1 F T + Q t .
The measurement update equations are
given by
xt t ; t = xt t ; t-1 + K t (z t - H t xt t ; t-1) (5)
Pt ; t = Pt ; t-1 -K t H t Pt ; t-1 ,
where
(6)
Prediction (Estimate)
???
[FIG3] Here, the prediction of the location of the train at time t = 1 and the level of uncertainty in that prediction is shown. The
confidence in the knowledge of the position of the train has decreased, as we are not certain if the train has undergone any
accelerations or decelerations in the intervening period from t = 0 to t = 1.
Measurement (Noisy)
???
Prediction (Estimate)
[FIG4] Shows the measurement of the location of the train at time t = 1 and the level of uncertainty in that noisy measurement,
represented by the blue Gaussian pdf. The combined knowledge of this system is provided by multiplying these two pdfs
together.
Measurement (Noisy)
???
Prediction (Estimate)
[FIG5] Shows the new pdf (green) generated by multiplying the pdfs associated with the prediction and measurement of the
trains location at time t = 1. This new pdf provides the best estimate of the location of the train, by fusing the data from the
prediction and the measurement.
1 e - (r - n21) #
2v 1
2rv 21
1 e - (r - n21) . (8)
2v 1
2rv 21
1
2r
2 2
v1 v2
e -e
(r - n 1) 2
2
2v 1
1 e - (r - n22)
2v 2
2rv 22
+
(r - n 2) 2
2
2v 2
1
e
2
2rv fused
(r - n fused) 2
2
2v fused
(11)
where
2
n fused =
n1 v2 + n2 v1
2
v1 + v2
2
v1
(n 2 - n 1)
2
(12)
v1 + v2
and
2
v fused =
4
v1 v2
v
= v 21 - 2 1 2 .
2
2
v1 + v2
v1 + v2
(13)
y fused (r; n 1, v 1, n 2, v 2)
=
= n1 +
information from the prediction and measurement, and our best current estimate of
the system, is therefore given by the product of these two Gaussian functions
y 1 (r; n 1, v 1) _
1 e - (r - n22) . (9)
2v 2
2rv 22
y 2 (r; n 2, v 2) _
(10)
[lecture NOTES]
continued
y 1 (s; n 1, v 1, c) _
n1 2
m
c
2
2` v 1 j
c
cs-
(14)
and
1 e - ^ s - n22h ,
2v 2
2rv 22
(15)
2
y 2 (s; n 2, v 2) _
n1
`
+
v1
j c n2 2
v1
n1
2
j + v2
2
2
N
J
v1
O
K
c
O $ c n 2 - n 1 m.
& n fused = n 1 + K
c
K v1 2 + v2 O
K` j
2O
P
L c
(16)
(17)
c2
=`
v1
j -
v1
v1
It is now easy to see how the standard Kalman filter equations relate to
(17) and (18) derived above:
n fused = n 1 +e
v fused = v 1 - e
2
j + v2
2
Hv 21
o $ ^ n 2 - Hn 1 h
H v 21 + v 22
2
" xt t ; t = xt t ; t - 1 + K t ^z t = H t xt t ; t - 1 h
c
N
J
v 21
O 2
K
c
O v1 .
& v 2fused = v 21 - K
K v1 2 + v2 O c
K` j
2O
P
L c
2
2
= v 1 - KHv 1
(18)
=P t ; t-1 H Tt (H t P t ; t-1 H Tt +R t) -1 :
the Kalman gain.
Hv 21
" Kt
H v 21 + v 22
K=
Hv 21
2
o Hv 1
H v 21 + v 22
2
" Pt ; t = Pt ; t - 1 - K t H t P t ; t - 1 .
CONCLUSIONS
The Kalman filter can be taught using a
simple derivation involving scalar mathematics, basic algebraic manipulations,
and an easy-to-follow thought experiment. This approach should permit students lacking a strong mathematical
interest to understand the core mathematics underlying the Kalman filter in
an intuitive manner and to understand
that the recursive property of the filter
are provided by the unique multiplicative property of the Gaussian function.
AUTHOR
Ramsey Faragher (ramsey@cantab.net)
is a principal scientist at the BAE
Systems Advanced Technology Centre,
United Kingdom.
REFERENCES