CSE-571 Probabilistic Robotics Bayes Filter Implementations - - PowerPoint PPT Presentation

CSE-571 Probabilistic Robotics

Bayes Filter Implementations Particle filters

2

§ So far, we discussed the § Kalman filter: Gaussian, linearization problems § Discrete filter: high memory complexity § Particle filters are a way to efficiently represent

non-Gaussian distributions

§ Basic principle § Set of state hypotheses (“particles”) § Survival-of-the-fittest

Motivation

Sample-based Localization (sonar)

1/21/12 3 Probabilistic Robotics

4

§ Particle sets can be used to approximate functions

Function Approximation

§ The more particles fall into an interval, the higher

the probability of that interval

§ How to draw samples form a function/distribution?

5

§ Let us assume that f(x)<1 for all x § Sample x from a uniform distribution § Sample c from [0,1] § if f(x) > c

keep the sample

• therwise

reject the sampe

Rejection Sampling

c x f(x) c ’ x ’ f(x’ )

OK

6

§ We can even use a different distribution g to

generate samples from f

§ By introducing an importance weight w, we can

account for the “differences between g and f ”

§ w = f / g § f is often called

target

§ g is often called

proposal

Importance Sampling Principle

Importance Sampling with Resampling: Landmark Detection Example

Distributions

Wanted: samples distributed according to p(x| z1, z2, z3)

This is Easy!

We can draw samples from p(x|zl) by adding noise to the detection parameters.

