Particle Filter

◀ Go to Bayes Filter

Particle Filter is a localization technique that utilizes Bayesian Updates. Instead of keeping track of a single exact position, the robot keeps many guesses called particles. Each particle represents a possible location the robot could be in. The algorithm gets rid of bad guesses and propagates more particles through good guesses. When more particles cluster in one area, it means the robot is more likely to actually be there. This method is especially useful when the robot’s sensors or movements are noisy or uncertain. In this tutorial, we will go over making a Particle Filter for a Differential Drive robot with wheel encoders and a LiDAR sensor.

Differential Drive Robot

Particle Filter Algorithm Overview

1. Initialization:
   Spread many particles randomly across the map. Each particle represents a possible robot pose.

2. Motion Update (Prediction):
   When the robot moves, update the position of each particle based on the robot’s movement. Add a bit of randomness to account for uncertainty in motion.

3. Measurement Update (Correction):
   For each LiDAR scan:

   • Predict what the scan would look like from each particle’s position.
   • Compare the predicted scan to the actual scan and assign a weight to each particle. The closer the match, the higher the weight.

4. Resampling:
   Generate a new set of particles by propagating a new set of particles from the current set. Particles with higher weights are more likely to be chosen, focusing the cloud of particles around the most probable locations.

5. Repeat:
   Continue steps 2–4 as the robot moves and collects new sensor data.

Step 1: Initialization

Let's start out by making \(n\) particles, each assigned weight \(\alpha_{n}\), which is \(\frac{1}{n}\) at the start.

Step 2: Motion Update

In this step we perform dead-reckoning using the wheel encoders and an IMU to update the particles of the robot. The IMU provides data on angular velocity, while the wheel encoders provide data on the linear velocity. Using this data, we are able to substitute it into our Differential Drive motion model and produce an initial estimation of the robot's pose.

In case you forgot, here is the Differential Drive motion model equation:

\[ \mathbf{x}_{t+1} = \begin{bmatrix} x_{t+1} \\ y_{t+1} \\ \theta_{t+1} \end{bmatrix} = f_d(\mathbf{x}_t, \mathbf{u}_t) = \mathbf{x}_t + \tau_t \begin{bmatrix} v_t \text{sinc}\left(\frac{\omega_t \tau_t}{2}\right) \cos\left(\theta_t + \frac{\omega_t \tau_t}{2}\right) \\ v_t \text{sinc}\left(\frac{\omega_t \tau_t}{2}\right) \sin\left(\theta_t + \frac{\omega_t \tau_t}{2}\right) \\ \omega_t \end{bmatrix} \]

Per each iteration of sensor updates or measurements taken on our robot, we use this equation \(n\) times for each of the \(n\) particles, to create \(n\) "guess" positions of where we can be at time \(t+1\). However, to make the guesses useful, they must be different from each other.

What if a random "noise" number \(W\) was added before plugging the velocities (linear and angular) into the Differential Drive model. This would provide variance among each particle's "guess", and allow us to overall make a more accurate estimate. But how do we get this noise? That's right, we can model it on a Gaussian distribution of the angular and linear velocities.

Noise \(W\) is often modeled on a Gaussian distribution. A Gaussian distribution (also called a normal distribution) is a bell-shaped probability distribution that describes how likely values are to occur around a central point (the mean). Most values cluster near the mean, while values farther away are less likely.

What is the main reason noise is used?

A

To provide a diverse set of particles.

B

Noise doesn't effect particles.

C

To make the particles as similar to each other as possible.

D

To add randomness in the particle's predictions.

Submit

The variance (often represented by \( \sigma^2 \)) controls how spread out the values are. The square root of variance, \( \sigma \), is called the standard deviation. You can think of it as a “typical distance” from the mean:

Gaussian Distribution Example .

• About 68% of values fall within ±1σ of the mean.
• About 95% of values fall within ±2σ of the mean.
• About 99.7% of values fall within ±3σ of the mean.

In other words, a small σ makes the curve narrow and tall (most values close to the mean), while a large σ makes the curve wider (values more spread out).

In motion models, we tend to represent the variance through a matrix, called the covariance matrix. The covariance matrice represents how much each part of your X, Y, \(\theta\), or pose in general can vary, and how errors are correlated to it.

Here is an equation we can follow for modeling the noise \(W\):

\[ W \sim \mathcal{N}\Bigg( \begin{bmatrix} 0 \\ 0 \end{bmatrix}, \; \begin{bmatrix} \sigma_v^2 & 0 \\ 0 & \sigma_\omega^2 \end{bmatrix} \Bigg) \]

The matrix \(\begin{bmatrix}0 \\0\end{bmatrix}\) is the center of the distribution, which is \(0\) m/s linear velocity and \(0\) rad/s angular velocity. \(\begin{bmatrix} \sigma_v^2 & 0 \\ 0 & \sigma_\omega^2 \end{bmatrix}\), is the covariance matrix which shows us how much we want the linear velocity noise and angular velocity noise to vary by. More specifically, \(\sigma_v^2\) represents the variance of our linear velocity noise, and \(\sigma_\omega^2\) represents the variance for the noise of our angular velocity. In real-world scenarios, we tend to choose the variance we want through extensive tuning and seeing which values work best. However, it is important to remember not to make it too small otherwise our guesses won't be diverse resulting in localization drift a common limitation of many localization techniques, which was explained in the Introduction to Localization Section.

