slam seminar

Robotics & Intelligent System Laboratory Department of Electrical Engineering Seoul National University

RISL | Robotics & Intelligent Systems Lab.Seoul National University

Simultaneous Localization And Mapping : Part 1

August , 2016

JunHyuk Shin



Contents

1. Introduction

2. Formulation of SLAM problem

3. Structure of Probabilistic SLAM

4. Solutions to the SLAM Problem- EKF SLAM- Rao-Blackwellized Filter (FastSLAM)

5. Conclusion & Limitation



1. Introduction

SLAM Problem (Simultaneous localization and mapping)- Mobile robot to be placed at an unknown location in an unknown environment : the robot to build a map of environment while simultaneously determining its location within the map Implement domain

1) From indoor to outdoor2) Underwater3) Airborne systems

Elements of SLAM Localization : determine pose given a priori map Mapping : generate map when pose is accurately known from auxiliary source- SLAM allows robots to operate in an environment without a priori knowledge of a map and without access to independent position information

Page 1




A. Preliminaries Defined Vector- : state vector describing the location and orientation of the vehicle- : control vector applied at time k-1 to drive the vehicle to a state at time - : location vector of the th landmark whose location is assumed time invariant- : observation taken from the vehicle at the location of the th landmarks at time k. with no specific landmark, Defined Set- - - -

Page 2




B. Probabilistic SLAM SLAM probability distribution

: the joint posterior density of the landmark locations and vehicle state given the recorded observation and control inputs up with initial state of the vehicle

Page 3

Hidden Markov Model (HMM): true state is assumed to be unob-served Markov process, and the measurements are the observed states - each state has a probability distri-bution over the possible output token- conditional probability distribution at future steps depends only on the current state of the system, and not additionally previous steps

(1)




Recursive Bayes filters: general probabilistic approach for estimating an unknown pdf recursively over time for given system model, but no analytical solution in general - Observation model - Motion model Prediction step (a-priori)

: use system model to predict forward (Chapman-Kolmogorov identity + Markov property)

Update step (a-posteriori): update the prediction with observed data

where

Page 4

𝑃 (𝒁 𝑘|𝒙𝑘 ,𝒎 )𝑃 (𝒙𝑘|𝒙𝑘− 1 ,𝒖𝑘 )

Motion model Previous Posterior

Observation model Current prior

Normalization constant

(2) (3)

(4)

(5)



3. Structure of Probabilistic SLAM

Page 6

Nearly independent measurement Correlation between landmark- Error in landmark location is highly correlated : relative location between any two landmark, may be known with high accuracy- Joint probability density(and correlation) on all landmarks becomes monotonically more peaked as more observation are made- This convergence occurs because the relative loca-tion of observed two landmark is nearly independent of the coordinate frame of the vehicle Spring network analogy: Network of spring connect all landmarks together with correlation spring- As observations move through environment, springs of network become increasingly stiffer- Localization accuracy is bounded only by relative location accuracy



4. Solutions to the SLAM Problem

No Analytic solution on Recursive Bayes filters EKF(Extended Kalman Filter) SLAM

- Representation of (4) and (5) in the form of a state-space model- Additive Gaussian noise- Linearize non-linear system model

Fast SLAM- Describe the vehicle motion model in (3) as a set of samples - General non-Gaussian probability distribution- Using Rao-Blackwellized particle filter- Directly represent the nonlinear process model (still linearize the observation model)

Page 7



4. Solutions to the SLAM Problem : EKF-SLAM

A. Basis for EKF-SLAM method System model definition

Vehicle motion model : additive, zero mean uncorrelated Gaussian disturbances with covariance

Observation model : additive, zero mean uncorrelated Gaussian errors with covariance

Joint posterior distribution Mean

Covariance

Page 8




B. Linearization: linearize to approximate the Gaussian, using Taylor expansion- : Jacobian of that

- : Jacobian of that

C. Prediction Step (Time-update): predict the robot motion from the control signal - Generally no need to perform a time-update for stationary landmarks

Page 9




D. Feature Extraction: extract stable salient features from environment Performance

- Stable features- Outlier rejection- Accuracy- Speed

Feature extraction algorithm- Expectation Maximization- RANSAC- Split and Merge- Hough Transform- Incremental Line Fitting (line tracking)

E. Data Association: find matches between features and landmarks to make general landmark observation For each feature

- calculate the predicted feature for each landmark (Observation model)- compute the Mahalanobis Distance- choose the feature/landmark with the lowest distance (Maximum Likelihood) below some threshold

Page 10




F. Update Step (observation-update): For each data association, update state and covariance estimates Where (Kalman Gain)G. Augment: state vector and covariance matrix grow as new landmarks are observed- If a feature has no matches, add it to the state vector as a new landmark- Linearize the new landmark equations

Page 11



4. Solutions to the SLAM Problem : EKF-SLAM issue

Convergence

Computational effort- Computation grows quadratically with the number of landmarks , : update step requires that all landmarks and the joint covariance matrix be updated every time an observation is made- Parameter memory of mean and covariance matrix grows quadratically : mean factors, covariance matrix factors for 2-dimensional vehicle

