Simultaneous Simultaneous Localization and Localization and
MappingMapping
Brian ClippBrian Clipp
Comp 790-072 RoboticsComp 790-072 Robotics
The SLAM ProblemThe SLAM Problem
► Given Given Robot controlsRobot controls Nearby measurementsNearby measurements
► EstimateEstimate Robot state (position, orientation)Robot state (position, orientation) Map of world featuresMap of world features
SLAM ApplicationsSLAM Applications
Images – Probabilistic RoboticsImages – Probabilistic Robotics
Indoors
Space
Undersea
Underground
OutlineOutline
►SensorsSensors►Probabilistic SLAMProbabilistic SLAM►Full vs. Online SLAMFull vs. Online SLAM►Example AlgorithmsExample Algorithms
Extended Kalman Filter (EKF) SLAMExtended Kalman Filter (EKF) SLAM FastSLAM (particle filter)FastSLAM (particle filter)
Types of SensorsTypes of Sensors
► OdometryOdometry► Laser Ranging and Detection (LIDAR)Laser Ranging and Detection (LIDAR)► Acoustic (sonar, ultrasonic)Acoustic (sonar, ultrasonic)► RadarRadar► Vision (monocular, stereo etc.)Vision (monocular, stereo etc.)► GPSGPS► Gyroscopes, Accelerometers (Inertial Gyroscopes, Accelerometers (Inertial
Navigation)Navigation)► Etc.Etc.
Sensor CharacteristicsSensor Characteristics
► NoiseNoise► Dimensionality of OutputDimensionality of Output
LIDAR- 3D pointLIDAR- 3D point Vision- Bearing only (2D ray in space)Vision- Bearing only (2D ray in space)
► RangeRange► Frame of ReferenceFrame of Reference
Most in robot frame (Vision, LIDAR, etc.)Most in robot frame (Vision, LIDAR, etc.) GPS earth centered coordinate frameGPS earth centered coordinate frame Accelerometers/Gyros in inertial coordinate Accelerometers/Gyros in inertial coordinate
frameframe
A Probabilistic ApproachA Probabilistic Approach
►The following algorithms take a The following algorithms take a probabilistic approachprobabilistic approach
tu
tz
m
tx
uzmx
t
t
t
ttt
to1 timefrom imputs Control
to1 timefrom inputsSensor
tenvironmen theof Map
at timerobot theof State
),|,p(
:1
:1
:1:1
Full vs. Online SLAMFull vs. Online SLAM
► Full SLAM calculates the robot state Full SLAM calculates the robot state over all time up to time tover all time up to time t
),|,p( :1:1:1 ttt uzmx
► Online SLAM calculates the robot state for Online SLAM calculates the robot state for the current time tthe current time t
121:1:1:1:1:1 ...),|,(),|,p( ttttttt dxdxdxuzmxpuzmx
Full vs. Online SLAMFull vs. Online SLAM
),|,p( :1:1:1 ttt uzmx
Full SLAM Online SLAM
121:1:1:1:1:1 ...),|,(),|,p( ttttttt dxdxdxuzmxpuzmx
Two Example SLAM Two Example SLAM AlgorithmsAlgorithms
►Extended Kalman Filter (EKF) SLAMExtended Kalman Filter (EKF) SLAM Solves online SLAM problemSolves online SLAM problem Uses a linearized Gaussian probability Uses a linearized Gaussian probability
distribution modeldistribution model
►FastSLAMFastSLAM Solves full SLAM problemSolves full SLAM problem Uses a sampled particle filter distribution Uses a sampled particle filter distribution
modelmodel
Extended Kalman Filter SLAMExtended Kalman Filter SLAM
►Solves the Online SLAM problem using Solves the Online SLAM problem using a linearized Kalman filtera linearized Kalman filter
►One of the first probabilistic SLAM One of the first probabilistic SLAM algorithmsalgorithms
►Not used frequently today but mainly Not used frequently today but mainly shown for its explanatory valueshown for its explanatory value
Process and Measurement Process and Measurement ModelsModels
► Non-linear Dynamic ModelNon-linear Dynamic Model Describes change of robot state with timeDescribes change of robot state with time
► Non-Linear Measurement ModelNon-Linear Measurement Model Predicts measurement value given robot statePredicts measurement value given robot state
EKF EquationsEKF Equations
►PredictPredict
►CorrectCorrect
EKF ExampleEKF Example
t=0
•Initial State and Uncertainty
•Using Range Measurements
EKF ExampleEKF Example
t=1
•Predict Robot Pose and Uncertainty at time 1
EKF ExampleEKF Example
t=1
•Correct pose and pose uncertainty
•Estimate new feature uncertainties
EKF ExampleEKF Example
t=2
•Predict pose and uncertainty of pose at time 2
•Predict feature measurements and their uncertainties
EKF ExampleEKF Example
t=2
•Correct pose and mapped features
•Update uncertainties for mapped features
•Estimate uncertainty of new features
Application from Probabilistic Application from Probabilistic RoboticsRobotics
[courtesy by John Leonard]
Application from Probabilistic Application from Probabilistic RoboticsRobotics
odometry estimated trajectory
[courtesy by John Leonard]
Correlation Between Correlation Between Measurement Association and Measurement Association and
State ErrorsState Errors
► Association between measurements and features is unknownAssociation between measurements and features is unknown► Errors in pose and measurement associations are correlatedErrors in pose and measurement associations are correlated
Robot poseuncertainty
Correct Associations
Robot’s Associations
Measurement AssociationsMeasurement Associations
► Measurements must be associated with particular Measurements must be associated with particular featuresfeatures If the feature is new add it to the mapIf the feature is new add it to the map Otherwise update the feature in the mapOtherwise update the feature in the map
► Discrete decision must be made for each feature Discrete decision must be made for each feature association, cassociation, ctt
tu
tz
c
m
tx
uzcmx
t
t
t
t
tttt
to1 timefrom imputs Control
to1 timefrom inputsSensor
t timea nsassociatio feature t toMeasuremen
tenvironmen theof Map
at timerobot theof State
),|,,p(
:1
:1
:1:1
Problems With EKF SLAMProblems With EKF SLAM► Uses uni-modal Gaussians to model non-Uses uni-modal Gaussians to model non-
Gaussian probability density functionGaussian probability density functionP
roba
bilit
y
Position
Problems With EKF SLAMProblems With EKF SLAM
►Only one set of measurement to Only one set of measurement to feature associations consideredfeature associations considered Uses maximum likelihood associationUses maximum likelihood association Little chance of recovery from bad Little chance of recovery from bad
associationsassociations
►O(NO(N33) matrix inversion required) matrix inversion required
FastSLAMFastSLAM
►Solves the Full SLAM problem using a Solves the Full SLAM problem using a particle filterparticle filter
Particle FiltersParticle Filters
► Represent probability distribution as a set Represent probability distribution as a set of discrete particles which occupy the state of discrete particles which occupy the state spacespace
Particle Filter Update CycleParticle Filter Update Cycle
►Generate new particle distribution Generate new particle distribution given motion model and controls given motion model and controls appliedapplied
►For each particleFor each particle Compare particle’s prediction of Compare particle’s prediction of
measurements with actual measurementsmeasurements with actual measurements Particles whose predictions match the Particles whose predictions match the
measurements are given a high weightmeasurements are given a high weight
►Resample particles based on weightResample particles based on weight
ResamplingResampling
►Assign each particle a weight Assign each particle a weight depending on how well its estimate of depending on how well its estimate of the state agrees with the the state agrees with the measurementsmeasurements
►Randomly draw particles from Randomly draw particles from previous distribution based on weights previous distribution based on weights creating a new distributioncreating a new distribution
Particle Filter AdvantagesParticle Filter Advantages
► Can represent multi-modal distributionsCan represent multi-modal distributionsP
roba
bilit
y
Position
Particle Filter DisadvantagesParticle Filter Disadvantages
►Number of particles grows Number of particles grows exponentially with the dimensionality exponentially with the dimensionality of the state spaceof the state space 1D – n particles1D – n particles 2D – n2D – n22 particles particles mD – nmD – nmm particles particles
FastSLAM FormulationFastSLAM Formulation
►Decouple map of features from poseDecouple map of features from pose Each particle represents a robot poseEach particle represents a robot pose Feature measurements are correlated Feature measurements are correlated
thought the robot posethought the robot pose If the robot pose was known all of the If the robot pose was known all of the
features would be uncorrelatedfeatures would be uncorrelated Treat each pose particle as if it is the true Treat each pose particle as if it is the true
pose, processing all of the feature pose, processing all of the feature measurements independentlymeasurements independently
Factored Posterior Factored Posterior (Landmarks)(Landmarks)
SLAM posterior
Robot path posterior
landmark positions
poses map observations & movements
Factorization first introduced by Murphy in 1999
Factored PosteriorFactored Posterior
Robot path posterior(localization problem) Conditionally
independent landmark positions
Rao-BlackwellizationRao-Blackwellization
Dimension of state space is drastically reduced by factorization making particle filtering possible
FastSLAMFastSLAM Rao-Blackwellized particle filtering based on
landmarks [Montemerlo et al., 2002]
Each landmark is represented by a 2x2 Extended Kalman Filter (EKF)
Each particle therefore has to maintain M EKFs
Landmark 1 Landmark 2 Landmark M…x, y,
Landmark 1 Landmark 2 Landmark M…x, y, Particle#1
Landmark 1 Landmark 2 Landmark M…x, y, Particle#2
ParticleN
…
FastSLAM – Action UpdateFastSLAM – Action Update
Particle #1
Particle #2
Particle #3
Landmark #1Filter
Landmark #2Filter
FastSLAM – Sensor UpdateFastSLAM – Sensor Update
Particle #1
Particle #2
Particle #3
Landmark #1Filter
Landmark #2Filter
FastSLAM – Sensor UpdateFastSLAM – Sensor Update
Particle #1
Particle #2
Particle #3
Weight = 0.8
Weight = 0.4
Weight = 0.1
FastSLAM ComplexityFastSLAM Complexity
► Update robot particles Update robot particles based on control ubased on control ut-1t-1
► Incorporate observation zIncorporate observation ztt into Kalman filtersinto Kalman filters
► Resample particle setResample particle set
N = Number of particlesM = Number of map features
O(N)Constant time per particle
O(N•log(M))Log time per particle
O(N•log(M))
O(N•log(M))Log time per particle
Log time per particle
Multi-Hypothesis Data Multi-Hypothesis Data AssociationAssociation
► Data association is Data association is done on a per-done on a per-particle basisparticle basis
► Robot pose error is Robot pose error is factored out of data factored out of data association association decisionsdecisions
Per-Particle Data AssociationPer-Particle Data Association
Was the observationgenerated by the redor the blue landmark?
P(observation|red) = 0.3 P(observation|blue) = 0.7
► Two options for per-particle data associationTwo options for per-particle data association Pick the most probable matchPick the most probable match Pick an random association weighted by Pick an random association weighted by
the observation likelihoodsthe observation likelihoods
► If the probability is too low, generate a new landmarkIf the probability is too low, generate a new landmark
MIT Killian CourtMIT Killian Court
The “infinite-corridor-dataset” at MIT
MIT Killian CourtMIT Killian Court
ConclusionConclusion
►SLAM is a hard problem which is not yet SLAM is a hard problem which is not yet fully solved fully solved
►Probabilistic methods which take Probabilistic methods which take account of sensor and process model account of sensor and process model error tend to work besterror tend to work best
►Effective algorithms must be robust to Effective algorithms must be robust to bad data associations which EKF SLAM bad data associations which EKF SLAM is notis not
►Real time operation limits complexity of Real time operation limits complexity of algorithms which can be appliedalgorithms which can be applied
References on EKF SLAMReferences on EKF SLAM
► P. Moutarlier, R. Chatila, "Stochastic Multisensory Data Fusion for P. Moutarlier, R. Chatila, "Stochastic Multisensory Data Fusion for Mobile Robot Localization and Environment Modelling", In Proc. of the Mobile Robot Localization and Environment Modelling", In Proc. of the International Symposium on Robotics Research, Tokyo, 1989.International Symposium on Robotics Research, Tokyo, 1989.
► R. Smith, M. Self, P. Cheeseman, "Estimating Uncertain Spatial R. Smith, M. Self, P. Cheeseman, "Estimating Uncertain Spatial Relationships in Robotics", In Autonomous Robot Vehicles, I. J. Cox Relationships in Robotics", In Autonomous Robot Vehicles, I. J. Cox
and G. T. Wilfong, editors, pp. 167-193, Springer-Verlag, 1990.and G. T. Wilfong, editors, pp. 167-193, Springer-Verlag, 1990.
► Ali Azarbayejani, Alex P. Pentland, "Recursive Estimation of Motion, Ali Azarbayejani, Alex P. Pentland, "Recursive Estimation of Motion, Structure, and Focal Length," Structure, and Focal Length," IEEE Transactions on Pattern Analysis IEEE Transactions on Pattern Analysis and Machine Intelligenceand Machine Intelligence ,vol. 17, no. 6, pp. 562-575, June, 1995. ,vol. 17, no. 6, pp. 562-575, June, 1995.
References on FastSLAMReferences on FastSLAM► M. Montemerlo, S. Thrun, D. Koller, and B. Wegbreit. FastSLAM: A M. Montemerlo, S. Thrun, D. Koller, and B. Wegbreit. FastSLAM: A
factored solution to simultaneous localization and mapping, factored solution to simultaneous localization and mapping, AAAI02AAAI02
► D. Haehnel, W. Burgard, D. Fox, and S. Thrun. An efficient FastSLAM D. Haehnel, W. Burgard, D. Fox, and S. Thrun. An efficient FastSLAM algorithm for generating maps of large-scale cyclic environments algorithm for generating maps of large-scale cyclic environments from raw laser range measurements, IROS03from raw laser range measurements, IROS03
► M. Montemerlo, S. Thrun, D. Koller, B. Wegbreit. FastSLAM 2.0: An M. Montemerlo, S. Thrun, D. Koller, B. Wegbreit. FastSLAM 2.0: An Improved particle filtering algorithm for simultaneous localization Improved particle filtering algorithm for simultaneous localization and mapping that provably converges. IJCAI-2003and mapping that provably converges. IJCAI-2003
► G. Grisetti, C. Stachniss, and W. Burgard. Improving grid-based G. Grisetti, C. Stachniss, and W. Burgard. Improving grid-based slam with rao-blackwellized particle filters by adaptive proposals slam with rao-blackwellized particle filters by adaptive proposals and selective resampling, ICRA05and selective resampling, ICRA05
► A. Eliazar and R. Parr. DP-SLAM: Fast, robust simultanous A. Eliazar and R. Parr. DP-SLAM: Fast, robust simultanous localization and mapping without predetermined landmarks, IJCAI03localization and mapping without predetermined landmarks, IJCAI03
Additional ReferenceAdditional Reference
►Many of the slides for this presentation Many of the slides for this presentation are from the book Probabilistic are from the book Probabilistic Robotic’s websiteRobotic’s website http://www.probabilistic-robotics.orghttp://www.probabilistic-robotics.org