Professional Documents
Culture Documents
Kleeman Kalman Basics
Kleeman Kalman Basics
Kalman Filtering
Lindsay Kleeman
Department of Electrical and Computer Systems Engineering
Monash University, Clayton
Introduction
Objectives:
1. Provide a basic understanding of Kalman Filtering and assumptions
behind its implementation.
2. Limit (but cannot avoid) mathematical treatment to broaden appeal.
3. Provide some practicalities and examples of implementation.
4. Provide C++ software overview.
Word examples:
cov( x1 , x2 )
=
E [( x1 x1 )( x2 x2 )]
( x1 x1 )( x 2 x 2 ) p( x1 , x1 )dx1dx 2
2x1 x2
12
x21x2
x1 x 2
, 1 12 +1
cov( x )
=
E [( x x )( x x )' ]
Pxx
2x i x j
D to E
D
C to D
A to B
B to C
C
B
3
10000 Monte-Carlo runs for k L = k R = 10 m 2 , B=0.5 m
1
Means
Covariance Matrix
10
11
x ( k + 1) = F ( k )x ( k ) + G ( k ) u ( k ) + v ( k )
k = 0,1,...
where x(k) is the nx dimensional state vector, u(k) is the nu dimensional known
input vector, v(k) is (unknown) zero mean white process noise with covariance
E [ v( k ) v( k )' ] = Q ( k )
Measurement equation:
z( k ) =
(k ) (k ) +
(k )
k = 1,....
w(k) is unknown zero mean white measurement noise with known covariance
E[ w ( k ) w ( k )' ] = R( k )
12
..
y(t ) = g
y ( t ) = y ( t0 ) g ( t t0 )
g
y ( t ) = y ( t 0 ) + y ( t0 )( t t 0 ) ( t t0 )2
2
As a discrete time system with time increment of t-t0=1
13
g
y ( k + 1) = y ( k ) + y ( k )
2
the height y(k+1) depends on the previous velocity and height at time k.
We can define the state as
14
1 1
0.5
x (k + 1) =
x (k) + ( g )
0 1
1
= F x( k ) + G u
Assuming we observe or measure the height of the ball directly.
measurement equation is:
The
H x ( k ) + w( k )
15
16
State Estimation
0. Known are x$ ( k | k ), u( k ), P( k | k ) and the new measurement z(k+1).
1. State Prediction x$ ( k + 1| k ) = F( k ) x$ ( k | k ) + G ( k ) u( k )
2. Measurement Prediction: z$ ( k + 1| k ) = H ( k ) x$ ( k + 1| k )
3. Measurement Residual: v( k + 1) = z( k + 1) z$ ( k + 1| k )
Time update
measurement
update
17
S( k + 1) = H ( k + 1)P( k + 1| k )H ( k + 1)' + R ( k + 1)
3. Filter Gain W( k + 1) = P( k + 1| k )H ( k + 1)' S( k + 1) 1
4. Updated state covariance
18
19
This is the Riccati equation and can be obtained from the Kalman filter
equations above.
The solution of the Riccati equation in a time invariant system converges to
steady state (finite) covariance if the pair {F, H} is completely observable (ie
the state is visible from the measurements alone).
20
FH
Q0 =
...
nx 1
FH
has full rank of nx.
The convergent solution to the Riccati equation yields the steady state gain for
the Kalman Filter.
21
22
True position
101
measurement
99
97
95
93
Estimate
91
89
87
85
1
23
True values
Estimates
Errors in Estimate
t=kT
x1
x2
x$1 ( k )
x$ 2 ( k )
P11(k)
P22(k)
100.0
95.0
1.0
10.0
1.0
99.5
-1.0
100.0
99.63
0.38
0.92
0.92
98.0
-2.0
97.9
98.43
-1.16
0.67
0.58
95.5
-3.0
94.4
95.21
-2.91
0.66
0.30
92.0
-4.0
92.7
92.35
-3.70
0.61
0.15
87.5
-5.0
87.3
87.68
-4.84
0.55
0.08
z(k)
24
25
Validation Gate
Recall the measurement prediction covariance:
S( k + 1) = H ( k + 1)P( k + 1| k )H ( k + 1)' + R ( k + 1)
and the measurement prediction: z$ ( k + 1| k ) = H ( k ) x$ ( k + 1| k )
and measurement residual: v( k + 1) = z( k + 1) z$ ( k + 1| k )
measurement
update
e 2 = v( k + 1)S 1 ( k + 1)v' ( k + 1) g 2
where g2 is chosen to for a confidence level. Normalised error e2 varies as a
Chi-Squared distribution with number of measurements degrees of freedom.
26
27
29
30
31
Kalman filtering classes, for defining and implementing KF, EKF and IEKF
-allows numerical checking of Jacobian functions
Software source is available to collaborators for non-commercial use provided
appropriately acknowledged in any publication. Standard disclaimers apply!
contact Lindsay.Kleeman@monash.edu.au
33
Further Reading
Bar-Shalom and Xiao-Rong Li, Estimation and Tracking: Principles,
Techniques and Software, Artech House Boston, 1993.
Jazwinski, A. H. . Stochastic Processes and Filtering Theory. New York,
Academic Press, 1970.
Bozic, S M, Digital and Kalman Filtering, Edward Arnold, London 1979.
Maybeck, P. S. The Kalman filter: An introduction to concepts. Autonomous
Robot Vehicles. I. J. Cox and G. T. Wilfong. New York, Springer-Verlag: 194204, 1990.
34
35
Our approach integrates noise over the entire path for a closed form error
covariance - more efficient and accurate
36
37