Skip to the lesson
FixTheRobot Explore Robotics / Localization & SLAM / M3 Mapping and State Estimation / M3-T8

Localization & SLAM · Theory · M3

M3-T8 — Particle Filters and FastSLAM

Represent the belief by many guesses of the pose, and let the good guesses breed.

  • Lesson 8 of 8
  • Module 3: Mapping and State Estimation
  • Theory
  • About 9 minutes to read

A Gaussian belief has one peak. A robot in a symmetric building can be in several places, and a particle filter can hold all of them. This lesson builds Monte Carlo localisation, explains its failure modes (depletion and the kidnapped robot), and shows how FastSLAM splits SLAM into a particle filter over the path and many tiny Kalman filters over the landmarks.

Learning objectives

After this lesson, you should be able to:

  • Describe the particle filter cycle (move, weight, resample) and what each particle represents.
  • Implement low-variance resampling and compute the effective sample size.
  • Explain particle depletion, the kidnapped-robot problem and the usual remedies.
  • Explain the Rao-Blackwellisation in FastSLAM and why it makes particle SLAM practical.

The problem and the intuition

Scatter a thousand flags over the map, each one a guess of where the robot might be. As the robot moves, move every flag the same way, with some random noise. When the robot takes a laser scan, ask each flag: “if the robot were here, would the scan look like this?” Flags that fit well get a high weight; those that do not, a low weight. Then throw away the low-weight flags and duplicate the high-weight ones. The cloud of flags concentrates where the robot really is, and if two places fit equally well, both keep flags.

A particle filter is “survival of the fittest” for pose hypotheses, driven by the likelihood of the measurement.

The particle filter cycleFour stages in a loop: move each particle with a noisy motion model, weight each particle by how well the measurement fits, resample in proportion to weight, and the new set of particles is the updated belief.1 Movesample from the motion model 2 Weightw = p(z | particle) 3 Resampledraw in proportion to w beliefN particles repeat for every control and measurement
Monte Carlo localisation: move, weight, resample. The set of particles approximates the belief.

The algorithm: Monte Carlo localisation and FastSLAM

Problem (localisation). Given a known map, estimate the pose by a set of M weighted samples {(x[k], w[k])} that approximates the belief bel(xt).

Assumptions. A known map; a motion model that can be sampled; a measurement model that can be evaluated as a likelihood; enough particles to cover the plausible region.

The cycle

  1. Sample (predict). For each particle draw xt[k] ~ p(xt | xt−1[k], ut): apply the odometry plus random noise.
  2. Weight (correct). w[k] = p(zt | xt[k], map). For a laser scan with independent beams, multiply the beam likelihoods (add log-likelihoods).
  3. Normalise the weights so that they sum to one.
  4. Resample: draw M new particles with probability proportional to the weights, and reset all weights to 1/M. Only resample when the effective sample size Neff = 1 / Σ(w[k])² falls below, say, M/2: resampling every step adds noise.

Low-variance resampling

Draw a single random number r ∈ [0, 1/M) and select the particles at the cumulative-weight positions r, r + 1/M, r + 2/M, …. It has O(M) cost and lower variance than M independent draws:

Low-variance resampling
ck=∑i≤k wic_{k}=\sum _{i\le k}\,w_{i}
choose particle k for each um=r+m−1M with c(k−1)<um≤ck\text{choose particle}\,k\,\text{for each}\,u_{m}=r+\frac{m-1}{M\,\text{with}\,c_{(k-1)}}<u_{m}\le c_{k}

Failure modes

  • Particle depletion. After many resamplings, all particles descend from a few ancestors. Diversity is lost, and if they are in the wrong place the filter cannot recover.
  • Kidnapped robot. If the robot is moved without the filter knowing, no particle is near the truth. Remedy: inject a small fraction of random particles (Augmented MCL), whose rate is driven by a falling average likelihood.
  • Too few particles. The cloud fails to cover a large uncertainty; the number needed grows with the dimension and the initial uncertainty (global localisation needs thousands).
  • Overconfident sensor model. A very peaked likelihood gives one particle all the weight (Neff ≈ 1). Inflate the sensor noise or use a mixture with a uniform outlier term.

FastSLAM and Rao-Blackwellisation

For SLAM, the state (path and map) is too large to sample directly. FastSLAM (Montemerlo et al., 2002) factorises the posterior:

FastSLAM factorisation
p(x1: t, m ∣z1: t, u1: t)=p(x1: t ∣z1: t, u1: t)⋅∏j p(mj ∣x1: t, z1: t)p(x_{1:\,t},\,m\,\mid z_{1:\,t},\,u_{1:\,t})=p(x_{1:\,t}\,\mid z_{1:\,t},\,u_{1:\,t})\cdot \prod _{j}\,p(m_{j}\,\mid x_{1:\,t},\,z_{1:\,t})

The key fact: given the whole path, the landmarks are independent (M3-T2). So each particle carries one sampled path and N separate 2×2 EKFs, one per landmark. Weights come from the measurement likelihood, and a landmark update touches one small filter. The cost per step is O(M log N) with a tree structure for the landmarks, instead of O(N²). Grid-based variants (Grisetti et al., 2007: GMapping) put an occupancy grid in each particle and use scan matching in the proposal.

PropertyEKF-SLAMFastSLAM / GMapping
BeliefOne GaussianMany particles, each a possible path with its own map
Multiple hypothesesNoYes (data association per particle)
Cost per updateO(N²)O(M log N)
WeaknessLinearisation, scalingNeeds many particles in large loops; each carries a full map; depletion after loop closure

