Every Particle Takes the Step, Give or Take

A particle filter keeps track of a robot's pose not as one guess but as a cloud of many guesses, the particles. Each particle is a pose (x, y, theta): a position in metres (x to the right, y up) and a heading in radians, counter-clockwise from the +x axis.

When the wheel odometry reports a motion, every particle makes that motion too, but each with its own small error, because the real wheels slip by an unknown amount. Here the odometry reports one step: first turn on the spot by turn radians, then drive dist metres straight ahead along the new heading. The errors are given to you, one pair per particle, so nothing is random: particle i turns by turn + turn_noise[i] and then drives dist + dist_noise[i]. Driving d along heading \theta moves a particle by (d\cos\theta,\ d\sin\theta).

Write move_particles(particles, turn, dist, turn_noise, dist_noise) that returns the list of moved particles, in the same order, each a tuple (x, y, theta) with theta wrapped to (-\pi, \pi] (whole turns of 2\pi added or subtracted; exactly -\pi is reported as \pi). Collect the positions and press Run with plot(xs, ys, kind="scatter") to see the cloud before and after.

Examples

Input:  particles = [(0, 0, 0), (0, 0, 0)], turn = 0.0, dist = 1.0,
        turn_noise = [0.0, 0.1], dist_noise = [0.0, -0.2]
Output: [(1.0, 0.0, 0.0), (0.7960033322224207, 0.07986673331746252, 0.1)]
Explanation: the second particle turns by 0.1 rad first, then drives 0.8 m along heading 0.1.

Input:  particles = [(3, 3, 3.0)], turn = 0.3, dist = 0.5, turn_noise = [0.0], dist_noise = [0.0]
Output: [(2.5062601150455674, 2.921127152928376, -2.9831853071795864)]
Explanation: the heading becomes 3.3 rad, reported as 3.3 - 2*pi; the drive goes 0.5 m along 3.3 rad.

Constraints

• turn_noise and dist_noise have one entry per particle
• answers are compared with a tolerance of 1e-6; do not round

Goals

• Move every particle of a particle filter by the same odometry plus its own noise draw
• Apply a turn-then-drive step in the right order: the drive uses the new heading
• Wrap each particle's heading into $(-\pi, \pi]$