Particle Filters: Theory and applications to mobile robot localization and map building Jose Luis Blanco Claraco August 2005 System Engineering and Automation Dpt. (I.S.A.) University of Málaga (Spain)

Aug, 2005Particle Filters: Theory and applications to mobile robotsJose Luis Blanco Table of contents 1.Introduction 2.Particle filters 3.Applications to localization and map building 4.Preliminary experiments

Aug, 2005Particle Filters: Theory and applications to mobile robotsJose Luis Blanco 1. Introduction 1.1. Tracking in real-world conditions 1.2. The Bayes Rule 1.3. Bayesian Tracking 1.4. Solutions to Bayesian Tracking

Aug, 2005Particle Filters: Theory and applications to mobile robotsJose Luis Blanco Estimation x0x0 Ground truth Time 1.1. Introduction: Tracking in real-world conditions Tracking in real-world conditions Tracking is the problem of estimating some state (vector) variable using the available information, which are measurements or observations. x1x1 x2x2 x3x3 x0x0 Observation Action x1x1 x2x2 x3x3 Observation Action Observation Action

Aug, 2005Particle Filters: Theory and applications to mobile robotsJose Luis Blanco 1.1. Introduction: Tracking in real-world conditions Tracking in real-world conditions It should be enough to take into account just the actions... isn’t it? Lets think in an experiment: a blind moving vehicle. We consider the vehicle state to be just the position along an X axis. Initially, I know where I am Action: To move forward 5 meters Actions are noisy But actions in real- world are not perfect: actual state will never be the predicted one Here comes into play the observations: in ideal conditions, they will solve our uncertainty.

Aug, 2005Particle Filters: Theory and applications to mobile robotsJose Luis Blanco 1.1. Introduction: Tracking in real-world conditions Tracking in real-world conditions It should be enough to take into account just the actions... isn’t it? Lets think in an experiment: a blind moving vehicle. We consider the vehicle state to be just the position along an X axis. Actions are noisy Observations are noisy But they are noisy too!! So the best they can do is to reduce the uncertainty. Is there an optimal way to estimate this? Yes! Bayesian Tracking I can see two trees from here...or here

Aug, 2005Particle Filters: Theory and applications to mobile robotsJose Luis Blanco 1.2. Introduction: The Bayes Rule The Bayes Rule It gives an expresion for updating our belief about some event, given some new information about it. It is derived from: P(A,B) = P(A | B) · P(B) = P(B | A) · P(A) = P(B,A) P(A|B) =· P(A) P(B|A) P(B) Update

Aug, 2005Particle Filters: Theory and applications to mobile robotsJose Luis Blanco 1.2. Introduction: The Bayes Rule The Bayes Rule An example: P(inf) : Likelihood of being infected of some strange disease P(n.inf) = 1 – P(inf) P(pos) : Lik. of a test to give a positive result P(neg) : Lik. of a test to give a negative result  Test reliability is 95%  1% of people is infected ¿ P(inf) ? P(inf) = 0.01 ¿ P(inf | pos) ? P(inf | pos) = P(pos | inf) P(pos) · P(inf) = = · ·0.05 · 0.01 = 0.16 = 16%

Aug, 2005Particle Filters: Theory and applications to mobile robotsJose Luis Blanco 1.3. Introduction: Bayesian Tracking Bayesian Tracking It is a rigurous framework that provides an (ideal) solution to the optimal tracking problem. It consists of two recursive steps: (1) Prediction: Last posterior pdfNew prior pdfSystem model (here comes in the action) State vector at time “k” All observations until time “k-1” State vector at time “k-1”

Aug, 2005Particle Filters: Theory and applications to mobile robotsJose Luis Blanco (2) Update using Bayes: 1.3. Introduction: Bayesian Tracking Bayesian Tracking It is a rigurous framework that provides an (ideal) solution to the optimal tracking problem. It consists of two recursive steps: (1) Prediction: New posterior pdf Observation model New prior pdf This term is constant for a given x k, so it is usually only a proportionally factor.

Aug, 2005Particle Filters: Theory and applications to mobile robotsJose Luis Blanco (2) Update using Bayes: 1.3. Introduction: Bayesian Tracking Bayesian Tracking It is a rigurous framework that provides an (ideal) solution to the optimal tracking problem. It consists of two recursive steps: (1) Prediction: We “only” need these filters

Aug, 2005Particle Filters: Theory and applications to mobile robotsJose Luis Blanco 1.4. Introduction: Solutions to Bayesian Tracking Solutions to Bayesian Tracking In most situations an analytical closed-form solution for this recursion can’t be found. Popular solutions are:  Kalman filter: For lineal transitions and gaussian distributions, exact solutions are obtained using this filter.  Extended Kalman filter (EKF): A linearization of non-lineal systems with additive gaussian noise. It can only approximates pdf as gaussians.  Unscented Kalman Filter: Improves the EKF approximation for non- lineal systems, but pdf remains gaussians.  Particle Filters: An approximated Bayesian filter implementation via sequential Monte Carlo simulations. It is also known as: Bootstrap filtering, condensation algorithm, interactive particle approximations and survival of the fittest.

