1. An ISO 9001:2008 Registered Company
UNCLASSIFIED
Kalman Filter
Distribution Restrictions: < Enter any appropriate distribution restrictions in title master, (eg. Distribution D)>
Data Rights: <Enter any applicable date rights restrictions, (eg. SBIR data rights or other similar information>;
DP-FM-016, Rev 2
Effective Date: 22 February 2012
2. Kalman Filter Facts
Dr. Rudolf Kalman is alive and well today (82 years old)
Important and used everywhere: GPS (predict update
location), surface to air missiles (hit target), machine
vision (track targets), brain computer interface
Not really a filter, it is an optimal estimator (infers
parameters of interest from indirect, noise
measurements)
It is recursive – so when a new measurement arrives it is
processed and you get a new estimate
Performs Data Fusion usually between measured and
UNCLASSIFIED
estimated states
22FEB12
2 Notice: Use or disclosure of data contained on this sheet is subject to the restriction on the title page of this
document.
3. Conceptual Overview – Example Definition
y
Lost on the 1-dimensional line, boat is not moving
Imagine that you are guessing your position by looking at
the stars using sextant
UNCLASSIFIED
Position function of time: y(t)
Assume Gaussian distributed measurements (errors)
3
4. Conceptual Overview - Prediction
0.16
Sextant Measurement 0.14
at t1: Mean = z1 and 0.12
Variance = z1 0.1
probability
ŷ(t1) = z1
Optimal estimate of 0.08 Predicted Position
position is: ŷ(t1) = z1 0.06
Variance of error 0.04
[y(t1) - ŷ(t1)] estimate: 0.02
2 (t ) = 2 0
0 10 20 30 40 50 60 70 80 90 100
e 1 z1 z
Boat in same position
UNCLASSIFIED
at time t2 - Predicted What if we also had a
position is z1 GPS unit?
4
5. Conceptual Overview - Measurement
0.16
0.14 prediction ŷ-(t2)
State (by looking
0.12 at the stars at t2)
0.1
0.08 Measurement
using GPS z(t2)
0.06
0.04
0.02
0
0 10 20 30 40 50 60 70 80 90 100
• So we have the prediction ŷ-(t2)
• GPS Measurement at t2: Mean = z2 and Variance = z2
UNCLASSIFIED
• Need to correct the prediction by Sextant due to
measurement to get ŷ(t2)
5
6. Conceptual Overview – Data Fusion
0.16 Kalman filter: fuse
corrected optimal
0.14
estimate ŷ(t2) measurement and
0.12 prediction ŷ-(t2) prediction based on
0.1
confidence
0.08 measurement
z(t2) Corrected mean is
0.06
the new optimal
0.04
0.02
estimate of position
0
0 10 20 30 40 50 60 70 80 90 100
New variance is
smaller than either
UNCLASSIFIED
What if the boat is of the previous two
moving? variances
6
7. Conceptual Overview – Prediction Model
0.16
ŷ(t2)
At time t3, boat
0.14
moves with velocity
0.12
Naïve Prediction
dy/dt=u
0.1 (sextant) ŷ-(t3)
Naïve approach:
0.08
Shift probability to
0.06
0.04
the right to predict
0.02
This would work if
0
0 10 20 30 40 50 60 70 80 90 100
we knew the velocity
exactly (perfect
UNCLASSIFIED
Try and predict where model)
it winds up.
7
8. Conceptual Overview – Prediction Model
0.16
ŷ(t2)
But you may not be
0.14
so sure about the
0.12
Naïve Prediction
exact velocity
0.1 (sextant) ŷ-(t3)
Better to assume
0.08
Prediction ŷ-(t3)
imperfect model by
0.06
0.04
adding Gaussian
0.02
noise
0
0 10 20 30 40 50 60 70 80 90 100
dy/dt = u + w
Distribution for
UNCLASSIFIED
Assumptions: prediction is prediction moves
linear, noise is Gaussian and spreads out
8
9. Conceptual Overview – Update
0.16
Corrected optimal estimate ŷ(t3)
• Now we take a
0.14 Updated Sextant position using
GPS
measurement (GPS)
0.12
at t3
0.1 Measurement z(t3) GPS
• Need to once again
0.08
correct the
0.06
Prediction ŷ-(t3) Sextant
0.04
prediction (fusion)
0.02
• Recursive – rinse
0
0 10 20 30 40 50 60 70 80 90 100
and repeat as time
goes on
UNCLASSIFIED
Update, recursively
9
10. Conceptual Overview
Optimal estimator only if:
Prediction model is linear (function of measurements)
All error (noise) is Gaussian: model error, measurement
error
Why is Kalman Filter so popular
Good results in practice due to optimality and structure.
Convenient form for online real time processing.
Easy to formulate and implement given a basic
understanding.
Measurement equations need not be inverted.
UNCLASSIFIED
10
11. State Space Equations
Estimated Estimated Control
State State Input
(now) (before)
UNCLASSIFIED
Observed
Measurement How do you find A,B,H?
22FEB12
AWGN?
11 Notice: Use or disclosure of data contained on this sheet is subject to the restriction on the title page of this
document.
12. Input
Output
Update Equations Place holder
Description Equation
State Prediction
Where do we end up
Covariance Prediction
When we get there, how much error
Innovation
Compare Reality to Prediction
Innovation Covariance
Compare real error to predicted error
Kalman Gain
What do you trust more?
State Update
UNCLASSIFIED
New estimate of where we are
22FEB12
Covariance Update
New estimate of error
12 Notice: Use or disclosure of data contained on this sheet is subject to the restriction on the title page of this
document.
13. Algorithm
Correction (Measurement Update)
Prediction (Time Update)
(1) Compute the Kalman Gain
(1) Project the state ahead
(2) Update estimate with measurement zk
(2) Project the error covariance ahead
(3) Update Error Covariance
UNCLASSIFIED
22FEB12
13 Notice: Use or disclosure of data contained on this sheet is subject to the restriction on the title page of this
document.
14. Measuring Constant Voltage (Classic Example 1)
UNCLASSIFIED
22FEB12
14 Notice: Use or disclosure of data contained on this sheet is subject to the restriction on the title page of this
document.
15. Predicting Trajectory of Projectile (Angry Bird)
UNCLASSIFIED
22FEB12
15 Notice: Use or disclosure of data contained on this sheet is subject to the restriction on the title page of this
document.
16. Equations
UNCLASSIFIED
22FEB12
16 Notice: Use or disclosure of data contained on this sheet is subject to the restriction on the title page of this
document.
17. Simulation Results
UNCLASSIFIED
22FEB12
17 Notice: Use or disclosure of data contained on this sheet is subject to the restriction on the title page of this
document.
18. Modified TWS example
State : y
{ x , y , x , y}
Cov : Q E [ ww *]
UNCLASSIFIED
22FEB12
18 Notice: Use or disclosure of data contained on this sheet is subject to the restriction on the title page of this
document.
19. Derivation
UNCLASSIFIED
22FEB12
19 Notice: Use or disclosure of data contained on this sheet is subject to the restriction on the title page of this
document.
20. What if Assumptions don’t hold
UNCLASSIFIED
22FEB12
20 Notice: Use or disclosure of data contained on this sheet is subject to the restriction on the title page of this
document.