2. From Bayesian Filtering to Particle Filters Dieter Fox University of Washington Joint work with W. Burgard, F. Dellaert, C. Kwok, S. Thrun

3. Bayes Filters: Framework Given: Stream of observations z and action data u: Sensor model P(z|x). Action model P(x|u,x’). Prior probability of the system state P(x). Wanted: Estimate of the state X of a dynamical system. The posterior of the state is also called Belief:

4. Graphical Model Representation Underlying Assumptions Static world Independent noise Perfect model, no approximation errors

5. Bayes Filters Bayes z = observation u = action x = state Markov Total prob.

6. Bayes Filters are Familiar! Kalman filters (linear, extended, unscented) Particle filters (Rao-Blackwellized) Hidden Markov models Dynamic Bayesian networks Estimator in Partially Observable Markov Decision Processes (POMDPs)

7. Robot Localization Given Map of the environment. Sequence of sensor measurements. Wanted Estimate of the robot’s position. Problem classes Position tracking Global localization Kidnapped robot problem (recovery) “Using sensory information to locate the robot in its environment is the most fundamental problem to providing a mobile robot with autonomous capabilities.” [Cox ’91]

8. Bayes Filters for Robot Localization

9. Representations for Bayesian Robot Localization Discrete approaches (’95) Topological representation (’95) uncertainty handling (POMDPs) occas. global localization, recovery Grid-based, metric representation (’96) global localization, recovery Multi-hypothesis (’00) multiple Kalman filters global localization, recovery Particle filters (’99) sample-based representation global localization, recovery Kalman filters (late-80s) Gaussians approximately linear models position tracking Discrete Continuous

10. Particle Filters

11.  Represent belief 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] Particle Filters

12. Sample-based Representation

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

14. Importance Sampling with Resampling: Landmark Detection Example

15. Distributions

16. Wanted: samples distributed according to p(x| z 1, z 2, z 3 )

17. This is Easy! We can draw samples from p(x|z l ) by adding noise to the detection parameters.

18. Importance Sampling with Resampling

19. Weighted samples After resampling

20. Particle Filters

21. Sensor Information: Importance Sampling

22. Robot Motion

23. Sensor Information: Importance Sampling

24. Robot Motion

25. draw x i t  1 from Bel (x t  1 ) draw x i t from p ( x t | x i t  1,u t  1 ) Importance factor for x i t : Particle Filter Algorithm

26. Resampling Given: Set S of weighted samples. Wanted : Random sample, where the probability of drawing x i is given by w i. Typically done n times with replacement to generate new sample set S’.

27. w2w2 w3w3 w1w1 wnwn W n-1 Resampling w2w2 w3w3 w1w1 wnwn W n-1 Roulette wheel Binary search, n log n Stochastic universal sampling Systematic resampling Linear time complexity Easy to implement, low variance

28. Start Motion Model Example

29. Proximity Sensor Model Example Laser sensor Sonar sensor

30. Sample-based Localization (sonar)

31. Localization for AIBO robots

32. How Many Samples?

33. 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-  ), the KL-distance between the true posterior and the sample-based approximation is less than . Observation: For fixed  and , number of samples only depends on number k of bins with support: KLD-sampling [Fox NIPS-01, Fox IJRR-03]

34. Evaluation

35. KLD-Sampling: Sonar [Fox NIPS-01, Fox IJRR-03]

36. KLD-Sampling: Laser

37. Practical Considerations Dealing with highly peaked observations Add noise to observation model Better proposal distributions: e.g., perform Kalman filter step to determine proposal Recover from failure by selectively adding samples from observations Overestimating noise often reduces number of required samples Resample only when necessary (efficiency of representation measured by variance of weights) Try to avoid sampling whenever possible!

38. Rao-Blackwellised Particle Filter Example

39. Ball Tracking in RoboCup  Extremely noisy (nonlinear) motion of observer  Inaccurate sensing, limited processing power  Interactions between target and environment  Interactions between robot(s) and target Goal: Unified framework for modeling the ball and its interactions.

40. Tracking Techniques  Kalman Filter  Highly efficient, robust (even for nonlinear)  Uni-modal, limited handling of nonlinearities  Particle Filter  Less efficient, highly robust  Multi-modal, nonlinear, non-Gaussian  Rao-Blackwellised Particle Filter, MHT  Combines PF with KF  Multi-modal, highly efficient

41. Dynamic Bayesian Network for Ball Tracking k-2 b k-1 b k b r k-2 r k-1 r k z k-2 z k-1 z k u k-2 u k-1 z z k z k-2 k m k-1 m k-2 m Ball observation Ball location and velocity Ball motion mode Map and robot location Robot control Landmark detection Ball tracking Robot localization l l l b b b

