From 7ea694c5e95f77dbf3d4767659c2f528792e2261 Mon Sep 17 00:00:00 2001
From: Cam Mazzacane <clm357@cornell.edu>
Date: Thu, 15 Oct 2026 18:18:39 -0400
Subject: [PATCH] Carry partial encoder ticks between wheel odometry samples

Each sample rounded its own wheel travel to whole ticks and dropped the
remainder. The distance error then depended on speed: -8% at 0.2 m/s and
+15% at 0.04 m/s. Below half a tick per sample the odometry read zero.

The model now keeps the total travel of each wheel and quantizes that
total, as an encoder count does. The error stays below one tick.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
---
 app/autonomy/LocalizationMeasurements.js | 19 +++++++++++----
 tests/localization-sensors.test.js       | 30 ++++++++++++++++++++++++
 2 files changed, 45 insertions(+), 4 deletions(-)

diff --git a/app/autonomy/LocalizationMeasurements.js b/app/autonomy/LocalizationMeasurements.js
index d5156419..3d81ad2a 100644
--- a/app/autonomy/LocalizationMeasurements.js
+++ b/app/autonomy/LocalizationMeasurements.js
@@ -394,8 +394,15 @@ export function buildWheelOdometryMeasurement(snapshot, sensorConfig, rng, state
         calibration.trackWidth,
         calibration.slipFactor,
     );
-    const leftQuantized = quantizeTravel(leftTravel, calibration.wheelRadius, calibration.ticksPerRevolution);
-    const rightQuantized = quantizeTravel(rightTravel, calibration.wheelRadius, calibration.ticksPerRevolution);
+    // An encoder counts ticks of the total wheel rotation. Quantize the
+    // running total so fractions of a tick carry over to the next sample.
+    const tickLength = (2 * Math.PI * Math.max(Number.EPSILON, calibration.wheelRadius)) / calibration.ticksPerRevolution;
+    const leftTravelM = Number(state.leftTravelM || 0) + leftTravel;
+    const rightTravelM = Number(state.rightTravelM || 0) + rightTravel;
+    const leftTicks = Math.round(leftTravelM / tickLength);
+    const rightTicks = Math.round(rightTravelM / tickLength);
+    const leftQuantized = (leftTicks - Number(state.leftTicks || 0)) * tickLength;
+    const rightQuantized = (rightTicks - Number(state.rightTicks || 0)) * tickLength;
     const integrated = integrateWheelOdometry(
         { ...state.odometry, dt },
         leftQuantized,
@@ -432,8 +439,10 @@ export function buildWheelOdometryMeasurement(snapshot, sensorConfig, rng, state
         },
         nextState: {
             odometry: integrated,
-            leftTicks: Number(state.leftTicks || 0) + Math.round((leftQuantized / (2 * Math.PI * calibration.wheelRadius)) * calibration.ticksPerRevolution),
-            rightTicks: Number(state.rightTicks || 0) + Math.round((rightQuantized / (2 * Math.PI * calibration.wheelRadius)) * calibration.ticksPerRevolution),
+            leftTravelM,
+            rightTravelM,
+            leftTicks,
+            rightTicks,
             lastCaptureTimeNs: snapshot.captureTimeNs,
         },
     };
@@ -480,6 +489,8 @@ export function createMeasurementSeedState(sensorConfig, seed) {
         drift: { angular: { x: 0, y: 0, z: 0 }, acceleration: { x: 0, y: 0, z: 0 } },
         multipath: { x: 0, y: 0, z: 0 },
         odometry: { position: { x: 0, y: 0, z: 0 }, heading: 0 },
+        leftTravelM: 0,
+        rightTravelM: 0,
         leftTicks: 0,
         rightTicks: 0,
         inOutage: false,
diff --git a/tests/localization-sensors.test.js b/tests/localization-sensors.test.js
index fe0a249b..f0afbae5 100644
--- a/tests/localization-sensors.test.js
+++ b/tests/localization-sensors.test.js
@@ -138,6 +138,36 @@ test("encoder quantization is deterministic and slip reduces travel", () => {
     assert.ok(q1 > 0);
 });
 
+test("wheel odometry carries partial ticks so distance does not drift with speed", () => {
+    const config = normalizeRunSensor({
+        type: "wheel-odometry",
+        rateHz: 50,
+        calibration: { poseNoise: { x: 0, y: 0, z: 0 }, twistNoise: { x: 0, y: 0, z: 0 } },
+    });
+    const tickLength = (2 * Math.PI * config.calibration.wheelRadius) / config.calibration.ticksPerRevolution;
+    const stepNs = 20_000_000;
+    const captures = 1500;
+    // 0.01 m/s moves less than half a tick per sample, which rounded to zero
+    // when each sample was quantized on its own.
+    for (const speed of [0.01, 0.2, 1, 3]) {
+        const rng = new SeededRNG(`wheel-ticks-${speed}`);
+        const vehicle = mockVehicle({ velocity: { x: speed, y: 0, z: 0 } });
+        let measurementState = createMeasurementSeedState(config, rng);
+        let lastSnapshot = null;
+        let position = null;
+        for (let step = 1; step <= captures; step += 1) {
+            const snapshot = captureVehicleSnapshot(vehicle, step * stepNs, lastSnapshot, stepNs);
+            lastSnapshot = snapshot;
+            const result = buildWheelOdometryMeasurement(snapshot, config, rng.fork(`sample-${step}`), measurementState);
+            measurementState = { ...measurementState, ...result.nextState };
+            position = result.measurement.position;
+        }
+        const truth = speed * captures * (stepNs / 1e9);
+        const traveled = Math.hypot(position.x, position.y);
+        assert.ok(Math.abs(traveled - truth) <= tickLength, `${speed} m/s: traveled ${traveled} m, truth ${truth} m`);
+    }
+});
+
 test("wheel odometry integrates independently from truth pose", () => {
     const config = normalizeRunSensor({ type: "wheel-odometry", rateHz: 50 });
     const rng = new SeededRNG("wheel");