Bayesian Methods - Department of Computer Sciencesleonard/week09.pdf · 600.436/600.636 G.D. Hager S. Leonard Bayes Filter • Given a sequence of measurements z1, …, z k • Given

600.436/600.636 G.D. Hager S. Leonard

Bayesian Methods


Recall Robot Localization

• Given

– Sensor readings z1, z2, …, zt = z1:t

– Known control inputs u0, u1, … ut = u0:t

– Known model P(xt+1 | xt, ut) with initial P(x1 | u0)

– Known map P(zt | xt)

• Compute

– P(xt | z1:t-1, u0:t-1)

This is just a probabilistic representation of what you’ve already learned!

Let’s try to connect the dots and do a couple of examples

Most likely sensor reading given state x

Most likely state x at time t given a sequence of commands u0:t-1 and measurements z1:t-1


A Simple

Example


Why Not Just Use a Kalman Filter?


Bayes Filter

• Given a sequence of measurements z1, …, zk

• Given a sequence of commands u0, …, uk-1

• Given a sensor model P(zk | xk)

• Given a dynamic model P(xk | xk-1, uk-1 )

• Given a prior probability P(x0)

• Find P(xk | z1:k, u0:k-1 )

x0 xk-2 xk-1 xk

zk-2 zk-1 zk

u0 uk-2 uk-1


Bayesian Filtering

• Recall Bayes Theorem:

𝑃 𝑥 𝑧) =𝑃 𝑧 𝑥 𝑃(𝑥)

𝑃(𝑧)

• Also remember conditional independence

• Think of x as the state of the robot and z as the data we know

𝑃(𝒙𝑘|𝒖0:𝑘−1, 𝒛1:𝑘) Posterior Probability Distribution

𝑃 𝒙𝑘 𝒖0:𝑘−1, 𝒛1:𝑘 =𝑃 𝒛𝑘 𝒙𝑘, 𝒖0:𝑘−1, 𝒛1:𝑘−1 𝑃(𝒙𝑘|𝒖0:𝑘−1, 𝒛1:𝑘−1)

𝑃(𝒛𝑘|𝒖0:𝑘−1, 𝒛1:𝑘−1)

= 𝜂𝑘𝑃(𝒛𝑘|𝒙𝑘) 𝑃 𝒙𝑘 𝒖𝑘−1, 𝒙𝑘−1 𝑃(𝒙𝑘−1|𝒖0:𝑘−2, 𝒛1:𝑘−1)

𝒙𝑘−1

observation recursive instance state prediction


• What is the probability that a given state xk generates the measurement zk?

𝑃(𝒛𝑘|𝒙𝑘)

Same zk

Different xk

This is more likely

than that

but how do we measure?

Observation Model


Observation Model

• Compute the distance of end-point of each beam to nearest obstacle


Observation Model • How likely is the

measurement zk given a state xk?

• The measurement zk is what we get. So we have to stick to it

• However, not all states will likely produce that measurement

• We’re not interested in finding the most likely state. We want the whole distribution

X

zk

xk X

𝑃(𝑧𝑘|𝑥𝑘)




• The measurement zk is what we get. So we have to stick by it

• However, not all states will likely produce the measurement


X

X


zk

xk







X

X


zk

xk







X

X


zk

xk







X

X


zk

xk







X

X


zk

xk

Doesn’t have to be Gaussian


Observation Model Beam Model

zk

d

X xk


d

d

d

d

d

Observation Model Beam Model

• Combine all the distances di between the measured obstacle and the real obstacle (we do have the map, or part thereof)

• But other things must be considered

d5

d2

d3 d4

d1

d6

d7

d d


Range Finder Observation Model Beam Model

• For each laser beam we can have (given a map and a state) – Correct range with noise

𝑃hit 𝑧𝒌 𝒙𝑘 , 𝑚 = 𝜂hit𝑁 𝑧𝑘

∗ , 𝜎ℎ𝑖𝑡 if 0 ≤ 𝑧𝑘 ≤ 𝑧max

0 otherwise

