One of the classic problems in mobile robotics is: if a robot only has noisy sensors and noisy motion, how does it figure out where it actually is? This project is a 2D simulator I built to answer that question hands-on, using a particle filter (a form of Monte Carlo localization) to estimate a robot's position on a grayscale map as it moves around blind, guided only by imperfect readings.

The whole thing runs in Python with NumPy doing the heavy lifting on the particle math and OpenCV handling the map rendering and keyboard input. Let's break down how it actually works.

The Idea

Instead of tracking one single position estimate, a particle filter tracks thousands of "guesses" at once, each one a hypothesis of where the robot could be. Every time the robot moves, all the particles move with it (with some noise). Every time the robot takes a sensor reading, particles that would have produced a similar reading get weighted higher, and particles that don't match get weighted lower. Resampling then keeps the good guesses and discards the bad ones, so over time the particle cloud collapses around the robot's true position.

Setting Up the Map and Robot State

The map is just a grayscale image, loaded once and blurred slightly so brightness changes smoothly across the terrain, that smoothing matters later for how the sensor model works.

map_img = cv2.imread("map.png", cv2.IMREAD_GRAYSCALE)
if map_img is None:
    raise FileNotFoundError("Could not read map.png")
HEIGHT, WIDTH = map_img.shape

map_blur = cv2.GaussianBlur(map_img, (7, 7), 0)

# Robot pose (floats)
rx, ry, rtheta = (WIDTH / 4.0, HEIGHT / 4.0, 0.0)

STEP = 5.0
TURN = np.radians(25.0)
SIGMA_STEP = 0.5
SIGMA_TURN = np.radians(5.0)
NUM_PARTICLES = 3000

The robot's pose is just three floats: x, y, and heading angle theta. 3,000 particles are simulated per frame, each with its own (x, y, theta), which is plenty for smooth convergence without tanking frame rate.

Motion With Noise

Real robots never move exactly as commanded, wheels slip, motors aren't perfectly calibrated. That imperfection is simulated directly by adding Gaussian noise to every move:

def move_robot(rx, ry, rtheta, fwd, turn):
    fwd_noisy = fwd + float(np.random.normal(0.0, SIGMA_STEP))
    turn_noisy = turn + float(np.random.normal(0.0, SIGMA_TURN))

    rx += fwd_noisy * np.cos(rtheta)
    ry += fwd_noisy * np.sin(rtheta)
    rtheta += turn_noisy

    rx = np.clip(rx, 0.0, WIDTH - 1)
    ry = np.clip(ry, 0.0, HEIGHT - 1)
    return rx, ry, rtheta

The particles get the same treatment, but vectorized across all 3,000 at once instead of one at a time, which is where NumPy earns its keep:

def move_particles(particles, fwd, turn):
    if fwd == 0.0 and turn == 0.0:
        return particles
    particles[:, 0] += fwd * np.cos(particles[:, 2])
    particles[:, 1] += fwd * np.sin(particles[:, 2])
    particles[:, 2] += turn
    particles[:, 0] = np.clip(particles[:, 0], 0.0, WIDTH - 1)
    particles[:, 1] = np.clip(particles[:, 1], 0.0, HEIGHT - 1)
    return particles

The Sensor Model

The robot's only "sensor" is the brightness of the map at its current location, read off the blurred grayscale image, with noise added on top to simulate a real sensor's imprecision:

def sense(x, y, noisy=False):
    SIGMA_SENSOR = 5.0
    xi = int(np.clip(x, 0, WIDTH - 1))
    yi = int(np.clip(y, 0, HEIGHT - 1))
    reading = float(map_blur[yi, xi])
    if noisy:
        reading += float(np.random.normal(0.0, SIGMA_SENSOR))
    return reading

It's a simple model on purpose, brightness as a stand-in for whatever a real sensor might return (distance, signal strength, whatever). The blur applied to the map earlier is what makes this readable: without it, brightness would jump too sharply between neighboring pixels for the filter to converge smoothly.

Scoring the Particles

