1. Localization and Mapping (3)
Advanced Mobile Robotics
Localization and Mapping (3)
Dr. John (Jizhong) Xiao
Department of Electrical Engineering
City College of New York

2. Outline Localization Methods Current Research Topics Project Exercise
Markov Localization (review)
Kalman Filter Localization
Particle Filter (Monte Carlo Localization)
Current Research Topics
SLAM
Multi-robot Localization
Project Exercise

3. Review: Get Started
Fundamental problems to provide a mobile robot with autonomous capabilities:
how to create an environmental map with imperfect sensors?
how a robot can tell where it is on a map?
what if you’re lost and don’t have a map?
mapping
localization
robot SLAM

4. Representation of the Environment
Environment Representation
Continuos Metric ® x,y,q
Discrete Metric ® metric grid
Discrete Topological ® topological grid

5. Localization, Where am I?
Perception
Odometry, Dead Reckoning
Localization base on external sensors, beacons or landmarks
Probabilistic Map Based Localization

6. Localization Methods Markov Localization: Kalman Filtering
Central idea: represent the robot’s belief by a probability distribution over possible positions, and uses Bayes’ rule and convolution to update the belief whenever the robot senses or moves
Markov Assumption: past and future data are independent if one knows the current state
Kalman Filtering
Central idea: posing localization problem as a sensor fusion problem
Assumption: Gaussian distribution function
Particle Filtering
Monte-Carlo method
SLAM (simultaneous localization and mapping)
Multi-robot localization

7. Markov Localization Applying probability theory to robot localization
Markov localization uses an explicit, discrete representation for the probability of all position in the state space.
This is usually done by representing the environment by a grid or a topological graph with a finite number of possible states (positions).
During each update, the probability for each state (element) of the entire space is updated.

8. Markov Localization Example
Assume the robot position is one- dimensional
The robot is placed
somewhere in the
environment but it is not told its location
The robot queries its
sensors and finds out it is next to a door

9. Markov Localization Example
The robot moves one meter forward. To account for inherent noise in robot motion the new belief is smoother
The robot queries its
sensors and again it finds itself next to a door

10. Basic Notation
Bel(Lt=l ) Is the probability (density) that the robot assigns to the possibility that its location at time t is l
The belief is updated in response to two different types of events:
• sensor readings (SEE)
• odometry data (ACT)

11. Notation
Bayes’ Rule:
Goal:

12. Markov assumption (or static world assumption)

13. Markov Localization

14. Update Phase (SEE) a b c

15. Update Phase (SEE)

16. Prediction Phase (ACT)
Map from a belief state and an action to new belief state (ACT):
Summing over all possible ways in which the robot may have reached .

17. Summary

18. Markov Localization: Case Study
The Dervish Robot
Topological Localization with Sonar

19. Markov Localization: Case Study
Topological map of office-type environment

20. Markov Localization: Case Study
Update of believe state for position n given the percept-pair i
p(n¦i): new likelihood for being in position n
p(n): current believe state
p(i¦n): probability of seeing i in n (see table)
No action update ! (no encoder installed)
However, the robot is moving and therefore we can apply a combination of action and perception update
t-i is used instead of t-1 because the topological distance between n’ and n can very depending on the specific topological map

21. Markov Localization: Case Study
The calculation
is calculated by multiplying the probability of generating perceptual event i at position n by the probability of having failed to generate perceptual event s at all nodes between n’ and n.

22. Markov Localization: Case Study
Example calculation
Assume that the robot has two nonzero belief states
p(1-2) = ; p(2-3) = *
at that it is facing east with certainty
State 2-3 will progress potentially to 3 and 3-4 to 4.
State 3 and 3-4 can be eliminated because the likelihood of detecting an open door is zero.
The likelihood of reaching state 4 is the product of the initial likelihood p(2-3)= 0.2, (a) the likelihood of detecting anything at node 3 and the likelihood of detecting a hallway on the left and a door on the right at node 4 and (b) the likelihood of detecting a hallway on the left and a door on the right at node 4. (for simplicity we assume that the likelihood of detecting nothing at node 3-4 is 1.0)
This leads to:
0.2 × [0.6 × × 0.05] × 0.7 × [0.9 × 0.1] ® p(4) =
Similar calculation for progress from 1-2 ® p(2) = 0.3.
* Note that the probabilities do not sum up to one. For simplicity normalization was avoided in this example

23. Kalman Filter Localization

24. Introduction to Kalman Filter (1)
Two measurements
Weighted leas-square
Finding minimum error
After some calculation and rearrangements
Gaussian probability
density function
Take weight as:
Best estimate for the robot position

25. Introduction to Kalman Filter (2)
In Kalman Filter notation
Best estimate of the state at time K+1 is equal to the
best prediction of value before the new measurement
is taken, plus a weighting of value times the difference between
and the best prediction at time k