– Unexpected object

𝑃short 𝑧𝒌 𝒙𝑘 , 𝑚 = 𝜂short𝜆𝑒

−𝜆𝑧𝑘 if 0 ≤ 𝑧𝑘 ≤ 𝑧𝑘∗

0 otherwise

– Failures

𝑃max 𝑧𝒌 𝒙𝑘 , 𝑚 = 𝐼 𝑧𝑘 = 𝑧max 1 if 𝑧𝑘 = 𝑧max0 otherwise

– Random measurement

𝑃rand 𝑧𝒌 𝒙𝑘 , 𝑚 = 1

𝑧max 𝑖𝑓 0 ≤ 𝑧𝑘 ≤ 𝑧max

0 otherwise

True range Sensor noise

d

𝑧max

𝑃(𝑧𝑘|𝑥𝑘 , 𝑚)



Probabilistic Robotic


Constants

𝜂hit = 𝑁(𝑧𝑘∗ , 𝜎hit)𝑑𝑧𝑘

𝑧max

0

−1

𝜂short =1

1 − 𝑒−𝜆short𝑧𝑘∗



• Mixing all these cases together we get

𝑃 𝑧𝑘 𝒙𝑘 , 𝑚 =

𝑤hit𝑤short𝑤max𝑤rand

𝑇

𝑃hit(𝑧𝑘|𝒙𝑘, 𝑚)

𝑃short(𝑧𝑘|𝒙𝑘 , 𝑚)

𝑃max(𝑧𝑘|𝒙𝑘, 𝑚)𝑃rand(𝑧𝑘|𝒙𝑘, 𝑚)

where the weights are parameters 𝑤hit + 𝑤short + 𝑤max + 𝑤rand = 1

Probabilistic Robotic


Bayes Filtering

𝑃 𝒙𝑘 𝒖0:𝑘−1, 𝒛1:𝑘

= 𝜂𝑘𝑃(𝒛𝑘|𝒙𝑘) 𝑃 𝒙𝑘 𝒖𝑘−1, 𝒙𝑘−1 𝑃 𝒙𝑘−1 𝒖0:𝑘−2, 𝒛1:𝑘−1𝒙𝑘−1


Predict Motion (Prior Distribution)

• Suppose we have 𝑃 𝒙𝑘

• We have 𝑃(𝒙𝑘+1|𝒙𝑘 , 𝒖𝑘)

• Put together

𝑃 𝒙𝑘+1 = 𝑃(𝒙𝑘+1|𝒙𝑘 , 𝒖𝑘𝒙𝑘

)𝑃(𝒙𝑘)𝑑𝒙𝑘

What is the probability distribution for xk+1 given the command uk and all the previous states xk?


System Model

𝑥 ∈ 𝐴, 𝐵, … , 𝑍

𝑥𝑡+1 = 𝐴 𝑥𝑡 = ⋯

𝑥𝑡 = 𝐴

𝑥𝑡 = 𝑍

𝑢𝑡

𝑃 𝑥𝑡+1 = 𝐴 𝑥𝑡 = 𝐴, 𝑢𝑡)

𝑃 𝑥𝑡+1 = 𝐴 𝑥𝑡 = 𝐵, 𝑢𝑡)

𝑃 𝑥𝑡+1 = 𝐴 𝑥𝑡 = 𝑍, 𝑢𝑡)

…


System Model

𝑥 ∈ 𝐴, 𝐵, … , 𝑍

𝑥𝑡+1 = 𝐵 𝑥𝑡 = ⋯

𝑥𝑡 = 𝐴

𝑥𝑡 = 𝑍

𝑢𝑡

𝑃 𝑥𝑡+1 = 𝐵 𝑥𝑡 = 𝐴, 𝑢𝑡)

𝑃 𝑥𝑡+1 = 𝐵 𝑥𝑡 = 𝐵, 𝑢𝑡)

𝑃 𝑥𝑡+1 = 𝐵 𝑥𝑡 = 𝑍, 𝑢𝑡)

…


System Model

