chapter 1 introduction - uva · 2018. 10. 3. · [email protected] . images courtesy of sebastian...

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 [email protected]

Images courtesy of Sebastian Thrun, Wolfram Burghard, Dieter Fox,

Michael Montemerlo, Dick Hähnel, Pieter Abbeel and others.

https://staff.fnwi.uva.nl/a.visser/education/ProbabilisticRobotics/

mailto:[email protected]

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


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 =


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


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 −− −


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−



The observation of a landmark m2 introduces another constraint:

The information vector increases with the term:

))((1ttt

itt

Tt HhzQH µµ +−−



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 κ−Φ=Ω



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


Network of features

Approximate the sparse information matrix with the argument that not all features are strongly connected:


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 Ω+Ω−Ω←

−


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


for all observed features

Calculate endfor

return

SEIF_sparsification


Calculate

return

with

SEIF_motion_update


return

Canonical form of

SEIF_update_state_estimate


return

For most map features

For a few features

For the pose


The effect of sparsification

The computation requires ‘constant’ time:



The memory scales linearly:



The prize is less accuracy, due to the approximation:


The degree of sparseness

By choosing the number of active features, accuracy can be traded against efficiency :


Effect of approximation

The effect of sparsification is less links between landmarks, more confidence, but nearly same information matrix:


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


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


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 ξµ ∆∆∆ Σ=


Results

MIT building (multiple loops):


Results



Results


UvA approach Q-WSM


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µ

chapter 1 introduction - uva · 2018. 10. 3. · [email protected] . images courtesy of sebastian...

Documents