Probabilistic Methods: Kalman Filtering (Intelligent

Probabilistic Methods:

Kalman Filtering

(Intelligent Autonomous Robotics)

Subramanian Ramamoorthy

School of Informatics

Story so far…

Estimation: Extract clean signal from noisy measurement

Assume a sensor model (y = Hx + e) where the state is

linearly transformed and corrupted by noise

Use one of many estimation procedures

Least squares

Weighted least squares

Recursive formulation of WLS

Maximum likelihood and Bayesian estimation

November 17, 2008 Kalman Filtering 2

Recap: Estimate Coefficients of a Cubic

Polynomial

Use k measurements at different t

to estimate coefficients x :

February 22, 2008 Estimation and Sensor Fusion 3

Main Theme for this lecture

Introduction of dynamics

How to estimate a state that evolves over time?

Incrementally - as measurements arrive one at a time

The Kalman filter algorithm: extends the least-squares

estimation concept to the case of dynamic systems

Discrete time, linear dynamics

Continuous time, linear dynamics

Brief discussion - Extension for nonlinear dynamics

November 17, 2008 Kalman Filtering 4

Historical Remarks

November 17, 2008 Kalman Filtering 5

Authoritative Resource on the subject:

http://www.cs.unc.edu/~welch/kalman/index.html

Let’s get started: Simple Estimation Problem

Consider a ship in choppy waters

Needs to navigate using a distant landmark (star)

To keep things simple, assume the model x = z + e

Can make multiple measurements of the same ground

truth (let us say – successively in time)

We know (a priori) one crucial fact – the variance of each

measurement

Then, what is the true value of the

distance to the landmark?

November 17, 2008 Kalman Filtering 6

Core Problem: Updating Estimated State

How to combine multiple sequential measurements?

November 17, 2008 Kalman Filtering 7

Answer: Variance-weighted Sum

November 17, 2008 Kalman Filtering 8

Remember this.We’ll see something similar again.

In pictures…

November 17, 2008 Kalman Filtering 9

What happens if we Add Dynamics?

November 17, 2008 Kalman Filtering 10

Effect of Adding System Dynamics

The uncertainty is amplified –

density function gets “squashed”

November 17, 2008 Kalman Filtering 11

Idea of the Kalman Filter

November 17, 2008 Kalman Filtering 12

Key Variables: First Two Statistical Moments

November 17, 2008 Kalman Filtering 13

Discrete Kalman Filter: Key Ingredients

Discrete process model

State change over time

Linear difference equation

Discrete measurement model

Relationship between state and measurement

Linear function

Model parameters

Process noise characteristics

Measurement noise characteristics

November 17, 2008 Kalman Filtering 14

Discrete Kalman Filter: Two Models

November 17, 2008 Kalman Filtering 15

Discrete Kalman Filter: Model Equations

November 17, 2008 Kalman Filtering 16

Complete Model Specification

November 17, 2008 Kalman Filtering 17

How the Filter Works

Time Update or

Prediction (a priori

estimates) - Project state

and covariance forward in

time

Measurement Update or

Correction (a posteriori

estimates) - Update

variables based on a

noise measurement

November 17, 2008 Kalman Filtering 18

Discrete Kalman Filter – In Equations

November 17, 2008 Kalman Filtering 19

Same idea as inslide #8

Discrete Kalman Filter: The Complete Loop

November 17, 2008 Kalman Filtering 20

Example: Estimating a (Noisy) Constant

November 17, 2008 Kalman Filtering 21

Recap: What is the Kalman Filter?

An optimal, recursive data processing algorithm

Optimal in what sense?

If the system dynamics are linear

And system/sensor noise is Gaussian and white

Then, there is no alternate algorithm with lower MSE

Optimal Best Linear Unbiased Estimator

Bayesian View of the Kalman Filter:

Propagate conditional probability density of desired

quantities, conditioned on knowledge of actual data

November 17, 2008 Kalman Filtering 22

Dealing with Nonlinearities

November 17, 2008 Kalman Filtering 23

Extended Kalman Filter - Setup

November 17, 2008 Kalman Filtering 24

Extended Kalman Filter - Computation

Notice that the above equations look a lot like the linear

system dynamics and measurement equations

So, we can define a Kalman filter over this error

And then, use that linear error estimate to drive the main

estimation loop

November 17, 2008 Kalman Filtering 25

Extended Kalman Filter: The Loop

November 17, 2008 Kalman Filtering 26

Reference:

S. Thrun et al., Probabilistic Robotics, MIT Press, 2005.

Several pictures and equations in this presentation are

taken from the tutorial by Welch and Bishop

(http://www.cs.unc.edu/~welch/kalman/)

November 17, 2008 Kalman Filtering 27

Kalman Filter: Continuous Time-variant case

November 17, 2008 Kalman Filtering 28