Inroducion o Mobile Roboics Bayes Filer Kalman Filer Wolfram Burgard Cyrill Sachniss Giorgio Grisei Maren Bennewiz Chrisian Plagemann
Bayes Filer Reminder Predicion bel p u bel d Correcion bel η p z bel
Gaussians : ~ π e p N p - Univariae / / : ~ e p Ν p d π Mulivariae
~ ~ a b a N Y b ax Y N X Properies of Gaussians ~ ~ ~ N X p X p N X N X
We say in he Gaussian world as long as we sar wih Gaussians and perform only linear ransformaions. ~ ~ A A B A N Y B AX Y N X Mulivariae Gaussians ~ ~ ~ N X p X p N X N X
Discree Kalman Filer Esimaes he sae of a discree-ime conrolled process ha is governed by he linear sochasic difference equaion A B u ε wih a measuremen z C δ 6
Componens of a Kalman Filer A B C ε δ Mari nn ha describes how he sae evolves from o - wihou conrols or noise. Mari nl ha describes how he conrol u changes he sae from o -. Mari kn ha describes how o map he sae o an observaion z. Random variables represening he process and measuremen noise ha are assumed o be independen and normally disribued wih covariance R and Q respecively. 7
Kalman Filer Updaes in D 8
9 Kalman Filer Updaes in D wih Q C C C K K C I C z K bel wih obs K K z K bel
Kalman Filer Updaes in D R A A B u A bel ac a b u a bel
Kalman Filer Updaes
Linear Gaussian Sysems: Iniializaion Iniial belief is normally disribued: bel N ; 0 0 0 0
Dynamics are linear funcion of sae and conrol plus addiive noise: u B A ε Linear Gaussian Sysems: Dynamics R B u A N u p ; ; ~ ; ~ N R B u A N d bel u p bel
Linear Gaussian Sysems: Dynamics R A A B u A bel d B u A R B u A bel N R B u A N d bel u p bel ep ep ; ~ ; ~ η
Observaions are linear funcion of sae plus addiive noise: C z δ Linear Gaussian Sysems: Observaions Q C z N z p ; N Q C z N bel z p bel ; ~ ; ~ η
Linear Gaussian Sysems: Observaions wih ep ep ; ~ ; ~ Q C C C K K C I C z K bel C z Q C z bel N Q C z N bel z p bel η η
Kalman Filer Algorihm. Algorihm Kalman_filer - - u z :. Predicion: 3. A Bu 4. A A R 5. Correcion: 6. K C C C Q 7. K z C 8. I K C 9. Reurn
Kalman Filer Algorihm
Kalman Filer Algorihm Predicion Observaion Maching Correcion
0 he Predicion-Correcion-Cycle R A A u B A bel ac a u b a bel Predicion
he Predicion-Correcion-Cycle Q C C C K C K I C z K bel obs K K z K bel Correcion
he Predicion-Correcion-Cycle Q C C C K C K I C z K bel obs K K z K bel R A A u B A bel ac a u b a bel Correcion Predicion
Kalman Filer Summary Highly efficien: Polynomial in measuremen dimensionaliy k and sae dimensionaliy n: Ok.376 n Opimal for linear Gaussian sysems! Mos roboics sysems are nonlinear!
Nonlinear Dynamic Sysems Mos realisic roboic problems involve nonlinear funcions g u z h
Lineariy Assumpion Revisied
Non-linear Funcion
EKF Linearizaion
EKF Linearizaion
EKF Linearizaion 3
Predicion: Correcion: EKF Linearizaion: Firs Order aylor Series Epansion G u g u g u g u g u g H h h h h h
EKF Algorihm. Eended_Kalman_filer - - u z :. Predicion: 3. 4. 5. Correcion: 6. 7. 8. 9. Reurn u g R G G Q H H H K h z K H K I u g G h H u A B R A A Q C C C K C z K C K I
Localizaion Using sensory informaion o locae he robo in is environmen is he mos fundamenal problem o providing a mobile robo wih auonomous capabiliies. [Co 9] Given Map of he environmen. Sequence of sensor measuremens. Waned Esimae of he robo s posiion. Problem classes Posiion racking Global localizaion Kidnapped robo problem recovery
Landmark-based Localizaion
. EKF_localizaion - - u z m: Predicion:. 3. 4. 5. 6. u g V V M G G θ θ θ θ θ θ ' ' ' ' ' ' ' ' ' y y y y y y u g G v y v y v u u g V ω θ θ ω ω ' ' ' ' ' ' 4 3 0 0 v v M ω α α ω α α Moion noise Jacobian of g w.r. locaion Prediced mean Prediced covariance Jacobian of g w.r. conrol
. EKF_localizaion - - u z m: Correcion:. 3. 4. 5. 6. 7. 8. ˆ z z K H K I θ θ ϕ ϕ ϕ y y r r r m h H θ aan ˆ y y y y m m m m z Q H H S S H K 0 0 r r Q Prediced measuremen mean Pred. measuremen covariance Kalman gain Updaed mean Updaed covariance Jacobian of h w.r. locaion
EKF Predicion Sep
EKF Observaion Predicion Sep
EKF Correcion Sep
Esimaion Sequence
Esimaion Sequence
Comparison o Groundruh
EKF Summary Highly efficien: Polynomial in measuremen dimensionaliy k and sae dimensionaliy n: Ok.376 n No opimal! Can diverge if nonlineariies are large! Works surprisingly well even when all assumpions are violaed!
EKF Localizaion Eample Line and poin landmarks
EKF Localizaion Eample Line and poin landmarks
EKF Localizaion Eample Lines only Swiss Naional Ehibiion Epo.0 Quickime and a MPEG-4 Video decompressor are needed o see his picure.
Linearizaion via Unscened ransform EKF UKF
UKF Sigma-Poin Esimae EKF UKF
UKF Sigma-Poin Esimae 3 EKF UKF
Unscened ransform n i n w w n n w n w i c i m i i c m... for 0 0 0 ± λ λ χ β α λ λ λ λ χ Sigma poins Weighs i i g χ ψ n i i i i c n i i i m w w 0 0 ' ' ψ ψ ψ Pass sigma poins hrough nonlinear funcion Recover mean and covariance
UKF_localizaion - - u z m: Predicion: 4 3 0 0 v v M ω α α ω α α 0 0 r r Q a 0 0 0 0 a Q M 0 0 0 0 0 0 a a a a a a γ γ χ u u g χ χ χ L i i i i w c 0 χ χ L i i i w m 0 χ Moion noise Measuremen noise Augmened sae mean Augmened covariance Sigma poins Predicion of sigma poins Prediced mean Prediced covariance
UKF_localizaion - - u z m: Correcion: Ζ z χ h χ L i zˆ w m S i 0 L w z i 0 L i 0 i c w Ζ i Ζ z Ζ zˆ i c i ˆ i χ Ζ zˆ i i Measuremen sigma poins Prediced measuremen mean Pred. measuremen covariance Cross-covariance K S z K zˆ z Kalman gain Updaed mean K S K Updaed covariance
. EKF_localizaion - - u z m: Correcion:. 3. 4. 5. 6. 7. 8. ˆ z z K H K I θ θ ϕ ϕ ϕ y y r r r m h H θ aan ˆ y y y y m m m m z Q H H S S H K 0 0 r r Q Prediced measuremen mean Pred. measuremen covariance Kalman gain Updaed mean Updaed covariance Jacobian of h w.r. locaion
UKF Predicion Sep
UKF Observaion Predicion Sep
UKF Correcion Sep
EKF Correcion Sep
Esimaion Sequence EKF PF UKF
Esimaion Sequence EKF UKF
Predicion Qualiy EKF UKF
UKF Summary Highly efficien: Same compleiy as EKF wih a consan facor slower in ypical pracical applicaions Beer linearizaion han EKF: Accurae in firs wo erms of aylor epansion EKF only firs erm Derivaive-free: No Jacobians needed Sill no opimal! 60
Kalman Filer-based Sysem [Arras e al. 98]: Laser range-finder and vision High precision <cm accuracy Couresy of K. Arras
Mulihypohesis racking
Localizaion Wih MH Belief is represened by muliple hypoheses Each hypohesis is racked by a Kalman filer Addiional problems: Daa associaion: Which observaion corresponds o which hypohesis? Hypohesis managemen: When o add / delee hypoheses? Huge body of lieraure on arge racking moion correspondence ec.
MH: Implemened Sysem Hypoheses are eraced from LRF scans Each hypohesis has probabiliy of being he correc one: H i ˆ { i i i P H} Hypohesis probabiliy is compued using Bayes rule P Hi s Hypoheses wih low probabiliy are deleed. New candidaes are eraced from LRF scans. C j P s Hi P Hi P s { zj j R} [Jensfel e al. 00]
MH: Implemened Sysem Couresy of P. Jensfel and S. Krisensen
MH: Implemened Sysem 3 Eample run # hypoheses PH bes Map and rajecory #hypoheses vs. ime Couresy of P. Jensfel and S. Krisensen