Probabilistic Robotics

1
Probabilistic Robotics
Robot Localization
2
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 robots position.
• Problem classes
• Position tracking
• Global localization
• Kidnapped robot problem (recovery)

3
Localization
4
Localization
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
EKF Linearization First Order Taylor Series
Expansion

• Prediction
• Correction

13
EKF Algorithm

1. Extended_Kalman_filter( mt-1, St-1, ut, zt)
2. Prediction
5. Correction
9. Return mt, St

14

1. EKF_localization ( mt-1, St-1, ut, zt,
   m)Prediction

Jacobian of g w.r.t location
Jacobian of g w.r.t control
Motion noise
Predicted mean
Predicted covariance
15

1. EKF_localization ( mt-1, St-1, ut, zt,
   m)Correction

Predicted measurement mean
Jacobian of h w.r.t location
Pred. measurement covariance
Kalman gain
Updated mean
Updated covariance
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(k2.376 n2)
• 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 ( 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
32

• 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
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
Kalman Filter-based System

• Arras et al. 98
• Laser range-finder and vision
• High precision (lt1cm accuracy)

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_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
(No Transcript)
52
(No Transcript)
53
(No Transcript)
54
(No Transcript)
55
(No Transcript)
56
(No Transcript)
57
(No Transcript)
58
(No Transcript)
59
(No Transcript)
60
(No Transcript)
61
(No Transcript)
62
(No Transcript)
63
(No Transcript)
64
(No Transcript)
65
(No Transcript)
66
Monte Carlo (Particle Filter) Localization
67
Monte Carlo (Particle Filter) Localization
68
Multi-hypothesisTracking
69
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.

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