1. Probabilistic Robotics: Review/SLAM
Advanced Mobile Robotics
Probabilistic Robotics: Review/SLAM
Dr. Jizhong Xiao
Department of Electrical Engineering
CUNY City College

2. Bayes Filter Revisit
Prediction (Action)
Correction (Measurement)

3. Probabilistic Robotics
Probabilistic Sensor Models
Beam-based Model
Likelihood Fields Model
Feature-based Model

4. Beam-based Proximity Model
Measurement noise
Unexpected obstacles
zexp
zmax
zexp
zmax
Gaussian Distribution
Exponential Distribution

5. Beam-based Proximity Model
Random measurement
Max range
zexp
zmax
zexp
zmax
Uniform distribution
Point-mass distribution

6. Resulting Mixture Density
Weighted average, and
How can we determine the model parameters?
System identification method: maximum likelihood estimator (ML estimator)

7. Likelihood Fields Model
Project the end points of a sensor scan Zt into the global coordinate space of the map
Probability is a mixture of …
a Gaussian distribution with mean at distance to closest obstacle,
a uniform distribution for random measurements, and
a small uniform distribution for max range measurements.
Again, independence between different components is assumed.

8. Likelihood Fields Model
Distance to the nearest obstacles. Max range reading ignored

9. Example Example environment Likelihood field
The darker a location, the less likely it is to perceive an obstacle
P(z|x,m)
Sensor probability
Oi : Nearest point to obstacles O1 O2 O3
Zmax

10. Feature-Based Measurement Model
Feature vector is abstracted from the measurement:
Sensors that measure range, bearing, & a signature (a numerical value, e.g., an average color)
Conditional independence between features
Feature-Based map: with
i.e., a location coordinate in global coordinates & a signature
Robot pose:
Measurement model:
Zero-mean Gaussian error variables with standard deviations

11. Probabilistic Robotics
Probabilistic Motion Models

12. Odometry Model Robot moves from to . Odometry information .
Relative motion information, “rotation”  “translation”  “rotation”

13. Noise Model for Odometry
The measured motion is given by the true motion corrupted with independent noise.
How to calculate :

14. Calculating the Posterior Given xt, xt-1, and u
An initial pose Xt-1
A hypothesized final pose Xt
A pair of poses u obtained from odometry
Algorithm motion_model_odometry (xt, xt-1, u)
return p1 · p2 · p3
odometry values (u)
values of interest (xt-1, xt)
Implements an error distribution over a with zero mean and standard deviation b

15. Application
Repeated application of the sensor model for short movements.
Typical banana-shaped distributions obtained for 2d-projection of 3d posterior.
p(xt| u, xt-1) x’ x’ u u
Posterior distributions of the robot’s pose upon executing the motion command illustrated by the solid line. The darker a location, the more likely it is.

16. Velocity-Based Model
Rotation radius
control

17. Equation for the Velocity Model
Instantaneous center of curvature (ICC) at (xc , yc)
Initial pose
Keeping constant speed, after ∆t time interval, ideal robot will be at
Corrected, -90

18. Velocity-based Motion Model
With and are the state vectors at time t-1 and t respectively
The true motion is described by a translation velocity and a rotational velocity
Motion Control with additive Gaussian noise
Circular motion assumption leads to degeneracy ,
2 noise variables v and w  3D pose
Assume robot rotates when arrives at its final pose

19. Velocity-based Motion Model
1 to 4 are robot-specific error parameters determining the velocity control noise
5 and 6 are robot-specific error parameters determining the standard deviation of the additional rotational noise

20. Probabilistic Motion Model
How to compute ?
Move with a fixed velocity during ∆t resulting in a circular trajectory from to
Center of circle:
with
Radius of the circle:
Change of heading direction:
(angle of the final rotation)

21. Posterior Probability for Velocity Model
Center of circle
Radius of the circle
Change of heading direction
Motion error: verr ,werr and

22. Examples (velocity based)

