Probabilistic Robotics

Transcription

1 Probabilisic Roboics Bayes Filer Implemenaions Gaussian filers

2 Bayes Filer Reminder Predicion bel p u bel d Correcion bel η p z bel

3 Gaussians : ~ π e p N p - Univariae / / : ~ μ μ μ e p Ν p d π Mulivariae

4 ~ ~ a b a N Y b ax Y N X Properies of Gaussians ~ ~ ~ N X p X p N X N X

5 ~ ~ A A B A N Y B AX Y N X Mulivariae Gaussians ~ ~ ~ N X p X p N X N X

6 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

7 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

8 Kalman Filer Updaes in D 8

9 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

10 0 Kalman Filer Updaes in D R A A B u A bel ac a b u a bel

11 Kalman Filer Updaes

12 Linear Gaussian Sysems: Iniializaion Iniial belief is normally disribued: bel N ;

13 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

14 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 ; ~ ; ~ η

15 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 ; ~ ; ~ η

16 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 η η

17 Kalman Filer Algorihm. Algorihm Kalman_filer - - u z :. Predicion: A B u Correcion: K C C C Q K z C A A 9. Reurn R I K C 7

18 8 he Predicion-Correcion-Cycle R A A B u A bel ac a b u a bel Predicion

19 9 he Predicion-Correcion-Cycle Q C C C K K C I C z K bel obs K K z K bel Correcion

20 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

21 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!

22 Nonlinear Dynamic Sysems Mos realisic roboic problems involve nonlinear funcions g u z h

23 Lineariy Assumpion Revisied 3

24 Non-linear Funcion 4

25 EKF Linearizaion 5

26 EKF Linearizaion 6

27 EKF Linearizaion 3 7

28 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

29 9 EKF Algorihm. Eended_Kalman_filer - - u z :. Predicion: Correcion: 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

30 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

31 Landmark-based Localizaion 3

32 3. EKF_localizaion - - u z m: Predicion: 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 ω θ θ ω ω ' ' ' ' ' ' v v M ω α α ω α α Moion noise Jacobian of g w.r. locaion Prediced mean Prediced covariance Jacobian of g w.r. conrol

33 33. EKF_localizaion - - u z m: Correcion: ˆ 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

34 EKF Predicion Sep 34

35 EKF Observaion Predicion Sep 35

36 EKF Correcion Sep 36

37 Esimaion Sequence 37

38 Esimaion Sequence 38

39 Comparison o Groundruh 39

40 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

41 Linearizaion via Unscened ransform EKF UKF 4

42 UKF Sigma-Poin Esimae EKF UKF 4

43 UKF Sigma-Poin Esimae 3 EKF UKF 43

44 44 Unscened ransform n i n w w n n w n w i c i m i i c m... for ± λ λ χ β α λ λ λ λ χ 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 45 UKF_localizaion - - u z m: Predicion: v v M ω α α ω α α 0 0 r r Q a a Q M 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

46 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 47. EKF_localizaion - - u z m: Correcion: ˆ 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

48 UKF Predicion Sep 48

49 UKF Observaion Predicion Sep 49

50 UKF Correcion Sep 50

51 EKF Correcion Sep 5

52 Esimaion Sequence EKF PF UKF 5

53 Esimaion Sequence EKF UKF 53

54 Predicion Qualiy EKF UKF 54

55 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

56 Kalman Filer-based Sysem [Arras e al. 98]: Laser range-finder and vision High precision <cm accuracy [Couresy of Kai Arras] 56

57 Mulihypohesis racking 57

58 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

59 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

60 MH: Implemened Sysem Couresy of P. Jensfel and S. Krisensen 60

61 MH: Implemened Sysem 3 Eample run # hypoheses PH bes Map and rajecory Couresy of P. Jensfel and S. Krisensen #hypoheses vs. ime 6