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

One Answer From the Whole Cloud

particle filter · weighted mean · circular mean · atan2 · angle wrapping

A particle filter describes a robot's pose by many weighted guesses, the particles (x, y, theta) (metres, x to the right, y up; heading in radians, counter-clockwise from +x). The robot's planner wants one pose. Take the weighted mean: the position is

\bar{x} = \frac{\sum_i w_i x_i}{\sum_i w_i}, \qquad \bar{y} = \frac{\sum_i w_i y_i}{\sum_i w_i}.

The heading needs more care, because angles go round in a circle: headings of 3.0 and -3.0 are both nearly \pi (facing -x), yet their plain average is 0 (facing +x). Instead, think of each heading as an arrow of length w_i pointing in direction \theta_i, add up the arrows, and take the direction of the sum. That direction is the mean heading, reported wrapped to (-\pi, \pi] (exactly -\pi is reported as \pi).

If the arrows cancel out, there is no mean heading: when the length of the summed arrow is less than 1e-9 times \sum_i w_i, report the heading as None.

Write mean_pose(particles, weights) that returns the tuple (x, y, theta). The weights are not necessarily normalised.

Examples

Input:  particles = [(1, 2, 3.0), (3, 2, -3.0)], weights = [1, 1]
Output: (2.0, 2.0, 3.141592653589793)
Explanation: both particles face almost straight along -x, so their mean does too.

Input:  particles = [(0, 0, 0.0), (2, 4, math.pi / 2)], weights = [3, 1]
Output: (0.5, 1.0, 0.3217505543966422)
Explanation: three arrows' worth along +x and one along +y add up to (3, 1),
whose direction is atan2(1, 3) = 0.3218 rad, not the plain weighted average 0.3927.

Input:  particles = [(1, 1, 0.0), (3, 1, math.pi)], weights = [0.5, 0.5]
Output: (2.0, 1.0, None)
Explanation: two equal arrows in opposite directions cancel out.

Constraints

  • weights has one entry per particle, each at least 0, with a positive sum
  • every heading in the input is in (-\pi, \pi]
  • answers are compared with a tolerance of 1e-6; do not round

Goals

  • Turn a weighted particle cloud into one pose estimate
  • Average headings on the circle, through their cosines and sines, not as plain numbers
  • Recognise when the headings cancel out and no average heading exists
Starting Python…