feat(imu,gnss): IMU lever arm centripetal, GPS pre-heading option, TF auto-resolve#17
Conversation
… auto-resolve
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.
|
Really solid PR.... Two things to fix before we merge: 1. Stale comment in fusion_node.cpp 2. fusioncore.yaml not updated
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! |
|
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. |
|
Hey @cedbossneo, just following up on this. If you're busy I'm happy to take those two small fixes myself and push them to get this merged. Just say the word and I'll close it out. Also: five PRs in and I realize I still have no idea what you're actually building with FusionCore. What's the platform? I'm at a stage where understanding real deployments matters more to me than almost anything else. If you'd ever be open to a short call (20 min), I'd genuinely value hearing about your setup. Happy to answer anything about the roadmap in return. |
2 participants
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.
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.
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.
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.
wrapper can populate the arm from TF after construction without
re-instantiating the filter.
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.
preserving existing behaviour).
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.
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:
gnss.apply_lever_arm_pre_heading.
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.
using msg->header.frame_id (typically "gnss_link" or "gps") as
the lookup target.
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.