CSE-571 Robotics. Sample-based Localization (sonar) Motivation. Bayes Filter Implementations. Particle filters. Density Approximation

Transcription

1 Moivaion CSE57 Roboics Bayes Filer Implemenaions Paricle filers So far, we discussed he Kalman filer: Gaussian, linearizaion problems Paricle filers are a way o efficienly represen nongaussian disribuions Basic principle Se of sae hypoheses paricles Survivalofhefies 2 Samplebased Localizaion sonar Densiy Approimaion Paricle ses can be used o approimae densiies The more paricles fall ino an inerval, he higher he probabiliy of ha inerval How o draw samples form a funcion/disribuion? 0/8/6 Probabilisic Roboics 3 4

2 Rejecion Sampling Le us assume ha f<= for all Sample from a uniform disribuion Sample c from [0,] if f > c keep he sample oherwise rejec he sampe c f f c OK Imporance Sampling Principle We can even use a differen disribuion g o generae samples from f By inroducing an imporance weigh w, we can accoun for he differences beween g and f w = f / g f is ofen called arge g is ofen called proposal 5 6 Imporance Sampling wih Resampling: Landmark Deecion Eample Disribuions Waned: samples disribued according o p z, z 2, z 3 2

3 This is Easy! We can draw samples from p z l by adding noise o he deecion parameers. Imporance Sampling wih Resampling Targe disribuion f : p z, z,..., z = 2 n Õ k p z p p z, z,..., z k 2 n Sampling disribuion g: p z l = pz l p pz l Imporance weighs w : f g p zl p zk p z, z2,..., zn k ¹ l = = p z p z, z,..., z l Õ 2 n Imporance Sampling wih Resampling Resampling Given: Se S of weighed samples. Waned : Random sample, where he probabiliy of drawing i is given by w i. Weighed samples Afer resampling Typically done n imes wih replacemen o generae new sample se S. 3

4 Resampling Resampling Algorihm. Algorihm sysemaic_resamplings,n: Wn wn w w2 w3 Wn wn w w2 w3 2. S ' = Æ, c = w 3. For i = 2!n Generae cdf i 4. c i = ci + w 5. u ~ U[0, n ], i Iniialize hreshold = Roulee wheel Binary search, n log n Sochasic universal sampling Sysemaic resampling Linear ime compleiy Easy o implemen, low variance 6. For j =!n Draw samples 7. While u j > c i Skip unil ne hreshold reached 8. i = i + i 9. S' = S' È { <, n > } Inser 0. u = u + n Incremen hreshold j j. Reurn S Also called sochasic universal sampling Paricle Filers Sensor Informaion: Imporance Sampling Bel w a p z Bel a p z Bel Bel = a p z 4

5 Robo Moion ò Bel p u, ' Bel ' d ' Sensor Informaion: Imporance Sampling Bel w a p z Bel a p z Bel Bel = a p z Robo Moion ò Bel p u, ' Bel ' d ' Paricle Filer Algorihm. Algorihm paricle_filer S, u z : 2. S = Æ, h = 0 3. For i =!n Generae new samples 4. Sample inde ji from he discree disribuion given by w i 5. Sample from using j i p, u and i i 6. w = p z Compue imporance weigh i 7. h = h + w Updae normalizaion facor i i 8. S = S È{ <, w > } Inser 9. For i =!n u i i 0. w = w /h Normalize weighs 5

6 6 draw i from Bel draw i from p i,u Imporance facor for i :,, disribuion proposal arge disribuion i z p Bel u p Bel u p z p w µ = = h, ò = d Bel u p z p Bel h Paricle Filer Algorihm Sar Moion Model Reminder Proimiy Sensor Model Reminder Laser sensor Sonar sensor 24

7

8

9

10

11 Using Ceiling Maps for Localizaion 4 [Dellaer e al. 99] Visionbased Localizaion Under a Ligh Measuremen z: Pz : z Pz h

12 Ne o a Ligh Elsewhere Measuremen z: Pz : Measuremen z: Pz : Global Localizaion Using Vision Recovery from Failure 2

13 Localizaion for AIBO robos Adapive Sampling KLDSampling Sonar KLDSampling Laser Adap number of paricles on he fly based on saisical approimaion measure 3

14 Paricle Filer Projecion Densiy Eracion Sampling Variance CSE57 Roboics Bayes Filer Implemenaions Discree filers SA 4

15 Piecewise Consan Discree Bayes Filer Algorihm. Algorihm Discree_Bayes_filer Bel,d : 2. h=0 3. If d is a percepual daa iem z hen 4. For all do 5. Bel ' = P z Bel 6. h =h + Bel' 7. For all do 8. Bel' =h Bel' 9. Else if d is an acion daa iem u hen 0. For all do. Bel' 2. Reurn Bel =å ' P u, ' Bel ' 0/8/6 CSE57 Probabilisic Roboics 57 0/8/6 CSE57 Probabilisic Roboics 58 Piecewise Consan Represenaion Gridbased Localizaion Bel =<, y, q > 0/8/6 CSE57 Probabilisic Roboics 59 0/8/6 CSE57 Probabilisic Roboics 60 5

16 Sonars and Occupancy Grid Map Treebased Represenaion Idea: Represen densiy using a varian of Ocrees 0/8/6 CSE57 Probabilisic Roboics 6 0/8/6 CSE57 Probabilisic Roboics 62 Treebased Represenaions Efficien in space and ime Muliresoluion Localizaion Algorihms Comparison Kalman filer Mulihypohesis racking Topological maps Gridbased fied/variable Paricle filer Sensors Gaussian Gaussian Feaures NonGaussian Non Gaussian Poserior Gaussian Mulimodal Piecewise consan Piecewise consan Samples Efficiency memory /o +/++ Efficiency ime o/+ +/++ Implemenaion + o + +/o ++ Accuracy /++ ++ Robusness /++ Global localizaion No Yes Yes Yes Yes 0/8/6 CSE57 Probabilisic Roboics 63 6