26. Introduction to Kalman Filter (3)
Dynamic Prediction (robot moving)
u = velocity w = noise
Motion
Combining fusion and dynamic prediction

27. Kalman Filter for Mobile Robot Localization
Five steps: 1) Position Prediction, 2) Observation, 3) Measurement prediction, 4) Matching, 5) Estimation

28. Kalman Filter for Mobile Robot Localization
Robot Position Prediction
In the first step, the robots position at time step k+1 is predicted based on its old location (time step k) and its movement due to the control input u(k):
f: Odometry function

29. Robot Position Prediction: Example
Odometry

30. Kalman Filter for Mobile Robot Localization
Observation
The second step it to obtain the observation Z(k+1) (measurements) from the robot’s sensors at the new location at time k+1
The observation usually consists of a set n0 of single observations zj(k+1) extracted from the different sensors signals. It can represent raw data scans as well as features like lines, doors or any kind of landmarks.
The parameters of the targets are usually observed in the sensor frame {S}.
Therefore the observations have to be transformed to the world frame {W} or
the measurement prediction have to be transformed to the sensor frame {S}.
This transformation is specified in the function hi (seen later).

31. Raw Date of Laser Scanner
Observation: Example
Raw Date of Laser Scanner
Extracted Lines
Extracted Lines
in Model Space
Sensor
(robot) frame

32. Kalman Filter for Mobile Robot Localization
Measurement Prediction
In the next step we use the predicted robot position and the map M(k) to generate multiple predicted observations zt.
They have to be transformed into the sensor frame
We can now define the measurement prediction as the set containing all ni predicted observations
The function hi is mainly the coordinate transformation between the world frame and the sensor frame

33. Measurement Prediction: Example
For prediction, only the walls that are in the field of view of the robot are selected.
This is done by linking the individual lines to the nodes of the path

34. Measurement Prediction: Example
The generated measurement predictions have to be transformed to the robot frame {R}
According to the figure in previous slide the transformation is given by
and its Jacobian by

35. Kalman Filter for Mobile Robot Localization
Matching
Assignment from observations zj(k+1) (gained by the sensors) to the targets zt (stored in the map)
For each measurement prediction for which an corresponding observation is found we calculate the innovation:
and its innovation covariance found by applying the error propagation law:
The validity of the correspondence between measurement and prediction can e.g. be evaluated through the Mahalanobis distance:

36. Matching: Example

37. Matching: Example
To find correspondence (pairs) of predicted and observed features we use the Mahalanobis distance
with

38. Estimation: Applying the Kalman Filter
Kalman filter gain:
Update of robot’s position estimate:
The associate variance

39. Estimation: 1D Case
For the one-dimensional case with we can show that the estimation corresponds to the Kalman filter for one-dimension presented earlier.

40. Estimation: Example
Kalman filter estimation of the new robot position :
By fusing the prediction of robot position (magenta) with the innovation gained by the measurements (green) we get the updated estimate of the robot position (red)

41. Monte Carlo Localization
One of the most popular particle filter method for robot localization
Current Research Topics
Reference:
Dieter Fox, Wolfram Burgard, Frank Dellaert, Sebastian Thrun, “Monte Carlo Localization: Efficient Position Estimation for Mobile Robots”, Proc. 16th National Conference on Artificial Intelligence, AAAI’99, July 1999

42. MCL in action
“Monte Carlo” Localization -- refers to the resampling of the distribution each time a new observation is integrated

43. Monte Carlo Localization
the probability density function is represented by samples
randomly drawn from it
it is also able to represent multi-modal distributions, and thus localize the robot globally
considerably reduces the amount of memory required and can integrate measurements at a higher rate
state is not discretized and the method is more accurate than the grid-based methods
easy to implement

44. “Probabilistic Robotics”
- Sebastian Thrun
p( B | A ) • p( A )
Bayes’ rule
p( A | B ) =
p( B )
Definition of marginal probability S
p( A ) = p( A  B )
all B S
p( A ) = p( A | B ) • p(B)
all B
Definition of conditional probability

45. Setting up the problem
The robot does (or can be modeled to) alternate between
sensing -- getting range observations o1, o2, o3, …, ot-1, ot
acting -- driving around (or ferrying?) a1, a2, a3, …, at-1
“local maps”
whence?

46. Setting up the problem
The robot does (or can be modeled to) alternate between
sensing -- getting range observations o1, o2, o3, …, ot-1, ot
acting -- driving around (or ferrying?) a1, a2, a3, …, at-1
We want to know rt the position of the robot at time t
but we’ll settle for p(rt) -- the probability distribution for rt !
What kind of thing is p(rt) ?

