FastSLAM: An efficient solution to the SLAM with unknown data association Young Ki Baik, Computer Vision Lab.

FastSLAM: An efficient solution to the SLAM with unknown data association

Young Ki Baik, Computer Vision Lab.

2

Fast SLAM

• References• Fastslam: An efficient solution to the

simultaneous localization and mapping problem with unknown data association

• S. Thrun et. al. (IJCAI 2003)

• Fastslam: A Factored Solution to the Simultaneous Localization and Mapping problem with unknown data association

• Michael Montemerlo (Thesis 2003)

3

Fast SLAM

• Contents• SLAM

• EKF-based SLAM

• Problems of EKF-based SLAM

• FastSLAM

• Experimental Results

• Conclusion

4

Fast SLAM

• SLAM• Simultaneous Localization and Mapping

problem

Real location

Location with error

Refined location

5

Fast SLAM

• SLAM• If we have the solution to the SLAM problem…

• Allow robots to operate in an environment without a priori knowledge of a map

• Open up a vast range of potential application for autonomous vehicles and robot

6

Fast SLAM

• EKF-based SLAM• Extended Kalman Filter

• Prediction

• Estimation

• Correction

kSJkPkK THxe

1)(

xFJFx

xHJHx

kxHkz ee

kzkzki em )(

kikKkxkx e )( kKkSkKkPkP T

e )(

kQkJkPkJkP TFxFxe 1

kukxFkxe ,1

kRJkPJkS THxeHx )(

: Previous value

: Input and measure

: Function

: Computed value

iFL LFJi

7

Fast SLAM

• EKF-based SLAM

• Assumption• Linear system and Gaussian noise

• Example (2D motion)• S : Object position• Θ : Landmark

• Setting state vector and covariance matrix

sss yx ,, yx ,

t

ts

x yx

yx

),(1

),,(

00

00

002

2

1e

esp

8

Fast SLAM

• Problems of EKF-based SLAM• Quadratic complexity (scaling problem)

• NxN computational complexity

t

ts

x yx

yx

),(1

),,(

00

00

002

2

1e

esp

sizevector

landmarkofnumberN

:#

:

#N

9

Fast SLAM

• Problems of EKF-based SLAM• Data association problem

• EKF-SLAM use single hypothesis• Correspondence problem

12

3

?4 ?? 32

10

Fast SLAM

• Modified EKF-based SLAM methods• Quadratic complexity (Scaling problem)

• Submap method (Compressed EKF) Update submap only → constant time Slow convergence

• Suboptimal method Reduced number of landmark Divergence problem

Reduced landmark distribution → bad

• Etc.

11

Fast SLAM

• Modified EKF-based SLAM methods• Data association problem

• Local Map Sequencing Corner and line segment (RANSAC)

• Joint Compatibility Branch and Bound Multi hypothesis for observation Exponential time

• Multi Hypothesis Tracking

12

Fast SLAM

• FastSLAM• Features

• Particle filter based SLAM Non-linear, non-Gaussian system can be

represented.

• Factored solution (for scaling problem) Faster then EKF-based SLAM Can treat plenty of landmarks About 1 million…

• Multi-hypothesis (for data association) Each particle means independent hypothesis.

13

Fast SLAM

• FastSLAM• Posterior Representation

• Posterior over maps and robot pose

tttt nuzsp ,,|,

tt

t

t

t

n

t

tn

tu

tz

n

n

ts

?,,? ? all ofset :?

at timen observatio ofn associatio data :

at time controlrobot :

at timen observatiosensor :

positionslandmark all ofset :

landmarkth - th ofposition :

at timerobot theof pose:

1

14

Fast SLAM

• FastSLAM• Factored Posterior Representation

• Posterior over maps and robot pose

• Posterior over maps and robot path

tttt nuzsp ,,|,

tttt nuzsp ,,|,

15

Fast SLAM

• FastSLAM• Factoring the SLAM problem

• If the true path of the robot is known, the position of landmark is conditionally independent of other landmark.

tttt nuzsp ,,|,

1s 2s ts

1

2

Rao-Blackwellized Particle Filter

16

Fast SLAM

• FastSLAM• Factored Posterior Representation

• Posterior over maps and robot path

tttttttttttt nuzspnuzspnuzsp ,,,|,,|,,|,

N

n

ttttn

tttttttt nuzspnuzspnuzsp1

,,,|,,|,,|,

Path posteriorLandmark estimators

17

Fast SLAM

• FastSLAM • State Vector has robot pose and landmark

position• Each particle has robot pose & Map• Each landmark has it’s own mean and variance

and state is solved using EKF

yx11 u 22 u N Nu

yx11 u 22 u N Nu

yx11 u 22 u N Nu

Particle 1:Particle 1:

Particle 2:Particle 2:

Particle M:Particle M:

Robot poseRobot pose Landmark 1Landmark 1

Landmark 2Landmark 2

Landmark NLandmark N

18

Fast SLAM

• FastSLAM • Prediction stage

• Each particle is modified according to the existing state transition model.

• Update stage• Revaluate each particle’s weight based on

observation.• Remove small weight particle.• Resampling : add a new particles

19

Fast SLAM

• EKF-based SLAM vs FastSLAM

1) EKF-based SLAM 2) PF-based SLAM - Correction - Selection

20

Fast SLAM

• Experimental Results• Victoria park (for comparison)

• Provider : University of Sydney• The vehicle was driven around for approximately

30 minutes, covering a distance of over 4 km.• Ground truth : GPS

21

Fast SLAM

• Experimental Results• Victoria park

Odometry FastSLAM

22

Fast SLAM

• Experimental Results• Accuracy

23

Fast SLAM

• Experimental Results• Run time (with 100 particles)

24

Fast SLAM

• Experimental Results• Odometry noise (EKF)

25

Fast SLAM

• Experimental Results• Odometry noise (FastSLAM)

26

Fast SLAM

• Experimental Results• Odometry noise (EKFSLAM vs FastSLAM)

27

Fast SLAM

• Conclusion• EKF-based SLAM has problems

• Gaussian assumption• High computational complexity

Scaling problem• Data association problem

Single hypothesis

• Fast SLAM• Non-Gaussian system• Factored representation and particle filter

Low computational complexity relative to EKF-base SLAM

Multiple hypothesis