drv/pose_estimator: add an EKF on odometry and position-only LH2 fixes

[geonnave]

Sits directly on main now that #32 has merged, so the diff below is this PR alone. The app side is DotBots/DotBot-firmware#426. The single-target steering is stacked on this one as #34.

Adds drv/pose_estimator, an extended Kalman filter on [x, y, heading] of the wheel-axle midpoint, fed by wheel odometry and LH2 fixes, plus the effective-track model it and the wheel loop's twist mixing now share. No hardware calls, so it builds on the host for its tests.

Design

• Predict on every 10 ms scheduler tick from the encoder deltas, over the effective track (below). Process noise grows with distance travelled, never with the call rate: position variance per mm the axle midpoint travels, and heading variance per mm travelled plus a turning term per mm of |d_right - d_left|, since slip is dominated by turning. Above a reference wheel-speed difference (250 mm/s, a spin at 125 mm/s per wheel) the turning term scales up in proportion.
• Update once per new LH2 fix, with a position-only measurement of the photodiode. The photodiode sits a lever arm ahead of the axle (51.5 mm, fitted to spins in place; the board figure stays in DB_LH2_LEVER_ARM), and that offset is what makes heading observable when the robot turns in place. The caller feeds each fix sequence once; a fix applied twice would shrink the covariance with no new information.
• Fix-age compensation. A fix is some ticks old when it arrives (2 ticks, measured on the floor; up to 8 supported). The estimator keeps the odometry of the last predicts in every state and moves each fix forward by the photodiode travel over its age before gating it. The seed chain consumes each odometry step only once it leaves that age window, so its odometry spans the capture times of its fixes, and a seeded pose is carried forward over the window to the present. Without that, a pose seeded while moving was the one at capture: 6 mm behind at 300 mm/s, 5.6 degrees behind in a 200 mm/s spin.
• Covariance update in Joseph form, P = (I - K H) P (I - K H)^T + K R K^T. On a robot at rest the fixes shrink P toward rank 1, and the plain P - K H P update let it go indefinite in float32 (25 of 36,000 updates over a simulated hour at rest).
• Gate. A fix whose squared Mahalanobis distance exceeds 23.0 (chi-square, 2 degrees of freedom, 99.999 %, about 14 to 18 mm per axis while tracking) is rejected. 13.8 (99.9 %) locked out the fixes that would correct a 40 degree heading error.
• Timeout and reseed. With no accepted fix for 1 s, TRACKING becomes LOST. Predict carries on; a fix inside the gate returns to TRACKING, and a fresh consistent chain of fixes (below) reseeds the whole pose. Heading and pose are only reported while TRACKING.
• Kidnap recovery. A robot picked up or moved by hand used to wait out the 1 s timeout and then need a consistent chain, about 1.3 s before its position followed. Now, while TRACKING, 3 rejected fixes in a row (DB_POSE_ESTIMATOR_KIDNAP_FIXES, 0.3 s at 10 Hz) that agree within the 20 mm seed tolerance, with at most 2 mm of summed wheel travel (DB_POSE_ESTIMATOR_KIDNAP_STILL_MM, |d_left| + |d_right|) since the first of them, count as a kidnap: the estimator returns to SEEDING at once with those fixes as the start of its seed chain. Heading stays unknown (the advert sends -1000 and falls back to the raw solve for position) until motion re-acquires it through the usual chain. Rejected fixes while the wheels turn keep the old path: hold the pose until the timeout, then reseed. A kidnaps counter joins the others for the debugger. kidnap_fixes = 0 disables it.
• A hard stop is not a kidnap. After a 500 to 700 mm/s sprint stops hard, the wheels slip or skid (more in reverse, and with some unseen yaw), so the estimate ends 25 to 45 mm off fixes that are steady while the wheels stand. That tripped the kidnap rule and threw the heading away. Two changes fix it. Process noise now also grows with each change of wheel speed beyond a deadband (the speed filtered with a 30 ms time constant; 0.06 mm^2 of position and 0.012 deg^2 of heading per mm/s), so a stop from 600 mm/s widens the gate enough to take its own slip. And a kidnap now also needs the wheels to have stood for 0.5 s before the first rejected fix (kidnap_settle_ticks). Consistent rejected fixes that arrive sooner after driving, within 60 mm of the estimate (reanchor_mm), re-anchor instead: the position moves onto the fix, the heading is kept and its variance raised by (5 deg)^2, and a reanchors counter records it. A larger jump right after driving is still a kidnap. Zero values keep the old behaviour.

Passive initial-heading acquisition

The estimator starts in SEEDING with no pose and needs no calibration manoeuvre. It builds a chain of fixes and, alongside it, the axle travel and rotation odometry saw since the chain's first fix, in that fix's body frame. A fix joins the chain when its distance from the first fix agrees with the photodiode travel odometry predicts to within 20 mm, otherwise the chain restarts. Once the chain holds 3 consistent fixes and the photodiode has moved at least 40 mm in the body frame, the heading is solved from the chain and the pose is seeded, with a heading variance from the fix noise over that baseline plus what odometry added along the chain. Driving normally, or spinning in place thanks to the lever arm, is enough.

Radius-dependent effective track