47. Setting up the problem
The robot does (or can be modeled to) alternate between
sensing -- getting range observations o1, o2, o3, …, ot-1, ot
acting -- driving around (or ferrying?) a1, a2, a3, …, at-1
We want to know rt the position of the robot at time t
but we’ll settle for p(rt) -- the probability distribution for rt !
What kind of thing is p(rt) ?
We do know m the map of the environment
p( o | r, m )
-- the sensor model
p( rnew | rold, a, m )
-- the accuracy of desired action a

48. potential observations o
Robot modeling
map m and location r
p( o | r, m ) sensor model
p( | r, m ) = .75
p( | r, m ) = .05
potential observations o

49. Robot modeling p( o | r, m ) sensor model
map m and location r
p( o | r, m ) sensor model
p( rnew | rold, a, m ) action model
p( | r, m ) = .75
p( | r, m ) = .05
potential observations o
“probabilistic kinematics” -- encoder uncertainty
red lines indicate commanded action
the cloud indicates the likelihood of various final states

50. Robot modeling: how-to
p( o | r, m ) sensor model
p( rnew | rold, a, m ) action model
(0) Model the physics of the sensor/actuators (with error estimates)
theoretical modeling
(1) Measure lots of sensing/action results and create a model from them
empirical modeling
take N measurements, find mean (m) and st. dev. (s) and then use a Gaussian model
or, some other easily-manipulated model...
0 if |x-m| > s
if |x-m| > s
p( x ) =
p( x ) =
1 otherwise
1- |x-m|/s otherwise
(2) Make something up...

51. Monte Carlo Localization
Start by assuming p( r0 ) is the uniform distribution.
take K samples of r0 and weight each with an importance factor of 1/K

52. Monte Carlo Localization
Start by assuming p( r0 ) is the uniform distribution.
take K samples of r0 and weight each with an importance factor of 1/K
Get the current sensor observation, o1
For each sample point r0 multiply the importance factor by p(o1 | r0, m)

53. Monte Carlo Localization
Start by assuming p( r0 ) is the uniform distribution.
take K samples of r0 and weight each with an importance factor of 1/K
Get the current sensor observation, o1
For each sample point r0 multiply the importance factor by p(o1 | r0, m)
Normalize (make sure the importance factors add to 1)
You now have an approximation of p(r1 | o1, …, m)
and the distribution is no longer uniform

54. Monte Carlo Localization
Start by assuming p( r0 ) is the uniform distribution.
take K samples of r0 and weight each with an importance factor of 1/K
Get the current sensor observation, o1
For each sample point r0 multiply the importance factor by p(o1 | r0, m)
Normalize (make sure the importance factors add to 1)
You now have an approximation of p(r1 | o1, …, m)
and the distribution is no longer uniform
Create r1 samples by dividing up large clumps
each point spawns new ones in proportion to its importance factor

55. Monte Carlo Localization
Start by assuming p( r0 ) is the uniform distribution.
take K samples of r0 and weight each with an importance factor of 1/K
Get the current sensor observation, o1
For each sample point r0 multiply the importance factor by p(o1 | r0, m)
Normalize (make sure the importance factors add to 1)
You now have an approximation of p(r1 | o1, …, m)
and the distribution is no longer uniform
Create r1 samples by dividing up large clumps
each point spawns new ones in proportion to its importance factor
The robot moves, a1
For each sample r1, move it according to the model p(r2 | a1, r1, m)

56. Monte Carlo Localization
Start by assuming p( r0 ) is the uniform distribution.
take K samples of r0 and weight each with an importance factor of 1/K
Get the current sensor observation, o1
For each sample point r0 multiply the importance factor by p(o1 | r0, m)
Normalize (make sure the importance factors add to 1)
You now have an approximation of p(r1 | o1, …, m)
and the distribution is no longer uniform
Create r1 samples by dividing up large clumps
each point spawns new ones in proportion to its importance factor
The robot moves, a1
For each sample r1, move it according to the model p(r2 | a1, r1, m)

57. MCL in action
“Monte Carlo” Localization -- refers to the resampling of the distribution each time a new observation is integrated

58. Discussion: MCL

59. References
Dieter Fox, Wolfram Burgard, Frank Dellaert, Sebastian Thrun, “Monte Carlo Localization: Efficient Position Estimation for Mobile Robots”, Proc. 16th National Conference on Artificial Intelligence, AAAI’99, July 1999
Dieter Fox, Wolfram Burgard, Sebastian Thrun, “Markov Localization for Mobile Robots in Dynamic Environments”, J. of Artificial Intelligence Research 11 (1999)
Sebastian Thrun, “Probabilistic Algorithms in Robotics”, Technical Report CMU-CS , School of Computer Science, Carnegie Mellon University, Pittsburgh, USA, 2000