In the ROS2 Humble simulation instance, the ODOM fused data exhibits strong fluctuations.

[hellodss]

Hi manankharwar,

I tried running a simulation instance on ROS2 Humble, and why is the jumping so severe with GPS enabled? I tried adjusting the parameters but failed. Also, it seems there's no node maintenance for the TensorFlow map to ODOM. This doesn't seem to be very friendly to Nav2 navigation?Thank you very much for your answers.

Parameter file

fusioncore:
  ros__parameters:
    base_frame: base_link
    odom_frame: odom
    publish_rate: 50.0

    imu.gyro_noise: 0.005
    imu.accel_noise: 0.1
    imu.has_magnetometer: false

    encoder.vel_noise: 0.02
    encoder.yaw_noise: 0.01

    gnss.base_noise_xy: 4.0   # baseline sigma (m) at HDOP=1. When NavSatFix provides
                               # covariance, fusion_node sets fix.hdop = sqrt(var_xy),
                               # so sigma = base_noise_xy * sqrt(var_xy). With base=1.0
                               # this equals sqrt(var_xy) directly: the correct value.
                               # Do NOT set this to NOISE_H; that double-shrinks R.
    gnss.base_noise_z: 8.0    # same reasoning for vertical: set to 1.0, not NOISE_V
    gnss.max_hdop: 1.2
    # Disable GNSS fusion: simulated fixes are GPS_FIX with 4 satellites,
    # so these quality gates reject every GNSS measurement.
    gnss.min_satellites: 4
    gnss.min_fix_type: 1
    gnss.heading_topic: ""

    outlier_rejection: true
    outlier_threshold_gnss: 7.0

    adaptive.imu: true
    adaptive.encoder: true
    adaptive.gnss: false

Node startup status

[gz_pose_to_gps-6] [INFO] [1776846682.062273543] [gz_pose_to_gps]: GPS publisher ready. Origin: 43.2557, -79.8711
[fusioncore_node-5] [INFO] [1776846696.613661040] [fusioncore]: Configuring FusionCore...
[fusioncore_node-5] [INFO] [1776846696.618565691] [fusioncore]: IMU gravity removal: disabled
[fusioncore_node-5] [INFO] [1776846696.618627711] [fusioncore]: GNSS min_fix_type: 1 (1=GPS, 2=DGPS, 3=RTK_FLOAT, 4=RTK_FIXED)
[fusioncore_node-5] [INFO] [1776846696.628205343] [fusioncore]: PROJ: transform ready (EPSG:4326 → EPSG:4978)
[fusioncore_node-5] [INFO] [1776846696.628290203] [fusioncore]: PROJ: using first GPS fix as local reference origin
[fusioncore_node-5] [INFO] [1776846696.635718069] [fusioncore]: Dual antenna heading disabled.
[fusioncore_node-5] [INFO] [1776846696.635772826] [fusioncore]: FusionCore configured. base_frame=base_link odom_frame=odom rate=50Hz
[fusioncore_node-5] [INFO] [1776846697.135874736] [fusioncore]: --- TF Validation ---
[fusioncore_node-5] [INFO] [1776846697.136041591] [fusioncore]:   [OK]      imu_link -> base_link
[fusioncore_node-5] [INFO] [1776846697.136067223] [fusioncore]:   [OK]      base_link -> odom
[fusioncore_node-5] [INFO] [1776846697.136080074] [fusioncore]: ---------------------
[fusioncore_node-5] [INFO] [1776846697.139815414] [fusioncore]: Activating FusionCore...
[fusioncore_node-5] [INFO] [1776846697.144963398] [fusioncore]: Filter initialized at t=1776846697.145 (first IMU)
[fusioncore_node-5] [WARN] [1776846697.145016548] [fusioncore]: IMU frame mismatch: TF validation checked 'imu_link' but first message has frame_id 'fusioncore_robot/imu_link/imu_sensor'. Set imu.frame_id: "fusioncore_robot/imu_link/imu_sensor" in your config to fix the startup validation warning.
[fusioncore_node-5] [INFO] [1776846697.151523326] [fusioncore]: FusionCore active. Listening for sensors.
[fusioncore_node-5] [INFO] [1776846697.210175959] [fusioncore]: GNSS reference set: lat=43.255699 lon=-79.871094

Heading warnings and GPS filtering status

