Particle Filtering

Sensors and Uncertainty Real world sensors are noisy and suffer from missing data (e.g., occlusions, GPS blackouts) Use sensor models to estimate ground truth, unobserved variables, make forecasts

Hidden Markov Model Use observations to get a better idea of where the robot is at time t X0X0 X1X1 X2X2 X3X3 z1z1 z2z2 z3z3 Hidden state variables Observed variables Predict – observe – predict – observe…

Last Class Kalman Filtering and its extensions Exact Bayesian inference for Gaussian state distributions, process noise, observation noise What about more general distributions? Key representational issue How to represent and perform calculations on probability distributions?

Particle Filtering (aka Sequential Monte Carlo) Represent distributions as a set of particles Applicable to non-gaussian high-D distributions Convenient implementations Widely used in vision, robotics

Simultaneous Localization and Mapping (SLAM) Mobile robots Odometry Locally accurate Drifts significantly over time Vision/ladar/sonar Inaccurate locally Global reference frame Combine the two State: (robot pose, map) Observations: (sensor input)

General problem x t ~ Bel(x t ) (arbitrary p.d.f.) x t+1 = f(x t,u,  p ) z t+1 = g(x t+1,  o )  p ~ arbitrary p.d.f.,  o ~ arbitrary p.d.f. Process noise Observation noise

Particle Representation Bel(x t ) = {(w k,x k ), k=1,…,n} w k are weights, x k are state hypotheses Weights sum to 1 Approximates the underlying distribution

Monte Carlo Integration If P(x) ≈ Bel(x) = {(w k,x k ), k=1,…,N} E P [f(x)] = integral[ f(x)P(x)dx ] ≈  k w k f(x k ) What might you want to compute? Mean: set f(x) = x Variance: f(x) = x 2 (recover Var(x) = E[x 2 ]-E[x] 2 ) P(y): set f(x) = P(y|x) Because P(y) = integral[ P(y|x)P(x)dx ]

Recovering the Distribution Kernel density estimation P(x) =  k w k K(x,x k ) K(x,x k ) is the kernel function Better approximation as # particles, kernel sharpness increases

Filtering Steps Predict Compute Bel’(x t+1 ): distribution of x t+1 using dynamics model alone Update Compute a representation of P(x t+1 |z t+1 ) via likelihood weighting for each particle in Bel’(x t+1 ) Resample to produce Bel(x t+1 ) for next step

Predict Step Given input particles Bel(x t ) Distribution of x t+1 =f(x t,u t,  ) determined by sampling  from its distribution and then propagating individual particles Gives Bel’(x t+1 )

Particle Propagation

Update Step Goal: compute a representation of P(x t+1 | z t+1 ) given Bel’(x t+1 ), z t+1 P(x t+1 | z t+1 ) =  P(z t+1 | x t+1 ) P(x t+1 ) P(x t+1 ) = Bel’(x t+1 ) (given) Each state hypothesis x k  Bel’(x t+1 ) is reweighted by P(z t+1 | x t+1 ) Likelihood weighting: w k  w k P(z t+1 |x t+1 =x k ) Then renormalize to 1

Update Step w k  w k ’ * P(z t+1 | x t+1 =x k ) 1D example: g(x,  o ) = h(x) +  o  o ~ N( ,  ) P(z t+1 | x t+1 =x k ) = C exp(- (h(x)-z t+1 ) 2 / 2  2 ) In general, distribution can be calibrated using experimental data

Resampling Likelihood weighted particles may no longer represent the distribution efficiently Importance resampling: sample new particles proportionally to weight

Sampling Importance Resampling (SIR) variant Predict Update Resample

Particle Filtering Issues Variance Std. dev. of a quantity (e.g., mean) computed as a function of the particle representation ~ 1/sqrt(N) Loss of particle diversity Resampling will likely drop particles with low likelihood They may turn out to be useful hypotheses in the future

Other Resampling Variants Selective resampling Keep weights, only resample when # of “effective particles” < threshold Stratified resampling Reduce variance using quasi-random sampling Optimization Explicitly choose particles to minimize deviance from posterior …

Storing more information with same # of particles Unscented Particle Filter Each particle represents a local gaussian, maintains a local covariance matrix Combination of particle filter + Kalman filter Rao-Blackwellized Particle Filter State (x 1,x 2 ) Particle contains hypothesis of x 1, analytical distribution over x 2 Reduces variance

Recap Bayesian mechanisms for state estimation are well understood Representation challenge Methods: Kalman filters: highly efficient closed-form solution for Gaussian distributions Particle filters: approximate filtering for high-D, non- Gaussian distributions Implementation challenges for different domains (localization, mapping, SLAM, tracking)