Aug, 2005Particle Filters: Theory and applications to mobile robotsJose Luis Blanco 2. Particle Filters 2.1. The whole pdf vs. Particles 2.2. Concretizing particles to an estimation 2.3. The basic algorithm: SIS 2.4. The SIR algorithm 2.4. Simulation is easier

Aug, 2005Particle Filters: Theory and applications to mobile robotsJose Luis Blanco 2.1. Particle Filters: The whole pdf. vs Particles The whole pdf vs. Particles Many pdf in real world can be modelled as monomodal or multimodal gaussian pdf’s, but other are too much different to these. Since the storing requirements for an universal pdf is of an intractable size for most useful situations, some approximation must be used. Using particles is a sample-based representation method, consisting in saving just a set of “samples” (particles) from the estate vector, and assigning to them a weight proportional to the estimated pdf. So a “particle” is a couple: { x k i, w k i } k: time, i:particle And a set of particles is: S k = { x k i, w k i }

Aug, 2005Particle Filters: Theory and applications to mobile robotsJose Luis Blanco 2.1. Particle Filters: The whole pdf. vs Particles The whole pdf vs. Particles For example, think in sampling this simple one-dimension pdf: p(x) x Lets take a set of random state vector values

Aug, 2005Particle Filters: Theory and applications to mobile robotsJose Luis Blanco 2.1. Particle Filters: The whole pdf. vs Particles The whole pdf vs. Particles For example, think in sampling this simple one-dimension pdf: p(x) x And set their weights proportional to p(x)

Aug, 2005Particle Filters: Theory and applications to mobile robotsJose Luis Blanco 2.1. Particle Filters: The whole pdf. vs Particles The whole pdf vs. Particles For example, think in sampling this simple one-dimension pdf: p(x) x These particles give the approximation to the pdf. The bigger the particles set, the closer to the real pdf.

Aug, 2005Particle Filters: Theory and applications to mobile robotsJose Luis Blanco 2.2. Particle Filters: Concretizing particles to an estimation Concretizing particles to an estimation There is a simple way of estimating any desired moment of random variable x, the state vector, from the set of particles. It is proven that true values are approched when M  ∞ For example, to get an estimation of the current estate x, one can use “f(x) = x” (In fact this is just one way of getting an estimation of the current state)

Aug, 2005Particle Filters: Theory and applications to mobile robotsJose Luis Blanco 2.3. Particle Filters: The basic algorithm: SIS The basic algorithm: SIS The basic algorithm is SIS (Sequential Importance Sampling), a sequential Monte Carlo solution to non-linear Bayesian tracking. The conditional resampling step is also practically applied in all implementations Why to resample particles?

Aug, 2005Particle Filters: Theory and applications to mobile robotsJose Luis Blanco 2.3. Particle Filters: The basic algorithm: SIS The basic algorithm: SIS The degeneracy problem: This problem involves all PF algorithms, since it has been proven that the variance in particles ALWAYS increases. So a solution is necesary to this degeneration of the pdf approximation. The usual approach is to resample the particles, but there many diferent ways of doing it and deciding when to do it.

Aug, 2005Particle Filters: Theory and applications to mobile robotsJose Luis Blanco 2.3. Particle Filters: The basic algorithm: SIS The basic algorithm: SIS [ {x i k,w i k } i=1:Ns ] = SISParticleFilter({x i k-1,w i k-1 } i=1:Ns, z k ) (1) Prediction: Draw x i k ~ q( x k | x i k-1, z k ) q: Importance density function Last pdf estimation New observation New pdf estimation (2) Update: Update the weights: w i k  w i k-1 p(z k |x i k ) p(x i k |x i k-1 ) / q(x i k | x i k-1,z k ) (3) Normalization: Normalize w i k so they sum up to 1. (4) Resampling: If the estimated Effective Sample Size (ESS) falls below a given threshold, perform a resampling of the particles. For example, applying Select with Replacement.

Aug, 2005Particle Filters: Theory and applications to mobile robotsJose Luis Blanco 2.4. Particle Filters: The SIR algorithm The SIR algorithm The Sampling Importance Resampling (SIR) particle filter is more practical than SIS since weaker requirements are made for implementing it. It was proposed for first time in For implementing SIR it is necesary:  To know the action and observation models. x k = f k ( x k-1, v k-1 ) ACTION MODEL Process noise Last state New state Action model

Aug, 2005Particle Filters: Theory and applications to mobile robotsJose Luis Blanco 2.4. Particle Filters: The SIR algorithm The SIR algorithm The Sampling Importance Resampling (SIR) particle filter is more practical than SIS since weaker requirements are made for implementing it. It was proposed for first time in For implementing SIR it is necesary:  To know the action and observation models. x k = f k ( x k-1, v k-1 ) ACTION MODEL z k = h k ( x k, n k ) OBSERVATION MODEL Process noise Current state Observation Observation model

