Mobile Robot Localization (ch. 7)

1
Mobile Robot Localization (ch. 7)

• Mobile robot localization is the problem of
  determining the pose of a robot relative to a
  given map of the environment. Because,
• Unfortunately, the pose of a robot can not be
  sensed directly, at least for now. The pose has
  to be inferred from data.
• A single sensor measurement is enough?
• The importance of localization in robotics.
• Mobile robot localization can be seen as a
  problem of coordinate transformation. One point
  of view.

2
Mobile Robot Localization

• Localization techniques have been developed for a
  broad set of map representations.
• Feature based maps, location based maps,
  occupancy grid maps, etc. (what exactly are
  they?) (See figure 7.2)
• (You can probably guess What is the mapping
  problem?)
• Remember, in localization problem, the map is
  given, known, available.
• Is it hard? Not really, because,

3
Mobile Robot Localization

• Most localization algorithms are variants of
  Bayes filter algorithm.
• However, different representation of maps, sensor
  models, motion model, etc lead to different
  variant.
• Here is the agenda.

4
Mobile Robot Localization

• We want to know different kinds of maps.
• We want to know different kinds of localization
  problems.
• We want to know how to solve localization
  problems, during which process, we also want to
  know how to get sensor model, motion model, etc.

5
Mobile Robot Localization(We want to know
different kinds of maps. )

• Different kinds of maps.
• At a glance, .
• feature-based, location-based, metric,
  topological map, occupancy grid map, etc.
• see figure 7.2
• http//www.cs.cmu.edu/afs/cs/project/jair/pub/volu
  me11/fox99a-html/node23.html

6
Mobile Robot Localization A Taxonomy (We want
to know different kinds of localization
problems.)

• Different kinds of Localization problems.
• A taxonomy in 4 dimensions
• Local versus Global (initial knowledge)
• Static versus Dynamic (environment)
• Passive versus active (control of robots)
• Single robot or multi-robot

7
Mobile Robot Localization

• Solved already, the Bayes filter algorithm. How?
• The straightforward application of Bayes filters
  to the localization problem is called Markov
  localization.
• Here is the algorithm (abstract?)

8
Mobile Robot Localization

• Algorithm Bayes_filter ( )
• for all do
• endfor
• return

9
Mobile Robot Localization

• Algorithm Markov Locatlization (
  )
• for all do
• endfor
• return
• The Markov Localization algorithm
  addresses the global localization problem, the
  position tracking problem, and the kidnapped
  robot problem in static environment.

10
Mobile Robot Localization

• Revisit Figure 7.5 to see how Markov localization
  algorithm in working.
• The algorithm Markov Localization is still very
  abstract. To put it in work (eg. your project),
  we need a lot of more background knowledge to
  realize motion model, sensor model, etc.
• We start with Guassian Filter (also called Kalman
  filter)

11
Bayes Filter Implementations (1) Kalman
Filter (Gaussian filters) (back to Ch.3)
12
Bayes Filter Reminder

• Prediction
• Correction

13
Gaussians
14
Properties of Gaussians
15
Multivariate Gaussians

• We stay in the Gaussian world as long as we
  start with Gaussians and perform only linear
  transformations.
• Review your probability textbook

http//en.wikipedia.org/wiki/Multivariate_normal_d
istribution
16
Kalman Filter
Estimates the state x of a discrete-time
controlled process that is governed by the linear
stochastic difference equation
with a measurement
17
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 ut
changes the state from t to t-1.
Matrix (kxn) that describes how to map the state
xt to an observation zt.
Random variables representing the process and
measurement noise that are assumed to be
independent and normally distributed with
covariance Rt and Qt respectively.
18
Kalman Filter Algorithm

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

19
Kalman Filter Updates in 1D
20
Kalman Filter Updates in 1D
21
Kalman Filter Updates in 1D
22
Kalman Filter Updates
23
Linear Gaussian Systems Initialization

• Initial belief is normally distributed

24
Linear Gaussian Systems Dynamics

