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