Aug, 2005Particle Filters: Theory and applications to mobile robotsJose Luis Blanco 2.4. Particle Filters: The SIR algorithm The SIR algorithm The Sampling Importance Resampling (SIR) particle filter is more practical than SIS since weaker requirements are made for implementing it. It was proposed for first time in For implementing SIR it is necesary:  To know the action and observation models.  To sample (random) realisations from v k-1 (the action model noise process)  To evaluate p(z k | x k ) pointwise, at least up to proportionality. Moreover, a few modifications to SIS algorithm are used

Aug, 2005Particle Filters: Theory and applications to mobile robotsJose Luis Blanco 2.4. Particle Filters: The SIR algorithm The SIR algorithm Modifications from SIS algorithm:  The importance density function q(·) is chosen to be just the prior: q( x k | x i k-1, z k ) = p( x k | x k-1 ) This implies we need to generate samples x i k ~ p( x k | x k-1 ), but this is easy since we know the action model f(·,·) and can draw v k samples: x i k = f k (x i k-1, v i k-1 ) Simulation  The resampling step is executed after each iteration.

Aug, 2005Particle Filters: Theory and applications to mobile robotsJose Luis Blanco 2.4. Particle Filters: The SIR algorithm The SIR algorithm [ {x i k,w i k } i=1:Ns ] = SIRParticleFilter({x i k-1,w i k-1 } i=1:Ns, z k ) (1) Prediction: Draw x i k ~ p( x k | x i k-1 ) using simulations. (2) Update: Update the weights: w i k = p(z k |x i k ) (Much easier than SIS!!) (3) Normalization: Normalize w i k so they sum up to 1. (4) Resampling: For example, applying Select with Replacement.

Aug, 2005Particle Filters: Theory and applications to mobile robotsJose Luis Blanco 2.5. Particle Filters: Simulation is easier Simulation is easier What is the advantage of using SIR? We need just the action and observation model and use them to:  Run particles evolution simulations.  Evaluate the observation inverse model pointwise. This is clearly easier than trying to build the true posterior pdf theoretically, as required by ideal Bayesian tracking framework.

Aug, 2005Particle Filters: Theory and applications to mobile robotsJose Luis Blanco 3. Applications to localization and map building 3.1. Localization 3.2. Map Building

Aug, 2005Particle Filters: Theory and applications to mobile robotsJose Luis Blanco 3.1. Applications: Localization Localization Typical localization applications include: - Position tracking - Global localization With particles filter, both approaches can be unified in one common framework. The usual approach assumes a known global map, and uses robot poses as particles. The p(z | x) is just the inverse sensor model.

Aug, 2005Particle Filters: Theory and applications to mobile robotsJose Luis Blanco NOTE: It may be necesary to suppose a sensor noise bigger than the real noise for particles solution to converge Applications: Localization Localization

Aug, 2005Particle Filters: Theory and applications to mobile robotsJose Luis Blanco 3.2. Applications: Map Building Map Building (Rao-Blackwellized Particle Filter) In these approaches each particle must represents a full world model, which is achieved letting the state vector x to include the sequence of poses and corresponding observations taken at each pose. A way to estimate the likelihood of an observation given all the set of previous observations must be developed. There are many other complex problems, for example, to recover the particles diversity after a successfull loop closure, etc… In these approaches there is not a “current world model”, but a set of them with associated probabilities of being the correct ones.

Aug, 2005Particle Filters: Theory and applications to mobile robotsJose Luis Blanco 3.2. Applications: Map Building FastSLAM: (Burgard, Fox, Thrun) Map Building (Rao-Blackwellized Particle Filter)

Aug, 2005Particle Filters: Theory and applications to mobile robotsJose Luis Blanco 4. Preliminary experiments Next are shown two particles simulations experiments: 1)A simmulation of robot pose tracking without updating, using just prediction stage, with particles. 2) A localization experiment.

Aug, 2005Particle Filters: Theory and applications to mobile robotsJose Luis Blanco 4. Preliminary experiments The 3D poses are projected over the 2D workspace for clarity.

Aug, 2005Particle Filters: Theory and applications to mobile robotsJose Luis Blanco 4. Preliminary experiments

Aug, 2005Particle Filters: Theory and applications to mobile robotsJose Luis Blanco References - A. Doucet, S. Godsill, C. Andrieu, “On Sequential Monte Carlo Sampling Methods for Bayesian Filtering” - I. M. Rekleitis, “A Particle Filter Tutorial for Mobile Robot Localization”, Tech. Report, S. Maskell, “A Tutorial on Particle Filters for On-line Nonlinear / Non-Gaussian Bayesian Tracking”, A. Doucet, N. de Freitas, K. Murphy, S. Russel. “Rao-Blackwellised Particle Filtering for Dynamic Bayesian Networks”.