• Dynamics are linear function of state and control
  plus additive noise

25
Linear Gaussian Systems Dynamics
26
Linear Gaussian Systems Observations

• Observations are linear function of state plus
  additive noise

27
Linear Gaussian Systems Observations
See page 45-54 for mathematical derivation.
28
The Prediction-Correction-Cycle
29
The Prediction-Correction-Cycle
30
The Prediction-Correction-Cycle
31
Kalman Filter Summary

• Highly efficient Polynomial in measurement
  dimensionality k and state dimensionality n
  O(k2.376 n2)
• Optimal for linear Gaussian systems!
• However, most robotics systems are nonlinear,
  unfortunately!

32
Nonlinear Dynamic Systems

• Most realistic robotic problems involve nonlinear
  functions

33
Linearity Assumption Revisited
34
Non-linear Function
35
EKF Linearization (1)
36
EKF Linearization (2)
37
EKF Linearization (3)
38
EKF Linearization First Order Taylor Series
Expansion

• Prediction
• Correction

39
EKF Algorithm

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

40
Bayes Filter Implementations (2) Particle filters
41
Sample-based Localization (sonar)
42
Particle Filters

• Represent belief by random samples
• Estimation of non-Gaussian, nonlinear processes
• Monte Carlo filter, Survival of the fittest,
  Condensation, Bootstrap filter, Particle filter
• Filtering Rubin, 88, Gordon et al., 93,
  Kitagawa 96
• Computer vision Isard and Blake 96, 98
• Dynamic Bayesian Networks Kanazawa et al., 95

43
Particle Filters
44
Sensor Information Importance Sampling
45
Robot Motion

46
Sensor Information Importance Sampling
47
Robot Motion
48
Particle Filter Algorithm

• Algorithm particle_filter( St-1, ut-1 zt)
• For
  Generate new samples
• Sample index j(i) from the discrete
  distribution given by wt-1
• Sample from using
  and
• Compute importance weight
• Update normalization factor
• Insert
• For
• Normalize weights

49
Particle Filter Algorithm
50
Importance Sampling
Weight samples w f / g
http//en.wikipedia.org/wiki/Importance_sampling
51
Importance Sampling with Resampling
52
Importance Sampling with Resampling
Weighted samples
After resampling
53
Resampling

• Given Set S of weighted samples.
• Wanted Random sample, where the probability of
  drawing xi is given by wi.
• Typically done n times with replacement to
  generate new sample set S.

54
Resampling

• Stochastic universal sampling
• Systematic resampling
• Linear time complexity
• Easy to implement, low variance

• Roulette wheel
• Binary search, n log n

55
Resampling Algorithm

1. Algorithm systematic_resampling(S,n)
3. For Generate cdf
5. Initialize threshold
6. For Draw samples
7. While ( ) Skip until next threshold
   reached
9. Insert
10. 
    Increment threshold
11. Return S

Also called stochastic universal sampling
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
(No Transcript)
67
(No Transcript)
68
(No Transcript)
69
(No Transcript)
70
(No Transcript)
71
(No Transcript)
72
(No Transcript)
73
(No Transcript)
74
(No Transcript)
75
Sample-based Localization (sonar)
76
Initial Distribution
77
After Incorporating Ten Ultrasound Scans
78
After Incorporating 65 Ultrasound Scans
79
Estimated Path
80
Using Ceiling Maps for Localization
81
Vision-based Localization
82
Under a Light
Measurement z
P(zx)
83
Next to a Light
Measurement z
P(zx)
84
Elsewhere
Measurement z
P(zx)
85
Global Localization Using Vision
86
Robots in Action Albert
87
Limitations

• The approach described so far is able to
• track the pose of a mobile robot and to
• globally localize the robot.
• How can we deal with localization errors (i.e.,
  the kidnapped robot problem)?

88
Approaches

• Randomly insert samples (the robot can be
  teleported at any point in time).
• Insert random samples proportional to the average
  likelihood of the particles (the robot has been
  teleported with higher probability when the
  likelihood of its observations drops).