On carpet the robot rotates less than the 78 mm geometric track predicts. db_track_effective_mm(left, right) returns 81 mm for a spin in place, rising linearly with |left + right| / |right - left| to 85 mm for arcs of 100 mm radius and wider, and held there. Only the ratio matters, so it takes speeds or distances. The estimator's predict and db_wheel_control_from_twist() both use it; the twist mixing resolves the track/radius dependency with a short fixed-point iteration. Spin speed is not modelled (the in-place figure reads 83 at 200 mm/s per wheel and 87 at 300). v1/v2 fall back to their DB_TRACK, so nothing changes for them.

Validation and status

• Host tests (make test): 106 pose-estimator checks (including sprints at 500 and 600 mm/s, forward and reverse, stopping with 30 to 45 mm of odometry over-count and up to 25 mm sideways that keep tracking, keep the heading and sit on the fixes 0.5 s later; the same stop re-anchoring with the slip noise off, and reading as a kidnap with the guard off too; a 150 mm move right after driving still a kidnap; a hand-move with the wheels still reseeding within 3 fixes, outliers and scattered rejected fixes while still not reseeding, rejected fixes while driving and a carry longer than the timeout both taking the timeout path, a wild fix breaking a seed chain, the seeded pose matching the present one at speed, and P staying symmetric positive definite over an hour at rest) and 154 wheel-control checks (the 150 that drv/wheel_control: add a per-wheel speed loop, brake to zero, and correct the v3 distance per count #32 brought to main, whose suite runs on two gain sets, plus 2 per set for the effective-track mixing), all passing. make check-format clean with clang-format 18.
• Replay of the floor data through this C file with the slip fix: the 700 mm/s hard-stop runs, which gave 3 false kidnaps (14 to 32 mm across the track, 8 to 10 mm along it), now give none, with no fix rejected on one robot. The one remaining kidnap in that capture is a 240 mm jump. Every other recorded block (straights at 100 to 600 mm/s, spins, arcs, full turns, about 15,000 fixes) is unchanged, with no rejections, and the genuine hand-move captures keep their kidnap and timeout outcomes. Not re-run as a hard-stop sprint on the floor since; the steering floor tests of drv/steering: add a single-target outer loop over the wheel speeds #34 (about 4,700 advertisements, starts and stops at up to 300 mm/s) showed no spontaneous heading loss with it.
• Floor test on two v3 robots, run before the seed carry-forward and the Joseph-form update, which have host tests only so far (untethered, dev-corner carpet, one LH2 station, telemetry build, OTA; about 11,000 fixes on straights, spins, arcs and brake legs, 0 rejected, squared distance at most 12 against the gate of 23). Rejection counts come from replaying this same C file on the recorded encoder counts and fixes, which matches the advertised heading within 1 degree.
  • Acquisition: from a straight after 40 to 44 mm (error within 2.5 degrees at that moment); from a spin in place after 46 to 73 degrees of rotation (within 4.3 degrees). After one full turn at 200 mm/s per wheel, the heading is within 1.3 degrees of LH2 truth at rest.
  • While tracking: heading at rest before and after each manoeuvre within 2 degrees on average (worst 3.8); position within 2 to 3 mm RMS of the fixes at 100 to 200 mm/s, about 6 mm at 300.
  • Kidnap: a slow carry is detected after 3 rejected fixes (about 0.4 s from the first displaced fix). A fast carry, with consecutive fixes more than 20 mm apart, is caught by the 1 s timeout instead. Either way the advert follows the raw fix for the rest of the carry and is right at touchdown, and the heading re-acquires within 44 mm of driving.
  • Fix age: measured against the encoders on straights at 100 to 600 mm/s, the fix is 1 to 4 ticks old, about 2 on average, and longer driving one way across the station than the other. At the original 4 ticks the estimate ran 42 ms ahead of the raw fix and about 7 mm ahead of truth at 300 mm/s. At 2 ticks it runs 22 ms ahead of the raw fix and within about 1 mm of truth on average (from 4.6 mm ahead to 2.2 mm behind per run).
• CI: all checks pass (the build matrix, host-tests, style, doc).
• The constants are provisional. The process-noise terms, the turn reference speed and the gate carry TODOs in drv/pose_estimator.h naming what they were sized from and what measurement would settle them.

File	+	-
tests/test_pose_estimator.c	+735	-0
drv/pose_estimator/pose_estimator.c	+472	-0
drv/pose_estimator.h	+304	-0
drv/geometry.h	+50	-6
6 small files: Makefile, doc/sphinx/drv.md, drv/drv.emProject, drv/wheel_control.h, drv/wheel_control/wheel_control.c, tests/test_wheel_control.c	+35	-5
Total, 10 files	+1596	-11

Merge chain

Merged on 2026-09-25: #32, DotBots/PyDotBot#298 and DotBots/DotBot-firmware#425. The rest go one PR at a time, in this order: this PR, DotBots/DotBot-firmware#426, #34, DotBots/DotBot-firmware#427, #35, DotBots/DotBot-firmware#428, DotBots/PyDotBot#301, DotBots/PyDotBot#302, #36, DotBots/DotBot-firmware#429, DotBots/PyDotBot#303. Next to merge overall: this PR. Next after this one: DotBots/DotBot-firmware#426. Before each firmware PR merges, its dotbot-libs submodule is re-bumped to the DotBot-libs main commit its libs PR lands as, as DotBots/DotBot-firmware#425 was re-bumped to caa788b after #32.