SEIF,EnKF,EKFSLAM

PieterAbbeelUCBerkeleyEECS

TexPoint fonts used in EMF. Read the TexPoint manual before you delete this box.: AAAAAAAAAAAAA

n  Fromananaly6calpointofview==Kalmanfilter

n  Difference:keeptrackoftheinversecovarianceratherthanthecovariancematrix[maFerofsomelinearalgebramanipula6onstogetintothisform]

n  Whyinteres6ng?

n  Inversecovariancematrix=0iseasiertoworkwiththancovariancematrix=infinity(caseofcompleteuncertainty)

n  InversecovariancematrixisoPensparserthanthecovariancematrix---forthe“insiders”:inversecovariancematrixentry(i,j)=0ifxiiscondi6onallyindependentofxjgivensomeset{xk,xl,…}

n  Downside:whenextendedtonon-linearseZng,needtosolvealinearsystemtofindthemean(aroundwhichonecanthenlinearize)

n  SeeProbabilis6cRobo6cspp.78-79formorein-depthpros/consandProbabilis6cRobo6csChapter12foritsrelevancetoSLAM(thenoPenreferredtoasthe“sparseextendedinforma6onfilter(SEIF)”)

Informa6onFilter

n  KalmanfilterexactunderlinearGaussianassump6ons

n  Extensiontonon-linearseZng:

n  ExtendedKalmanfilter

n  UnscentedKalmanfilter

n  ExtensiontoextremelylargescaleseZngs:

n  EnsembleKalmanfilter

n  SparseInforma6onfilter

n  Mainlimita6on:restrictedtounimodal/Gaussianlookingdistribu6ons

n  Canalleviatebyrunningmul6pleXKFs+keepingtrackofthelikelihood;butthisiss6lllimitedintermsofrepresenta6onalpowerunlessweallowaverylargenumberofthem

KFSummary

EKF/UKFSLAM

n  State:(nR,eR,θR,nA,eA,nB,eB,nC,eC,nD,eD,nE,eE,nF,eF,nG,eG,nH,eH)

n  Nowmap=loca6onoflandmarks(vs.gridmaps)

n  Transi6onmodel:

n  Robotmo6onmodel;Landmarksstayinplace

B E

FC

H

AG

D

R

SimultaneousLocaliza6onandMapping(SLAM)

n  Inprac6ce:robotisnotawareofalllandmarksfromthebeginning

n  Moreover:nouseinkeepingtrackoflandmarkstherobothasnotreceivedanymeasurementsabout

àIncrementallygrowthestatewhennewlandmarksgetencountered.

n  Landmarkmeasurementmodel:robotmeasures[xk;yk],theposi6onoflandmarkkexpressedincoordinateframeaFachedtotherobot:n  h(nR,eR,θR,nk,ek)=[xk;yk]=R(θ)([nk;ek]-[nR;

eR])

n  OPenalsosomeodometrymeasurementsn  E.g.,wheelencoders

SimultaneousLocaliza6onandMapping(SLAM)

VictoriaParkDataSetVehicle

[courtesy by E. Nebot]

DataAcquisi6on

[courtesy by E. Nebot]

VictoriaParkDataSet

[courtesy by E. Nebot]

Es6matedTrajectory

[courtesy by E. Nebot]

EKFSLAMApplica6on

[courtesy by J. Leonard]

12

EKFSLAMApplica6on

odometry estimated trajectory

[courtesy by John Leonard]

Landmark-basedLocaliza6on

EKF-SLAM:prac6calchallengesn  Defininglandmarks

n  Laserrangefinder:Dis6nctgeometricfeatures(e.g.useRANSACtofindlines,thenusecornersasfeatures)

n  Camera:“interestpointdetectors”,textures,color,…

n  OPenneedtotrackmul6plehypotheses

n  Dataassocia6on/Correspondenceproblem:whenseeingfeaturesthatcons6tutealandmark---Whichlandmarkisit?

n  Closingtheloopproblem:howtoknowyouareclosingaloop?

à  Cansplitoffmul6pleEKFswheneverthereisambiguity;

à  KeeptrackofthelikelihoodscoreofeachEKFanddiscardtheoneswithlowlikelihoodscore

n  Computa6onalcomplexitywithlargenumbersoflandmarks.