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