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