Par$cle Filters. Pieter Abbeel UC Berkeley EECS. Many slides adapted from Thrun, Burgard and Fox, ProbabilisAc RoboAcs

Transcription

1 Par$cle Filters Pieter Abbeel UC Berkeley EECS May slides adapted from Thru, Burgard ad Fox, ProbabilisAc RoboAcs

2 MoAvaAo For coauous spaces: oee o aalyacal formulas for Bayes filter updates SoluAo 1: Histogram Filters: (ot studied i this course) ParAAo the state space Keep track of probability for each paraao Challeges: What is the dyamics for the paraaoed model? What is the measuremet model? OEe very fie resoluao required to get reasoable results SoluAo 2: ParAcle Filters: Represet belief by radom samples Ca use actual dyamics ad measuremet models Naturally allocates computaaoal resources where required (~ adapave resoluao) Aka Mote Carlo filter, Survival of the fiuest, CodesaAo, Bootstrap filter

3 Sample-based LocalizaAo (soar)

4 Problem to be Solved Give a sample-based represetaao of Bel(x t ) = P(x t z 1,, z t, u 1,, u t ) S t = {x t 1, x t 2,..., x t N } Fid a sample-based represetaao S t+1 = {x 1 t+1, x 2 t+1,..., x N t+1 } of Bel(x t+1 ) = P(x t+1 z 1,, z t, z t+1, u 1,, u t+1 )

5 Dyamics Update Give a sample-based represetaao of Bel(x t ) = P(x t z 1,, z t, u 1,, u t ) S t = {x t 1, x t 2,..., x t N } Fid a sample-based represetaao of P(x t+1 z 1,, z t, u 1,, u t+1 ) SoluAo: For i=1, 2,, N Sample x i t+1 from P(X t+1 X t = xi t, u t+1 )

6 ObservaAo Update Give a sample-based represetaao of P(x t+1 z 1,, z t ) Fid a sample-based represetaao of {x 1 t+1, x 2 t+1,..., x N t+1 } P(x t+1 z 1,, z t, z t+1 ) = C * P(x t+1 z 1,, z t ) * P(z t+1 x t+1 ) SoluAo: For i=1, 2,, N w (i) t+1 = w(i) t * P(z t+1 X t+1 = x(i) t+1 ) the distribuao is represeted by the weighted set of samples {< x 1 1 t+1, w t+1 >,< x 2 2 t+1, w t+1 >,...,< x N t+1, w N t+1 >}

7 SequeAal Importace Samplig (SIS) ParAcle Filter Sample x 1 1, x2 1,, xn 1 from P(X 1 ) Set w i 1 = 1 for all i=1,,n For t=1, 2, Dyamics update: For i=1, 2,, N Sample x i t+1 from P(X t+1 X t = xi t, u t+1 ) ObservaAo update: For i=1, 2,, N w i t+1 = wi t * P(z t+1 X t+1 = xi t+1 ) At ay Ame t, the distribuao is represeted by the weighted set of samples { <x i t, wi t > ; i=1,,n}

8 SIS paracle filter major issue The resulag samples are oly weighted by the evidece The samples themselves are ever affected by the evidece à Fails to cocetrate paracles/computaao i the high probability areas of the distribuao P(x t z 1,, z t )

9 SequeAal Importace Resamplig (SIR) At ay Ame t, the distribuao is represeted by the weighted set of samples { <x i t, wi t > ; i=1,,n} à à Sample N Ames from the set of paracles The probability of drawig each paracle is give by its importace weight à More paracles/computaao focused o the parts of the state space with high probability mass

10 SequeAal Importace Resamplig (SIR) ParAcle Filter 1. Algorithm particle_filter( S t-1, u t, z t ): 2. S t 3. For i =1 Geerate ew samples 4. Sample idex j(i) from the discrete distributio give by w t-1 i j ( i) 5. Sample x from p(x t x t 1,u t ) usig ad i i 6. w = p( z x ) Compute importace weight i 7. η = η + w t Update ormalizatio factor i i 8. S = S { < x, w > } Isert 9. For =, η = 0 i i 10. w t = wt /η Normalize weights 11. Retur S t t t t i =1 t t t t t xt 1 u t

11 ParAcle Filters

12 Sesor IformaAo: Importace Samplig

13 Robot MoAo

14 Sesor IformaAo: Importace Samplig

15 Robot MoAo

16

17

18

19

20

21

22

23

24

25

26

27

28

29

30

31

32

33

34 Noise Domiated by MoAo Model [Grisetti, Stachiss, Burgard, T-RO2006] à Most particles get (ear) zero weights ad are lost.

35 Importace Samplig TheoreAcal jusaficaao: for ay fucao f we have: f could be: whether a grid cell is occupied or ot, whether the posiao of a robot is withi 5cm of some (x,y), etc.

36 Importace Samplig Task: sample from desity p(.) SoluAo: sample from proposal desity π(.) Weight each sample x (i) by p(x (i) ) / π(x (i) ) E.g.: p π Requiremet: if π(x) = 0 the p(x) = 0.

37 ParAcle Filters Revisited 1. Algorithm particle_filter( S t-1, u t, z t ): 2. S t 3. For i =1 Geerate ew samples 4. Sample idex j(i) from the discrete distributio give by w t-1 i 5. Sample xt from 6. i Compute importace weight i 7. η = η + w t Update ormalizatio factor i i 8. St = St { < xt, wt > } Isert 9. For =, η = 0 i i 10. w t = wt /η Normalize weights 11. Retur S t w i t = p(z t x i t )p(x i i t x t 1 π (x i t x t 1,u t, z t ) i =1 π (x t x j(i) t 1,u t, z t ),u t )

