Probabilisic Roboics Bayes Filer Implemenaions Gaussian filers
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
~ ~ 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
0 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
3 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
4 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 ; ~ ; ~ η
5 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 ; ~ ; ~ η
6 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: A B u 3. 4. 5. Correcion: K C C C Q K z C 6. 7. 8. A A 9. Reurn R I K C 7
8 he Predicion-Correcion-Cycle R A A B u A bel ac a b u a bel Predicion
9 he Predicion-Correcion-Cycle Q C C C K K C I C z K bel obs K K z K bel Correcion
0 he Predicion-Correcion-Cycle Q C C C K K C I C z K bel obs K K z K bel R A A B u A bel ac a b u 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 3
Non-linear Funcion 4
EKF Linearizaion 5
EKF Linearizaion 6
EKF Linearizaion 3 7
8 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
9 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 30
Landmark-based Localizaion 3
3. 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
33. 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 34
EKF Observaion Predicion Sep 35
EKF Correcion Sep 36
Esimaion Sequence 37
Esimaion Sequence 38
Comparison o Groundruh 39
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! 40
Linearizaion via Unscened ransform EKF UKF 4
UKF Sigma-Poin Esimae EKF UKF 4
UKF Sigma-Poin Esimae 3 EKF UKF 43
44 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
45 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 χ Measuremen sigma poins 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 Prediced measuremen mean Pred. measuremen covariance Cross-covariance K S z K zˆ z Kalman gain Updaed mean K S K Updaed covariance 46
47. 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 48
UKF Observaion Predicion Sep 49
UKF Correcion Sep 50
EKF Correcion Sep 5
Esimaion Sequence EKF PF UKF 5
Esimaion Sequence EKF UKF 53
Predicion Qualiy EKF UKF 54
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! 55
Kalman Filer-based Sysem [Arras e al. 98]: Laser range-finder and vision High precision <cm accuracy [Couresy of Kai Arras] 56
Mulihypohesis racking 57
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. 58
MH: Implemened Sysem Hypoheses are eraced from LRF scans Each hypohesis has probabiliy of being he correc one: H i { ˆ i i P H i } Hypohesis probabiliy is compued using Bayes rule P H i s P s H i P H P s Hypoheses wih low probabiliy are deleed. New candidaes are eraced from LRF scans. C j { z j R j} i [Jensfel e al. 00] 59
MH: Implemened Sysem Couresy of P. Jensfel and S. Krisensen 60
MH: Implemened Sysem 3 Eample run # hypoheses PH bes Map and rajecory Couresy of P. Jensfel and S. Krisensen #hypoheses vs. ime 6