Compared to if we had a covariance matrix with both σ_v=0.001 and σ_w=0.001, if we had σ_v=0.5 and σ_w=0.5 what would be different?

A

The latter would have a similar variety of particle guesses.

B

The latter would have a greater variety of particle guesses.

C

The latter would have a lesser variety of particle guesses.

D

Covariance matrix values have no effect the variety of particle guesses.

Submit

With this noise "\(W\)" added, we can derive a new equation for the Differential Drive Motion Model:

\[ \mathbf{x}_{t+1} = \begin{bmatrix} x_{t+1} \\ y_{t+1} \\ \theta_{t+1} \end{bmatrix} = f_d(\mathbf{x}_t, \mathbf{u}_t)+W = \mathbf{x}_t + \tau_t \begin{bmatrix} v_t \text{sinc}\left(\frac{\omega_t \tau_t}{2}\right) \cos\left(\theta_t + \frac{\omega_t \tau_t}{2}\right) \\ v_t \text{sinc}\left(\frac{\omega_t \tau_t}{2}\right) \sin\left(\theta_t + \frac{\omega_t \tau_t}{2}\right) \\ \omega_t \end{bmatrix}+W \]

Here, \(W\) is a random noise vector sampled independently for each particle at every timestep from the Gaussian distribution we discussed earlier:

\[ W \sim \mathcal{N}\Bigg( \begin{bmatrix} 0 \\ 0 \end{bmatrix}, \; \begin{bmatrix} \sigma_v^2 & 0 \\ 0 & \sigma_\omega^2 \end{bmatrix} \Bigg) \]

Adding this noise allows each particle to have slightly different motion predictions, creating a diverse set of particle guesses at time \(t+1\). This diversity is crucial for the particle filter to maintain multiple hypotheses about the robot’s pose, helping it stay robust to motion and sensor uncertainty and preventing localization drift.

Step 3: Measurment Update

In this step, we update the weights of each of our particle's guesses, implementing Bayes Update to do so.

As stated before, lets assume that have \(n\) weights for the \(n\) particles, each having value \(a_{n}\). To update the weights of each particle, we multiply our belief of the particle's "guess" being right. We find this belief using the data we obtain from a LiDAR sensor.

The general equation for performing an update on particle \(n\) is:

\[ a_{n}=a_{n} \times p(z_{t}|x_{n}) \]

The probability \(p(z_{t}|x_{n})\) is taken from the Bayesian Measurement Update in the previous lesson on Bayes Filter. It represents our current belief of being at pose \(x_{n}\) based on the most recent lidar scan. Essentially, it tells us the probability of receiving measurement \(z_{t}\) given the robot is at pose \(x_{n}\). To implement a calculation for it however, is much more complex than knowing the expression.

Once we have \(p(z_{t} \mid x_{n})\) using the method discussed in the previous lesson on the Bayesian Measurement Update, we multiply it with the particle’s prior weight \(a_n\) to obtain the updated weight. After updating all particle weights, we normalize them so that they sum to 1, ensuring they represent a proper probability distribution that is between 0 and 1:

\[ a_{n} = \frac {a_n}{\sum_{k=1}^{N} a_k} \]

Particles whose predicted scans align well with the map will receive higher weights, while poorly aligned particles will receive lower weights. This weighting process is the key step that allows the particle filter to focus on the most-likely correct poses over time.

Step 4: Resampling

As we perform many iterations, a common issue can arise: a few particles may end up with very high weights while most particles have weights close to zero. This makes the weights vector very sparse and reduces the diversity of our particles. To address this problem, we perform a process called resampling, which focuses computational resources on the most likely particles while throwing away the unlikely ones. This keeps the particle distribution representative of the robot’s true belief.

To determine when resampling is necessary, we calculate the effective number of particles, \(N_{eff}\), as:

\[N_{eff} = \frac{1}{\sum_{i=1}^{N} a_i^2}\]

where \(a_i\) are the normalized weights of the particles. This value represents how many particles effectively contribute to the estimate. If \(N_{eff}\) falls below a chosen threshold, often times \(\frac{n}{2}\) , resampling is triggered.

Once resampling is triggered, we generate a new set of \(N\) particles based on the current weights. First, we compute the cumulative sum of the particle weights, a series \(c\) of numbers, with each element \(c_{i}\) given by:

\[c_i = \sum_{j=1}^{i} a_j\]

Then, for each new particle, we use this cumulative sum to assign it the pose of one of the old particles according to their weights. Each new particle is assigned a weight of \(1/N\).

After all new particles are selected, reset all particle weights to \(1/N\) so that the total weight sums to 1. This new set of particles is now ready to continue in the particle filter loop. The particle poses will then be updated in the prediction step, where motion updates and noise are applied according to the motion model.

Outputting the Robot's Pose

When we want to obtain a prediction for our robot's pose, we can do this by calculating a combined weighted-pose using the weights of each particle:

\[ X = \sum_{i=1}^{n} (a_{i} \times x_{i}) \]

where \(a_i\) is the weight, and \(x_{i}\) is the pose estimate of particle \(i\).

This way, we are able to take into account all the particles’ contributions to the robot’s overall estimated position. Since each particle represents a possible pose with an associated probability (weight), the weighted average gives a more accurate estimate than just picking a single particle.

Demo

A Google Colab demo of Particle Filter localization algorithm.