Problem 601691 · medium · Robotics · Level 06 Estimation and Autonomy

The Hospital Runner in the Long Corridor

particle filter · Monte Carlo localization · resampling · effective sample size · Gaussian likelihood

A delivery robot in a hospital drives down a long straight corridor. Its position is one number x (metres along the corridor), and the corridor ends in a wall at x = wall. A sonar facing the wall reads the distance to it, wall - x, with Gaussian noise of standard deviation sigma.

The robot localises itself with a particle filter: particles is a list of guesses of x, all starting with the same weight 1/n. Each entry of steps is a tuple (move, noise, z, u), and one cycle does, in this order:

  1. Move. Particle i moves by move + noise[i] (the noise draws are given, one per particle).
  2. Weigh. Multiply each weight by the likelihood \exp\!\left(-\frac{(z - (\text{wall} - x_i))^2}{2\sigma^2}\right) of the reading z, then divide all weights by their sum.
  3. Estimate. Record the weighted mean \sum_i w_i x_i.
  4. Resample, if needed. Compute the effective sample size N_\text{eff} = 1 / \sum_i w_i^2. If it is strictly less than n / 2, resample with the offset u: with running sums C_i = w_0 + \dots + w_i, the tooth p_k = (u + k)/n for k = 0, \dots, n-1 picks the first particle i with C_i > p_k (if rounding leaves no such i, the last particle with a positive weight). The picked positions, in the order of k, become the new particles, and every weight is reset to 1/n. Otherwise the particles and their weights carry over unchanged.

Write corridor_filter(particles, wall, sigma, steps) that returns a tuple (estimates, resampled): the list of recorded estimates, one per step, and the list of the step indices (counting from 0) at which the filter resampled.

Examples

Input:  particles = [0.0, 1.0, 2.0, 3.0], wall = 10.0, sigma = 0.5,
        steps = [(1.0, [0.0, 0.1, -0.1, 0.0], 7.2, 0.5), (1.0, [0.05, 0.0, -0.05, 0.1], 6.1, 0.25)]
Output: ([2.7291688511972234, 3.840839946847217], [0])
Explanation: after the first move the particles are at 1, 2.1, 2.9 and 4. The reading 7.2 fits
2.9 best (it expects 7.1): its weight is 0.694, N_eff = 1.81 < 2, so the filter resamples
to [2.1, 2.9, 2.9, 2.9]. After the second cycle N_eff is 3.56: no resampling.

Input:  the same, with sigma = 2.0
Output: ([2.5801099256398845, 3.6626864343373824], [])
Explanation: a vaguer sonar spreads the weight more evenly and the filter never resamples.

Constraints

  • noise has one entry per particle in every step; 0 <= u <= 0.999; sigma > 0
  • no reading is so far off that every weight becomes 0 (single weights may come out as exactly 0)
  • answers are compared with a tolerance of 1e-6; do not round

Goals

  • Run whole particle filter cycles: move, weigh, estimate, and resample when needed
  • Resample only when the effective sample size drops below half the particles
  • Keep the weights from one cycle to the next when no resampling happens
Starting Python…