Importance Sampling with Resampling

) ,..., , ( ) ( ) | ( ) ,..., , | ( : f

• n

distributi Target

2 1 2 1 n k k n

z z z p x p x z p z z z x p

∏

= ) ( ) ( ) | ( ) | ( : g

• n

distributi Sampling

l l l

z p x p x z p z x p = ) ,..., , ( ) | ( ) ( ) | ( ) ,..., , | ( : w weights Importance

2 1 2 1 n l k k l l n

z z z p x z p z p z x p z z z x p g f

∏

≠

= =

Weighted samples After resampling

Importance Sampling with Resampling

) ,..., , ( ) ( ) | ( ) ,..., , | ( : f

• n

distributi Target

2 1 2 1 n k k n

z z z p x p x z p z z z x p

∏

= ) ( ) ( ) | ( ) | ( : g

• n

distributi Sampling

l l l

z p x p x z p z x p = ) ,..., , ( ) | ( ) ( ) | ( ) ,..., , | ( : w weights Importance

2 1 2 1 n l k k l l n

z z z p x z p z p z x p z z z x p g f

∏

≠

= =

Importance Sampling with Resampling

Weighted samples After resampling

Particle Filter Projection

Density Extraction

Sampling Variance

Particle Filters

) | ( ) ( ) ( ) | ( ) ( ) | ( ) ( x z p x Bel x Bel x z p w x Bel x z p x Bel α α α = ← ←

− − −

Sensor Information: Importance Sampling

∫

←

−

' d ) ' ( ) ' | ( ) (

,

x x Bel x u x p x Bel

Robot Motion

) | ( ) ( ) ( ) | ( ) ( ) | ( ) ( x z p x Bel x Bel x z p w x Bel x z p x Bel α α α = ← ←

− − −

Sensor Information: Importance Sampling

Robot Motion

∫

←

−

' d ) ' ( ) ' | ( ) (

,

x x Bel x u x p x Bel

• 1. Algorithm particle_filter( St-1, ut-1 zt):

2.

• 3. For Generate new samples

4. Sample index j(i) from the discrete distribution given by wt-1 5. Sample from using and 6. Compute importance weight 7. Update normalization factor 8. Insert

• 9. For

10. Normalize weights

Particle Filter Algorithm

, = ∅ = η

t

S

n i … 1 = } , { > < ∪ =

i t i t t t

w x S S

i t

w + =η η

i t

x

) , | (

1 1 − − t t t

u x x p

) ( 1 i j t

x −

1 − t

u

) | (

i t t i t

x z p w = n i … 1 = η /

i t i t

w w =

draw xi

t-1 from Bel(xt-1)

draw xi

t from p(xt | xi t-1,ut-1)

Importance factor for xi

t:

) | ( ) ( ) , | ( ) ( ) , | ( ) | (

• n

distributi proposal

• n

distributi target

1 1 1 1 1 1 t t t t t t t t t t t t i t

x z p x Bel u x x p x Bel u x x p x z p w ∝ = =

− − − − − −

η

1 1 1 1

) ( ) , | ( ) | ( ) (

− − − −

∫

=

t t t t t t t t

dx x Bel u x x p x z p x Bel η

Particle Filter Algorithm

Resampling

• Given: Set S of weighted samples.
• Wanted : Random sample, where the

probability of drawing xi is given by wi.

• Typically done n times with replacement to

generate new sample set S’.

w2 w3 w1 wn Wn-1

Resampling

w2 w3 w1 wn Wn-1

• Roulette wheel
• Binary search, n log n
• Stochastic universal sampling
• Systematic resampling
• Linear time complexity
• Easy to implement, low variance

• 1. Algorithm systematic_resampling(S,n):

2.

• 3. For

Generate cdf 4. 5. Initialize threshold

• 6. For

Draw samples … 7. While ( ) Skip until next threshold reached 8. 9. Insert

• 10. Increment threshold
• 11. Return S’

Resampling Algorithm

1 1

, ' w c S = ∅ =

n i … 2 =

i i i

w c c + =

−1

1 ], , [ ~

1 1

=

−

i n U u n j … 1 =

1 −

+ = n u u

j j i j

c u >

{ }

> < ∪ =

−1

, ' ' n x S S

i

1 + = i i

Also called stochastic universal sampling

Start

Motion Model Reminder

Proximity Sensor Model Reminder

Laser sensor Sonar sensor

28

29

30

31

32

33

34

35

36

37

38

39

40

41

42

43

44

45

Using Ceiling Maps for Localization

[Dellaert et al. 99]

Vision-based Localization

P(z|x) h(x) z

Under a Light

Measurement z: P(z|x):

Next to a Light

Measurement z: P(z|x):

Elsewhere

Measurement z: P(z|x):

Global Localization Using Vision

Recovery from Failure

Localization for AIBO robots

Adaptive Sampling

• Idea:
• Assume we know the true belief.
• Represent this belief as a multinomial distribution.
• Determine number of samples such that we can guarantee

that, with probability (1- d), the KL-distance between the true posterior and the sample-based approximation is less than e.

• Observation:
• For fixed d and e, number of samples only depends on

number k of bins with support:

KLD-sampling

3 1 2

) 1 ( 9 2 ) 1 ( 9 2 1 2 1 ) 1 , 1 ( 2 1 ⎭ ⎬ ⎫ ⎩ ⎨ ⎧ − + − − − ≅ − − Χ =

−δ

ε δ ε z k k k k n

• 1. Algorithm adaptive_particle_filter( St-1, ut-1 zt, ):

2.

• 3. Do

Generate new samples 4. Sample index j(n) from the discrete distribution given by wt-1 5.

Sample from using and

6. Compute importance weight 7. Update normalization factor 8. Insert 9. If ( falls into an empty bin b) Update bins with support 10. k=k+1, b = non-empty 11. n=n+1

• 12. While ( )
• 13. For

14. Normalize weights

Adaptive Particle Filter Algorithm

} , { > < ∪ =

n t n t t t

w x S S

n t

w + =η η

n t

x

) , | (

1 1 − − t t t

u x x p

) ( 1 n j t

x −

1 − t

u

) | (

n t t n t

x z p w = n i … 1 = η /

i t i t

w w = δ ε, , Δ

∅ = = = = ∅ = b k n St , , , , α

n t

x ) 1 , 1 ( 2 1

2

δ ε − − Χ < k n

Example Run Sonar

Example Run Laser

Evaluation

Localization Algorithms - Comparison

Kalman filter Multi- hypothesis tracking Topological maps Grid-based

(fixed/variable)

Particle filter Sensors Gaussian Gaussian Features Non-Gaussian Non- Gaussian Posterior Gaussian Multi-modal Piecewise constant Piecewise constant Samples

Efficiency (memory)

++ ++ ++

• /o

+/++ Efficiency (time) ++ ++ ++

• /+

+/++ Implementation +

• +

+/o ++ Accuracy ++ ++

• +/++

++ Robustness

• +

+ ++ +/++ Global localization No Yes Yes Yes Yes