Odometry & dead reckoning

Where am I?

In step 1, rover.pose told the rover exactly where it was. Real robots have no such oracle. The oldest answer is dead reckoning: start from a known pose and add up every small move since.

The unicycle model

Drive at $(v, \omega)$ for a short time $\Delta t$. The rover turns by $\omega\,\Delta t$ and travels $v\,\Delta t$, but its heading changes during the step. The midpoint rule moves it along the heading halfway through:

Here $(x, y, \theta)$ is the pose before the step and $(x', y', \theta')$ the pose after it. Chain many small steps and you follow any drive. Using $\theta$ instead of $\theta_m$ (an Euler step) cuts every arc slightly short.

Odometry

A real rover can't trust its commands: wheels slip and motors lag. So it measures instead. Encoders count wheel turns, which give the distance and the turn of each step, and odometry feeds those measured moves through the same update. rover.odometry() returns its estimate.

Every measurement is slightly off, though, and a wheel whose size is a fraction of a percent wrong is off the same way every time. Integration adds all these errors up, and nothing ever takes them away. Heading errors hurt most: 1° off makes every later metre land 1.7 cm to the side.

This step

The simulated wheels never slip, so integrating the commands predicts the drive almost exactly. You'll predict two laps of a rounded square before the rover drives them (orange), then compare with the truth (green) and the odometry (blue), whose wheel readings carry noise and a small scale error. The Plots tab shows how far each is from the truth.

Your task

1. Write integrate(pose, v, w, dt): one midpoint step, returning [x, y, theta] with theta wrapped by rover.wrap.
2. Write dead_reckon(start, segments, dt): split each (v, w, duration) segment into round(duration / dt) steps and return every pose, starting with start.

The grader tests both on fixed cases, and wants your prediction of the square within 5 mm of the true path.