Finding a lost rover with particles

When the rover could be anywhere

Switched on somewhere in a maze, the rover could be in any corridor. One Gaussian can't say "here, or there, or over there", but a crowd of guesses can. A particle filter keeps $N$ particles: particle $i$ is a pose $[x_i, y_i, \theta_i]$ with a weight $w_i$. It runs the same predict–update loop as your histogram filter.

Predict and update

Predict: move every particle with step 1's sample_motion, so each takes its own noisy version of the trip.

Update: the lidar returns hit points $p_j$ in the rover's frame. Seen from particle $i$, a point lands in the world at

From the right pose every point lands on a wall. The likelihood field wall_distance gives each point's distance $d_{ij}$ to the nearest wall, and

with $\sigma$ = SIGMA. Log-likelihoods can be hundreds below zero, and $e^{-1000}$ is 0 in floating point, so subtract the largest $\log L$ before exponentiating: it cancels when you normalise.

Resample

Soon most particles carry almost no weight. When the effective number $N_\text{eff} = 1/\sum_i w_i^2$ falls below $N/2$, resample: copy particles in proportion to their weights, then reset every weight to $1/N$. Low-variance resampling draws one $r$ in $[0, 1/N)$ and lays $N$ evenly spaced pointers $r + k/N$ along the running sum of the weights. Each pointer picks the particle whose slice it lands in.

The program

The rover wanders a maze it knows from an unknown spot, with a lidar that sees 240° ahead. The compass gives the heading, so 10,000 particles start on free floor, all facing that way. Watch the cloud split into a few clusters, then collapse onto the rover.

Your task

Write log_likelihood(particles, points), reweight(weights, loglik), low_variance_resample(particles, weights, r) and estimate(particles, weights), the weighted mean pose. Work on every particle at once with numpy. The grader checks each on fixed cases, then that your estimate stays within 3 cm of the rover from 6 s on.