Problem 641782 · easy · Robotics · Level 06 Estimation and Autonomy

Which Guesses Agree With the Beacon?

particle filter · importance weights · Gaussian likelihood · normalisation · effective sample size

A robot keeps a particle filter: a list of guesses of its pose (x, y, theta) (metres, x to the right, y up; heading in radians), the particles, each with a weight saying how much the filter believes it. A radio beacon stands at the known spot landmark = (lx, ly), and the robot measures its distance to it: the reading is z metres, with Gaussian noise of standard deviation sigma.

A particle at distance d_i from the beacon would expect to read d_i. The likelihood of the actual reading for that particle is

L_i = \exp\!\left(-\frac{(z - d_i)^2}{2\sigma^2}\right)

(the usual factor \frac{1}{\sigma\sqrt{2\pi}} is the same for every particle, so it can be left out). The new weight of particle i is its old weight times L_i, and then all new weights are divided by their sum so that they add up to 1. The heading plays no part: a range reading does not depend on which way the robot faces.

When a few particles take almost all the weight, the cloud is really smaller than it looks. The effective sample size N_\text{eff} = 1 / \sum_i w_i^2 (with the normalised weights) counts how many particles still matter: it equals the number of particles when all weights are equal, and 1 when one particle has all of it.

Write weigh_particles(particles, weights, landmark, z, sigma) that returns a tuple (new_weights, n_eff): the list of normalised weights, in the particles' order, and the effective sample size.

Examples

Input:  particles = [(1, 0, 0), (0, 0, 0), (2, 0, 1.0)], weights = [1/3, 1/3, 1/3],
        landmark = (4, 0), z = 3.0, sigma = 0.5
Output: ([0.7869860421615984, 0.10650697891920075, 0.10650697891920075], 1.5575486897936257)
Explanation: the particles are 3, 4 and 2 m from the beacon. The first explains the reading
exactly (L = 1); the others are 1 m = 2 sigma off, L = exp(-2) = 0.1353.

Input:  particles = [(0, 0, 0), (1, 1, 0), (2, 2, 0), (3, 3, 0)], weights = [0.1, 0.2, 0.3, 0.4],
        landmark = (0, 3), z = 2.5, sigma = 1.0
Output: ([0.09549455507019204, 0.20901088985961594, 0.31351633478942387, 0.38197822028076817], 3.3669510480233638)
Explanation: the first and last particles are both 3 m away, so they get the same likelihood,
but the last one keeps four times the weight because it started with four times as much.

Constraints

  • weights has one positive entry per particle and they add up to 1 (within 1e-9); sigma > 0
  • every particle's expected range is within 20 * sigma of z, so no likelihood is zero
  • answers are compared with a tolerance of 1e-6; do not round

Goals

  • Weight each particle by how well it explains a range reading, with a Gaussian likelihood
  • Multiply by the old weights and normalise so the weights add up to 1
  • Measure how many particles still count with the effective sample size $1/\sum w_i^2$
Starting Python…