Embed Size (px)
Citation preview
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
