Robot Localization with Particle Filters: An Intuitive Guide

Imagine a little robot entered a new home…

Since the robot doesn't know where it is, it creates many imaginary copies of itself — called particles — scattered randomly throughout the house. Each particle is a guess of where it might be.

Step-1 Initialize:

Each particle usually represents a full pose, which includes:

• x: horizontal position
• y: vertical position
• θ (theta): heading direction (orientation angle, usually in radians)

particle = [x, y, θ]

To initialize:

num_of_particle = 500
particles = []
for i in range (num_of_particle):
    x = robot()
    x.set_noise(0.02, 0.02, 3.0)
    particles.append(x) 

The Robot needs some landmarks to help it decide where it is. Just like us human, when we see a bed, we know we are in the bedroom.

What are Landmarks:

• In 2D LIDAR-based systems, landmarks often appear as "blobs" or line segments (walls, table edges).
• In vision-based systems, landmarks may be objects like a picture frame, TV, or doorframe.
• Advanced systems may even use semantic mapping, recognizing landmarks like "fridge" or "couch" from camera data using object detection.

Quality of Landmark:

1. Detectable: The robot's sensors (e.g., LIDAR, camera, sonar) can reliably detect and recognize it.
2. Locally unique: The object has a distinctive position or appearance, not easily confused with other similar objects.
3. Fixed position: It does not move often (e.g., a bed or wall is better than a rolling chair).

Step-2 Move:

After the initialization, the particles start to move. Because the heading direction was randomly initiated, the particles will move towards all directions.

# Move
particles_temp = []
for i in range(num_particles):
    particles_temp.append(particles[i].move(0.1, 5.0)). # move(direction, distance)
particles = particles_temp

Step-3 Measure:

1. The Robot Takes Real Measurements

• The robot uses its actual sensors (e.g. lidar, sonar, depth camera) to measure distances to known landmarks.
• These are the actual observations, denoted as z_real.

2. Each Particle Predicts What It Would Measure

• Each particle represents a hypothetical robot with its own (x, y, θ) position and heading.
• Based on its position and the known map (with landmark locations), the particle predicts what it would measure, denoted as z_expected.

3. Find out how important each particle is

Importance Weight = How Close z_real ≈ z_expected

• The closer a particle's predicted measurement is to the real sensor reading, the higher its importance weight.
• Mathematically:

prob = 1.0
dist = sqrt((self.x - landmarks[i][0]) ** 2 + (self.y - landmarks[i][1]) ** 2)
prob *= self.Gaussian(dist, self.sense_noise, measurement[i])

Step-4 Resample:

Resample particles randomly based on their importance weights to form a new set — favoring larger weights(better matching).

Common algorithms are:

1. Naive Resampling (roulette wheel)

import random

def resample(particles, weights):
    N = len(particles)
    new_particles = []
    index = int(random.random() * N)
    beta = 0.0
    mw = max(weights)
    for _ in range(N):
        beta += random.random() * 2.0 * mw
        while beta > weights[index]:
            beta -= weights[index]
            index = (index + 1) % N
        new_particles.append(particles[index])
    return new_particles

2. Systematic Resampling (more efficient and deterministic)

def systematic_resample(particles, weights):
    N = len(particles)
    positions = (random.random() + np.arange(N)) / N
    indexes = []
    cumulative_sum = np.cumsum(weights)
    i, j = 0, 0
    while i < N:
        if positions[i] < cumulative_sum[j]:
            indexes.append(j)
            i += 1
        else:
            j += 1
    return [particles[i] for i in indexes]

Step-5 Repeat:

Move → Measure → Resample

This cycle continues until the robot reaches its goal, or until localization is confident enough — or indefinitely in continuous tracking tasks.

Reference:

Thrun, Sebastian. AI for Robotics. Available at: https://www.udacity.com/course/ai-for-robotics--cs373

📘 Also check out my related articles about AI in Robotics: - [Bayes' Rule in Robot Localization] - [SLAM: How Lost Robots Build a Map] - [Kalman Filter — Combining Messy Sensors with Math] - [Particle Filters: An Intuitive Guide to Robot Localization] - [Where AI Robots Take Us, Practically and Philosophically]