This is the core of the filter: for every particle, compare what it would have sensed against what the robot actually sensed. Particles that match closely get high weight, particles that don't get pushed toward zero.

def compute_weights(particles, robot_sensor):
    xs = particles[:, 0].astype(np.int32)
    ys = particles[:, 1].astype(np.int32)
    particle_vals = map_blur[ys, xs].astype(np.float32)

    errors = np.abs(robot_sensor - particle_vals)
    max_err = errors.max()
    weights = np.ones_like(errors) if max_err == 0 else (max_err - errors)

    # Kill off particles that drifted off the map
    edge_mask = (
        (particles[:, 0] <= 0) | (particles[:, 0] >= WIDTH - 1) |
        (particles[:, 1] <= 0) | (particles[:, 1] >= HEIGHT - 1)
    )
    weights[edge_mask] = 0.0

    # Sharpen the distribution so good matches dominate
    weights = weights ** 3

    if np.sum(weights) == 0:
        weights[:] = 1.0
    return weights

Two details worth calling out: particles that have drifted off the edge of the map get their weight zeroed out immediately (they're not physically valid guesses), and the raw weights get cubed before use. Cubing sharpens the distribution, small differences in error become much larger differences in weight, which makes the filter converge faster instead of staying spread out indefinitely.

Resampling and Jitter

Once every particle has a weight, the filter resamples: it draws a new set of 3,000 particles from the old set, with particles more likely to be picked the higher their weight. This is what actually collapses the cloud toward the true position over time.

def resample(particles, weights):
    probabilities = weights / np.sum(weights)
    idx = np.random.choice(NUM_PARTICLES, size=NUM_PARTICLES, p=probabilities)
    return particles[idx, :]

Left alone, resampling alone would eventually collapse every particle onto a single duplicated point, losing the ability to recover if that point turns out wrong. A small amount of noise gets added back in afterward to keep the particle cloud diverse enough to adapt:

def add_noise(particles):
    SIGMA_PARTICLE_STEP = 2.0
    SIGMA_PARTICLE_TURN = np.pi / 24.0
    particles[:, 0] += np.random.normal(0, SIGMA_PARTICLE_STEP, NUM_PARTICLES)
    particles[:, 1] += np.random.normal(0, SIGMA_PARTICLE_STEP, NUM_PARTICLES)
    particles[:, 2] += np.random.normal(0, SIGMA_PARTICLE_TURN, NUM_PARTICLES)
    particles[:, 0] = np.clip(particles[:, 0], 0.0, WIDTH - 1)
    particles[:, 1] = np.clip(particles[:, 1], 0.0, HEIGHT - 1)
    return particles

Tying It Together

The main loop is straightforward once the pieces above exist: move the robot, propagate the particles the same way, and only run the sense → weigh → resample → jitter cycle when the robot actually moved forward (a pure turn doesn't give the sensor anything new to check against).

if fwd != 0.0:
    robot_sensor = sense(rx, ry, noisy=True)
    weights = compute_weights(particles, robot_sensor)
    particles = resample(particles, weights)
    particles = add_noise(particles)

On screen, the true robot position is drawn in green, the filter's best guess (the mean of all particles) in red, and the individual particles themselves in blue when toggled on with P. Watching the blue cloud shrink and chase the green dot around the map is honestly the most satisfying part of the whole project.

Why This Matters

Particle filters show up constantly in real robotics, GPS-denied indoor navigation, SLAM front-ends, sensor fusion pipelines. Building one from scratch, even in a toy 2D simulator, makes the tradeoffs concrete in a way that just reading the theory doesn't: how much noise to inject, how aggressively to sharpen the weight distribution, when resampling helps versus when it collapses diversity too fast.

Try It Yourself

Full code is open-source under the MIT License on GitHub: Sharkyy-eng/robot-localization-sim.

Controls: arrow keys or W/A/D to drive, P to toggle the particle cloud visualization, Q or Esc to quit. Try swapping in your own map image, or tune NUM_PARTICLES and the sigma values to see how convergence speed changes.