**Probabilistic Robotics** SLAM and FastSLAM

**The SLAM Problem** • SLAM stands for simultaneous localization and mapping • originally developed by Hugh Durrant-Whyte and John J. Leonard • “Mobile robot localization by tracking geometric beacons” - 1991 • The task of building a map while estimating the pose of the robot relative to this map • Why is SLAM hard? Chicken and egg problem: • A map is needed to localize the robot and a pose estimate is needed to build a map • Errors correlate!

**Indoors** Undersea Underground Space SLAM Applications

**Typical Robot Equipment** • Mobile robot • Typically want error under • 2 cm per meter moved • 2° per 45° degrees turned

**Typical Robot Equipment** • Range finding sensors • Sonar • 10 mm accuracy • 4 m range • SICK laser • 10 mm accuracy • 80 m range • IR range finder • 1.5 m range • Vision • Increasingly viable

**Outline of typical SLAM Process**

**Representations** • Grid maps or scans [Lu & Milios, 97; Gutmann, 98: Thrun 98; Burgard, 99; Konolige & Gutmann, 00; Thrun, 00; Arras, 99; Haehnel, 01;…] • Landmark-based [Leonard et al., 98; Castelanos et al., 99: Dissanayake et al., 2001; Montemerlo et al., 2002;…

**The SLAM Problem** Given: • The robot’s controls • Observations of nearby features Estimate: • Map of features • Path of the robot A robot moving though an unknown, static environment

**Why is SLAM a hard problem?** SLAM: robot path and map are both unknown! Robot path error increases errors in the map

**Structure of the Landmark-based SLAM-Problem **

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

**Detecting Landmarks** • Look for spikes in range data • Red dots show table legs • Fit geometric primitives • RANSAC line fitting algorithm • Don’t want to fit to all data • Pick a random subset • Fit a primitive • Classify remaining points as outlier or not • Keep if enough points classify well • Keep a full occupancy map • Each cell is a landmark

**(E)KF SLAM** • Represent robot state and landmark position all in one big state • ( rx, ry, l1x, l1y, l2x, l2y,…) • Apply Kalman filter to state update and observations • Uncertainty represented in large covariance matrix

**Kalman Filter and Linearity** • For many applications, the time update and measurement equations are NOT linear • The KF is not directly applicable • Linearize around the non-linearities • Use of the KF is extended to more realistic situations

**Kalman Filter ** Extended Kalman Filter The Extended Kalman Filter (EKF) • The Extended Kalman (EKF) is a sub-optimal extension of the original KF algorithm • The EKF allows for estimation of non-linear processes or measurement relationships

**Linearity Assumption Revisited**

**Non-linear Function**

**EKF Linearization (1)**

**EKF Linearization (2) **

**EKF Linearization (3)**

**(E)KF-SLAM** • Map with N landmarks:(3+2N)-dimensional Gaussian • Can handle hundreds of dimensions

**EKF-SLAM** Map Correlation matrix

**EKF-SLAM** Map Correlation matrix

**EKF-SLAM** Map Correlation matrix

**EKF-SLAM** • Can be quadratic in number of landmarks • Non-linearities a problem • Has led to other approaches

**Dependencies** • Is there a dependency between the dimensions of the state space? • If so, can we use the dependency to solve the problem more efficiently? • In the SLAM context • The map depends on the poses of the robot. • We know how to build a map if the position of the sensor is known.

**x, y, ** Landmark 1 Landmark 2 Landmark M … FastSLAM • 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 Particle #1 x, y, Landmark 1 Landmark 2 Landmark M … Particle #2 x, y, Landmark 1 Landmark 2 Landmark M … … Particle N

**FastSLAM – Action Update** Landmark #1 Filter Particle #1 Landmark #2 Filter Particle #2 Particle #3

**FastSLAM – Sensor Update** Landmark #1 Filter Particle #1 Landmark #2 Filter Particle #2 Particle #3

**Weight = 0.8** Weight = 0.4 Weight = 0.1 FastSLAM – Sensor Update Particle #1 Particle #2 Particle #3

**Multi-Hypothesis Data Association** • Data association is done on a per-particle basis • Robot pose error is factored out of data association decisions

**Improved Proposal** • The proposal adapts to the structure of the environment

**Per-Particle Data Association** Was the observation generated by the red or the blue landmark? P(observation|red) = 0.3 P(observation|blue) = 0.7 • Two options for per-particle data association • Pick the most probable match • Pick an random association weighted by the observation likelihoods • If the probability is too low, generate a new landmark

**Results – Victoria Park** • 4 km traverse • < 5 m RMS position error • 100 particles Blue = GPS Yellow = FastSLAM Dataset courtesy of University of Sydney

**Selective Re-sampling** • Re-sampling is dangerous, since important samples might get lost(particle depletion problem) • Key question: When should we re-sample?

**visiting new areas** closing the first loop visiting known areas second loop closure Typical Evolution of particle number

**Intel Lab** • 15 particles • four times faster than real-timeP4, 2.8GHz • 5cm resolution during scan matching • 1cm resolution in final map

**More Details on FastSLAM** • M. Montemerlo, S. Thrun, D. Koller, and B. Wegbreit. FastSLAM: A factored solution to simultaneous localization and mapping, AAAI02 • D. Haehnel, W. Burgard, D. Fox, and S. Thrun. An efficient FastSLAM algorithm for generating maps of large-scale cyclic environments from raw laser range measurements, IROS03 • M. Montemerlo, S. Thrun, D. Koller, B. Wegbreit. FastSLAM 2.0: An Improved particle filtering algorithm for simultaneous localization and mapping that provably converges. IJCAI-2003 • G. Grisetti, C. Stachniss, and W. Burgard. Improving grid-based slam with rao-blackwellized particle filters by adaptive proposals and selective resampling, ICRA05 • A. Eliazar and R. Parr. DP-SLAM: Fast, robust simultanous localization and mapping without predetermined landmarks, IJCAI03