23. Map-Consistent Motion Model
Obstacle grown by robot radius
Map free estimate of motion model
“consistency” of pose in the map
“=0” when placed in an occupied cell
Approximation:

24. Summary
We discussed motion models for odometry-based and velocity-based systems
We discussed ways to calculate the posterior probability p(x| x’, u).
Typically the calculations are done in fixed time intervals t.
In practice, the parameters of the models have to be learned.
We also discussed an extended motion model that takes the map into account.

25. Probabilistic Robotics
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]

26. Localization, Where am I?
Given
Map of the environment.
Sequence of measurements/motions.
Wanted
Estimate of the robot’s position.
Problem classes
Position tracking (initial robot pose is known)
Global localization (initial robot pose is unknown)
Kidnapped robot problem (recovery)

27. Markov Localization

28. Bayes Filter Revisit
Prediction (Action)
Correction (Measurement)

29. EKF Linearization
First Order Taylor Expansion
Prediction:
Correction:

30. EKF Algorithm Extended_Kalman_filter( mt-1, St-1, ut, zt): Prediction:
Correction:
Return mt, St

31. 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 covariance
Matrix from the control
Predicted mean
Predicted covariance

32. Velocity-based Motion Model
With and are the state vectors at time t-1 and t respectively
The true motion is described by a translation velocity and a rotational velocity
Motion Control with additive Gaussian noise

33. Velocity-based Motion Model

34. Velocity-based Motion Model
Derivative of g along x’ dimension, w.r.t. x at
Jacobian of g w.r.t location

35. Velocity-based Motion Model
Mapping between the motion noise in control space to the motion noise in state space
Jacobian of g w.r.t control
Derivative of g w.r.t. the motion parameters, evaluated at and

36. 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

37. Feature-Based Measurement Model
Jacobian of h w.r.t location
Is the landmark that corresponds to the measurement of

38. EKF Localization with known correspondences

39. EKF Localization with unknown correspondences
Maximum likelihood estimator

40. EKF Prediction Step
Initial estimate is represented by the ellipse centered at
Moving on a circular arc of 90cm & turning 45 degrees to the left, the predicted position is centered at
Small noise in translational & rotation
High rotational noise
High translational noise
High noise in both translation & rotation

41. EKF Measurement Prediction Step
True robot (white circle) & the observation (bold line circle)
Innovations (white arrows) : differences between observed & predicted measurements

42. EKF Correction Step Measurement Prediction Resulting correction
Update mean estimate & reduce position uncertainty ellipses

43. Estimation Sequence (1)
EKF localization with an accurate landmark detection sensor
Dashed line: estimated robot trajectory
Dashed line: corrected robot trajectory
Uncertainty ellipses: before (light gray) & after (dark gray) incorporating landmark detection
Solid line: true robot motion

44. Estimation Sequence (2)
EKF localization with a less accurate landmark detection sensor
Uncertainty ellipses: before (light gray) & after (dark gray) incorporating landmark detection

45. Comparison to Ground Truth

46. UKF Localization Given Wanted UKF localization Map of the environment.
Sequence of measurements/motions.
Wanted
Estimate of the robot’s position.
UKF localization

47. Unscented Transform Sigma points Weights
Pass sigma points through nonlinear function
For n-dimensional Gaussian
λ is scaling parameter that determine how far the sigma points are spread from the mean
If the distribution is an exact Gaussian, β=2 is the optimal choice.
Recover mean and covariance

48. 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

49. UKF_localization ( mt-1, St-1, ut, zt, m):
Correction:
The predicted robot locations are used to generate the measurement sigma points
Measurement sigma points
Predicted measurement mean
Pred. measurement covariance
Cross-covariance
Kalman gain
Updated mean
Updated covariance

50. UKF Prediction Step High rotational noise
Moving on a circular arc of 90cm & turning 45 degrees to the left, the predicted position is centered at
High translational noise
High noise in both translation & rotation

51. UKF Measurement Prediction Step
Predicted Sigma points
Measurement Prediction

