CSE-473. A Gentle Introduction to Particle Filters

Transcription

1 CSE-473 A Genle Inroducion o Paricle Filers

2 Bayes Filers for Robo Localizaion Dieer Fo 2

3 Bayes Filers: Framework Given: Sream of observaions z and acion daa u: d Sensor model Pz. = { u, z2, u 1, z 1 Dynamics model P u,. Prior probabiliy of he sysem sae P. Waned: Esimae of he sae X of a dynamical sysem. The poserior of he sae is also called Belief: Bel = P u, z2, u 1, z } 1 Dieer Fo 3

4 Bayes Filers,,,,,,, u z u P u z u z P =η Bayes z = observaion u = acion = sae,,, 1 1 z u z u P Bel = Markov,,, 1 1 u z u P z P =η 1 1 1, = d Bel u P z P η Markov ,,,, = d u z u P u P z P η = η Pz P u 1,z 1,,u, 1 P 1 u 1,z 1,,u d 1 Toal prob. Dieer Fo 4

5 Bayes Filers are Familiar! Bel = η P z P u, 1 Bel 1 d 1 Kalman filers Paricle filers Hidden Markov models Dynamic Bayesian neworks Parially Observable Markov Decision Processes POMDPs Dieer Fo 5

6 Gaussians :, ~ σ µ πσ σ µ = e p N p -s s m Univariae / 2 / : ~ µ Σ µ Σ µ Σ = e p, Ν p d π m Mulivariae Dieer Fo 6

7 Kalman Filer Updaes in 1D Dieer Fo 7

8 Kalman Filer Updaes in 1D Dieer Fo 8

9 Kalman Filer Updaes Dieer Fo 9

10 Sample-based Localizaion sonar Dieer Fo 10

11 Paricle Filers Dieer Fo 11

12 z p Bel Bel z p w Bel z p Bel α α α = Sensor Informaion: Imporance Sampling Dieer Fo 12

13 Robo Moion Bel p u, ' Bel ' d ' Dieer Fo 13

14 z p Bel Bel z p w Bel z p Bel α α α = Sensor Informaion: Imporance Sampling Dieer Fo 14

15 Robo Moion Bel p u, ' Bel ' d ' Dieer Fo 15

16 Paricle Filer Algorihm 1. Algorihm paricle_filer S -1, u -1 z : 2. S 3. For i =1 n Generae new samples 4. Sample inde ji from he discree disribuion given by w -1 i j i 5. Sample from p, u using and i i 6. w = p z Compue imporance weigh i 7. η = η + w Updae normalizaion facor i i 8. S = S { <, w > } Inser 9. For =, η = 0 i =1 n 1 1 u 1 1 i i 10. w = w /η Normalize weighs Dieer Fo 16

17 draw i -1 from Bel -1 draw i from p i -1,u -1 Imporance facor for i :,, disribuion proposal arge disribuion i z p Bel u p Bel u p z p w = = η , = d Bel u p z p Bel η Paricle Filer Algorihm Dieer Fo 17

18 Resampling Given: Se S of weighed samples. Waned : Random sample, where he probabiliy of drawing i is given by w i. Typically done n imes wih replacemen o generae new sample se S. Dieer Fo 18

19 Resampling W n-1 w n w 1 w 2 W n-1 w n w 1 w 2 w 3 w 3 Roulee wheel Binary search, n log n Sochasic universal sampling Sysemaic resampling Linear ime compleiy Easy o implemen, low variance Dieer Fo 19

20 Resampling Algorihm 1. Algorihm sysemaic_resamplings,n: 1 2. S ' =, c1 = w 3. For i = 2 n Generae cdf i 4. c i = ci 1 + w 1 5. u ~ U[0, n ], i 1 Iniialize hreshold 1 = 6. For j =1 n Draw samples 7. While u j > c i Skip unil ne hreshold reached 8. i = i { i 1 S' = S' <, n > } Inser 10. u = u 1 + n Incremen hreshold j j 11. Reurn S Also called sochasic universal sampling Dieer Fo 20

21 Museum Tour-Guide Minerva Dieer Fo 21

22 Dieer Fo 22

23 Dieer Fo 23

24 Dieer Fo 24

25 Dieer Fo 25

26 Dieer Fo 26

27 Dieer Fo 27

28 Dieer Fo 28

29 Dieer Fo 29

30 Dieer Fo 30

31 Dieer Fo 31

32 Dieer Fo 32

33 Dieer Fo 33

34 Dieer Fo 34

35 Dieer Fo 35

36 Dieer Fo 36

37 Dieer Fo 37

38 Dieer Fo 38

39 Dieer Fo 39

40 Using Ceiling Maps for Localizaion [Dellaer e al. 99] Dieer Fo 40

41 Vision-based Localizaion z Pz h Dieer Fo 41

42 Under a Ligh Measuremen z: Pz : Dieer Fo 42

43 Ne o a Ligh Measuremen z: Pz : Dieer Fo 43

44 Elsewhere Measuremen z: Pz : Dieer Fo 44

45 Global Localizaion Using Vision Dieer Fo 45

46 Localizaion for AIBO robos CSE-473: Arificial Inelligence Dieer Fo 46

47 From Images o Objecs Approach: Erac relevan colors CSE-473: Arificial Inelligence Dieer Fo 47

48 Disribuions for Pz Dieer Fo 48

49 Velociy Based Moion Model Dieer Fo 49

50 Muli-Sep Moion Sar Dieer Fo 50

51 Eample CSE-473: Arificial Inelligence Dieer Fo 51

52 Kalman Filer Highly efficien, robus Uni- modal, limied handling of nonlineariies Paricle Filer Less efficien, highly robus Muli- modal, nonlinear, non- Gaussian Rao- Blackwellised Paricle Filer, MHT Combines PF wih KF Muli- modal, highly efficien CSE-473: Arificial Inelligence Dieer Fo 52

53 Ball Tracking Dieer Fo 53