Particle Filters. Ioannis Rekleitis

Transcription

1 Partcle Flters Ioanns Reklets

2 Bayesan Flter Estmate state x from data Z What s the probablty of the robot beng at x? x could be robot locaton, map nformaton, locatons of targets, etc Z could be sensor readngs such as range, actons, odometry from encoders, etc ) Ths s a general formalsm that does not depend on the partcular probablty representaton Bayes ;lter recursvely computes the posteror dstrbuton: Bel ( xt ) = P( xt ZT ) CSCE-774 Robotc Systems 2

3 Itera.ng the Bayesan Flter Propagate the moton model: Bel ( xt ) = P( xt at 1, xt 1) Bel( xt 1) dxt 1 Compute the current state estmate before takng a sensor readng by ntegratng over all possble prevous state estmates and applyng the moton model Update the sensor model: Bel( xt ) =ηp( ot xt ) Bel ( xt ) Compute the current state estmate by takng a sensor readng and multplyng by the current estmate based on the most recent moton hstory CSCE-774 Robotc Systems 3

4 Moble Robot Localza.on (Where Am I?) A moble robot moves whle collectng sensor measurements from the envronment. Two steps, acton and sensng: (X,Y,θ) Predcton/Propagaton: what s the robots pose x after acton A? Update: Gven measurement z, correct the pose x What s the probablty densty functon (pdf ) that descrbes the uncertanty P of the poses x and x? CSCE-774 Robotc Systems 4

5 State Es.ma.on Propagaton P( x x, α) t + 1 t Update + P( x x, z ) t+ 1 t+ 1 t+ 1 CSCE-774 Robotc Systems 5

6 Trad.onal Approach Kalman Flter Optmal for lnear systems wth Gaussan nose Extended Kalman ;lter: Lnearzaton Gaussan nose models Fast! CSCE-774 Robotc Systems 6

7 Monte- Carlo State Es.ma.on (Par.cle Flterng) Employng a Bayesan Monte- Carlo smulaton technque for pose estmaton. A partcle ;lter uses N samples as a dscrete representaton of the probablty dstrbuton functon (pdf ) of the varable of nterest: S " = [ x, w : = 1 N]! where x s a copy of the varable of nterest and w s a weght sgnfyng the qualty of that sample. In our case, each partcle can be regarded as an alternatve hypothess for the robot pose. CSCE-774 Robotc Systems 7

8 Par.cle Flter (cont.) The partcle ;lter operates n two stages: Predcton: After a moton (α) the set of partcles S s mod;ed accordng to the acton model S ʹ = f ( S, α, ν ) where (ν) s the added nose. The resultng pdf s the pror estmate before collectng any addtonal sensory nformaton. CSCE-774 Robotc Systems 8

9 Par.cle Flter (cont.) Update: When a sensor measurement (z) becomes avalable, the weghts of the partcles are updated based on the lkelhood of (z) gven the partcle x w ʹ = P( z x! ) w The updated partcles represent the posteror dstrbuton of the movng robot. CSCE-774 Robotc Systems 9

10 Remarks: In theory, for an n;nte number of partcles, ths method models the true pdf. In practce, there are always a ;nte number of partcles. CSCE-774 Robotc Systems 10

11 Resamplng For ;nte partcle populatons, we must focus populaton mass where the PDF s substantve. Falure to do ths correctly can lead to dvergence. Resamplng needlessly also has dsadvantages. One way s to estmate the need for resamplng based on the varance of the partcle weght dstrbuton, n partcular the coef;cent of varance: cv 2 t ESS var( w ( )) M t = = 2 E ( wt ( )) M = 1 t M = 1+ cv 2 t 1 ( Mw t ( ) 1) CSCE-774 Robotc Systems 11 2

12 Predc.on: Odometry Error Modelng Pecewse lnear moton: a smple example. Rotaton: Corrupted by Gaussan Nose. Translaton: Smulated by multple steps. Each step models translatonal and rotatonal error. Sngle step: Small rotatonal error (drft) before and after the translaton. Translatonal error proportonal to the dstance traveled. All errors drawn from a Normal Dstrbuton. CSCE-774 Robotc Systems 12

