Professional Documents
Culture Documents
I
k
A
k
B
Kalman Filter
k
w
1 k k k k k k
k k k k
x A x B u w
z H x v
+
= + +
= +
l
k
n m
k
l n
k
m n
k
m
k
n
k
m
k
n
k
U
H
B
A
v
w
z
x
9 e
9 e
9 e
9 e
9 e
9 e
9 e
9 e
Noise Statistics
Process Noise
Covariance
Measurement Noise
Covariance
Gaussian
Process & measurement noises are independent,
white, zero mean and normal distributed
0
0
k T
n k
k T
n k
Q n k
E w w
n k
R n k
E v v
n k
=
(
=
=
(
=
=
0 and
T
n k
E v w n k
(
=
) , 0 ( ~ ) (
) , 0 ( ~ ) (
k
k
R N v P
Q N w P
STATE ESTIMATION
a-priori State Estimate
a-priori (predicted)
state estimate
a-priori estimation
error
1 1 1 1
k k k k k
x A x B u
= + Update equation
k
x
n
9 e
=
k k k
x x e
a-posteriori State Estimate
a-posteriori state
estimate
a-posteriori estimation
error
( )
k k k k k k
x x K z H x
= +
Update equation
Innovation
(residual)
Kalman Gain
k k k
x x e
=
n
9 e k
x
Covariance Matrices
T
k k k
P E e e
(
=
a-priori estimation
error covariance
a-posteriori estimation
error covariance
T
k k k
P E e e
(
=
KALMAN GAIN COMPUTATION
Optimization Criterion
Choose Kalman gain that minimizes a-posteriori error covariance
Kalman Filter
T
k k k
P E e e
(
=
Steps to Kalman Gain Computation
Substitute into
Substitute into
Perform expectations in
Choose so that terms containing are
Solve for to get
Kalman Filter
( )
k k k k k k
x x K z H x
= +
k k k
e x x =
( )
k k k k k k k
e x x K z H x
=
T
k k k
P E e e
(
=
k
K
k
K
T
k k k
P E e e
(
=
( )
1
T T
k k k k k k k
K P H H P H R
= +
0
k
K
Kalman Gain
( )( )
( ) { } ( ) { }
( ) { }
( ) { }
(
=
(
=
(
=
(
= +
(
+
(
T
k k k
T
k k k k
T
k k k k k k k k k k k k
k k k k k k k k
T
k k k k k k k k
P E e e
E x x x x
E x x K z H x x x K z H x
E x x K H x v H x
x x K H x v H x
a-posteriori estimate
error covariance
Kalman Gain (continued)
( ) { }
( ) { }
( )( ) ( )( )
( )( )
( )( )
= +
(
+
(
= +
+
(
+ + +
(
k k k k k k k k k
T
k k k k k k k k
T T
T
k k k k k k k k k k k k
T
k k k k k k k k
T
T
k k k k k k k k k k k k
P E x x K H x v H x
x x K H x v H x
E x x x x x x H x v H x K
K H x v H x x x
K H x v H x H x v H x K
Kalman Gain (continued)
( ) ( ) ( ) ( )
( ) ( )
( ) ( )
( ) ( )
( ) ( )
( )
T T
T
k k k k k k k k k k k k k
T
k k k k k k k k
T
T
k k k k k k k k k k k k
T
T
k k k k k k k k k k
T
k k k k k k k k
k k k k k k
P E x x x x x x H x v H x K
K H x v H x x x
K H x v H x H x v H x K
P P E x x H x v H x K
E K H x v H x x x
E K H x v H x
= +
+
(
+ + +
(
(
= +
(
(
+
(
+ +
( )
T
T
k k k k k k
H x v H x K
(
+
(
Kalman Filter
Kalman Gain (continued)
( ) ( )
( ) ( )
( ) ( )
T
T
k k k k k k k k k k
T
k k k k k k k k
T
T
k k k k k k k k k k k k
P P E x x H x v H x K
E K H x v H x x x
E K H x v H x H x v H x K
(
= +
(
(
+
(
(
+ + +
(
T
k k k
E v v R
(
=
( )
T T T T
k k k k k k k k k k k k k k
P P K H P P H K K H P H R K
= + +
Linear in Kalman gain
Quadratic in Kalman gain
Kalman Gain (continued)
Kalman Filter
T T T T
k k k k k k k k k k k k
P P K H P P H K K S S K
= +
Put
( )
T T T T
k k k k k k k k k k k k k k
P P K H P P H K K H P H R K
= + +
into
T T T T T T
k k k k k k k k k k k k
P P K H P P H K K S S K
= + + ++ ++
Used for completing squares
k
T
k k k
T
k k
R H P H S S + =
A
Kalman Gain (continued)
Kalman Filter
T T T T T T
k k k k k k k k k k k k
P P K H P P H K K S S K
= + + ++ ++
( )( )
T
T
k k k k k k
T T T T T T T
k k k k k k k k k
P P K S K S
P K S S K K S S K
= + + + ++
= + + + + ++ ++
T T
k k k
P H S
+ =
T T T T T
k k k k k k k k k k
K S S K K H P P H K
+ + + = +
Compare the two equations
Holds if
Kalman Gain (continued)
Kalman Filter
( )( )
T
T
k k k k k k
P P K S K S
= + + + ++
T T
k k k
P H S
+ =
Can be minimized if
1
k k
k k
K S
K S
= +
= +
( )
( )
1
1
1
T T
k k k k k
T T
k k k k
T T
k k k k k k
K P H S S
P H S S
P H H P H R
=
=
= +
k
T
k k k
T
k k
R H P H S S + =
A
Interpretation of Kalman Gain
( )
1
T T
k k k k k k k
K P H H P H R
= +
Kalman Gain weights residual
more heavily
1
0
lim
k
k k
R
K H
=
0
lim 0
k
k
P
K
=
Kalman Gain weights residual less
heavily
( )
k k k k k k
x x K z H x
= +
residual
Kalman Gain
Measurement error
covariance
a-priori estimation
error covariance
A- POSTERIORI ESTIMATE ERROR
COVARIANCE
k
P
Computation of (continued)
k
P
T T T T
k k k k k k k k k k k
P P K H P P H K P H K
= +
( )
k k k k
P I K H P
=
Computation of
( )
T T T T
k k k k k k k k k k k k k k
P P K H P P H K K H P H R K
= + +
Substitute
k
P
( )
1
T T
k k k k k k k
K P H H P H R
= +
into
( ) ( )
1
T T
k k k k k k k k
T T T T
k k k k k k k k k k k
P P K H P P H K
P H H P H R H P H R K
=
+ + +
I
A-PRIORI ESTIMATE ERROR
COVARIANCE
k
P
Computation of
k
P
1 1
k k k
x A x
=
a-priori state
estimate
( )
1 1 1 1 1
1 1 1 1
1 1 1
k k k
k k k k k
k k k k
k k k
e x x
A x w A x
A x x w
A e w
=
= +
= +
= +
a-priori state
estimate error
Computation of (continued)
Kalman Filter
k
P
( ) ( )
1 1 1 1 1 1
1 1 1 1 1 1 1
1 1 1 1 1
T
k k k
T
k k k k k k
T T T
k k k k k k k
T T T
k k k k k
P E e e
E A e w A e w
A E e e A A E e E w
E w E e A E w w
(
=
(
= + +
(
( ( (
= +
( ( (
+ +
a-priori state
estimate error
covariance
1 k
Q
1 1 1 1
T
k k k k k
P A P A Q
= +
1 1 1
T
k k k
P E e e
(
=
| | | | 0 0
1 1
= . =
T
k k
w E w E
Discrete Kalman Filter
1 1 1 1
1 1 1
k k k k k
T
k k k k k
x A x B u
P A P A Q
= +
= +
Kalman Filter
( )
( )
( )
1
T T
k k k k k k k
k k k k k k
k k k k
K P H H P H R
x x K z H x
P I K H P
= +
= +
=
Time Update
(predict)
Measurement Update
(Correct)
The time update projects
the current state estimate
ahead in time
The measurement update
adjusts the projected estimate by an
actual measurement at that time
Kalman Filter - Complete Picture
Kalman Filter
1 1 1
T
k k k k k
P A P A Q
= +
1 1 1 1
k k k k k
x A x B u
= +
( )
1
T T
k k k k k k k
K P H H P H R
= +
( )
k k k k k k
x x K z H x
= +
( )
k k k k
P I K H P
=
1
k
x
1 k
P
MATLAB SIMULATION
SCALAR KALMAN FILTER
Let's examine the use of a Kalman filter to estimate the
value of a system with a gain a=1. The output gain (h) is
set equal to 3; h=3. The measurement noise (v) covariance
(R) is set to 1; R=1. The initial guess of the state is 1.5;
x0=1.5, and the initial estimate of the a posteriori
covariance is 1; p0=1.
ANALYSING DIFFERENT CASES
Case 1 - No Process Noise
Case 2 - Add Some Process Noise
Case 3 - Increase Process Noise
Case 4 - Increase the Measurement Noise
CASE 1:NO PROCESS NOISE
a=1, h=3, Q=0, R=1; x0=1.5, p0=1
Case 2 - Add Some Process Noise
a=1, h=3, Q=0.01, R=1; x0=1.5, p0=1
Case 3 - Increase Process Noise
a=1, h=3, Q=0.1, R=1; x0=1.5, p0=1
Case 4 - Increase the Measurement Noise
a=1, h=3, Q=0.01, R=2; x0=1.5, p0=1
REFERENCE
Greg Welch and Gary Bishop An Introduction to the Kalman Filter ,
TR 95-041 , Department of Computer Science , University of North
Carolina at Chapel Hill , Chapel Hill, NC 27599-3175 , Updated: Monday,
July 24, 2006
Kalman, R. E. 1960. A New Approach to Linear Filtering and Prediction
Problems, Transaction of the ASME--Journal of Basic Engineering, pp.
35-45 (March 1960).
Saa Fratina and Samo Korpar Using Kalman Filter to Track
Particles , 2004-01-13