Introduction to Mobile Robotics Bayes Filter Kalman Filter Wolfram Burgard, Cyrill Stachniss, Maren Bennewitz, Giorgio Grisetti, Kai Arras 1
Bayes Filter Reminder 1. Algorithm Bayes_filter( Bel(x),d ): 2. η=0 3. If d is a perceptual data item z then 4. For all x do 5. 6. 7. For all x do 8. 9. Else if d is an action data item u then 10. For all x do 11. 12. Return Bel (x) 2
Kalman Filter Bayes filter with Gaussians Developed in the late 1950's Most relevant Bayes filter variant in practice Applications range from economics, wheather forecasting, satellite navigation to robotics and many more. The Kalman filter "algorithm" is a bunch of matrix multiplications! 3
Gaussians µ Univariate -σ σ µ Multivariate
Gaussians 1D 3D 2D Video
Properties of Gaussians
Multivariate Gaussians (where division " " denotes matrix inversion) We stay Gaussian as long as we start with Gaussians and perform only linear transformations
Discrete Kalman Filter Estimates the state x of a discrete-time controlled process that is governed by the linear stochastic difference equation with a measurement 8
Components of a Kalman Filter Matrix (nxn) that describes how the state evolves from t to t-1 without controls or noise. Matrix (nxl) that describes how the control u t changes the state from t to t-1. Matrix (kxn) 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. 9
Bayes Filter Reminder Prediction Correction
Kalman Filter Updates in 1D 11
Kalman Filter Updates in 1D ) = µ t = µ t + K t (z t C t µ t ) Σ t = (I K t C t )Σt with K t = ΣtC t T (C t ΣtC t T + R t ) 1 How to get the blue one? > Kalman correction step 12
Kalman Filter Updates in 1D How to get the magenta one? > State prediction step ) = µ t = A t µ t 1 + B t u t Σt = A t Σ t 1 A T t + Q t
Kalman Filter Updates
Linear Gaussian Systems: Initialization Initial belief is normally distributed:
Linear Gaussian Systems: Dynamics Dynamics are linear function of state and control plus additive noise: p(x t u t,x t 1 ) = N( x t ;A t x t 1 + B t u t,q ) t ) = p(x t u t,x t 1 ) 1 ) dx t 1 ~ N( x t ;A t x t 1 + B t u t,q ) t ~ N( x t 1 ;µ t 1,Σ ) t 1
Linear Gaussian Systems: Dynamics ) = p(x t u t, x t 1 ) 1 ) dx t 1 ~ N( x t ;A t x t 1 + B t u t,q t ) ~ N( x t 1 ;µ t 1,Σ t 1 ) ) = η exp 1 2 (x t A t x t 1 B t u t ) T Q 1 t (x t A t x t 1 B t u t ) exp 1 2 (x µ t 1 t 1 )T Σ 1 t 1 (x t 1 µ t 1 ) dx t 1 ) = µ t = A t µ t 1 + B t u t Σt = A t Σ t 1 A T t + Q t
Linear Gaussian Systems: Observations Observations are linear function of state plus additive noise: p(z t x t ) = N( z t ;C t x t,r ) t ) = η p(z t x t ) ) ~ N( z t ;C t x t,r ) t ~ N( x t ;µ ) t,σt
Linear Gaussian Systems: Observations ) = η p(z t x t ) ) ~ N( z t ;C t x t,r t ) ~ N( x t ;µ ) t,σt ) = η exp 1 2 (z C x t t t )T R 1 t (z t C t x t ) exp 1 2 (x µ t t )T Σ 1 t (x t µ t ) ) = µ t = µ t + K t (z t C t µ t ) Σ t = (I K t C t )Σt with K t = ΣtC t T (C t ΣtC t T + R t ) 1
Kalman Filter Algorithm 1. Algorithm Kalman_filter( µ t-1, Σ t-1, u t, z t ): 2. Prediction: 3. µ t = A t µ t 1 + B t u t 4. Σt = A t Σ t 1 A T t + Q t 5. Correction: 6. K t = ΣtC T t (C t ΣtC T t + R t ) 1 7. µ t = µ t + K t (z t C t µ t ) 8. Σ t = (I K t C t )Σt 9. Return µ t, Σ t
Kalman Filter Algorithm
Kalman Filter Algorithm Prediction Observation Matching Correction
The Prediction-Correction-Cycle Prediction ) = µ t = a t µ t 1 + b t u t σ 2 t = a t2 σ 2 2 t + σ act,t ) = µ t = A t µ t 1 + B t u t Σt = A t Σ t 1 A T t + Q t 23
The Prediction-Correction-Cycle ) = µ t = µ t + K t (z t µ t ), K σ 2 2 t = (1 K t )σ t = t ) = µ t = µ t + K t (z t C t µ t ),K Σ t = (I K t C t = ΣtC T t (C t ΣtC T t + R t ) 1 t )Σt σ t 2 σ 2 2 t + σ obs,t Correction 24
The Prediction-Correction-Cycle Prediction ) = µ t = µ t + K t (z t µ t ), K σ 2 2 t = (1 K t )σ t = t ) = µ t = µ t + K t (z t C t µ t ),K Σ t = (I K t C t = ΣtC T t (C t ΣtC T t + R t ) 1 t )Σt σ t 2 σ 2 2 t + σ obs,t ) = µ t = a t µ t 1 + b t u t σ 2 t = a t2 σ 2 2 t + σ act,t ) = µ t = A t µ t 1 + B t u t Σt = A t Σ t 1 A T t + Q t Correction 25
Kalman Filter Summary Highly efficient: Polynomial in the measurement dimensionality k and state dimensionality n: O(k 2.376 + n 2 ) Optimal for linear Gaussian systems! Most robotics systems are nonlinear!
Nonlinear Dynamic Systems Most realistic robotic problems involve nonlinear functions
Linearity Assumption Revisited
Non-linear Function
EKF Linearization (1)
EKF Linearization (2)
EKF Linearization (3)
EKF Linearization: First Order Taylor Series Expansion Prediction: Correction:
EKF Algorithm 1. Extended_Kalman_filter( µ t-1, Σ t-1, u t, z t ): 2. Prediction: 3. 4. Σt = G t Σ t 1 G T t + Q t 5. Correction: 6. K t = ΣtH T t (H t ΣtH T t + R t ) 1 7. 8. Σt = A t Σ t 1 A t T + Q t K t = ΣtC t T (C t ΣtC t T + R t ) 1 9. Return µ t, Σ t