Introduction to Mobile Robotics Bayes Filter Kalman Filter

Transcription

1 Introduction to Mobile Robotics Bayes Filter Kalman Filter Wolfram Burgard, Cyrill Stachniss, Maren Bennewitz, Giorgio Grisetti, Kai Arras 1

2 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 For all x do Else if d is an action data item u then 10. For all x do Return Bel (x) 2

3 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

4 Gaussians µ Univariate -σ σ µ Multivariate

5 Gaussians 1D 3D 2D Video

6 Properties of Gaussians

7 Multivariate Gaussians (where division " " denotes matrix inversion) We stay Gaussian as long as we start with Gaussians and perform only linear transformations

8 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

9 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

10 Bayes Filter Reminder Prediction Correction

11 Kalman Filter Updates in 1D 11

12 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

13 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

14 Kalman Filter Updates

15 Linear Gaussian Systems: Initialization Initial belief is normally distributed:

16 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

17 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

18 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

19 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

20 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

21 Kalman Filter Algorithm

22 Kalman Filter Algorithm Prediction Observation Matching Correction

23 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

24 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

25 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

26 Kalman Filter Summary Highly efficient: Polynomial in the measurement dimensionality k and state dimensionality n: O(k n 2 ) Optimal for linear Gaussian systems! Most robotics systems are nonlinear!

27 Nonlinear Dynamic Systems Most realistic robotic problems involve nonlinear functions

28 Linearity Assumption Revisited

29 Non-linear Function

30 EKF Linearization (1)

31 EKF Linearization (2)

32 EKF Linearization (3)

33 EKF Linearization: First Order Taylor Series Expansion Prediction: Correction:

34 EKF Algorithm 1. Extended_Kalman_filter( µ t-1, Σ t-1, u t, z t ): 2. Prediction: Σ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 ) Σ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