The Kalman Filter: A Study of Covariances
description
Transcript of The Kalman Filter: A Study of Covariances
David WheelerKyle Ingersoll
EcEn 670
December 5, 2013
A Comparison between Analytical and Simulated Results
The Kalman Filter: A Study of Covariances
2
Kalman Overview:
Common Applications1:• Inertial Navigation (IMU + GPS)• Global Navigation Satellite Systems• Estimating Constants in the Presence of
Noise• Simultaneous Localization and Mapping
(SLAM)• Object Tracking In Computer Vision• Economics
Predict (P) Forward One Step
Update (U)Use Measurements If Available
P P P P P P P P
U U U
3
Kalman Intuition: Predict Using Underlying Model
1 2 3 4 5 ?
4
Kalman Intuition: Predict Using Underlying Model
1 2 3 4 5 ?
5
Kalman Intuition: Update by Weighing Measurement and Model
1 2 3 4 5 ?
Measurement,
Model Estimate,
Residual
6
Kalman Intuition: Update by Weighing Measurement and Model
1 2 3 4 5 ?
Measurement Covariance,
State Covariance,
KalmanGain,
7
Kalman Intuition: Summary
1 2 3 4 5 ?
KalmanGain,
Predict Step(1)Predict state forward one step.(2)Predict covariance forward one step.Update Step(1)Determine Kalman Gain
(optimal weighting between and ).(2)Update state using Kalman gain and
residual.(3)Update state covariance .
8
Prediction Derivation: Linear:
Prediction Step: Linear Example
Current State
Recent State
Process Noise
Recent Input
k=1
𝑥
𝑦 Δ𝑥
Δy
𝑥 𝑙𝑜𝑐
𝑦 𝑙𝑜𝑐
k=2
Example 1
============
9
Update: Measurement:
Update Step: Linear Example
Measurement Model’s Guess for Measurement
Noise ResidualWeighting
𝑓 (𝑃 𝐾 ,𝑅)
10
Results: Linear Example
Ten Steps “Predict" Only:
500 runs 10 time steps
11
Results: Linear Example
Experimental covariance (Cyan dots)
MATLAB cov command Analytical covariance
(Red solid line)
Individual runs (Magenta dots)(Dark blue dots)
12
Results: Linear Example
Update Step:
500 runs
0.01
13
Results: Linear Example
Experimental covariance(Green dots)
MATLAB cov command Analytical covariance
(Magenta solid line)
Individual runs(Dark blue dots)
14
Linear Example: Comparing Covariance Trends
Experimental Covariance (Blue)Analytical Covariance (Red)
15
Linear Example: Convergence of Covariances
16
Process
Non-Linear Example
𝑥𝑘=[ 𝑥 𝑙𝑜𝑐𝑦 𝑙𝑜𝑐
𝜃h𝑒𝑎𝑑𝑖𝑛𝑔]𝑢=[𝐷 𝑓𝑜𝑟𝑤𝑎𝑟𝑑
Δ𝜃h ]
𝑥
𝑦
Δ𝑥
Δy
𝑥 𝑙𝑜𝑐
𝑦 𝑙𝑜𝑐
Δ𝜃h𝜃h
Example 2
𝑓 𝑥=[𝑥 𝑙𝑜𝑐+𝐷 𝑓 ∗ cos (𝜃h+Δ𝜃h2
)
𝑦 𝑙𝑜𝑐+𝐷 𝑓 ∗ sin (𝜃h+Δ𝜃h2
)
𝜃h+Δ𝜃h]
𝑥𝑏
𝑦 𝑏𝑧𝑘
17
Results: Non-linear Example
30 Time Steps 500 runs Input: Input Noise is Gaussian,
±5%
(known to start at origin) Analytical Covariance
(Cyan Ellipse) Beacon Location
(Red Circle)
18
Results: Non-linear Example
Beacon Location(Red Circle)
Measurement (7/500)(Green Lines)
Gaussian Noise on Measurement(Red Xs)
Covariance (before update)Analytical (Thin
Cyan)Experimental (Thick
Cyan)
𝑅
19
Results: Non-linear Example
Covariance Before update
Analytical (Thin Cyan) Experimental (Thick Cyan)
After update Analytical (Thin
Magenta) Experimental (Thick
Magenta)
Note – the update step reduces the uncertainty in the direction of the measurement only!
20
Under certain conditions, a Kalman filter causes the covariance to converge
Analytical and simulated covariances match closely Analytical and simulated covariances converge quickly if
seeded with different values Individual measurements can significantly reduce the
covariance of the state estimate
Conclusion
21
Questions & Discussion