Carry partial encoder ticks between wheel odometry samples

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,

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");