From 807e7b39ceec3443a04388a73dce5f64c954fc54 Mon Sep 17 00:00:00 2001 From: Cam Mazzacane Date: Thu, 15 Oct 2026 18:39:30 -0400 Subject: [PATCH] Report GNSS fixes at the antenna position GNSS fixes used the base_link position and ignored the sensor's mount pose. A real receiver reports the antenna position, so an antenna mounted away from base_link gives a heading-dependent offset. The fix now adds the mount offset, rotated by the vehicle orientation, to the base_link position. The default GNSS mount is 0.5 m above base_link, so default fixes report 0.5 m more altitude. Co-Authored-By: Claude Opus 5.5 --- app/autonomy/LocalizationMeasurements.js | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/app/autonomy/LocalizationMeasurements.js b/app/autonomy/LocalizationMeasurements.js index d5156419..5f282478 100644 --- a/app/autonomy/LocalizationMeasurements.js +++ b/app/autonomy/LocalizationMeasurements.js @@ -270,10 +270,12 @@ export function buildImuMeasurement(snapshot, sensorConfig, rng, state = {}) { export function buildGnssMeasurement(snapshot, sensorConfig, rng, state = {}) { const faults = sensorConfig.calibration.faults || {}; const datum = sensorConfig.calibration.datum || { latitude: 42.4430, longitude: -76.4840, altitude: 200 }; + // The fix is the antenna position: base_link plus the mount offset rotated into the map frame. + const antennaOffset = rotateVectorByQuaternion(vec3(sensorConfig.pose?.position), snapshot.orientation); const positionEnu = { - east: snapshot.pose.position.x, - north: snapshot.pose.position.y, - up: snapshot.pose.position.z, + east: snapshot.pose.position.x + antennaOffset.x, + north: snapshot.pose.position.y + antennaOffset.y, + up: snapshot.pose.position.z + antennaOffset.z, }; const dt = state.lastCaptureTimeNs != null && snapshot.captureTimeNs > state.lastCaptureTimeNs ? (snapshot.captureTimeNs - state.lastCaptureTimeNs) / 1e9