feat(imu,gnss): IMU lever arm centripetal, GPS pre-heading option, TF auto-resolve

[cedbossneo]

Two complementary additions for frame-aware sensor fusion on robots with
sensors offset from base_link, plus a ROS-wrapper improvement that lets
the URDF be the single source of truth for the geometry.

1. IMU lever arm compensation (core)
   The IMU rarely sits at the base_link origin. At offset r, the
   accelerometer reads
   a_imu = a_base + ω×(ω×r) + α×r + g_body
   Previously we dropped everything except a_base. With an IMU 30 cm
   from base_link on a robot spinning at 0.5-1.0 rad/s, the centripetal
   term contributes ~0.09-0.35 m/s² of apparent horizontal acceleration
   that the filter attributed (wrongly) to real motion or bias drift.

   • New sensors::ImuLeverArm struct and sensors::ImuParams::lever_arm.
   • New imu_measurement_function_with_lever_arm(...) factory returns
     a lambda that adds ω×(ω×r) = (ω·r)ω − (ω·ω)r to the predicted
     accel. The α×r tangential term is dropped (α not in state) and
     remains absorbed by accel_noise; negligible under typical
     ground-robot control bandwidth.
   • fusioncore.cpp selects the lever-arm-aware h() when the configured
     arm is non-zero, both for live update_imu() and the retrodiction
     replay path — so the compensation stays consistent when a delayed
     sensor forces the filter to re-apply buffered IMU frames.
   • New runtime setter FusionCore::set_imu_lever_arm() so the ROS
     wrapper can populate the arm from TF after construction without
     re-instantiating the filter.

2. GNSS "apply lever arm pre-heading" option (core)
   Default behaviour: the antenna lever arm only takes effect AFTER
   heading_validated_ flips true (dock compass / dual-GNSS / 5 m of
   straight driving). That gate exists because the initial yaw may be
   way off, and a wrong-yaw lever arm projection can feed spurious
   position innovations into the filter.

   • New GnssParams::apply_lever_arm_pre_heading (default false,
     preserving existing behaviour).
   • When enabled, the lever arm is applied from the first fix, turning
     every GPS observation into an active yaw correction via the
     position-orientation coupling in the measurement Jacobian. Safe
     with Mahalanobis gating on and especially useful with RTK-grade
     fixes: converges yaw in a few fixes instead of waiting for the
     straight-line accumulator.

3. ROS wrapper: auto-resolve lever arms from TF (fusion_node.cpp)
   Addresses "why doesn't FusionCore just adapt to URDF?". The core
   library stays framework-agnostic, but the ROS wrapper now:

   • Declares new params imu.lever_arm_x/y/z and
     gnss.apply_lever_arm_pre_heading.
   • On the first IMU message: if the user left the lever-arm params
     at zero, looks up base_frame -> imu_frame via tf2 and copies the
     translation into FusionCore's config via set_imu_lever_arm(). The
     rotation is already applied by the existing frame-rotation logic.
     A non-zero param acts as an explicit override.
   • On the first GPS fix: same treatment for the primary antenna,
     using msg->header.frame_id (typically "gnss_link" or "gps") as
     the lookup target.
   • TF lookup failures are logged (throttled) and the lever arm is
     left at zero — so missing-URDF installs still run, just without
     the centripetal/offset correction.

Together these close the observed gap between the filter's
position-velocity model and the physical geometry described by the
URDF, without requiring the user to maintain two copies of the same
numbers.

[manankharwar]

Really solid PR....

Two things to fix before we merge:

1. Stale comment in fusion_node.cpp
The comment references fc_->update_config_imu_lever_arm() but the actual function called is fc_->set_imu_lever_arm(). Looks like the function was renamed at some point but the comment wasn't updated.

2. fusioncore.yaml not updated
Four new parameters are declared in the code but missing from fusioncore_ros/config/fusioncore.yaml:

• imu.lever_arm_x: 0.0
• imu.lever_arm_y: 0.0
• imu.lever_arm_z: 0.0
• gnss.apply_lever_arm_pre_heading: false

The YAML is the single source of truth for users.... if a parameter isn't there, users won't know it exists. Please add all four with descriptions similar to the existing lever arm params.

Once those two are addressed, happy to merge!

[manankharwar]

Hey, just checking in on this one.... the two fixes are really small, and the underlying work is solid. If you get a chance to push those changes, I'm ready to merge.