Skip to content

Localization

Localization is a fundamental problem in robotics, revolving around the question: "Where am I?". One popular solution to this question is the Particle Filter localization algorithm, particularly when using LIDAR (Light Detection and Ranging) as a sensor. On this page, we'll dive deep into the basics of Particle Filter localization using LIDAR scans.

What is a Particle Filter?

A Particle Filter is a probabilistic technique that represents the robot's belief of its position as a set of particles. Each particle signifies a potential hypothesis of where the robot might be. The more particles in a specific area, the higher the belief that the robot is in that area.

Demo

A Google Colab demo of Particle Filter localization algorithm.

With user keyboard:

Open In Colab

With PyBullet:

Open In Colab

How Particle Filters Work with LIDAR

LIDAR provides distance measurements by sending out light beams and measuring the time it takes for them to reflect back. With a set of distance measurements (or scans) forming a 360° view around the robot, we can compare these scans against a known map to determine where the robot might be.

Steps

  1. Initialization: Spread out a large number of particles randomly throughout the map.
  2. Motion Update: Whenever the robot moves, shift each particle based on the robot's movement. Introduce some randomness to account for uncertainties in movement.

  3. Measurement Update: For each LIDAR scan:

    • Simulate what the scan would look like from the position and orientation of each particle.
    • Assign a weight to each particle based on how closely its simulated scan matches the actual LIDAR scan.
    • The better the match, the higher the weight.

  4. Resampling: Create a new set of particles by randomly drawing from the current set, with a higher chance of drawing particles with higher weights. This step focuses the particle cloud on areas of high likelihood.

  5. Repeat steps 2-4 as the robot continues to move and sense its environment.

Particle Filter Algorithm: Mathematical Representation

Particle Representation

Each particle \(i\) can be represented as \(\{x_i, y_i, \theta_i \}\)

  • \(x_i\) and \(y_i\) are the \(x\) and \(y\) coordinates of the particle.
  • \(\theta_i\) is the orientation of the particle.

Motion Update (Prediction Step)

Given a motion command \(u(t) = \{\Delta x, \Delta y, \Delta \theta \}\), the new position \(p_i'\) of each particle can be determined as follows:

\(x'_i = x_i + \Delta x + \epsilon_x\), \(y'_i = y_i + \Delta y + \epsilon_y\), \(\theta'_i = \theta_i + \Delta \theta + \epsilon_{\theta}\).

Where \(\epsilon_x\), \(\epsilon_y\), and \(\epsilon_{\theta}\) are random variables drawn from a Gaussian distribution with mean 0 and standard deviation \(\sigma_{x}\), \(\sigma_{y}\), and \(\sigma_{\theta}\) respectively. These standard deviations capture the uncertainty in the motion command.

Measurement Update (Correction Step)

Given a LIDAR scan \(z_t\) and the expected measurement \(\hat{z}_t(i)\) from each particle \(i\), the weight of each particle is computed as follows:

\(w_i = f(z_t, \hat{z}_t(i))\)

The function \(f\) computes the similarity, often using the likelihood of the observed measurement given the expected one. A common method is to use the Gaussian probability density function:

\(w_i = \frac{1}{\sigma \sqrt{2\pi}} \exp\left(-\frac{(z_t - \hat{z}_t(i))^2}{2\sigma^2}\right)\)

Where \(\sigma\) is the standard deviation capturing the uncertainty in measurements.

Resampling

Particles are resampled with a probability proportional to their weights. The probability of selecting particle \(i\) is:

\(P(p_i) = \frac{w_i}{\sum_{j=1}^{N} w_j}\)

Where \(N\) is the total number of particles.