Why odometry isn't enough

Dead reckoning

The rover's wheels count how far it drove and how far it turned. Adding those increments up gives odometry, a pose $[x, y, \theta]$ that needs no other sensor. Between two readings the wheels report an increment $u = [\Delta s, \Delta\theta]$, and the rover moves along its average heading $\bar\theta = \theta + \Delta\theta/2$: turn half, drive, turn the other half.

$$x \leftarrow x + \Delta s\cos\bar\theta, \qquad y \leftarrow y + \Delta s\sin\bar\theta, \qquad \theta \leftarrow \theta + \Delta\theta$$

Every increment is a little wrong

Wheels slip, so each increment misses the real motion by a random amount. The errors add up like a random walk, so their variance grows in proportion to the motion:

$$\Delta s' = \Delta s + \sigma_t\sqrt{|\Delta s|}\;\varepsilon_1, \qquad \Delta\theta' = \Delta\theta + \sqrt{\sigma_r^2|\Delta\theta| + \sigma_d^2|\Delta s|}\;\varepsilon_2$$

$\varepsilon_1$ and $\varepsilon_2$ are independent standard normal numbers (np.random.randn). $\sigma_t$ is the distance noise, $\sigma_r$ the turning noise and $\sigma_d$ the heading drift per metre; the program reads them from rover.config into NOISE $= [\sigma_t, \sigma_r, \sigma_d]$.

A cloud of possible rovers

One noisy draw is one trip the rover might have made. Move 200 copies of the start pose, each with its own draw, and the cloud shows everywhere the rover could be. It grows as you drive, and small heading errors swing the position sideways, so it curves into a banana.

The program

The rover drives one lap of the loop, steering itself. Ten times a second your cloud moves by the odometry increment. Every half metre the cloud is drawn in yellow and the true rover in green; raw odometry is grey. The Plots tab shows the cloud's size and the odometry's real error against distance.

Your task

1. Write move(poses, ds, dth). poses is an (N, 3) array; ds and dth are numbers or (N,) arrays. Return the moved poses, with θ wrapped by rover.wrap.
2. Write sample_motion(poses, u, noise): draw $\Delta s'$ and $\Delta\theta'$ for every pose, then move them.

The grader checks both on fixed cases, then checks that the true rover ends inside your cloud. Nothing here looks at the world, so the cloud can only grow. The rest of the course fixes that.