Probabilistic Models of Sensing and Movement

1. Probabilistic Models of Sensing and Movement • Move to probability models of sensing and movement • Project 2 is about complex behavior using sensing • Sensor interpretation is difficult – simple interpretation in this section • Artifacts [goal-directed motion] and reactive behaviors • Lectures • Probabilistic sensor models • Probabilistic representation of uncertain movement • Particle filter implementation • Project • PF for motion model • Markov localization with PF • Stretch – feature-based localization Slides thanks to Steffen Gutmann CS225B Kurt Konolige

2. Markov Localization • Bayes filter • Motion: • Sense: • Implementations • Discrete filter • Particle filter • Kalman filter • Multi-Hypotheses tracking (MHT) • … CS225B Kurt Konolige

3. Discrete Filter • Piecewise constant • Histogram • Probability grid CS225B Kurt Konolige

4. Discrete Bayes Filter Algorithm • Algorithm Discrete_Bayes_filter( Bel(x),d ): • h=0 • Ifd is a perceptual data item z then • For all x do • For all x do • Else ifd is an action data item uthen • For all x do • ReturnBel’(x) CS225B Kurt Konolige

5. Piecewise Constant Representation CS225B Kurt Konolige

6. Grid-based Localization CS225B Kurt Konolige

7. Xavier: Localization in a Topological Map [Courtesy of Reid Simmons] CS225B Kurt Konolige

8. Sample-based Localization (Sonar) CS225B Kurt Konolige

9. Particle Filters • Represent belief by random samples + weight state + weight • 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] CS225B Kurt Konolige

10. Robot Motion Sample: CS225B Kurt Konolige

11. Motion Model in Particle Filter Start CS225B Kurt Konolige

12. Sensor Information: Importance Sampling CS225B Kurt Konolige

13. Importance Sampling Weight samples: w = f / g CS225B Kurt Konolige

14. Importance Sampling with Resampling Draw sample i with probability Weighted samples After resampling CS225B Kurt Konolige

15. draw xit-1from Bel(xt-1) draw xitfrom p(xt | xit-1,ut) Importance factor for xit: Particle Filter Algorithm CS225B Kurt Konolige

16. Particle Filter Algorithm • Algorithm particle_filter(Xt-1, ut, zt): • for Generate new samples • Insert into temporary set • endfor • forResample • draw i with probability • Insert into final set • endfor • return Xt CS225B Kurt Konolige

17. Resampling • Given: Set X 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 X’. CS225B Kurt Konolige

18. w1 wn w2 Wn-1 w1 wn w2 Wn-1 w3 w3 Resampling • Stochastic universal sampling • Systematic resampling • Linear time complexity • Easy to implement, low variance • Roulette wheel • Binary search, n log n CS225B Kurt Konolige

19. Resampling Algorithm • Algorithm systematic_resampling(): • Initialize threshold • for • Increment threshold • while Skip until next threshold • endwhile • Insert • endfor • returnXt CS225B Kurt Konolige

20. CS225B Kurt Konolige

21. CS225B Kurt Konolige

22. CS225B Kurt Konolige

23. CS225B Kurt Konolige

24. CS225B Kurt Konolige

25. CS225B Kurt Konolige

26. CS225B Kurt Konolige

27. CS225B Kurt Konolige

28. CS225B Kurt Konolige

29. CS225B Kurt Konolige

30. CS225B Kurt Konolige

31. CS225B Kurt Konolige

32. CS225B Kurt Konolige

33. CS225B Kurt Konolige

34. CS225B Kurt Konolige

35. CS225B Kurt Konolige

36. CS225B Kurt Konolige

37. CS225B Kurt Konolige

38. Berkeley Street Localization [Freuh] CS225B Kurt Konolige

39. Limitations • The approach described so far is able • to globally localize the robot, and • to track the pose of a mobile robot • How can we deal with localization errors such as • the kidnapped robot problem, or • recovery from localization failures? CS225B Kurt Konolige

40. Approaches • Random samples: • During re-sampling, insert a few random samples (the robot can be teleported at any point in time) • Insert random samples inverse proportional to the average likelihood of the particles (the robot has been teleported with higher probability when the likelihood of its observations drops). • Samples drawn from observations: p(x|z) • Inverse proportional to average likelihood of particles (Sensor Resetting Localization) [Lenser, Veloso, 2000] • Constant number but weight each sample by motion model (Mixture MCL) [Thrun et al., 2000] • Proportional to noise in observations estimated over time (Adaptive MCL) [Fox 2002] CS225B Kurt Konolige

41. Particle Filter with Random Samples • Algorithm particle_filter_random(Xt-1, ut, zt): • for Generate new samples • Insert into temporary set • endfor • forResample • with probability pranddo • Insert random pose • else • draw i with probability • Insert sample • endwith • endfor • return Xt CS225B Kurt Konolige

42. Recovery from Failure failure CS225B Kurt Konolige

43. Random SamplesVision-Based Localization 936 Images, invariant features for matching images Trajectory of the robot: CS225B Kurt Konolige

44. Kidnapping the Robot CS225B Kurt Konolige

45. Adaptive Monte Carlo Localization • Algorithm Augmented_MCL(Xt-1, ut, zt): • static wslow, wfast • forGenerate new samples • Insert into temporary set • endfor • forResample • with probability max{0, 1– wfast / wslow} do • Insert random pose • else • draw i with probability • Insert sample • endwith • endfor • return Xt CS225B Kurt Konolige