1. Probabilistic Robotics Bayes Filter Implementations Gaussian filters

2. Markov  Kalman Filter Localization Markov localization localization starting from any unknown position recovers from ambiguous situation. However, to update the probability of all positions within the whole state space at any time requires a discrete representation of the space (grid). The required memory and calculation power can thus become very important if a fine grid is used. Kalman filter localization tracks the robot and is inherently very precise and efficient. However, if the uncertainty of the robot becomes to large (e.g. collision with an object) the Kalman filter will fail and the position is definitively lost.

3. Kalman Filter Localization

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

5. Prediction Correction Bayes Filter Reminder

6. 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 couple of matrix multiplications! 6

7. Gaussian s p(x) ~ N(, 2 ) :p(x) ~ N(, 2 ) : p(x) p(x)  1 22  e 2 1 ( x  )21 ( x  )2  2 2 --   Univariate  Multivariate

8. Gaussians 1D 2D 3D Video correlation

9. Properties of Gaussians Multivariate Univariate We stay in the “Gaussian world” as long as we start with Gaussians and perform only linear transformations

10. Introduction to Kalman Filter (1) Two measurements no dynamics Weighted least-square Finding minimum error After some calculation and rearrangements Another way to look at it – weigthed mean If w i = 1/σ i 2

11. 11 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 Matrix (nxn) that describes how the state evolves from t - 1 to t without controls or noise. Matrix (nxl) that describes how the control u t changes the state from t-1 to t. 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 R t and Q t respectively. Process dynamics Observation model

12. Kalman Filter Updates in 1D prediction measurement correction It's a weighted mean! 12

13. Kalman Filter Updates in 1D 13 gain innovation

14. 1 Kalman Filter Updates in 1D 4

15. 1 Kalman Filter Updates 5

16. Linear Gaussian Systems: Initialization 16 Initial belief is normally distributed:

17. Dynamics are linear function of state and control plus additive noise: Linear Gaussian Systems: Dynamics 17

18. Linear Gaussian Systems: Dynamics 18

19. Observations are linear function of state plus additive noise: Linear Gaussian Systems: Observations 19

20. Linear Gaussian Systems: Observations 20

21. Kalman Filter Algorithm 1. Algorithm Kalman_filter(  t-1,  t-1, u t, z t ): 2.Prediction: 3. 4. 5.Correction: 6. 7. 8. 9. Return  t,  t 21

22. Kalman Filter Algorithm

23.  Predictio n  Observation  Matchin g  Correctio n

24. The Prediction-Correction-Cycle Predictio n 24

25. The Prediction-Correction-Cycle Correctio n 25

26. The Prediction-Correction-Cycle Correctio n Predictio n 26

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

28. 28 Nonlinear Dynamic Systems Most realistic robotic problems involve nonlinear functions Extended Kalman filter relaxes linearity assumption xt g(ut, xt 1 )xt g(ut, xt 1 ) zt h(xt )zt h(xt )

29. Second-Order Error Propagation Rarely used (complex expressions) Monte-Carlo Non- parametric representation of uncertainties 1.Sampling from p(X ) 2.Propagation of samples 3.Histogramming 4.Normalization Other Error Prop. Techniques 29

30. First-Order Error Propagation X,Y assumed to be Gaussian Y = f(X) Taylor series expansion Wanted:, 30

31. Jacobian Matrix It ’ s a non-square matrixin general Suppose you have a vector-valued function Let the gradient operator be the vector of (first-order) partial derivatives Then, the Jacobian matrix is defined as 31

32. It ’ s the orientation of the tangent plane to the vector- valued function at a given point Generalizes the gradient of a scalar valued function Heavily used for first-order error propagation... Jacobian Matrix 32

33. First-Order Error Propagation Putting things together... with “ Is there a compact form?... ” 33

34. First-Order Error Propagation...Yes! Given Input covariance matrix C X Jacobian matrix F X the Error Propagation Law computes the output covariance matrix C Y 34

35. State Prediction 35 Update Map Feature/Landmark Extraction Data Association Odometry or IMU Sensors Measurement Prediction Landmark-based Localization EKF Localization: Basic Cycle

36. State Prediction 36 posterior state Update Map Data Association Odometry or IMU raw sensory data Sensors landmarks Feature/Landmark Extraction Measurement Prediction Landmark-based Localization EKF Localization: Basic Cycle innovation from matched landmarks predicted measurements in sensor coordinates landmarks in global coordinates encoder measurements predicted state

37. State Prediction (Odometry) Landmark-based Localization Control u k : wheel displacements s l, s r Error model: linear growth Nonlinear process model f : 37

38. State Prediction (Odometry) Landmark-based Localization Control u k : wheel displacements s l, srsr Error model: linear growth Nonlinear process model f : 38

39. Landmark-based Localization Landmark Extraction (Observation) Extracted lines Hessian line model Extracted lines in model space Raw laser range data 39

40. Landmark-based Localization Measurement Prediction...is a coordinate frame transform world-to-sensor Given the predicted state (robot pose), predicts the location uncertainty and location of expected observations in sensor coordinates model space 40

41. Data Association (Matching) Associates predicted measurements with observations Innovation and innovation covariance Matching on significance level alpha Landmark-based Localization 62 model space Green: observation Magenta: measurement prediction

42. Landmark-based Localization Update Kalman gain State update (robot pose) State covariance update Red: posterior estimate 63