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

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:

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

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

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)

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]

Bayes Filters for Robot Localization

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

Particle Filters

 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

Sample-based Representation

Weight samples: w = f / g Importance Sampling

Importance Sampling with Resampling: Landmark Detection Example

Distributions

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

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

Importance Sampling with Resampling

Weighted samples After resampling

Particle Filters

Sensor Information: Importance Sampling

Robot Motion

Sensor Information: Importance Sampling

Robot Motion

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

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

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

Start Motion Model Example

Proximity Sensor Model Example Laser sensor Sonar sensor

Sample-based Localization (sonar)

Localization for AIBO robots

How Many Samples?

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]

Evaluation

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

KLD-Sampling: Laser

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!

Rao-Blackwellised Particle Filter Example

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.

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

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

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

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

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 )

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

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

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

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

Particle Filter for Robot Localization

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

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:

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

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

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

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

Ball-Environment Interaction

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

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

Error vs. Prediction Time

Reference * Observations Reference * Observations Simulation Runs

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

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

Orientation Errors Orientation Error [degrees] Time [sec] RBPF KF*

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)