Bayesian Methods / G.D. Hager S. Leonard

Transcription

1 Bayesian Methods

2 Recall Robot Localization Given Sensor readings z 1, z 2,, z t = z 1:t Known control inputs u 0, u 1, u t = u 0:t Known model t+1 t, u t ) with initial 1 u 0 ) Known map z t t ) Compute t z 1:t-1, u 0:t-1 ) Most liely sensor reading given state Most liely state at time t given a sequence of commands u 0:t-1 and measurements z 1: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 eamples

3 A Simple Eample

4 Why Not Just Use a Kalman Filter?

5 Bayes Filter Given a sequence of measurements z 1,, z Given a sequence of commands u 0,, u -1 Given a sensor model z ) Given a dynamic model -1 ) Given a prior probability 0 ) Find z 1:, u 0:-1 ) u 0 u -2 u z -2 z -1 z

6 Recall Bayes Theorem: Bayesian Filtering P z) = P z ) z) Also remember conditional independence Thin of as the state of the robot and z as the data we now u 0: 1, z 1: ) Posterior Probability Distribution P u 0: 1, z 1: = P z, u 0: 1, z 1: 1 u 0: 1, z 1: 1 ) z u 0: 1, z 1: 1 ) = η z ) 1 P u 1, 1 1 u 0: 2, z 1: 1 ) observation state prediction recursive instance

7 Observation Model What is the probability that a given state generates the measurement z? z ) Same z Different This is more liely than that but how do we measure?

8 Observation Model Compute the distance of end-point of each beam to nearest obstacle

9 How liely is the measurement z given a state? The measurement z is what we get. So we have to stic by it However, not all states will liely produce the measurement We re not interested in finding the most liely state. We want the whole distribution Observation Model z ) z X X

10 How liely is the measurement z given a state? The measurement z is what we get. So we have to stic by it However, not all states will liely produce the measurement We re not interested in finding the most liely state. We want the whole distribution Observation Model z ) z X X

11 How liely is the measurement z given a state? The measurement z is what we get. So we have to stic by it However, not all states will liely produce the measurement We re not interested in finding the most liely state. We want the whole distribution Observation Model z ) z X X

12 How liely is the measurement z given a state? The measurement z is what we get. So we have to stic by it However, not all states will liely produce the measurement We re not interested in finding the most liely state. We want the whole distribution Observation Model z ) z X X

13 How liely is the measurement z given a state? The measurement z is what we get. So we have to stic by it However, not all states will liely produce the measurement We re not interested in finding the most liely state. We want the whole distribution Observation Model z ) z X X

14 How liely is the measurement z given a state? The measurement z is what we get. So we have to stic by it However, not all states will liely produce the measurement We re not interested in finding the most liely state. We want the whole distribution Observation Model z ) z Doesn t have to be Gaussian X X

15 Observation Model Beam Model d z X

16 Observation Model Combine all the distances d i between the measured obstacle and the real obstacle (we do have the map, or part thereof) But other things must be considered Beam Model d 1 d d d 2 d 3 d 4 d d 5 d d d 6 d 7 d d

17 Range Finder Observation Model Beam Model For each laser beam we can have (given a map and a state) Correct range with noise P hit z, m = η hit N z, σ hit if 0 z z ma 0 otherwise Unepected object Failures P short z, m = η short λe λz if 0 z z 0 otherwise Pma z, m = I z = zma 1 if z = zma 0 otherwise Random measurement P rand z, m = True range 1 zma if 0 z 0 otherwise Sensor noise zma

18 Range Finder Observation Model Beam Model Probabilistic Robotic

19 Range Finder Observation Model Beam Model Miing all these cases together we get P z, m = w hit w short wma w rand T P hit (z, m) P short (z, m) Pma(z, m) P rand (z, m) where the weights are parameters w hit + w short + wma + w rand = 1 Probabilistic Robotic

20 Constants η hit = 0 η short = zma N(z, σ 2 )dz 1 1 e λ shortz 1

21 Range Finder Lielihood Field Model P hit z, m = ε 2 σhit (d 2 ) Probabilistic Robotic

22 Range Finder Lielihood Field Model

23 Range Finder Lielihood Field Model Precompute the shortest distance to obstacle Distance transformation Brushfire algorithm

24 Bayes Filtering P u 0: 1, z 1: = η z ) 1 P u 1, 1 P 1 u 0: 2, z 1: 1

25 Predict Motion (Prior Distribution) Suppose we have P We have +1, u ) Put together P +1 = +1, u ) )d What is the probability distribution for +1 given the command u and all the previous states?

