kalman filter upload
TRANSCRIPT
UN
CLA
SSIF
IED
An ISO 9001:2008 Registered Company
Kalman FilterDistribution 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
UN
CLA
SSIF
IED
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 estimated states
22
FEB
12
Notice: Use or disclosure of data contained on this sheet is subject to the restriction on the title page of this document.
2
UN
CLA
SSIF
IED
3
Conceptual Overview – Example Definition
Lost on the 1-dimensional line, boat is not moving
Imagine that you are guessing your position by looking at the stars using sextant
Position function of time: y(t)
Assume Gaussian distributed measurements (errors)
y
UN
CLA
SSIF
IED
4
Conceptual Overview - Prediction
Sextant Measurement at t1: Mean = z1 and Variance = z1
Optimal estimate of position is: ŷ(t1) = z1
Variance of error [y(t1) - ŷ(t1)] estimate:
2e (t1) = 2
z1
Boat in same position at time t2 - Predictedposition is z1
0 10 20 30 40 50 60 70 80 90 1000
0.02
0.04
0.06
0.08
0.1
0.12
0.14
0.16
z
pro
babili
ty ŷ(t1) = z1
Predicted Position
What if we also had a GPS unit?
UN
CLA
SSIF
IED
5
Conceptual Overview - Measurement
• So we have the prediction ŷ-(t2)
• GPS Measurement at t2: Mean = z2 and Variance = z2
• Need to correct the prediction by Sextant due to measurement to get ŷ(t2)
0 10 20 30 40 50 60 70 80 90 1000
0.02
0.04
0.06
0.08
0.1
0.12
0.14
0.16
prediction ŷ-(t2)
State (by looking
at the stars at t2)
Measurement
using GPS z(t2)
UN
CLA
SSIF
IED
6
0 10 20 30 40 50 60 70 80 90 1000
0.02
0.04
0.06
0.08
0.1
0.12
0.14
0.16
measurement
z(t2)
corrected optimal
estimate ŷ(t2)
prediction ŷ-(t2)
Conceptual Overview – Data Fusion
Kalman filter: fuse measurement and prediction based on confidence
Corrected mean is the new optimal estimate of position
New variance is smaller than either of the previous two variances
What if the boat is moving?
UN
CLA
SSIF
IED
7
Conceptual Overview – Prediction Model
0 10 20 30 40 50 60 70 80 90 1000
0.02
0.04
0.06
0.08
0.1
0.12
0.14
0.16
ŷ(t2)
Naïve Prediction
(sextant) ŷ-(t3)
At time t3, boat moves with velocity dy/dt=u
Naïve approach: Shift probability to the right to predict
This would work if we knew the velocity exactly (perfect model)Try and predict where
it winds up.
UN
CLA
SSIF
IED
8
Conceptual Overview – Prediction Model
But you may not be so sure about the exact velocity
Better to assume imperfect model by adding Gaussian noise
dy/dt = u + w
Distribution for prediction moves and spreads out
Assumptions: prediction is linear, noise is Gaussian
0 10 20 30 40 50 60 70 80 90 1000
0.02
0.04
0.06
0.08
0.1
0.12
0.14
0.16
Prediction ŷ-(t3)
ŷ(t2)
Naïve Prediction
(sextant) ŷ-(t3)
UN
CLA
SSIF
IED
9
0 10 20 30 40 50 60 70 80 90 1000
0.02
0.04
0.06
0.08
0.1
0.12
0.14
0.16
Prediction ŷ-(t3) Sextant
Measurement z(t3) GPS
Corrected optimal estimate ŷ(t3)
Updated Sextant position using
GPS
• Now we take a measurement (GPS) at t3
• Need to once again correct the prediction (fusion)
• Recursive – rinse and repeat as time goes on
Conceptual Overview – Update
Update, recursively
UN
CLA
SSIF
IED
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.
UN
CLA
SSIF
IED
State Space Equations
22
FEB
12
Notice: Use or disclosure of data contained on this sheet is subject to the restriction on the title page of this document.
11
EstimatedState(now)
ObservedMeasurement
EstimatedState
(before)
ControlInput
How do you find A,B,H?AWGN?
UN
CLA
SSIF
IED
Update Equations
22
FEB
12
Notice: Use or disclosure of data contained on this sheet is subject to the restriction on the title page of this document.
12
Description Equation
State Prediction Where do we end up
Covariance Prediction When we get there, how much error
InnovationCompare Reality to Prediction
Innovation CovarianceCompare real error to predicted error
Kalman GainWhat do you trust more?
State UpdateNew estimate of where we are
Covariance UpdateNew estimate of error
Input
Output
Place holder
UN
CLA
SSIF
IED
Algorithm
22
FEB
12
Notice: Use or disclosure of data contained on this sheet is subject to the restriction on the title page of this document.
13
Prediction (Time Update)
(1) Project the state ahead
(2) Project the error covariance ahead
Correction (Measurement Update)
(1) Compute the Kalman Gain
(2) Update estimate with measurement zk
(3) Update Error Covariance
UN
CLA
SSIF
IED
Measuring Constant Voltage (Classic Example 1)
22
FEB
12
Notice: Use or disclosure of data contained on this sheet is subject to the restriction on the title page of this document.
14
UN
CLA
SSIF
IED
Predicting Trajectory of Projectile (Angry Bird)
22
FEB
12
Notice: Use or disclosure of data contained on this sheet is subject to the restriction on the title page of this document.
15
UN
CLA
SSIF
IED
Equations
22
FEB
12
Notice: Use or disclosure of data contained on this sheet is subject to the restriction on the title page of this document.
16
UN
CLA
SSIF
IED
Simulation Results
22
FEB
12
Notice: Use or disclosure of data contained on this sheet is subject to the restriction on the title page of this document.
17
UN
CLA
SSIF
IED
Modified TWS example
22
FEB
12
Notice: Use or disclosure of data contained on this sheet is subject to the restriction on the title page of this document.
18
*][:
},,,{:
wwEQCov
yxyxState y
UN
CLA
SSIF
IED
Derivation
22
FEB
12
Notice: Use or disclosure of data contained on this sheet is subject to the restriction on the title page of this document.
19
UN
CLA
SSIF
IED
What if Assumptions don’t hold
22
FEB
12
Notice: Use or disclosure of data contained on this sheet is subject to the restriction on the title page of this document.
20