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