[fusioncore_node-5] [WARN] [1776847086.507848098] [fusioncore]: GNSS fix rejected (fix_type=1, min=1, hdop=0.50, quality check or Mahalanobis gate)
[fusioncore_node-5] [INFO] [1776847086.507905090] [fusioncore]: Heading not yet validated: lever arm inactive. Distance traveled: 2.3m (need 5.0m), or provide dual antenna / IMU orientation.
[fusioncore_node-5] [WARN] [1776847091.611310030] [fusioncore]: GNSS fix rejected (fix_type=1, min=1, hdop=0.50, quality check or Mahalanobis gate)
[fusioncore_node-5] [INFO] [1776847091.611385181] [fusioncore]: Heading not yet validated: lever arm inactive. Distance traveled: 3.3m (need 5.0m), or provide dual antenna / IMU orientation.
[fusioncore_node-5] [WARN] [1776847096.751245830] [fusioncore]: GNSS fix rejected (fix_type=1, min=1, hdop=0.50, quality check or Mahalanobis gate)
[fusioncore_node-5] [INFO] [1776847096.751310016] [fusioncore]: Heading not yet validated: lever arm inactive. Distance traveled: 3.3m (need 5.0m), or provide dual antenna / IMU orientation.
[fusioncore_node-5] [WARN] [1776847101.894905517] [fusioncore]: GNSS fix rejected (fix_type=1, min=1, hdop=0.50, quality check or Mahalanobis gate)
[fusioncore_node-5] [WARN] [1776847106.968660593] [fusioncore]: GNSS fix rejected (fix_type=1, min=1, hdop=0.50, quality check or Mahalanobis gate)
[fusioncore_node-5] [WARN] [1776847112.173092855] [fusioncore]: GNSS fix rejected (fix_type=1, min=1, hdop=0.50, quality check or Mahalanobis gate)
[fusioncore_node-5] [WARN] [1776847117.832813982] [fusioncore]: GNSS fix rejected (fix_type=1, min=1, hdop=0.50, quality check or Mahalanobis gate)
[fusioncore_node-5] [WARN] [1776847123.235430471] [fusioncore]: GNSS fix rejected (fix_type=1, min=1, hdop=0.50, quality check or Mahalanobis gate)
[fusioncore_node-5] [WARN] [1776847128.582131961] [fusioncore]: GNSS fix rejected (fix_type=1, min=1, hdop=0.50, quality check or Mahalanobis gate)

[manankharwar]

@hellodss Great! Thanks for creating a new Issue. I will take a look at the debug and get back to you latest by tonight!... Thanks again...

[manankharwar]

Hi, thanks for the detailed report. I can see a few issues in your config and logs causing this.... all fixable.

1. outlier_threshold_gnss: 7.0: this is why GPS is being rejected

You lowered it from the default 16.27. At 7.0 (chi² ~97th percentile), any normal GPS noise spike trips the gate and rejects the fix. The default 16.27 is chi²(3) at 99.9th percentile.... that's the correct statistical threshold for 3D position. Change it back:

outlier_threshold_gnss: 16.27

2. gnss.base_noise_xy: 4.0 is incorrect

With hdop=0.50 from the message, the effective noise becomes 4.0 × 0.5 = 2.0m sigma. That's very large for simulation and makes every GPS correction look like a big jump to the filter. For Gazebo, use the default:

gnss.base_noise_xy: 1.0
gnss.base_noise_z: 2.0

3. Conflicting odom→base_link TF publisher

Your startup log shows [OK] base_link -> odom.... but FusionCore itself publishes that TF, so it cannot exist before the filter starts. Something else in your launch file is publishing a static odom→base_link transform. Remove it. Two nodes publishing the same TF causes the jumps you're seeing.

4. map→odom: this is by design

FusionCore is an odometry filter. It publishes odom→base_link only. The map→odom transform is the job of a global localizer.... slam_toolbox, AMCL, or similar. Run one of those alongside FusionCore for full Nav2 navigation.

Fix 1, 2, and 3 first.... I expect the jumping to disappear entirely. Let me know what you see.

[manankharwar]

Hey @hellodss, just checking in: did those three changes sort out the jumping? Curious especially if removing the duplicate odom→base_link publisher made a difference, that one tends to cause the most chaos.

[sbetts2]

@manankharwar is this package compatible with humble? From your docs it seems jazzy is the minimum requirement but this thread makes it seem like it could work with humble?

[manankharwar]

@sbetts2... Jazzy is the only officially supported and tested distro. That's what the benchmarks ran on, what the apt package targets, and what CI tests against.

That said, FusionCore only uses standard ROS 2 APIs (lifecycle nodes, standard message types) and the core filter has zero ROS dependency. It will very likely build from source on Humble without changes. @hellodss appears to have gotten it running.

If you test it on Humble and hit a build issue, open a PR.... happy to fix anything that's a trivial compact change

[manankharwar]

@sbetts2 Update since my last comment: Humble is now officially supported.

We just added it to CI: it builds and tests clean on Ubuntu 22.04 / Humble as of today. Install from source:

mkdir -p ~/ros2_ws/src && cd ~/ros2_ws/src
git clone https://github.com/manankharwar/fusioncore.git
cd ~/ros2_ws
source /opt/ros/humble/setup.bash
rosdep install --from-paths src --ignore-src -r -y
colcon build && source install/setup.bash

Then follow the rest of the Getting Started docs: everything there applies to Humble, just replace jazzy with humble in any shell commands.

[sbetts2]

@manankharwar Awesome, thanks! Been struggling with RL for months, can't wait to try this.