Probabilistic Robotics Robot Localization

2 Localization Given Map of the environment. Sequence of sensor measurements. Wanted Estimate of the robot’s position. Problem classes Position tracking Global localization Kidnapped robot problem (recovery) “Using sensory information to locate the robot in its environment is the most fundamental problem to providing a mobile robot with autonomous capabilities.” [Cox ’91]

3 Localization

4 Position tracking

5 Localization Global localization

6 Landmark-based Localization

7 Linearity Assumption Revisited

8 Non-linear Function

9 EKF Linearization (1)

10 EKF Linearization (2)

11 EKF Linearization (3)

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

13 EKF Algorithm 1.Extended_Kalman_filter (  t-1,  t-1, u t, z t ): 2. Prediction: Correction: Return  t,  t

14 1.EKF_localization (  t-1,  t-1, u t, z t, m): Prediction: Motion noise Jacobian of g w.r.t location Predicted mean Predicted covariance Jacobian of g w.r.t control

15 1.EKF_localization (  t-1,  t-1, u t, z t, m): Correction: Predicted measurement mean Pred. measurement covariance Kalman gain Updated mean Updated covariance Jacobian of h w.r.t location

16 EKF Prediction Step (known correspondences)

17 EKF Correction Step (known correspondences)

18 EKF Prediction Step (unknown correspondences)

19 EKF Correction Step (unknown correspondences)

20 EKF Prediction Step

21 EKF Observation Prediction Step

22 EKF Correction Step

23 Estimation Sequence (1)

24 Estimation Sequence (2)

25 Comparison to GroundTruth

26 EKF Summary Highly efficient: Polynomial in measurement dimensionality k and state dimensionality n: O(k n 2 ) Not optimal! Can diverge if nonlinearities are large! Works surprisingly well even when all assumptions are violated!

27 Linearization via Unscented Transform EKF UKF

28 UKF Sigma-Point Estimate (2) EKF UKF

29 UKF Sigma-Point Estimate (3) EKF UKF

30 Unscented Transform Sigma points Weights Pass sigma points through nonlinear function Recover mean and covariance

31 UKF_localization (  t-1,  t-1, u t, z t, m): Prediction: Motion noise Measurement noise Augmented state mean Augmented covariance Sigma points Prediction of sigma points Predicted mean Predicted covariance

32 UKF_localization (  t-1,  t-1, u t, z t, m): Correction: Measurement sigma points Predicted measurement mean Pred. measurement covariance Cross-covariance Kalman gain Updated mean Updated covariance

33 UKF Prediction Step

34 UKF Observation Prediction Step

35 UKF Correction Step

36 EKF Correction Step

37 Estimation Sequence EKF PF UKF

38 Estimation Sequence EKF UKF

39 Prediction Quality EKF UKF

40 UKF Summary Highly efficient: Same complexity as EKF, with a constant factor slower in typical practical applications Better linearization than EKF: Accurate in first two terms of Taylor expansion (EKF only first term) Derivative-free: No Jacobians needed Still not optimal!

41 [Arras et al. 98]: Laser range-finder and vision High precision (<1cm accuracy) Kalman Filter-based System [Courtesy of Kai Arras]

42 Map-based Localization

43 Monte Carlo (Particle Filter) Localization

44 Resampling Algorithm

45 Monte Carlo (Particle Filter) Localization

46 Monte Carlo (Particle Filter) Localization 1.Algorithm sample_normal_distribution( b ): 2.return 1.Algorithm sample_triangular_distribution( b ): 2.return

47 Monte Carlo (Particle Filter) Localization

48 Monte Carlo (Particle Filter) Localization

49 Monte Carlo (Particle Filter) Localization

50 Monte Carlo (Particle Filter) Localization

51

52

53

54

55

56

57

58

59

60

61

62

63

64

65

66 Monte Carlo (Particle Filter) Localization

67 Monte Carlo (Particle Filter) Localization

68 Multi- hypothesis Tracking

69 Belief is represented by multiple hypotheses Each hypothesis is tracked by a Kalman filter Additional problems: Data association: Which observation corresponds to which hypothesis? Hypothesis management: When to add / delete hypotheses? Huge body of literature on target tracking, motion correspondence etc. Localization With MHT

70 MHT: Implemented System (2) Courtesy of P. Jensfelt and S. Kristensen

71 MHT: Implemented System (3) Example run Map and trajectory # hypotheses #hypotheses vs. time P(H best ) Courtesy of P. Jensfelt and S. Kristensen