State space X = { 1, 2, 3, 4}

P(xk+1 | xk, uk) Xk+1=1 Xk+1=2 Xk+1=3 Xk+1=4

Xk=1 0.25 0.5 0.25 0

Xk=2 0 0.25 0.5 0.25

Xk=3 0 0 0.25 0.75

Xk=4 0 0 0 1 xk

P(xk)

1 2 3 4

0.5

Prior probability distribution 𝑃(𝑥𝑘)

Compute 𝑃 𝑥𝑘+1 = 𝑃(𝑥𝑘+1|𝑥𝑘 , 𝑢𝑘𝑥𝑘)𝑃(𝑥𝑘)𝑑𝑥𝑘

Transition matrix: The probability P(j|i) of moving from i to j is given by Pi,j. Each row must sum to 1.


System Model State space X = { 1, 2, 3, 4}


Xk=1 0.25 0.5 0.25 0

Xk=2 0 0.25 0.5 0.25

Xk=3 0 0 0.25 0.75

Xk=4 0 0 0 1

0

5.00005.00025.0

)4(),4|1(

)3(),3|1(

)2(),2|1(

)1(),1|1(

)(),|1()1(

1

1

1

1

11

kkkk

kkkk

kkkk

kkkk

Xx

kkkkk

xPuxxP

xPuxxP

xPuxxP

xPuxxP

xPuxxPxP

k

xk

P(xk)

1 2 3 4

0.5

Prior 𝑃(𝑥𝑘)


State space X = { 1, 2, 3, 4}


Xk=1 0.25 0.5 0.25 0

Xk=2 0 0.25 0.5 0.25

Xk=3 0 0 0.25 0.75

Xk=4 0 0 0 1

125.0

5.00005.025.005.0

)4(),4|2(

)3(),3|2(

)2(),2|2(

)1(),1|2(

)(),|2()2(

1

1

1

1

11

kkkk

kkkk

kkkk

kkkk

Xx

kkkkk

xPuxxP

xPuxxP

xPuxxP

xPuxxP

xPuxxPxP

k

System Model

xk

P(xk)

1 2 3 4

0.5





Xk=1 0.25 0.5 0.25 0

Xk=2 0 0.25 0.5 0.25

Xk=3 0 0 0.25 0.75

Xk=4 0 0 0 1

25.0

5.00025.05.05.0025.0

)4(),4|3(

)3(),3|3(

)2(),2|3(

)1(),1|3(

)(),|3()3(

1

1

1

1

11

kkkk

kkkk

kkkk

kkkk

Xx

kkkkk

xPuxxP

xPuxxP

xPuxxP

xPuxxP

xPuxxPxP

k

xk

P(xk)

1 2 3 4

0.5





Xk=1 0.25 0.5 0.25 0

Xk=2 0 0.25 0.5 0.25

Xk=3 0 0 0.25 0.75

Xk=4 0 0 0 1

625.0

5.01075.05.025.000

)4(),4|4(

)3(),3|4(

)2(),2|4(

)1(),1|4(

)(),|4()4(

1

1

1

1

11

kkkk

kkkk

kkkk

kkkk

Xx

kkkkk

xPuxxP

xPuxxP

xPuxxP

xPuxxP

xPuxxPxP

k

xk

P(xk)

1 2 3 4

0.5



System Model

kkkkkkdxxPuxxPxP )(),|()(

11

Xk+1

P(xk+1)

1 2 3 4

0.625

0.25 0.125


Bayes Filter Recap

𝑃(𝒙𝑘−1 = A)

𝑃(𝒙𝑘−1 = B)

𝑃(𝒙𝑘−1 = Z)

…

𝑃(𝒙𝑘 = A |

𝒙𝑘−1 𝒖𝑘−1)

𝑃(𝒙𝑘 = B|


𝑃(𝒙𝑘 = Z |


Apply command 𝒖𝑘−1

…

Obtain z k and apply 𝑃(𝒛𝑘|𝒙𝑘) 𝑃(𝒙𝑘 = A |

𝒖𝑘−1 𝒛𝑘)

𝑃(𝒙𝑘 = B |


𝑃(𝒙𝑘 = Z |


…

𝑃 𝒙𝑘 𝒖0:𝑘−1, 𝒛1:𝑘 𝑃 𝒙𝑘−1 𝒖0:𝑘−2, 𝒛1:𝑘−1 𝑃 𝒙𝑘 𝒖𝑘−1, 𝒙𝑘−1


Bayes Filter Recap

• Given a measurement 𝒛𝑘, compute the probability that 𝒛𝑘 was generated from the robot in state 𝒙𝑘

• Prior distribution (recursive) – The probability of all previous

states

• State transition: – Given a prior distribution over

all states 𝒙𝑘−1 and an action 𝒖𝑘−1, compute the probability of the robot transitioning to state 𝒙𝑘

𝑃 𝒙𝑘 𝒖0:𝑘−1, 𝒛1:𝑘 = 𝜂𝑘𝑃(𝒛𝑘|𝒙𝑘) 𝑃 𝒙𝑘 𝒖𝑘−1, 𝒙𝑘−1 𝑃 𝒙𝑘−1 𝒖0:𝑘−2, 𝒛1:𝑘−1 𝑑𝒙𝑘−1𝒙𝑘−1


Discrete Bayes Filter Algorithm Algorithm Discrete_Bayes_filter( u0:k-1, z1:k, P(x0) )

1. P(x) = P(x0) (if you don’t know: uniform distribution)

2. for i=1:k

3. for all states x X

4.

5. end for

6. h=0 Normalization constant


8.

9. h = h + P(x)

10. end for


12. P(x) = P(x) / h

13. end for

14. end for

Xx

xPxuxPxP )(),|()(

)()|()( xPxzPxP

Prediction given prior dist. and command

Update using measurement

Normalize to 1


• 𝑃 𝒙𝑘 𝒖0:𝑘−1, 𝒛1:𝑘 = 𝜂𝑘𝑃(𝒛𝑘|𝒙𝑘) 𝑃 𝒙𝑘 𝒖𝑘−1, 𝒙𝑘−1 𝑃 𝒙𝑘−1 𝒖0:𝑘−2, 𝒛1:𝑘−1 𝑑𝒙𝑘−1𝒙𝑘−1

• It is a probability distribution

• What do we do with it?

– Maximum likelihood:

– Mean Squared Error:

Note About the Posterior Distribution

x

P(x)

]))ˆ()([(2

xPxPE

),|(maxarg mzxPkk

x


Kalman vs Bayes

Kalman Filter Bayes Filter

Isard 1998


Bayes Filter Recap

𝑃(𝒙𝑘−1 = A)

𝑃(𝒙𝑘−1 = B)

𝑃(𝒙𝑘−1 = Z)

…

𝑃(𝒙𝑘 = A |


𝑃(𝒙𝑘 = B|


𝑃(𝒙𝑘 = Z |


Apply command 𝒖𝑘−1

…

Obtain z k and apply 𝑃(𝒛𝑘|𝒙𝑘) 𝑃(𝒙𝑘 = A |


𝑃(𝒙𝑘 = B |


𝑃(𝒙𝑘 = Z |


…



Piecewise Constant Representation

• Updating from observations is a simple multiplication of prior probability by likelihood of observation

• Updating using dynamical model is simply a discrete convolution (blurring) of the prior by the driving noise of the planned motion


Piecewise Constant Representation (Mobile Robot)

),,( >=< qyxxBel t

Position of a mobile robot: (x, y, q)



Particle Filter

• Computing 𝑃(𝒛𝑘|𝒙𝑘 , 𝑚) and 𝑃(𝒙𝑘+1|𝒙𝑘 , 𝒖𝑘) is not easy

• In practice, it is never directly computable

• Need to propagate an entire conditional distribution, not just one

state like we did with Kalman,

• Represent probability distribution by random samples

• Estimation of non-Gaussian, nonlinear processes

• Monte Carlo filter, Survival of the fittest, Condensation, Bootstrap filter,

Particle filter

• Filtering: [Rubin, 88], [Gordon et al., 93], [Kitagawa 96]

• Computer vision: [Isard and Blake 96, 98]

• Dynamic Bayesian Networks: [Kanazawa et al., 95]


Particles/Samples

• Given a distribution 𝑃 𝒙 , randomly sample the distribution N times

• Assign a weight w to each sample according to the probability

• Ensure that 𝑤𝑖 = 1𝑁𝑖=1

Isard 1998

600.436/600.636 G.D. Hager

S. Leonard

600.436/600.636 G.D. Hager

S. Leonard

Odometry Motion Model

Odometry measures the

distance and rotation travelled

by the robot during a time

interval (this is a sensor-based

method).

This isn’t a prediction anymore

(command are not used in this

model). Instead, the model uses

measurements to predict where

the robot might be.

Odometry provides relative

motion: 𝒖𝑡 = ∆𝑥 ∆𝑦 ∆𝜃 𝑇.

Probabilistic Robotics

(d)

(a)

(b)

Dx

Dy

st

st-1

600.436/600.636 G.D. Hager

S. Leonard

Odometry Motion Model

• Sampling from P(st | ut, st-1)

1. 𝛼 = atan2 ∆𝑦, ∆𝑥 − 𝜃𝑡−1

2. 𝛽 = ∆𝜃 − 𝛼

3. 𝑑 = ∆𝑥2 + ∆𝑦2

4. 𝛼′ = 𝛼 − 𝑁(0, 𝛼1𝛼2 + 𝛼2𝑑

2)

5. 𝛽′ = 𝛽 − 𝑁(0, 𝛼1𝛽2 + 𝛼2𝑑

2)

6. 𝑑′ = 𝑑 − 𝑁 0, 𝛼3𝑑2 + 𝛼4𝛼

2 + 𝛼4𝛽2

7. 𝑥𝑡 = 𝑥𝑡−1 + 𝑑′cos (𝜃𝑡−1 + 𝛼′)

8. 𝑦𝑡 = 𝑦𝑡−1 + 𝑑′ sin 𝜃𝑡−1 + 𝛼′

9. 𝜃𝑡 = 𝜃𝑡−1 + 𝛼′ + 𝛽′

Probabilistic Robotics

(d)

(a)

(b)

Dx

Dy

st

st-1

600.436/600.636 G.D. Hager

S. Leonard

Start

Motion Model

600.436/600.636 G.D. Hager

S. Leonard

Particle Filter Algorithm Algorithm Particle_filter( u0:k-1, z1:k, set of N samples M = {xj, wj} )

1. for i=1:k

2. for j=1:N

3. compute a new state x by sampling according to P( x | ui-1, xj )

4. xj = x

5. end

6. h=0

7. for j=1:N

8. wj = P( zi | xj )

9. h += wj

10. end

11. for j=1:N

12. wj = wj / h

13. end

14. resample (M ) according to weights wj

15. end

…

Apply command 𝒖𝑘−1 with noise

…

Obtain z k and apply 𝑃(𝒛𝑘|𝒙𝑘)

…


600.436/600.636 G.D. Hager

S. Leonard

w2

w3

w1 wn

Wn-1

Resampling

w2

w3

w1 wn

Wn-1

• Roulette wheel

• Binary search, log n

• Stochastic universal sampling

• Systematic resampling

• Linear time complexity

• Easy to implement, low variance


Sample-based Localization (sonar)

rse-lab.cs.washington.edu


Burgard

Tour of the Smithsonian National Museum of American History


Using Ceiling Maps for Localization

[Dellaert et al. 99]


P(z|x)

Vision-Based Localization


Measurement z: P(z|x):

Under a Light



Next to a Light



Elsewhere

Bayesian Methods - Department of Computer Sciencesleonard/week09.pdf · 600.436/600.636 G.D. Hager S. Leonard Bayes Filter • Given a sequence of measurements z1, …, z k • Given

Documents