38 OpAmal SequeAal Proposal π(.) OpAmal = à π (x t x i t 1,u t, z t ) p(x t x i t 1,u t, z t ) w i t = p(z t x i t)p(x i t x i t 1,u t ) (x i t x i t 1,u t,z t ) = p(z t x i t)p(x i t x i t 1,u t ) p(x i t x i t 1,u t,z t ) Applyig Bayes rule to the deomiator gives: p(x i t x i t 1,u t,z t )= p(z t x i t,u t,x i t 1)p(x i t x i t 1,u t ) p(z t x i t 1,u t) SubsAtuAo ad simplificaao gives

39 OpAmal SequeAal Proposal π(.) OpAmal = à π (x t x i t 1,u t, z t ) p(x t x i t 1,u t, z t ) Challeges: Typically difficult to sample from p(x t x i t 1,u t, z t ) Importace weight: typically expesive to compute itegral

40 Example 1: π(.) = OpAmal Proposal Noliear Gaussia State Space Model Noliear Gaussia State Space Model: The: with Ad:

41 Example 2: π(.) = MoAo Model à the stadard paracle filter

42 Example 3: ApproximaAg OpAmal π for LocalizaAo [Grisetti, Stachiss, Burgard, T-RO2006] Oe (ot so desirable solutio): use smoothed likelihood such that more particles retai a meaigful weight --- BUT iformatio is lost Better: itegrate latest observatio z ito proposal π

43 Example 3: ApproximaAg OpAmal π for LocalizaAo: GeeraAg Oe Weighted Sample 1. IiAal guess Build Gaussia Approximatio to Optimal Sequetial Proposal 2. Execute sca matchig starag from the iiaal guess, resulag i pose esamate. 3. Sample K poits i regio aroud. 4. Proposal distribuao is Gaussia with mea ad covariace: 5. Sample from (approximately opamal) sequeaal proposal distribuao. Z 6. Weight = p(z t x 0,m)p(x 0 x i t 1,u t )dx 0 i x 0

44 Example 3: Example ParAcle DistribuAos [Grisetti, Stachiss, Burgard, T-RO2006] Particles geerated from the approximately optimal proposal distributio. If usig the stadard motio model, i all three cases the particle set would have bee similar to (c).

45 Resamplig Cosider ruig a paracle filter for a system with determiisac dyamics ad o sesors Problem: While o iformaao is obtaied that favors oe paracle over aother, due to resamplig some paracles will disappear ad aeer ruig sufficietly log with very high probability all paracles will have become ideacal. O the surface it might look like the paracle filter has uiquely determied the state. Resamplig iduces loss of diversity. The variace of the paracles decreases, the variace of the paracle set as a esamator of the true belief icreases.

46 Resamplig SoluAo I EffecAve sample size: Example: Normalized weights All weights = 1/N à EffecAve sample size = N All weights = 0, except for oe weight = 1 à EffecAve sample size = 1 Idea: resample oly whe effecave samplig size is low

47 Resamplig SoluAo I (ctd)

48 Resamplig SoluAo II: Low Variace Samplig M = umber of paracles r i [0, 1/M] Advatages: More systemaac coverage of space of samples If all samples have same importace weight, o samples are lost Lower computaaoal complexity

49 Resamplig SoluAo III Loss of diversity caused by resamplig from a discrete distribuao SoluAo: regularizaao Cosider the paracles to represet a coauous desity Sample from the coauous desity E.g., give (1-D) paracles sample from the desity:

50 ParAcle DeprivaAo = whe there are o paracles i the viciity of the correct state Occurs as the result of the variace i radom samplig. A ulucky series of radom umbers ca wipe out all paracles ear the true state. This has o-zero probability to happe at each Ame à will happe evetually. Popular soluao: add a small umber of radomly geerated paracles whe resamplig. Advatages: reduces paracle deprivaao, simplicity. Co: icorrect posterior esamate eve i the limit of ifiitely may paracles. Other beefit: iiaalizaao at Ame 0 might ot have goue aythig ear the true state, ad ot eve ear a state that over Ame could have evolved to be close to true state ow; addig radom samples will cut out paracles that were ot very cosistet with past evidece ayway, ad istead gives a ew chace at geug close the true state.

51 ParAcle DeprivaAo: How May ParAcles to Add? Simplest: Fixed umber. BeUer way: Moitor the probability of sesor measuremets which ca be approximated by: Average esamate over mulaple Ame-steps ad compare to typical values whe havig reasoable state esamates. If low, iject radom paracles.

52

53 Noise-free Sesors Cosider a measuremet obtaied with a oise-free sesor, e.g., a oise-free laser-rage fider---issue? All paracles would ed up with weight zero, as it is very ulikely to have had a paracle matchig the measuremet exactly. SoluAos: ArAficially iflate amout of oise i sesors BeUer proposal distribuao (e.g., opamal sequeaal proposal)

54 AdapAg Number of ParAcles: KLD-Samplig E.g., typically more paracles eed at the begiig of localizaao ru Idea: ParAAo the state-space Whe samplig, keep track of umber of bis occupied Stop samplig whe a threshold that depeds o the umber of occupied bis is reached If all samples fall i a small umber of bis à lower threshold

55 z_{1-±}: the upper 1-± quaale of the stadard ormal distribuao ± = 0.01 ad ² = 0.05 works well i pracace

56 KLD-samplig

57 KLD-samplig