Data Association: EKF SLAM is fragile to incorrect association of observations(feature) to landmarks- If many observations are incorporated into the EKF with wrong data associations which never be removed, the EKF will diverge

Nonlinearity: Non-linearity can be a significant problem in EKF-SLAM (Convergence, Consistency)

Page 12

- Convergence of map : monotonic convergence of the determinant of the map covariance matrix toward zero- The individual landmark variances converge toward a lower bound determined by initial uncertainties in robot position and observations



4. Solutions to the SLAM Problem : EKF-SLAM issue

Page 13

Data Association on Loop-closure Before closing the loop : landmark uncertainty increases as robot pose uncertainty increases. Then distributions of ob-servations from nearby landmarks will begin to overlap substantially. This overlap leads to am-biguity in the identity of the landmarks After closing the loop : revisiting a known landmark decreases both the robot pose uncertainty, and the uncertainty of landmarks previously observed Loop-closure problem : association problem is compounded in envi-ronments where landmarks are not simple points and indeed look different from different view-points

Robot Uncertainty Landmark Uncertainty



4. Solutions to the SLAM Problem : FastSLAM

A. Basis for FastSLAM method : FastSLAM decomposes the SLAM problem into particle filtering for one robot localiza-tion problem, and the EKF for a collection of landmark estimation problems Particle Filtering : represent the posterior pdf by a set of randomly chosen (Monte Carlo) weighted sam-ples Reduce sample space by Rao-Blackwellization

- Joint state is partitioned to - only need be sampled - marginal can be obtained

Rao-Blackwellized state: trajectory is represented by weighted samples and the map is computed analytically- The joint SLAM state

- Joint distribution at time - When conditioned on the trajectory, the map landmarks become independent

Page 14

Conditional Map Component Path Posterior Component




B. Sampling a New Pose- Cannot draw samples directly from the SLAM poste-rior at time : draw samples from a simpler distribution called the proposal distribution, and correct for the difference us-ing a technique called IS(importance sampling) Proposal Distribution : generate new particle at time given each particle

Samples drawn from the probabilistic motion model

C. Updating the Landmark Estimates: implements the update equation of each conditional landmark estimation using EKF

Page 15




D. Calculating Importance Weights- Samples from the proposal distribution are distributed according to , therefore do not match the desired posterior(target distribution) : this difference is corrected through IS (importance sampling) Importance Sampling

- In regions where the target distribution is larger than the proposal distribution, the samples receive higher weights: As a result, samples in this region will be picked more

Page 16




The degeneracy phenomenon : after a few iteration with sequential IS, most particles have negligible weights Measure of degeneracy

- Uniform weight : - Severe degeneracy :

F. Importance Resampling: When degeneracy is above some threshold, eliminate particles with low importance weights and multiply particles with high importance weight - The new set is generated by sampling with replacement such that - This step is necessary to insure that the particles are distributed according to the true posterior- Complexity :

Page 17




FastSLAM 1.0 The proposal distribution is the motion model

The samples are weighted according to the marginalized observation model

FastSLAM 2.0 The proposal distribution includes the current observation

=

Importance weight according to ( : normalization constant)

The advantage of FastSLAM 2.0 is that its proposal distribution is locally optimal: for each particle, it give the smallest possible variance in importance weight condi-tioned upon available information, , ,

Page 18



4. Solutions to the SLAM Problem : FastSLAM issue

Convergence- Theorem : Linear-Gaussian FastSLAM converges in expectation to the correct map with particle if all features are observed infinitely often, and the location of one feature is known in ad-vance

Page 19

Computational Complexity- Computation grows logarithmic increase with the number of landmarks , - Binary tree of landmark filters for a collection of landmark estimation𝑀 : In updating new particle process, instead of dupli-cate the entire tree, a single path is duplicated



4. Solutions to the SLAM Problem : FastSLAM issue

Data Association Motion ambiguity

- By Per-Particle Data Association - FastSLAM takes a Multi-Hypothesis approach to the data association problem due to particles- Particles that pick the correct data association will receive high weights : because they explain the observations well

Measurement ambiguity- Monte Carlo Data Association: following other methods..- Individual Compatibility- Joint Compatibility Branch and Bound - Combined Constrained Data Association- Random Joint Compatibility

Page 20



5. Conclusion

SLAM problem- Simultaneously determine localization and mapping- Probabilistic SLAM is recursive Bayesian filter problem- In this paper, two solution of SLAM which are EKF-SLAM and FastSLAM is intro-duced

Pros & Cons of SLAM solution

Page 21

EKF-SLAM FastSLAM

Pros

-Straightforward application of the EKF-Works reasonably well for small # of features and distinct landmarks-Retain covariance

-Complexity logarithmic with landmark M-Multi-hypothesis data association(robust)-No linearization of non-linear motion models-Solves both full SLAM and online SLAM

Cons

-Complexity quadratic with landmark M-No guarantee of convergence in non-linear case-Gaussian assumption-Can’t correct for data association errors

-Underestimated covariance cause lower map accuracy given data association-Harder to close the loop (by above)