52. UKF Correction Step
Measurement Prediction
Resulting correction

53. Estimation Sequence EKF UKF
Robot path estimated with different techniques, with UKF being slightly closer

54. Prediction Quality EKF UKF
Robot moves on a circle, estimates based on EKF prediction, & UKF prediction

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

56. Multi-hypothesis Tracking

57. MHT: Multi-Hypothesis Tracking filter
Localization With MHT
MHT: Multi-Hypothesis Tracking filter
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.

58. MHT: Implemented System (1)
Hypotheses are extracted from Laser Range Finder (LRF) scans
Each hypothesis has probability of being the correct one:
Hypothesis probability is computed using Bayes’ rule
Hypotheses with low probability are deleted.
New candidates are extracted from LRF scans.
[Jensfelt et al. ’00]

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

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

61. Probabilistic Robotics
SLAM

62. SLAM Problem : Chicken or Egg
Fundamental problems for localization and mapping
The task of SLAM is to build a map while estimating the pose of the robot relative to this map.
Without a map, robot cannot localize itself
Without knowing its location, robot cannot build a map
Which needed to be done first? Localization or mapping?

63. The SLAM Problem Given: Estimate:
A robot is exploring an unknown, static environment.
Given:
The robot’s controls (U1:t)
Observations of nearby features (Z1:t)
Estimate:
Map of features (m)
Pose / Path of the robot (xt)

64. Why is SLAM a hard problem?
Uncertanties
Error in pose
Error in observation
Error in mapping
Error accumulated

65. Why is SLAM a hard problem?
SLAM: robot path and map are both unknown
Robot path error correlates errors in the map

66. Why is SLAM a hard problem?
Robot pose
uncertainty
In the real world, the mapping between observations and landmarks is unknown
Picking wrong data associations can have catastrophic consequences

67. Data Association Problem
A data association is an assignment of observations to landmarks
In general there are more than (n observations, m landmarks) possible associations
Also called “assignment problem”

68. Nature of the SLAM Problem
Continuous Location of objects in
component the map
Robot’s own pose
Discrete Correspondence
component
reasoning
Object is the same or not

69. SLAM: Simultaneous Localization and Mapping
Full SLAM:
Estimates Entire pose (x1:t) and map (m)
Given Previous knowledge (Z1:t-1, U1:t-1) Current measurement (Zt, Ut)
Estimates entire path and map!

70. Graphical Model of Full SLAM:
Compute a joint posterior over the whole path of robot and the map

71. SLAM: Simultaneous Localization and Mapping
Online SLAM:
Estimates Most recent pose (xt) and map (m)
Given Previous knowledge (Z1:t-1, U1:t-1) Current measurement (Zt, Ut)
Estimates most recent pose and map!

72. Graphical Model of Online SLAM:
Estimate a posterior over the current robot pose, and the map
Integrations typically done one at a time

73. SLAM with Extended Kalman Filter
Pre-requisites
Maps are feature-based (landmarks)
small number (< 1000)
Assumption - Gaussian Noise
Process only positive sightings
No landmark = negative
Landmark = positive

74. EKF-SLAM with known correspondences
Correspondence Data association problem
Landmarks can’t be uniquely identified
True identity of observed feature
Correspondence variable (Cit)
between feature (fit) and real landmark
Make correspondence variables explicit

75. EKF-SLAM with known correspondences
Signature Numerical value (average color)
Characterize type of landmark (integer)
Multidimensional vector
(height and color)

76. EKF-SLAM with known correspondences
Similar development to EKF localization
Diff  robot pose + coordinates of all landmarks
Combined state vector
(3N + 3)
Online posterior

77. Iteration through measurements
Mean
Motion update
Covariance
Test for new landmarks
Initialization of elements
Expected measurement
Filter is updated
Iteration through measurements

78. EKF-SLAM with known correspondences
Observing a landmark improves robot pose estimate
eliminates some uncertainty of other landmarks
Improves position estimates of the landmark + other landmarks
We don’t need to model past poses explicitly

