chapter 1 introduction - uva · 2018. 10. 3. · a.visser@uva.nl . images courtesy of sebastian...
Post on 28-Aug-2021
4 Views
Preview:
TRANSCRIPT
Probabilistic Robotics The Sparse Extended Information Filter
MSc course Artificial Intelligence 2018 https://staff.fnwi.uva.nl/a.visser/education/ProbabilisticRobotics/
Arnoud Visser
Intelligent Robotics Lab Informatics Institute
Universiteit van Amsterdam A.Visser@uva.nl
Images courtesy of Sebastian Thrun, Wolfram Burghard, Dieter Fox,
Michael Montemerlo, Dick Hähnel, Pieter Abbeel and others.
Probabilistic Robotics Course at the Universiteit van Amsterdam 2
Simultaneous Localization and Mapping
A robot acquires a map while localizing itself relative to this map. Online SLAM problem Full SLAM problem Estimate map m and current position xt Estimate map m and driven path x1:t
),|,( :1:1 ttt uzmxp ),|,( :1:1:1 ttt uzmxp
Probabilistic Robotics Course at the Universiteit van Amsterdam 3
SEIF SLAM
SEIF SLAM reduces the state vector y again to the current position xt
This is the same state vector y as EKF SLAM
( )TNyNxNyxtt smmsmmxy ,,1,1,1 =
Probabilistic Robotics Course at the Universiteit van Amsterdam 4
State estimate
SEIF SLAM requires every timestep inference to estimate the state
The state estimated is also done by GraphSLAM, as a post-processing step.
ξµ ~~~ 1−Ω=t
Probabilistic Robotics Course at the Universiteit van Amsterdam 5
Sparseness of Information Matrix
After a while, all landmarks are correlated in EKF’s correlation matrix
The normalized information matrix is naturally sparse; most elements are
close to zero (but none is zero).
)),(()),(( 1jt
itt
Tjt
it mxhzQmxhz −− −
Probabilistic Robotics Course at the Universiteit van Amsterdam 6
Acquisition of the information matrix
The observation of a landmark m1 introduces a constraint:
The constraint is of the type: Where h(xt,mj) is the measurement model and Qt the covariance of the
measurement noise.
ttT
t HQH 1−
Probabilistic Robotics Course at the Universiteit van Amsterdam 7
Acquisition of the information matrix
The observation of a landmark m2 introduces another constraint:
The information vector increases with the term:
))((1ttt
itt
Tt HhzQH µµ +−−
Probabilistic Robotics Course at the Universiteit van Amsterdam 8
Acquisition of the information matrix
The movement of the robot from x1 to x2 also introduces an constraint:
The constraint is now between the landmarks m1 and m2 (and not between the path xt-1 to xt):
Which can be simplified to
111 ][ −−− +Ω=Ω xt
Tx
Ttttt FRFGG
ttt κ−Φ=Ω
Probabilistic Robotics Course at the Universiteit van Amsterdam 9
Acquisition of the information matrix
The information matrix can become really sparse by applying a sparsification step:
This is done by partition the set of features into three disjoint subsets: Where m- is the set of passive features and m+ ∩ m0 is the set of active
features. The number of features that are allowed to remain active (set m+) is thresholded to guarantee efficiency.
−+ ++= mmmm 0
Probabilistic Robotics Course at the Universiteit van Amsterdam 10
Network of features
Approximate the sparse information matrix with the argument that not all features are strongly connected:
Probabilistic Robotics Course at the Universiteit van Amsterdam 11
Updating the current state estimate
The current state estimate is needed every timestep:
Yet, from the current state estimate only subset is needed: i.e. the robot position xt and the locations of the active landmarks m+.
This can be done with an iterative hill climbing algorithm: Where Fi is a projection matrix to extract element i from matrix Ω.
ξµ ~~~ 1−Ω= tt
tµ
( )Ttt smmsmmxyyxyx
++++++= 21 ,2,2,1,1
( ) ][1µµξµ i
Tii
Tiii FFFFF Ω+Ω−Ω←
−
Probabilistic Robotics Course at the Universiteit van Amsterdam 12
Full Algorithm
The algorithm combines the four steps; two updates and two approximations:
)n_update(SEIF_motio,, 111 tt-t-t-ttt ,u,μ,Ωξ=Ω µξ),,timate(e_state_esSEIF_updat~
tttt µξµ Ω=)~ate(rement_updSEIF_measu, ttttttt ,c,zμ,Ω,ξ=Ωξ
),ification(SEIF_spars~~tttt , Ω=Ω ξξ
t~,~~return µtt Ω,ξ
)s(espondenceknown_corrSEIF_SLAM_ Algorithm 111 tttt-t-t- ,c,z,u,μ,Ωξ
SEIF_measurement_update
Probabilistic Robotics Course at the Universiteit van Amsterdam 13
for all observed features
Calculate endfor
return
SEIF_sparsification
Probabilistic Robotics Course at the Universiteit van Amsterdam 14
Calculate
return
with
SEIF_motion_update
Probabilistic Robotics Course at the Universiteit van Amsterdam 15
return
Canonical form of
SEIF_update_state_estimate
Probabilistic Robotics Course at the Universiteit van Amsterdam 16
return
For most map features
For a few features
For the pose
Probabilistic Robotics Course at the Universiteit van Amsterdam 17
The effect of sparsification
The computation requires ‘constant’ time:
Probabilistic Robotics Course at the Universiteit van Amsterdam 18
The effect of sparsification
The memory scales linearly:
Probabilistic Robotics Course at the Universiteit van Amsterdam 19
The effect of sparsification
The prize is less accuracy, due to the approximation:
Probabilistic Robotics Course at the Universiteit van Amsterdam 20
The degree of sparseness
By choosing the number of active features, accuracy can be traded against efficiency :
Probabilistic Robotics Course at the Universiteit van Amsterdam 21
Effect of approximation
The effect of sparsification is less links between landmarks, more confidence, but nearly same information matrix:
Probabilistic Robotics Course at the Universiteit van Amsterdam 22
Full Algorithm
To extend the algorithm for unknown correspondences, an estimate for the correspondence is needed:
),ˆ,,|( argmaxˆ 1:1:11:1 tttttc
t ccuzzpct
−−=
ttttttttc
t dycuzypcyzpct
∫ −−= )ˆ,,|(),|( argmaxˆ 1:1:11:1
tttt
cttttcttcttc
t dydxcuzyxpcyxzpc ∫ ∫ −−= )ˆ,,|,(),,|( argmaxˆ 1:1:11:1
Probabilistic Robotics Course at the Universiteit van Amsterdam 23
Estimating the correspondence
To probability can be approximated by the Markov blanket of all landmarks connected to robot pose xt and landmark yct
)ˆ,,|,( 1:1:11:1 −− tttct cuzyxpt
Probabilistic Robotics Course at the Universiteit van Amsterdam 24
Correspondence test
Based on the probability that mj corresponds to mk:
)()( kBjBB ∪=1)( −Ω=Σ T
BBB FFξµ BBB FΣ=
exp)2det(return 1212
1
∆−∆∆−∆ ΣΣ
− µµπ T
)test(spondence_SEIF_corre Algorithm kj ,c,m,, µξΩ
1)( −∆∆∆ Ω=Σ T
B FF
BF ξµ ∆∆∆ Σ=
Probabilistic Robotics Course at the Universiteit van Amsterdam 25
Results
MIT building (multiple loops):
Probabilistic Robotics Course at the Universiteit van Amsterdam 26
Results
MIT building (multiple loops):
Probabilistic Robotics Course at the Universiteit van Amsterdam 27
Results
MIT building (multiple loops):
UvA approach Q-WSM
Probabilistic Robotics Course at the Universiteit van Amsterdam 28
Conclusion
The Sparse Extended Information Filter: Solves the Online SLAM problem efficiently. Where EKF spread the information of each measurement over the full
map, SEIF limits the spread to ‘active features’. All information in the stored in the canonical parameterization. Yet, an
estimate of the mean is still needed. This estimate is found with a hill climbing algorithm (and not a inversion of the information matrix).
The accuracy and efficiency can be balanced by selecting an appropriate number of ‘active features’.
),|,( :1:1 ttt uzmxp
tµ
top related