26 System Model ) 0.5 Prior probability distribution ) State space X = { 1, 2, 3, 4} +1, u ) X +1 =1 X +1 =2 X +1 =3 X +1 =4 X = X = X = X = Transition matri: The probability j i) of moving from i to j is given by P i,j. Each row must sum to 1. Compute P +1 = +1, u ) )d

27 State space X = { 1, 2, 3, 4} System Model +1, u ) X +1 =1 X +1 =2 X +1 =3 X +1 =4 X = X = X = X = ) 0.5 Prior ) 1 1) X , u 2, u 3, u 4, u, u ) ) ) ) ) ) 1) 2) 3) 4)

28 State space X = { 1, 2, 3, 4} System Model +1, u ) X +1 =1 X +1 =2 X +1 =3 X +1 =4 X = X = X = X = ) 0.5 Prior ) 1 2) X , u 2, u 3, u 4, u, u ) ) ) ) ) 1) 2) 3) 4) )

29 State space X = { 1, 2, 3, 4} System Model +1, u ) X +1 =1 X +1 =2 X +1 =3 X +1 =4 X = X = X = X = ) 0.5 Prior ) 1 3) X , u 2, u 3, u 4, u, u ) ) ) ) ) 1) 2) 3) 4) )

30 State space X = { 1, 2, 3, 4} System Model +1, u ) X +1 =1 X +1 =2 X +1 =3 X +1 =4 X = X = X = X = ) 0.5 Prior ) 1 4) X , u 2, u 3, u 4, u, u ) ) ) ) ) 1) 2) 3) 4) )

31 System Model P ) ( 1) 1, u ) d +1 ) X +1

32 P u 0: 1, z 1: Bayes Filter Recap P u 1, 1 P 1 u 0: 2, z 1: 1 = A u 1 z ) Obtain z and apply z ) = A 1 u 1 ) Apply command u 1 1 = A) = B u 1 z ) = B 1 u 1 ) 1 = B) = Z u 1 z ) = Z 1 u 1 ) 1 = Z)

33 Bayes Filter Recap P u 0: 1, z 1: = η z ) 1 P u 1, 1 P 1 u 0: 2, z 1: 1 d 1 Given a measurement z, compute the probability that z 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 u 1, compute the probability of the robot transitioning to state

34 Discrete Bayes Filter Algorithm Algorithm Discrete_Bayes_filter( u 0:-1, y 1:, 0 ) ) 1. ) = 0 ) (if you don t now: uniform distribution) 2. for i=1: 3. for all states X 4. P ( ) u, ) ) 5. end for X 6. h=0 Normalization constant 7. for all states X h = h + ) 10. end for 11. for all states X 12. ) = ) / h 13. end for 14. end for ) z ) ) Prediction given prior dist. and command Update using measurement Normalize to 1

35 Note About the Posterior Distribution P u 0: 1, z 1: = η z ) 1 P u 1, 1 P 1 u 0: 2, z 1: 1 d 1 It is a probability distribution ) What do we do with it? Maimum lielihood: arg ma z, m) Mean Squared Error: E[( ) ˆ)) 2 ]

36 Kalman vs Bayes Kalman Filter Bayes Filter Isard 1998

37 Particle Filter Computing z, m) and +1, u ) is not easy In practice, it is never directly computable Need to propagate an entire conditional distribution, not just one state lie 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 Blae 96, 98] Dynamic Bayesian Networs: [Kanazawa et al., 95]

38 Particles/Samples Given a distribution P, randomly sample the distribution N times Assign a weight w to each sample according to the probability Ensure that i=1 N w i = 1 Isard 1998

39 Particle Filter

40 Particle Filter Algorithm Algorithm Particle_filter( u 0:-1, y 1:, 0 ), set of N samples M = { j, w j } ) 1. for i=1: 2. for j=1:n 3. compute a new state by sampling according to u i-1, j ) 4. j = 5. end 6. h=0 7. for j=1:n 8. w j = z i j ) 9. h += w j 10. end 11. for j=1:n 12. w j = w j / h 13. end 14. resample (M ) according to weights w j 15. end

41 Sample-based Localization (sonar) rse-lab.cs.washington.edu

42 Particles/Samples The more particles M, the better is the approimation Curse of dimensionality Independent of time sup E C M Eponential in the dimension of the state space Bound on epected error Particle filter wors well for low dimensional problems Localization dimension: 3 SLAM dimension: 3 + 2N (N can be large!)

43 Tour of the Smithsonian National Museum of American History Burgard

44 Using Ceiling Maps for Localization [Dellaert et al. 99]

45 Vision-Based Localization z )

46 Measurement z: Under a Light z ):

47 Measurement z: Net to a Light z ):

48 Measurement z: z ): Elsewhere