Skip to content

FusionLocalizer: GNSS fixes in east-north-up maps and from the antenna's mounting - #167

Merged
fmrico merged 3 commits into
rollingfrom
fusion_gps_init
Nov 3, 2026
Merged

fmrico merged 3 commits into
rollingfrom
fusion_gps_init

Conversation

@fmrico

@fmrico fmrico commented Nov 1, 2026

Copy link
Copy Markdown
Contributor

Closes #166.

  • projection parameter: utm (default, as before) or enu, east-north-up at the origin (GeographicLib LocalCartesian), for maps defined that way.
  • The first fix places the robot and not the antenna: the antenna's mounting (TF from the filter's base_link_frame to the fix's frame) is turned with the IMU's attitude and removed. Later fixes carry the IMU's attitude, so the filter removes the mounting in the right direction. Without IMU or without that TF, as before.
  • README: the GNSS parameters and the antenna.
  • Tests for both projections, an unknown projection, the antenna at the first and later fixes, without its TF or without IMU, and the fix of the estimate published back.

The projection parameter chooses how fixes become map coordinates: utm
(default, as before) or enu, east-north-up at the origin, as Gazebo's
spherical_coordinates and most local maps define it. Through UTM, a map
defined east-north-up was off by the grid's convergence: 1.9 m at 300 m
from the origin at Sonoma Raceway.

The first fix placed the robot at the antenna, and later fixes reached the
filter with an identity orientation, so it removed the antenna's mounting as
if the robot faced east. Now the first fix removes the mounting turned with
the IMU's attitude, and every fix carries that attitude.

Signed-off-by: Francisco Martín Rico <fmrico@gmail.com>
@midemig

midemig commented Nov 3, 2026

Copy link
Copy Markdown
Contributor

Two things I’d change before merging:

  1. The IMU’s mounting (TF) is not applied.
    update_rt uses IMUPerception.data.orientation as is, and every fix carries it. The IMU perception handler stores the raw message, so that orientation is imu_link’s, not base_link’s. With a mounted-turned IMU, the antenna offset is rotated with the wrong heading, on the first fix and on all later ones.

  2. A null IMU quaternion gives NaN.
    An IMU with no orientation (orientation_covariance[0] = -1, quaternion 0,0,0,0) makes q.normalized() in antenna_to_robot return NaN. The first fix is NaN and the filter stays poisoned afterwards.

… NaN without orientation

The IMU's orientation is its frame's: the robot's attitude now removes the IMU's mounting (TF).
An IMU without orientation (covariance -1, null or non-finite quaternion) or without a known
mounting gives no attitude instead of a NaN first fix; later fixes then use the estimated
heading. Later fixes carry the antenna frame's orientation, which is what the filter expects.

Signed-off-by: Francisco Martín Rico <fmrico@gmail.com>
@fmrico

fmrico commented Nov 3, 2026

Copy link
Copy Markdown
Contributor Author

Thanks, both were right.

  1. The IMU's orientation is its frame's: the robot's attitude now removes the IMU's mounting (TF from base_link_frame to the IMU's header.frame_id). Later fixes also carry the antenna frame's orientation, which is what robot_localization uses to remove the antenna's mounting.
  2. An IMU without orientation (orientation_covariance[0] == -1, a null or non-finite quaternion) or whose mounting is not in TF gives no attitude: the first fix places the robot at the antenna, with a warning, and later fixes use the filter's estimated heading. No more NaN.

d457a29, with 7 new tests (IMU and antenna mounted turned, no orientation, null/NaN quaternion, unknown mounting, no IMU, IMU appearing later). In the Prius playground with the IMU turned 90° and the antenna 1.5 m ahead, the error against ground truth was 4.4 m with a 90° heading error before, and is 0.16 m mean while driving now.

@fmrico
fmrico merged commit 2dd7206 into rolling Nov 3, 2026
1 check passed
@fmrico
fmrico deleted the fusion_gps_init branch November 3, 2026 12:10
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

FusionLocalizer: GNSS fixes off by meters in east-north-up maps and at the antenna

2 participants