I ntroduction to Mobile Robotics Bayes Filter Kalm an Filter - PowerPoint PPT Presentation

I ntroduction to Mobile Robotics Bayes Filter – Kalm an Filter Wolfram Burgard 1

Bayes Filter Rem inder  Prediction  Correction

Bayes Filter Rem inder  Prediction  Correction

Kalm an Filter  Bayes filter with Gaussians  Developed in the late 1950's  Most relevant Bayes filter variant in practice  Applications range from economics, weather forecasting, satellite navigation to robotics and many more.  The Kalman filter “algorithm” is a couple of m atrix m ultiplications ! 5

Gaussians µ Univariate - σ σ µ Multivariate

Gaussians 1 D 3 D 2 D

Properties of Gaussians  Univariate case

Properties of Gaussians  Multivariate case (where division "–" denotes matrix inversion)  We stay Gaussian as long as we start with Gaussians and perform only linear transform ations

Discrete Kalm an Filter Estimates the state x of a discrete-time controlled process that is governed by the linear stochastic difference equation with a measurement 10

Com ponents of a Kalm an Filter Matrix ( n × n ) that describes how the state evolves from t -1 to t without controls or noise. Matrix ( n × l ) that describes how the control u t changes the state from t -1 to t . Matrix ( k × n ) that describes how to map the state x t to an observation z t . Random variables representing the process and measurement noise that are assumed to be independent and normally distributed with covariance Q t and R t respectively. 11

Kalm an Filter Updates in 1 D prediction measurement correction It's a weighted mean! 13

Kalm an Filter Updates in 1 D How to get the blue one? Kalm an correction step 14

Kalm an Filter Updates in 1 D How to get the magenta one? State prediction step

Kalm an Filter Updates prediction correction measurement

Linear Gaussian System s: I nitialization Initial belief is normally distributed:

Linear Gaussian System s: Dynam ics Dynamics are linear functions of the state and the control plus additive noise:

Linear Gaussian System s: Dynam ics

Linear Gaussian System s: Observations Observations are a linear function of the state plus additive noise:

Linear Gaussian System s: Observations

Kalm an Filter Algorithm Algorithm Kalm an_ filter ( µ t-1 , Σ t-1 , u t , z t ): 1. 2. Prediction: 3. 4. 5. Correction: 6. 7. 8. Return µ t , Σ t 9.

Kalm an Filter Algorithm

The Prediction-Correction-Cycle Prediction 25

The Prediction-Correction-Cycle Correction 26

The Prediction-Correction-Cycle Prediction Correction 27

Kalm an Filter Sum m ary  Only two parameters describe belief about the state of the system  Highly efficient: Polynomial in the measurement dimensionality k and state dimensionality n : O(k 2.376 + n 2 )  Optim al for linear Gaussian system s !  However: Most robotics systems are nonlinear !  Can only model unimodal beliefs