Problem 474648 · medium · Robotics · Level 04 Belief and Optimisation

Two Metres to the Trolley

histogram localization · range sensor · Gaussian noise · likelihood · sense update

A hospital corridor is cut into n cells, numbered 0 to n - 1 from left to right. The map row has row[i] = 1 where a cell is blocked (a parked trolley, a cupboard) and 0 where it is free. Just past cell n - 1 the corridor ends in a solid wall, which counts as a blocked cell at index n.

A robot stands in some free cell, facing right (towards larger indices). Its belief is the list belief of probabilities (they add up to 1, and every blocked cell has probability 0). A laser rangefinder measures z, the distance in cells to the first blocked cell ahead of it. From cell i, the expected distance is j - i, where j > i is the index of the first blocked cell to the right (or n, the end wall, if there is none).

The rangefinder is noisy: from a cell whose expected distance is d, the reading z has the Gaussian (normal) density

p(z \mid i) = \frac{1}{\sigma\sqrt{2\pi}} \exp\!\left(-\frac{(z - d)^2}{2\sigma^2}\right)

with standard deviation \sigma = sigma cells.

Write range_update(belief, row, z, sigma) that returns the belief after the reading (Bayes' rule, then normalised to add up to 1), as a list in cell order. Blocked cells stay at 0.

Examples

Input:  belief = [1/7, 1/7, 1/7, 0, 1/7, 1/7, 1/7, 1/7], row = [0, 0, 0, 1, 0, 0, 0, 0],
        z = 2.2, sigma = 0.5
Output: [0.11050281, 0.36688226, 0.02231013, 0.0, 0.0006096, 0.11050281, 0.36688226, 0.02231013]
Explanation: the expected distances are 3, 2, 1 (to the trolley in cell 3) and
4, 3, 2, 1 (to the end wall at 8). Cells 1 and 6 both expect 2 and stay tied.

Input:  belief = [1/3, 1/3, 1/3], row = [0, 0, 0], z = 1.4, sigma = 0.3
Output: [0.00000122, 0.2476635, 0.75233528]
Explanation: with nothing in the way, the end wall is 3, 2 and 1 cells away.

Constraints

  • z is within 6 * sigma of the expected distance of at least one cell with a positive belief, so the normalising sum is never 0
  • answers are compared with a tolerance of 1e-6; do not round

Goals

  • Compute the distance a range sensor should read from every cell of a map
  • Turn the gap between expected and measured distance into a Gaussian likelihood
  • Use the likelihoods in a sense update, and see why a range reading can leave two places tied
Starting Python…