79. Example

80. EKF-SLAM with known correspondences
Example:
Uncertainty of landmarks are mainly due to robot’s pose uncertainty (persist over time)
Estimated location of landmarks are correlated

81. EKF-SLAM with unknown correspondences
No correspondences for landmarks
Uses an incremental maximum likelihood (ML) estimator
Determines most likely value of the correspondence variable
Takes this value for granted later on

82. EKF-SLAM with unknown correspondences
Mean
Motion update
Covariance
Hypotheses of new landmark

84. General Problem Gaussian noise assumption Unrealistic
Spurious measurements Fake landmarks
Outliers
Affect robot’s localization

85. Solutions to General Problem
Provisional landmark list
New landmarks do not augment the map
Not considered to adjust robot’s pose
Consistent observation regular map

86. Solutions to General Problem
Landmark Existence Probability
Landmark is observed
Observable variable (o) increased by fixed value
Landmark is NOT observed when it should
Observable variable decreased
Removed from map when (o) drops below threshold

87. Problem with Maximum Likelihood (ML)
Once ML estimator determines likelihood of correspondence, it takes value for granted
always correct
Makes EKF susceptible to landmark confusion
Wrong results

88. Solutions to ML Problem
Spatial arrangement
Greater distance between landmarks
Less likely confusion will exist
Trade off:
few landmarks harder to localize
Little is known about optimal density of landmarks
Signatures
Give landmarks a very perceptual distinctiveness
(e,g, color, shape, …)

89. EKF-SLAM Limitations Selection of appropriate landmarks
Reduces sensor reading utilization to presence or absence of those landmarks
Lots of sensor data is discarded
Quadratic update time
Limits algorithm to scarce maps (< features)
Low dimensionality of maps harder data association problem

90. EKF-SLAM Limitations Fundamental Dilemma of EKF-SLAM
It might work well with dense maps (millions of features)
It is brittle with scarce maps
BUT
It needs scarce maps because of complexity of the algorithm (update process)

91. SLAM Techniques EKF SLAM  (chapter 10) Graph-SLAM  (chapter 11)
SEIF (sparse extended information filter)  (chapter 12)
Fast-SLAM  (chapter 13)

92. Graph-SLAM Solves full SLAM problem
Represents info as a graph of soft constraints
Accumulates information into its graph without resolving it (lazy SLAM)
Computationally cheap
At the other end of EKF-SLAM
Process information right away (proactive SLAM)
Computationally expensive

93. Graph-SLAM Calculates posteriors over robot path (not incremental)
Has access to the full data
Uses inference to create map using stored data
Offline algorithm

94. Sparse Extended Information Filter (SEIF)
Implements a solution to online SLAM problem
Calculates current pose and map (as EKF)
Stores information representation of all knowledge (as Graph-SLAM)
Runs Online and is computationally efficient
Applicable to multi-robot SLAM problem

95. FastSLAM Algorithm Particle filter approach to the SLAM problem
Maintain a set of particles
Particles contain a sampled robot path and a map
The features of the map are represented by own local Gaussian
Map is created as a set of separate Gaussians
Map features are conditionally independent given the path
Factoring out the path (1 per particle)
Map feature become independent
Eliminates the need to maintain correlation among them

96. FastSLAM Algorithm Updating in FastSLAM
Sample new pose update the observed features
Update can be performed online
Solves both online and offline SLAM problem
Instances Feature-based maps
Grid-based algorithm

97. Approximations for SLAM Problem
Local submaps [Leonard et al.99, Bosse et al. 02, Newman et al. 03]
Sparse links (correlations) [Lu & Milios 97, Guivant & Nebot 01]
Sparse extended information filters [Frese et al. 01, Thrun et al. 02]
Thin junction tree filters [Paskin 03]
Rao-Blackwellisation (FastSLAM) [Murphy 99, Montemerlo et al. 02, Eliazar et al. 03, Haehnel et al. 03]

98. Thank You