CSE-571 So far, we discussed the Kalman filter: Gaussian, - - PowerPoint PPT Presentation

1

CSE-571 Robotics

Bayes Filter Implementations Particle filters

2

§ So far, we discussed the § Kalman filter: Gaussian, linearization problems § 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)

10/18/16 3 Probabilistic Robotics 4

§ Particle sets can be used to approximate densities Density Approximation § The more particles fall into an interval, the higher

the probability of that interval

§ How to draw samples form a function/distribution?

2

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)

3 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

Õ

= Sampling distribution g: p(x | zl) = p(zl | x)p(x) p(zl) ) ,..., , ( ) | ( ) ( ) | ( ) ,..., , | ( : 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

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’.

4

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

Particle Filters

) | ( ) ( ) ( ) | ( ) ( ) | ( ) ( x z p x Bel x Bel x z p w x Bel x z p x Bel a a a = ¬ ¬

• Sensor Information: Importance Sampling

5

ò

¬

• '

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 a a a = ¬ ¬

• 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

, = Æ = h

t

S n i ! 1 = } , { > < È =

i t i t t t

w x S S

i t

w + =h h

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 = h /

i t i t

w w =

6

draw xit-1 from Bel(xt-1) draw xit from p(xt | xit-1,ut-1) Importance factor for xit: ) | ( ) ( ) , | ( ) ( ) , | ( ) | (

• 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 µ = =

• h

1 1 1 1

) ( ) , | ( ) | ( ) (

• ò

=

t t t t t t t t

dx x Bel u x x p x z p x Bel h

Particle Filter Algorithm

Start

Motion Model Reminder

Proximity Sensor Model Reminder

Laser sensor Sonar sensor

24

7

25 26 27 28

8

29 30 31 32

9

33 34 35 36

10

37 38 39 40

11

41

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):

12 Next to a Light

Measurement z: P(z|x):

Elsewhere

Measurement z: P(z|x):

Global Localization Using Vision

Recovery from Failure

13

Localization for AIBO robots

Adaptive Sampling KLD-Sampling Sonar

Adapt number of particles on the fly based

• n statistical approximation measure

KLD-Sampling Laser

14 Particle Filter Projection Density Extraction Sampling Variance

SA-1

CSE-571 Robotics

Bayes Filter Implementations Discrete filters

15

CSE-571 - Probabilistic Robotics 10/18/16 57

Piecewise Constant

CSE-571 - Probabilistic Robotics 10/18/16 58

Discrete Bayes Filter Algorithm

1.

Algorithm Discrete_Bayes_filter( Bel(x),d ): 2. h=0 3. If d is a perceptual data item z then 4. For all x do 5. 6. 7. For all x do 8. 9. Else if d is an action data item u then 10. For all x do 11. 12. Return Bel’(x)

) ( ) | ( ) ( ' x Bel x z P x Bel = ) ( ' x Bel + =h h ) ( ' ) ( '

1

x Bel x Bel

• =h

å

=

'

) ' ( ) ' , | ( ) ( '

x

x Bel x u x P x Bel

CSE-571 - Probabilistic Robotics 10/18/16 59

Piecewise Constant Representation

) , , ( > =< q y x x Bel

t

CSE-571 - Probabilistic Robotics 10/18/16 60

Grid-based Localization

16

CSE-571 - Probabilistic Robotics 10/18/16 61

Sonars and Occupancy Grid Map

CSE-571 - Probabilistic Robotics 10/18/16 62

Tree-based Representation

Idea: Represent density using a variant of Octrees

CSE-571 - Probabilistic Robotics 10/18/16 63

Tree-based Representations

• Efficient in space and time
• Multi-resolution

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