Probabilistic Robotics Robot Localization 1 Localization Using sensory

Probabilistic Robotics Robot Localization 1

Localization “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] • 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) 2

Localization 3

Localization Position tracking 4

Localization Global localization 5

Landmark-based Localization 6

Linearity Assumption Revisited 7

Non-linear Function 8

EKF Linearization (1) 9

EKF Linearization (2) 10

EKF Linearization (3) 11

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

EKF Algorithm 1. Extended_Kalman_filter( mt-1, St-1, ut, zt): 2. 3. 4. Prediction: 5. 6. 7. 8. Correction: 9. Return mt, St 13

1. EKF_localization ( mt-1, St-1, ut, zt, m): Prediction: 2. Jacobian of g w. r. t location 3. Jacobian of g w. r. t control 4. Motion noise 5. Predicted mean 6. Predicted covariance 14

1. EKF_localization ( mt-1, St-1, ut, zt, m): Correction: 2. 3. Predicted measurement mean Jacobian of h w. r. t location 4. 5. Pred. measurement covariance 6. Kalman gain 7. Updated mean 8. Updated covariance 15

EKF Prediction Step (known correspondences) 16

EKF Correction Step (known correspondences) 17

EKF Prediction Step (unknown correspondences) 18

EKF Correction Step (unknown correspondences) 19

EKF Prediction Step 20

EKF Observation Prediction Step 21

EKF Correction Step 22

Estimation Sequence (1) 23

Estimation Sequence (2) 24

Comparison to Ground. Truth 25

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

Linearization via Unscented Transform EKF UKF 27

UKF Sigma-Point Estimate (2) EKF UKF 28

UKF Sigma-Point Estimate (3) EKF UKF 29

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

UKF_localization ( mt-1, St-1, ut, zt, m): Prediction: Motion noise Measurement noise Augmented state mean Augmented covariance Sigma points Prediction of sigma points Predicted mean Predicted covariance 31

UKF_localization ( mt-1, St-1, ut, zt, m): Correction: Measurement sigma points Predicted measurement mean Pred. measurement covariance Cross-covariance Kalman gain Updated mean Updated covariance 32

UKF Prediction Step 33

UKF Observation Prediction Step 34

UKF Correction Step 35

EKF Correction Step 36

Estimation Sequence EKF PF UKF 37

Estimation Sequence EKF UKF 38

Prediction Quality EKF UKF 39

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! 40

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

Map-based Localization 42

Monte Carlo (Particle Filter) Localization 43

Resampling Algorithm 44

Monte Carlo (Particle Filter) Localization 45

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

Monte Carlo (Particle Filter) Localization 47

Monte Carlo (Particle Filter) Localization 48

Monte Carlo (Particle Filter) Localization 49

Monte Carlo (Particle Filter) Localization 50

51

52

53

54

55

56

57

58

59

60

61

62

63

64

65

Monte Carlo (Particle Filter) Localization 66

Monte Carlo (Particle Filter) Localization 67

Multihypothesis Tracking 68

Localization With MHT • 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. 69

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

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