42. Robot Location k-2 b k-1 b k b r k-2 r k-1 r k z k-2 z k-1 z k u k-2 u k-1 z z k z k-2 k m k-1 m k-2 m Ball observation Ball location and velocity Ball motion mode Map and robot location Robot control Landmark detection Ball tracking Robot localization l l l b b b

43. Robot and Ball Location (and velocity) k-2 b k-1 b k b r k-2 r k-1 r k z k-2 z k-1 z k u k-2 u k-1 z z k z k-2 k m k-1 m k-2 m Ball observation Ball location and velocity Ball motion mode Map and robot location Robot control Landmark detection Ball tracking Robot localization l l l b b b

44. Ball-Environment Interactions NoneGrabbed Bounced Kicked (0.8) Robot loses grab (0.2) ( 1. 0 ) R o b o t k i c k s b a l l ( 0. 9 ) ( 1. 0 ) Within grab range and robot grabs (prob.from model) ( 0. 1 ) Kick fails(0.1) Deflected (residual prob.) ( 1. 0 ) C o l l i s i o n w i t h o b j e c t s o n m a p ( 1. 0 )

45. Integrating Discrete Ball Motion Mode k-2 b k-1 b k b r k-2 r k-1 r k z k-2 z k-1 z k u k-2 u k-1 z z k z k-2 k m k-1 m k-2 m Ball observation Ball location and velocity Ball motion mode Map and robot location Robot control Landmark detection Ball tracking Robot localization l l l b b b

46. Grab Example (1) k-2 b k-1 b k b r k-2 r k-1 r k z k-2 z k-1 z k u k-2 u k-1 z z k z k-2 k m k-1 m k-2 m Ball observation Ball location and velocity Ball motion mode Map and robot location Robot control Landmark detection Ball tracking Robot localization l l l b b b

47. Grab Example (2) k-2 b k-1 b k b r k-2 r k-1 r k z k-2 z k-1 z k u k-2 u k-1 z z k z k-2 k m k-1 m k-2 m Ball observation Ball location and velocity Ball motion mode Map and robot location Robot control Landmark detection Ball tracking Robot localization l l l b b b

48. Inference: Posterior Estimation k-2 b k-1 b k b r k-2 r k-1 r k z k-2 z k-1 z k u k-2 u k-1 z z k z k-2 k m k-1 m k-2 m Ball observation Ball location and velocity Ball motion mode Map and robot location Robot control Landmark detection Ball tracking Robot localization l l l b b b

49. Particle Filter for Robot Localization

50. Rao-Blackwellised PF for Inference  Factorize state space into different components  Represent some part(s) by samples and remaining part analytically, conditioned on samples  Each sample contains robot location, ball mode, ball location

51. Rao-Blackwellised Particle Filter for Inference k-1 b k b r r k k m m Ball location and velocity Ball motion mode Map and robot location Ball tracking Robot localization  Draw a sample from the previous sample set:

52. Generate Robot Location r k-1 r k u z k Map and robot location Robot control Landmark detection Robot localization l k-1 b m Ball location and velocity Ball motion mode Ball tracking k b k m

53. Generate Ball Motion Model k-1 b r r k u z k k m m Ball location and velocity Ball motion mode Map and robot location Robot control Landmark detection Ball tracking Robot localization l k b

54. Update Ball Location and Velocity k-1 b r r k u z k k m m Ball location and velocity Ball motion mode Map and robot location Robot control Landmark detection Ball tracking Robot localization l k b z k b

55. Importance Resampling  Weight sample by if observation is landmark detection and by if observation is ball detection.  Resample

56. Ball-Environment Interaction

58. Tracking and Finding the Ball  Cluster ball samples by discretizing pan / tilt angles  Uses negative information

59. Experiment: Real Robot  Robot kicks ball 100 times, tries to find it afterwards  Finds ball in 1.5 seconds on average

60. Error vs. Prediction Time

61. Reference * Observations Reference * Observations Simulation Runs

62. Comparison to KF* (optimized for straight motion) RBPF KF* Reference * Observations RBPF KF* Reference * Observations

63. RBPF KF’ Reference * Observations RBPF KF’ Reference * Observations Comparison to KF ’ (inflated prediction noise)

64. Orientation Errors 0 20 40 60 80 100 120 140 160 180 234567891011 Orientation Error [degrees] Time [sec] RBPF KF*

65. Discussion  Particle filters are intuitive and “ simple ”  Support point-wise thinking (reduced uncertainty)  It ’ s an art to make them work  Good for test implementation if system behavior is not well known  Inefficient compared to Kalman filters  Rao-Blackwellization for complex problems  Only sample discrete / highly non-linear parts of state space  Solve remaining part analytically (KF,discrete)