Particle filter

What it computes

A particle filter holds its belief about the pose as $N$ weighted samples, particles $(x, y, \theta)$. Unlike a Kalman filter's single Gaussian, a cloud can say "in one of these three corridors", so it can find a robot that starts lost. On a known map this is Monte Carlo localisation.

One step

Move each particle by the odometry $u = (dx, dy, d\theta)$ plus its own noise, in the particle's frame. The noise spreads the cloud as the robot grows less sure.

Weigh it with the likelihood field: place the scan's hit points in the world as seen from the particle, and look up each one's distance $d_{ij}$ to the nearest wall. From the right pose, every hit lands on a wall.

Resample once few particles carry the weight ($N_\text{eff} = 1/\sum_i w_i^2 < N/2$), then reset every $w_i$ to $1/N$. Low-variance resampling draws one $r \in [0, 1/N)$ and lays pointers $U_k = r + k/N$ along the running sum of the weights. Each pointer picks the particle whose slice it lands in, so particle $i$ is copied $\lfloor N w_i \rfloor$ or $\lceil N w_i \rceil$ times.

u = odom + noise                        # (N, 3): one draw per particle
x += cos(th) * u[0] - sin(th) * u[1]    # in each particle's own frame
y += sin(th) * u[0] + cos(th) * u[1]
th += u[2]
hits = (x, y) + R(th) @ scan            # (N, M, 2): the scan from each particle
weights *= exp(-sum(wall_distance(hits) ** 2) / (2 * SIGMA ** 2))
weights /= sum(weights)
if 1 / sum(weights ** 2) < N / 2:
    particles = low_variance_resample(particles, weights, uniform(0, 1 / N))
    weights = 1 / N

The program

The rover wanders the maze from an unknown spot. Finding its heading as well would take far more particles in a maze this regular. Real robots nearly always have a compass or IMU, so the particles start facing the compass heading. The first step uses 20,000 particles; after it, 1,000 are enough.

Tools

• wall_distance(points): the likelihood field (m) for points of shape (…, 2).
• SIGMA (lidar) and MOTION_SIGMA (motion noise per step).

Your task

Implement low_variance_resample and pf_step. The grader checks your resampling exactly with fixed $r$, and your motion and weights against a reference. Then your estimate must stay within 2 cm of the rover from 10 s on.