A worked example: weights, N<sub>eff</sub> and resampling

Four particles with weights after a measurement update: w = (0.1, 0.2, 0.6, 0.1).

  1. Effective sample size. Neff = 1 / (0.1² + 0.2² + 0.6² + 0.1²) = 1 / 0.42 = 2.38. Four particles contribute as if there were only 2.4 equally weighted ones. With a threshold M/2 = 2 the filter would not resample yet.
  2. Low-variance resampling with r = 0.06. M = 4, so the step is 0.25 and the pointers are 0.06, 0.31, 0.56, 0.81. The cumulative weights are 0.1, 0.3, 0.9, 1.0.
  3. Selection. 0.06 lies in (0, 0.1]: particle 1. 0.31 lies in (0.3, 0.9]: particle 3. 0.56 and 0.81 also lie in (0.3, 0.9]: particle 3 twice more. The new set is {1, 3, 3, 3}, all with weight 0.25.
  4. The cost. Particle 2 (weight 0.2) was dropped while particle 3 (weight 0.6) was cloned three times. Diversity fell from four distinct to two distinct particles after one step. After a few more such steps the cloud may collapse onto a single ancestor: depletion.
  5. Kidnapping. If the true pose now jumps 5 m away, every particle’s likelihood becomes tiny and the average likelihood falls sharply. An augmented filter notices that drop and replaces, say, 10% of the particles by uniform random poses; one of them may land near the truth and take over within a few steps.

The lab’s third stage lets you watch exactly this on a corridor.

Engineering implications and limitations

Engineering implications

  • Number of particles. A few hundred suffice for tracking with a good initial pose; thousands for global localisation. Adapt it with KLD-sampling (Fox, 2003) to the spread of the cloud.
  • Motion noise. Too little noise starves the cloud of diversity; too much blurs it. Tune against recorded runs.
  • Sensor model. Mix the beam Gaussian with a uniform “random reading” term; subsample beams (every 5–10th) and temper the likelihood to avoid overconfident weights.
  • Resample selectively (Neff threshold) and keep the cloud’s estimate as a weighted mean only when it is unimodal; otherwise report the best cluster.
  • Recovery. Inject random particles when the average likelihood drops; watch the divergence between the short-term and long-term averages (Augmented MCL).
  • Use particle SLAM for medium maps in 2D (GMapping, a classic ROS package) and graph-based methods for large or 3D ones.

Limits

Particle filters do not scale to high dimensions: the number of particles needed grows exponentially with the state size, which is why FastSLAM must factor out the map. In SLAM the particles represent paths, and long loops need many of them because a particle that is wrong early cannot be repaired. This weakness of particle SLAM is a main reason why graph optimisation became the standard (Module 4).

Common misconceptions

“More particles always fix the problem.”

More particles help coverage but not a wrong motion or sensor model, and do not stop depletion over a long loop.

“The particle with the highest weight is the pose.”

The belief is the whole cloud. The best particle can be a lucky outlier; use the weighted mean for a unimodal belief or cluster for several.

“Resample at every step.”

It discards diversity unnecessarily. Resample when Neff is low.

“A particle filter handles any amount of uncertainty.”

Coverage needs particles: large initial uncertainty needs a large number.

Engineering takeaways

  1. A particle filter represents the belief with weighted samples: move with the motion model, weight with the measurement likelihood, resample.
  2. Low-variance resampling is O(M) and low in variance; resample only when the effective sample size is low.
  3. Depletion and the kidnapped-robot problem are the main failures; random particle injection is the standard remedy.
  4. FastSLAM factorises SLAM: sampled paths plus a tiny EKF per landmark per particle, because landmarks are independent given the path.
  5. Particle SLAM is good for medium 2D maps and multiple hypotheses; it is limited by the number of particles needed for long loops.

References and further reading

Primary papers and authoritative documentation.

Knowledge check

Three conceptual questions. Write an answer, then reveal the explanation. Your answers stay in your browser.

Question 1

Why does FastSLAM keep a separate small EKF per landmark in each particle instead of one large filter?

Explanation

Because given the whole robot path, the landmark estimates are independent. Each particle carries one sampled path, so the landmark estimates for that path factor into many 2×2 filters. This turns an O(N²) joint update into O(log N) per observation per particle.

Question 2

The effective sample size drops to 1.2 out of 500 particles after one update. What does it mean and what should you check?

Explanation

Almost all the weight is on a single particle: the filter is effectively one hypothesis. Typically the sensor model is overconfident (likelihoods too peaked), or the true pose is outside the cloud. Check the sensor noise parameters, temper the likelihood, and check whether the robot was moved (kidnapped).

Question 3

After a long loop closure a particle SLAM system has lost the correct map. Why can’t more resampling repair it?

Explanation

Resampling can only duplicate paths that exist in the particle set. If, over a long loop, every particle accumulated errors independently and none of them stayed correct, the loop-closing observation has no consistent particle to favour. More particles at the start, or a better proposal using scan matching, are needed.

What’s next

Module 3 closes with a lab that has you tune a Monte Carlo localiser and implement EKF-SLAM. Module 4 then drops the filter altogether and treats SLAM as optimisation over a graph.

Practice this lesson

Explore Particle-Filter Localization

Tune and complete a Monte Carlo localisation filter and watch particle depletion and kidnapping.

Next lesson

M4-T1 — Front-End and Back-End SLAM

Split a SLAM system into data-association front end and optimisation back end and say what each owns.

Nothing below is required to finish this lesson. The last 3 entries are lessons of other courses on this site that teach the background or go further.