slam techniques and algorithms - crv · pdf filedefence research and development canada...
TRANSCRIPT
![Page 1: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/1.jpg)
Defence Research andDevelopment Canada
Recherche et développementpour la défense Canada Canada
SLAM Techniques and Algorithms
Jack Collier
![Page 2: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/2.jpg)
Defence R&D Canada • R & D pour la défense Canada
Goals
What will we learn
Gain an appreciation for what SLAM is and canaccomplish
Understand the underlying theory behind SLAM
Understand the terminology and fundamentalbuilding blocks of SLAM algorithms
Appreciate the deficiencies of SLAM and SLAMalgorithms
Won’t focus on the math, but the concept
Online non-linear feature based SLAM withunknown data association
![Page 3: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/3.jpg)
Defence R&D Canada • R & D pour la défense Canada
Outline
What is SLAM?
Probabilistic basis of SLAM
EKF SLAM
Data Association
Closing the Loop
FastSLAM
Sub-Mapping SLAM
Resources
Questions?
![Page 4: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/4.jpg)
Defence R&D Canada • R & D pour la défense Canada
What is SLAM?
Simultaneous Localization and Mapping
Given an unknown environment and vehicle pose:
Move through the environmentEstimate the robot poseGenerate a map of environmental features
Use the robot pose estimate to improve the maplandmark position estimates
Use the landmark estimates to improve the robotpose estimate
![Page 5: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/5.jpg)
Defence R&D Canada • R & D pour la défense Canada
What is SLAM?and why do we need it?
Robot motion models aren’t accurate
Wheel odometry error is cumulative
IMU sensors errors
Maps are generated from sensors on the vehicle. If wecan’t accurately predict the robot pose then how can weproduce an accurate map.
GPS can help but is not always available or reliable.
![Page 6: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/6.jpg)
Defence R&D Canada • R & D pour la défense Canada
What is SLAM?and why do we need it?
![Page 7: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/7.jpg)
Defence R&D Canada • R & D pour la défense Canada
What is SLAM?and why do we need it?
![Page 8: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/8.jpg)
Defence R&D Canada • R & D pour la défense Canada
What is SLAM?The SLAM problem
Given:
Robot control signal uk (or measurement)
A set of feature observations zK (sensormeasurements)
Estimate:
Map of landmarks Mk+1
Robot Pose Vk+1
Sources of Error:
Control signal
Mapping sensor
Motion model
Observation model
![Page 9: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/9.jpg)
Defence R&D Canada • R & D pour la défense Canada
Probabilistic Basis of SLAMProbability Theory
Probability Theory gives us a framework for dealingwith these sources of error
The online SLAM theory is:p(Vk+1,Mk+1|zk , uk)
Estimate the Joint Probability of Vk+1 and Mk+1
conditioned on zk , uk (Joint Conditional PDF)
![Page 10: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/10.jpg)
Defence R&D Canada • R & D pour la défense Canada
Probabilistic Basis of SLAMSLAM Probability toolkit
Conditional Probability p(x |y) = p(x ,y)p(y)
Product Rule p(x , y) = p(x |y)p(y)
Bayes Rule p(x |y) = p(y |x)p(x)p(y)
or
p(x |y) = ηp(y |x)p(x)
Gaussian PDFp(x) = det(2πΣ)−
12 exp−1
2(x − x̄)T Σ−1(x − x̄)
![Page 11: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/11.jpg)
Defence R&D Canada • R & D pour la défense Canada
Probabilistic Basis of SLAMSLAM as a Bayesian filter
p(Vk+1,Mk+1|zk+1, uk+1) =
ηp(zk+1|Vk+1,Mk+1)p(Vk+1,Mk+1|zk , uk+1) (Bayes Filter) =ηp(zk+1|Vk+1,Mk+1)
∫p(Vk+1,Mk+1|Vk , zk , uk+1)p(Vk |zk , uk+1)dvk
![Page 12: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/12.jpg)
Defence R&D Canada • R & D pour la défense Canada
Probabilistic Basis of SLAMSLAM as a Bayesian filter
p(Vk+1,Mk+1|zk+1, uk+1) =ηp(zk+1|Vk+1,Mk+1)p(Vk+1,Mk+1|zk , uk+1) (Bayes Filter)
=ηp(zk+1|Vk+1,Mk+1)
∫p(Vk+1,Mk+1|Vk , zk , uk+1)p(Vk |zk , uk+1)dvk
![Page 13: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/13.jpg)
Defence R&D Canada • R & D pour la défense Canada
Probabilistic Basis of SLAMSLAM as a Bayesian filter
p(Vk+1,Mk+1|zk+1, uk+1) =ηp(zk+1|Vk+1,Mk+1)p(Vk+1,Mk+1|zk , uk+1) (Bayes Filter) =ηp(zk+1|Vk+1,Mk+1)
∫p(Vk+1,Mk+1|Vk , zk , uk+1)p(Vk |zk , uk+1)dvk
![Page 14: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/14.jpg)
Defence R&D Canada • R & D pour la défense Canada
Probabilistic Basis of SLAMSLAM as a Bayesian filter
p(Vk+1,Mk+1|zk+1, uk+1) =ηp(zk+1|Vk+1,Mk+1)p(Vk+1,Mk+1|zk , uk+1) (Bayes Filter) =ηp(zk+1|Vk+1,Mk+1)
∫p(Vk+1,Mk+1|Vk , zk , uk+1)p(Vk |zk , uk+1)dvk
= ηp(zk+1|Vk+1,Mk+1)∫
p(Vk+1|Vk , uk)p(Vk ,Mk |zk , uk)dvk
No closed form solution
Approximate the solution using an Extended Kalman Filter
![Page 15: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/15.jpg)
Defence R&D Canada • R & D pour la défense Canada
Probabilistic Basis of SLAMSLAM as a Bayesian filter
p(Vk+1,Mk+1|zk+1, uk+1) =ηp(zk+1|Vk+1,Mk+1)p(Vk+1,Mk+1|zk , uk+1) (Bayes Filter) =ηp(zk+1|Vk+1,Mk+1)
∫p(Vk+1,Mk+1|Vk , zk , uk+1)p(Vk |zk , uk+1)dvk
= ηp(zk+1|Vk+1,Mk+1)∫
p(Vk+1|Vk , uk)p(Vk,Mk|zk,uk)dvk
Prior probability:Prior state and covariance estimates from the last filter iteration
![Page 16: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/16.jpg)
Defence R&D Canada • R & D pour la défense Canada
Probabilistic Basis of SLAMSLAM as a Bayesian filter
p(Vk+1,Mk+1|zk+1, uk+1) =ηp(zk+1|Vk+1,Mk+1)p(Vk+1,Mk+1|zk , uk+1) (Bayes Filter) =ηp(zk+1|Vk+1,Mk+1)
∫p(Vk+1,Mk+1|Vk , zk , uk+1)p(Vk |zk , uk+1)dvk
= ηp(zk+1|Vk+1,Mk+1)∫
p(Vk+1|Vk,uk)p(Vk ,Mk |zk , uk)dvk
Motion Model:Probabilistic motion model estimates the new vehicle pose covari-ance estimates from the prior estimate and the control
![Page 17: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/17.jpg)
Defence R&D Canada • R & D pour la défense Canada
Probabilistic Basis of SLAMSLAM as a Bayesian filter
p(Vk+1,Mk+1|zk+1, uk+1) =ηp(zk+1|Vk+1,Mk+1)p(Vk+1,Mk+1|zk , uk+1) (Bayes Filter) =ηp(zk+1|Vk+1,Mk+1)
∫p(Vk+1,Mk+1|Vk , zk , uk+1)p(Vk |zk , uk+1)dvk
= ηp(zk+1|Vk+1,Mk+1)∫
p(Vk+1|Vk , uk)p(Vk ,Mk |zk , uk)dvk
Measurement Model:Measurement model gives the expected value of the feature obser-vations.
![Page 18: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/18.jpg)
Defence R&D Canada • R & D pour la défense Canada
Probabilistic Basis of SLAMGaussian PDF
![Page 19: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/19.jpg)
Defence R&D Canada • R & D pour la défense Canada
Probabilistic Basis of SLAMGaussian PDF
![Page 20: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/20.jpg)
Defence R&D Canada • R & D pour la défense Canada
EKF SLAM
Simultaneous Localization and Mapping:
Joint estimate both robot pose and position ofunique landmarks
Landmark estimates improve robot pose estimatesand vice versa
Use standard Extended Kalman Filtering techniques
Assumes Gaussian noise and error.
Mk =
xm1
ym1...
xmn
ymn
, Vk =
x rk
y rk
θrk
![Page 21: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/21.jpg)
Defence R&D Canada • R & D pour la défense Canada
EKF SLAMAlgorithm
Xk =
(Vk
Mk
), Pk =
(Pv Pvm
PTvm Pm
)(1)
Prediction - A prediction of the new state vector andcovariance matrix is calculated from the previous stateand covariance, and the new control uk .
![Page 22: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/22.jpg)
Defence R&D Canada • R & D pour la défense Canada
EKF SLAMAlgorithm
Xk =
(Vk
Mk
), Pk =
(Pv Pvm
PTvm Pm
)(1)
Data Association - Find matches between the currentlandmarks Mk and the new set of observed features zk .
![Page 23: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/23.jpg)
Defence R&D Canada • R & D pour la défense Canada
EKF SLAMAlgorithm
Xk =
(Vk
Mk
), Pk =
(Pv Pvm
PTvm Pm
)(1)
Measurement Update - Calculate the Kalman gain foreach observed landmark and update the state and covari-ance values based on that Kalman gain and the measure-ment innovation.
![Page 24: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/24.jpg)
Defence R&D Canada • R & D pour la défense Canada
EKF SLAMAlgorithm
Xk =
(Vk
Mk
), Pk =
(Pv Pvm
PTvm Pm
)(1)
Augmentation - Integrate newly observed landmarks intothe state vector and covariance matrix.
![Page 25: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/25.jpg)
Defence R&D Canada • R & D pour la défense Canada
EKF SLAMLinearization
Kalman Filters work under a linear Gaussianassumption
Robot motion and measurement functions arenon-linear
Use Taylor Expansion to linearize (Approximate aGaussian)g ≈ g + g ′
g ′ is Jacobian of the function with respect to itsvariables
![Page 26: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/26.jpg)
Defence R&D Canada • R & D pour la défense Canada
EKF SLAMLinearization
![Page 27: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/27.jpg)
Defence R&D Canada • R & D pour la défense Canada
EKF SLAMPrediction
Predict the robot motion from the control signal
Linearize to approximate the Gaussian
V̄k+1 = Vk + ∆V
![Page 28: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/28.jpg)
Defence R&D Canada • R & D pour la défense Canada
EKF SLAMPrediction
Predict the robot motion from the control signal
Linearize to approximate the Gaussian
Vk+1 =
x rk
y rk
θrk
+
− vk
ωksin(θr
k) + vk
ωksin(θr
k + ωk∆t)vk
ωkcos(θr
k)− vk
ωkcos(θr
k + ωk∆t)
ωk∆t
![Page 29: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/29.jpg)
Defence R&D Canada • R & D pour la défense Canada
EKF SLAMPrediction
Predict the robot motion from the control signal
Linearize to approximate the Gaussian
P̄v(k+1) = GkPv(k)GTk
P̄vm(k+1) = RkQRTk
P̄k+1 =
(P̄v(k+1) P̄vm(k+1)
P̄Tvm(k+1) Pm
)
G linearizes the state transition functionR maps additional motion noise into the state space
![Page 30: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/30.jpg)
Defence R&D Canada • R & D pour la défense Canada
EKF SLAMFeature Extraction Algorithms
Extract stable salient features from environment):
![Page 31: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/31.jpg)
Defence R&D Canada • R & D pour la défense Canada
EKF SLAMFeature Extraction Algorithms
What do we want in feature extraction algorithm?
![Page 32: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/32.jpg)
Defence R&D Canada • R & D pour la défense Canada
EKF SLAMFeature Extraction Algorithms
What do we want in feature extraction algorithm?
Stable features
Outlier rejection
Accuracy
Speed
![Page 33: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/33.jpg)
Defence R&D Canada • R & D pour la défense Canada
EKF SLAMFeature Extraction Algorithms
What do we want in feature extraction algorithm?
Expectation Maximization
![Page 34: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/34.jpg)
Defence R&D Canada • R & D pour la défense Canada
EKF SLAMFeature Extraction Algorithms
What do we want in feature extraction algorithm?
RANSAC
![Page 35: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/35.jpg)
Defence R&D Canada • R & D pour la défense Canada
EKF SLAMFeature Extraction Algorithms
What do we want in feature extraction algorithm?
Split and Merge
![Page 36: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/36.jpg)
Defence R&D Canada • R & D pour la défense Canada
EKF SLAMFeature Extraction Algorithms
What do we want in feature extraction algorithm?
Hough Transform
![Page 37: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/37.jpg)
Defence R&D Canada • R & D pour la défense Canada
EKF SLAMFeature Extraction Algorithms
What do we want in feature extraction algorithm?
Incremental Line Fitting
![Page 38: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/38.jpg)
Defence R&D Canada • R & D pour la défense Canada
EKF SLAMLaser Feature Extraction
Extract features from Raw laserdata
Incremental algorithm (linetracking)
Fit a line to a set of pointsCompute the residualIf below error threshold addanother point and repeatIf above the last point addedis a feature
![Page 39: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/39.jpg)
Defence R&D Canada • R & D pour la défense Canada
EKF SLAMData Association
Find matches between features and landmarks
For each feature
Calculate the predicted feature for each landmark(measurement model)compute the Mahalanobis Distance:choose the feature/landmark with the lowestdistance (Maximum Likelihood) below somethreshold
End for
![Page 40: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/40.jpg)
Defence R&D Canada • R & D pour la défense Canada
EKF SLAMData Association
Measurement Model:
zmj =
(rmj
βmj
)=
( √∆y 2 + ∆x2
arctan (∆y ,∆x)
)
Mahalanobis Distance normalizes feature/landmarkdistance based on their covariances:
dij = (zi − zmj )T (Hj
¯Pk+1HjT + R)−1(zi − zm
j )
![Page 41: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/41.jpg)
Defence R&D Canada • R & D pour la défense Canada
EKF SLAMData Association
Measurement Model:
zmj =
(rmj
βmj
)=
( √∆y 2 + ∆x2
arctan (∆y ,∆x)
)Mahalanobis Distance normalizes feature/landmarkdistance based on their covariances:
dij = (zi − zmj )T (Hj
¯Pk+1HjT + R)−1(zi − zm
j )
![Page 42: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/42.jpg)
Defence R&D Canada • R & D pour la défense Canada
EKF SLAMWhy Mahalanobis Distance?
Why Mahalanobis Distance:
![Page 43: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/43.jpg)
Defence R&D Canada • R & D pour la défense Canada
EKF SLAMWhy Mahalanobis Distance?
![Page 44: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/44.jpg)
Defence R&D Canada • R & D pour la défense Canada
EKF SLAMWhy Mahalanobis Distance?
Points which lie on the same covariance ellipse have areequidistant
![Page 45: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/45.jpg)
Defence R&D Canada • R & D pour la défense Canada
EKF SLAMUpdate
For each data association
Calculate Kalman Gain:Kj = P̄k+1Hj
T (Hj P̄k+1HjT + R)−1
H linearizes the measurement modelUpdate state and covariance estimates:Xk+1 = X̄k+1 + Kj(zi − zm
j )
Pk+1 = P̄k+1 − KjHj P̄k+1
End for
Innovation vector - Larger → bigger correction
Kalman Gain - Weights the innovation vector.Smaller measurement error → higher gain (trustthe measurement)Smaller covariance → smaller gain (trust theprediction)
![Page 46: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/46.jpg)
Defence R&D Canada • R & D pour la défense Canada
EKF SLAMAugment
State Vector and Covariance matrix grow as newlandmarks are observed
If a feature has no matches, add it to the statevector as a new landmark
![Page 47: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/47.jpg)
Defence R&D Canada • R & D pour la défense Canada
EKF SLAMAugment
New Landmark Locations:
xmi = x r
k + ri cos(θrk + βi)
ymi = y r
k + ri sin(θrk + βi)
![Page 48: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/48.jpg)
Defence R&D Canada • R & D pour la défense Canada
EKF SLAMAugment
Append to state vector:
X =
Xxmi
ymi
![Page 49: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/49.jpg)
Defence R&D Canada • R & D pour la défense Canada
EKF SLAMAugment
Original Covariance Matrix:
P =
(Pv Pvm
PvmT Pm
)
![Page 50: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/50.jpg)
Defence R&D Canada • R & D pour la défense Canada
EKF SLAMAugment
Augmented Covariance Matrix:
Pa =
Pv Pvm PvTFi
T
PvmT Pm Pvm
TFiT
FiPv FiPvm FiPvFiT + MiRMi
T
F and M are the Jacobians which linearize the new land-mark equations with respect to vehicle pose and measure-ment variables respectively
![Page 51: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/51.jpg)
Defence R&D Canada • R & D pour la défense Canada
EKF SLAMOther Issues
Can use provisional landmark list to reject spuriousfeatures
Pruning (removing old/irrelevant landmarks)
Landmark Signatures (reflectivity/color/shape/etc.)
Landmark minimum distance
Intelligent update (Only update relevant landmarks)
Sub-Mapping (more on this later)
![Page 52: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/52.jpg)
Defence R&D Canada • R & D pour la défense Canada
EKF SLAMAdvantages
Straightforward application of the EKF
Large body of research to pull from
Works reasonably well for small number of featuresand distinct landmarks
![Page 53: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/53.jpg)
Defence R&D Canada • R & D pour la défense Canada
EKF SLAMDisadvantages
Complexity Quadratic with number of features
No guarantee of convergence in non-linear case
Make hard decisions about data associations
Can’t correct for erroneous data associations
Need sufficiently distinct landmarks (low clutter)
Gaussian Assumption
![Page 54: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/54.jpg)
Defence R&D Canada • R & D pour la défense Canada
EKF SLAMVariants
Unscented KF SLAM
Linearization using Sigma pointsPreserves the true mean and covariance of theposterior
Extended Information Filter SLAM
Dual representation of EKFLess complex measurement update
![Page 55: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/55.jpg)
Defence R&D Canada • R & D pour la défense Canada
Data AssociationErroneous data association WILL cause SLAM to fail!
1 feature to 2 landmarks
2 features to 1 landmark
clutter
Symmetry
Reduce the maxMahalanobis distance?
![Page 56: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/56.jpg)
Defence R&D Canada • R & D pour la défense Canada
Data Association
How do determine the correspondence between a featureand landmark?
Individual Compatibility
Joint Compatibility Branch and Bound
Combined Constrained Data Association
Randomized Joint Compatibility
Multi hypothesis Data Association
![Page 57: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/57.jpg)
Defence R&D Canada • R & D pour la défense Canada
Data AssociationIndividual Compatibility
Calculate Mahalanobis distance from each featureto landmark
Choose feature with smallest distance to landmarkwithin threshold (validation gate)
Disregard ambiguous associations
Advantage: Simple
Disadvantage: Can be error prone especially inclutter
![Page 58: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/58.jpg)
Defence R&D Canada • R & D pour la défense Canada
Data AssociationJoint Compatibility Branch and
Bound
Consider multiple associations simultaneously
Find the largest set of matches which correspond
Joint Mahalanobis distance measurement with jointvalidation gate
Branch and bound interpretation tree search
Advantage: Robust
Disadvantage: Tree search computation/May beAmbiguous sets
![Page 59: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/59.jpg)
Defence R&D Canada • R & D pour la défense Canada
Data AssociationJoint Compatibility Branch and
Bound
PotentialMatchesz1→ L1 or L2z2→ L2 or L3z3→ L4Null
![Page 60: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/60.jpg)
Defence R&D Canada • R & D pour la défense Canada
Data AssociationJoint Compatibility Branch and
Bound
![Page 61: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/61.jpg)
Defence R&D Canada • R & D pour la défense Canada
Data AssociationCombined Constrained Data
Association
Correspondence Graph technique
Nodes are individually compatible associations
Edges are compatible associations
Find the maximum clique of associations that aremutually compatible
Consider both relative and absolute constraints
Advantage: Can work with no pose info (relativeconstraints)
Disadvantage: May be ambiguous sets
![Page 62: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/62.jpg)
Defence R&D Canada • R & D pour la défense Canada
Data AssociationRandomized Joint Compatibility
JCBB with RANSAC
Uses relative constraints
Randomly select m of the n feature measurements
Advantage: Reduced complexity, no pose necessary
Disadvantage: Don’t always know Pfail and Pgood
![Page 63: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/63.jpg)
Defence R&D Canada • R & D pour la défense Canada
Closing the LoopWhat is loop closing?
Even with the best SLAM algorithm poseuncertainty will increase as the vehicle moves.
This pose uncertainty means that landmarklocations further from the map origin have a higheruncertainty
Revisiting a previously observed landmarkssignificantly reduces uncertainty in robot andlandmark pose estimates
Errors in landmark estimates are correlated withrobot pose
New pose info necessarily improved landmarkestimates
![Page 64: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/64.jpg)
Defence R&D Canada • R & D pour la défense Canada
Closing the LoopHow to Close the loop
Map Matching Techniques (feature matching)Can use pose estimate to constrain the searchExpectation Maximization AlgorithmNon-linear Optimization
![Page 65: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/65.jpg)
Defence R&D Canada • R & D pour la défense Canada
FastSLAMRepresent probability distributions by set of particlesRao-Blackwellized particle filter (low dimensionalEKFs)Each particle maintains its own pose and mapestimates (multi-hypothesis)Each landmark has its own EKFN landmarks and M particles we have MxN + 1filters
![Page 66: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/66.jpg)
Defence R&D Canada • R & D pour la défense Canada
FastSLAMAlgorithm
Do M times:
Retrieve pose from particle k
Predict a new pose (motion model)
Data Association
Measurement Update
Importance Weight
ENDRe-sample with Replacement using the importanceweight
Raw Odometry
FastSLAM
![Page 67: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/67.jpg)
Defence R&D Canada • R & D pour la défense Canada
FastSLAMAdvantages
Time logarithmic to number of landmarks
Multi-hypothesis data association (robustness)
No linearization of non-linear motion models
Solves both full SLAM and Online SLAM problems
![Page 68: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/68.jpg)
Defence R&D Canada • R & D pour la défense Canada
FastSLAMDisadvantages
Throws away correlations
Over optimistic pose estimate
Harder to close the loop
![Page 69: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/69.jpg)
Defence R&D Canada • R & D pour la défense Canada
Sub-Mapping SLAM
Partition SLAM into sub-mapsOptimally Fuse at a global level periodicallyReduces EKF linearization errorsReduces computational complexityClosing the loop techniques for sub-maps
![Page 70: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/70.jpg)
Defence R&D Canada • R & D pour la défense Canada
Sub-Mapping SLAMAlgorithms
ATLAS
Hybrid-SLAM
Decoupled Stochastic Mapping
Hierarchical SLAM
Conditionally In dependant SLAM
Divide and Conquer
EKF SLAM vs. D&C SLAM
![Page 71: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/71.jpg)
Defence R&D Canada • R & D pour la défense Canada
Other Topics
Visual SLAM
SIFT, Harris Corners, SURF, etc. for featuresNo range to featuresInverse Depth
Muti-robot SLAM
Teams map buildingInformation sharing/cooperation
Learning for SLAM
Adaptive SLAMLearnt sensor, observation, motion models
6-DOF SLAM/Mesh Models
Feature Extraction Algorithms
Scan matching
![Page 72: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/72.jpg)
Defence R&D Canada • R & D pour la défense Canada
Resources
Probabilistic Robotics
SLAM Summer School 2009http://www.acfr.usyd.edu.au/education/summerschool.shtml
openslam.org (MATLAB, C, Different algorithms,data sets, etc.)
![Page 73: SLAM Techniques and Algorithms - CRV · PDF fileDefence Research and Development Canada Recherche et d veloppement pour la d fense Canada SLAM Techniques and Algorithms Jack Collier](https://reader031.vdocuments.us/reader031/viewer/2022030403/5a795c957f8b9a0b118d758e/html5/thumbnails/73.jpg)
Defence R&D Canada • R & D pour la défense Canada
Questions?