The drift that never stops

Remember lesson 5? The wheels slipped, the encoders never noticed, and your dead reckoning peeled away from the truth. Back then you could only watch it happen. In lesson 13 you learned the counter-move: pull your belief toward a landmark fix. Case closed?

Not quite. The slip is back, for good this time, and your kidnapped robot trick has a blind spot. Slip does not shift where you are so much as it bends which way you face: the right wheel quietly underdelivers, the heading drifts, and the encoders swear everything is fine. Press Run with the starter and watch the error grow even though the position correction from lesson 13 is already in there, running every tick.

The trap is subtle and worth seeing clearly: to work out where the fix implies you are, you walk the range and bearing backward using your own heading. If the heading is wrong, the implied spot is wrong by the same twist, so the pull drags you toward a lie. A position nudge cannot repair a heading.

Here is the strange part: tower A cannot even see the lie. Twist your heading by some angle and swing your believed position around the tower by the same angle, and every range and bearing it sends still checks out perfectly. One landmark pins you to a family of rotated stories, all self-consistent. That is why the mat has a second tower: rotating around A moves you relative to B, so B notices what A never can.

So use B as your compass. You know where it truly sits, so from your (corrected) position you can compute the world angle it should appear at: math.atan2(by - y, bx - x). Your belief makes its own claim: tower B sits at theta + bear_b. If those two angles disagree, your heading is lying by the difference.

Then nudge it: wrap the disagreement into the -pi..pi range (the atan2(sin, cos) trick keeps a 359 degree error from reading as a huge turn) and pull theta a fraction h_gain toward the truth, every tick, exactly like the position pull. Small, steady, relentless: that is what beats a small, steady, relentless drift.

With the heading correction in, Run again and watch the difference: the slip never stops, but now every fix repairs a little of what the last two ticks of drift broke. The belief does not just lock on, it stays locked while the wheels keep lying, which is the real test of an estimator: not a snapshot, a fight it keeps winning.

If the steady checkpoint will not latch, tune in the code and re-run. Too little h_gain and the drift outruns the repair; too much and the noisy bearing (about 8 degrees of jitter per fix) shakes your heading around. Somewhere in between the belief goes quiet. This class is a keeper: pass the lesson and Estimator joins your mybot library, position and heading both under repair.