13 Odometry Error Modelng CSCE-774 Robotc Systems 13

14 Odometry Error Modelng P r e d c t o n CSCE-774 Robotc Systems 14

15 Odometry Error Modelng P r e d c t o n CSCE-774 Robotc Systems 15

16 Odometry Error Modelng P r e d c t o n CSCE-774 Robotc Systems 16

17 Odometry Error Modelng P r e d c t o n CSCE-774 Robotc Systems 17

18 Predc.on- Only Par.cle Dstrbu.on CSCE-774 Robotc Systems 18

19 Propaga.on of a dscrete.me system (δt=1 sec) t w t w v y y t w v x x t t t t t t t v t t t t v t t t δ ω φ φ φ δ φ δ ω ) ( sn ) ( cos ) ( = + + = + + = Where s the addtve nose for the lnear velocty, and s the addtve nose for the angular velocty w vt w ωt CSCE-774 Robotc Systems 19

20 Con.nuous mo.on example Dt=1sec Plottng 1 sample/sec all the partcles every 5 sec Constant lnear velocty Angular velocty changes randomly every 10 sec CSCE-774 Robotc Systems 20

21 Con.nuous mo.on example CSCE-774 Robotc Systems 21

22 Predc.on Examples Usng a PF Pecewse lnear moton (Translaton and Rotaton) Command success 70% Start at [- 8,0,0] Translate by 4m Rotate by 30 o Translate by 6m CSCE-774 Robotc Systems 22

23 Start [- 8,0,0 o ] CSCE-774 Robotc Systems 23

24 Translate by 4m 30% stayed CSCE-774 Robotc Systems 24

25 Rotate by 30 o 30% stayed CSCE-774 Robotc Systems 25

26 Translate by 6m CSCE-774 Robotc Systems 26

27 Propaga.on Known poston, known orentaton Bounded lnear velocty [ ] m/sec Bounded angular velocty Run 200 sec. Plottng every twenty ;fth sec. CSCE-774 Robotc Systems 27

28 Bounded Veloc.es ω [ ] rad / sec ω [ ] rad / sec ω [ ] rad / sec CSCE-774 Robotc Systems

29 Propaga.on Known poston, unknown orentaton Bounded lnear velocty [ ] m/sec Bounded angular velocty [ ] rad/sec Run 200 sec. Plottng every twenty ;fth sec. CSCE-774 Robotc Systems 29

30 Propaga.on CSCE-774 Robotc Systems 30

31 Propaga.on Known poston, known orentaton Bounded lnear velocty [ ] m/sec Bounded angular velocty [ ] rad/sec Run 200 sec. Plottng every twenty ;fth sec. For a partcle to stay at the orgn, t has to draw zero velocty 25 tmes n the row. CSCE-774 Robotc Systems 31

32 Bounded veloc.es CSCE-774 Robotc Systems 32

33 Update Examples Usng a PF CSCE-774 Robotc Systems 33

34 Envronment wth two red doors (unform dstrbu.on) CSCE-774 Robotc Systems 34

35 Envronment wth two red doors (Sensng the red door) CSCE-774 Robotc Systems 35

36 Sensng four walls CSCE-774 Robotc Systems 36

37 Four possble areas CSCE-774 Robotc Systems 37

38 Update Range only w t = w t πσ ρ e ( ρ ρ ) 2 r σ ρ 2 CSCE-774 Robotc Systems 38

39 Update Range only CSCE-774 Robotc Systems 39

40 Update Range only CSCE-774 Robotc Systems 40

41 Update Range only CSCE-774 Robotc Systems 41

42 Update Range only CSCE-774 Robotc Systems 42

43 Update Bearng only w t = w t πσ ϕ e ( ϕ ϕ ) 2 r σ ϕ 2 CSCE-774 Robotc Systems 43

44 Update Bearng only CSCE-774 Robotc Systems 44

45 Update Bearng only CSCE-774 Robotc Systems 45

46 Update Bearng only CSCE-774 Robotc Systems 46

47 Update Bearng only CSCE-774 Robotc Systems 47

48 Update Bearng only CSCE-774 Robotc Systems 48

49 Update Bearng only CSCE-774 Robotc Systems 49

50 Update Range and Bearng CSCE-774 Robotc Systems 50 ( ) ( ) ϕ σ ρ ρ ρ ρ σ ϕ ϕ πσ ϕ πσ r r e e w w t t =

51 Update Compass only w t = w t πσ ϑ e ( ϑ ϑ ) 2 r σ ϑ 2 CSCE-774 Robotc Systems 51

52 Update Compass only CSCE-774 Robotc Systems 52

53 Update Compass only CSCE-774 Robotc Systems 53

54 Coopera.ve Localza.on Pose of the movng robot s estmated relatve to the pose of the statonary robot. Statonary Robot observes the Movng Robot. Robot Tracker Returns: <ρ,θ,φ> x m est ( k + 1) = x y θ m m m xs + ρ cos = ys + π ( θ + θ ) ρ sn( θ + θ ) s ( φ ( θ + θ )) CSCE-774 Robotc Systems est 54 est est s s

55 Laser- Based Robot Tracker Robot Tracker Returns: <ρ,θ,φ> CSCE-774 Robotc Systems 55

56 Tracker Wegh.ng Func.on ( ) ( ) ( ) φ θ ρ σ φ φ φ σ θ θ θ σ ρ ρ ρ πσ πσ πσ e e e W = U p d a t e 56 CSCE-774 Robotc Systems

57 Example: Predc.on CSCE-774 Robotc Systems 57

58 Example: Update CSCE-774 Robotc Systems 58

59 Example: Predc.on CSCE-774 Robotc Systems 59

60 Example: Update CSCE-774 Robotc Systems 60

61 Vara.ons on PF Add some partcles unformly Add some partcles where the sensor ndcates Add some jtter to the partcles after propagaton Combne EKFs to track landmarks CSCE-774 Robotc Systems 61

62 Keep n Mnd: The number of partcles ncreases wth the dmenson of the state space CSCE-774 Robotc Systems 62

63 Complexty results for SLAM n=number of map features Problem: naïve methods have hgh complexty EKF models O(n^2) covarance matrx PF requres prohbtvely many partcles to characterze complex, nterdependent dstrbuton Soluton: explot condtonal ndependences Feature estmates are ndependent gven robot s path CSCE-774 Robotc Systems 63

64 Genera.ng Random Numbers From a unform RNG produce samples followng the Normal dstrbuton: The most basc form of the transformaton looks lke: y1 = sqrt( - 2 ln(x1) ) cos( 2 p x2 ) y2 = sqrt( - 2 ln(x1) ) sn( 2 p x2 ) The polar form of the Box- Muller transformaton s both faster and more robust numercally. The algorthmc descrpton of t s: ;loat x1, x2, w, y1, y2; do { x1 = 2.0 * ranf() - 1.0; x2 = 2.0 * ranf() - 1.0; w = x1 * x1 + x2 * x2; } whle ( w >= 1.0 ); w = sqrt( (- 2.0 * ln( w ) ) / w ); y1 = x1 * w; y2 = x2 * w; See: CSCE-774 Robotc Systems 64

65 Rao- Blackwellza.on Fgure from [Montemerlo et al Fast SLAM] CSCE-774 Robotc Systems 65

66 RBPF Implementa.on for SLAM 2 steps: Partcle ;lter to estmate robot s pose Set of low- dmensonal, ndependent EKF s (one per feature per partcle) E.g. FastSLAM whch ncludes several computatonal speedups to acheve O(M logn) complexty (wth M number of partcles) CSCE-774 Robotc Systems 66

67 Ques.ons For more nformaton on PF: CSCE-774 Robotc Systems 67