u/Snoo_92391

Hey everyone, I've been banging my head against this for a while and could really use some experienced eyes on my setup.

The robot: Segway RMP platform, differential drive. It's a ground robot that does long outdoor runs, typically 30-60 minutes, covering several kilometers on a university campus. Mix of open areas, tree-lined paths, and some areas with buildings nearby causing GPS multipath.

Sensors:

• IMU: Microstrain 3DM-GX3-45 (6-axis, no magnetometer, so no absolute heading)
• Wheel encoders publishing as nav_msgs/Odometry on /odom/wheels
• GPS through navsat_transform_node outputting to /gps/odometry
• All fused through ekf_node at 150Hz

The problem: Most of the time the filter works okay-ish, but on some runs it completely falls apart. Like, the estimated position jumps to somewhere millions of meters away after what I think is a GPS spike getting accepted, and then the filter never recovers. It just keeps publishing nonsense from that point on. On other runs the path length ratio is completely off (filter thinks the robot traveled either way more or way less than it actually did).

Also running ukf_node in parallel to compare, and that one is just spitting out Critical Error, NaNs were detected in the output state of the filter almost constantly. So the UKF option seems totally broken for my setup.

Current config:

rl_ekf:
  ros__parameters:
    frequency: 150.0
    sensor_timeout: 0.5
    two_d_mode: true
    transform_time_offset: 0.0
    transform_timeout: 0.1
    print_diagnostics: false
    debug: false
    map_frame: map
    odom_frame: odom
    base_link_frame: base_link
    world_frame: odom

    # Wheel odometry: fuse Vx and Vyaw only (differential drive, no lateral velocity)
    odom0: /odom/wheels
    odom0_config: [false, false, false,
                   false, false, false,
                   true,  false, false,
                   false, false, true,
                   false, false, false]
    odom0_differential: false
    odom0_relative: false
    odom0_queue_size: 10
    odom0_twist_rejection_threshold: 4.03

    # GPS via navsat_transform: fuse XY position
    odom1: /gps/odometry
    odom1_config: [true,  true,  false,
                   false, false, false,
                   false, false, false,
                   false, false, false,
                   false, false, false]
    odom1_differential: false
    odom1_relative: false
    odom1_queue_size: 10
    odom1_pose_rejection_threshold: 3.72

    # IMU: roll/pitch only (no magnetometer, no absolute yaw), angular vel, accel
    imu0: /imu/data
    imu0_config: [false, false, false,
                  true,  true,  false,
                  false, false, false,
                  true,  true,  true,
                  true,  true,  true]
    imu0_differential: false
    imu0_relative: false
    imu0_remove_gravitational_acceleration: true
    imu0_queue_size: 50

    process_noise_covariance: [0.05,  0.0,   0.0,   0.0,   0.0,   0.0,   0.0,    0.0,    0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,
                               0.0,   0.05,  0.0,   0.0,   0.0,   0.0,   0.0,    0.0,    0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,
                               0.0,   0.0,   0.06,  0.0,   0.0,   0.0,   0.0,    0.0,    0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,
                               0.0,   0.0,   0.0,   0.03,  0.0,   0.0,   0.0,    0.0,    0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,
                               0.0,   0.0,   0.0,   0.0,   0.03,  0.0,   0.0,    0.0,    0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,
                               0.0,   0.0,   0.0,   0.0,   0.0,   0.06,  0.0,    0.0,    0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,
                               0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.025,  0.0,    0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,
                               0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,    0.025,  0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,
                               0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,    0.0,    0.04,  0.0,   0.0,   0.0,   0.0,   0.0,   0.0,
                               0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,    0.0,    0.0,   0.01,  0.0,   0.0,   0.0,   0.0,   0.0,
                               0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,    0.0,    0.0,   0.0,   0.01,  0.0,   0.0,   0.0,   0.0,
                               0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,    0.0,    0.0,   0.0,   0.0,   0.02,  0.0,   0.0,   0.0,
                               0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,    0.0,    0.0,   0.0,   0.0,   0.0,   0.01,  0.0,   0.0,
                               0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,    0.0,    0.0,   0.0,   0.0,   0.0,   0.0,   0.01,  0.0,
                               0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.0,    0.0,    0.0,   0.0,   0.0,   0.0,   0.0,   0.0,   0.015]

Process noise covariance is diagonal, values roughly in the 0.01-0.06 range for position/orientation and 0.01-0.025 for velocity/acceleration. I basically eyeballed these from examples online.

Specific questions:

1. The GPS divergence issue: my rejection threshold is sqrt(chi2(2, 0.999)) = 3.72. Is this too loose? Too tight? When a GPS spike comes in and passes the gate, is there any way to recover without restarting the node?
2. No magnetometer means I have no absolute yaw reference at startup. I'm not fusing yaw from the IMU at all (the false in that position). The filter figures out heading from GPS travel. How long does this take to converge and is there a better way to handle it?
3. odom0_differential: false with wheel odometry publishing absolute twist... is this correct or should it be true? I've seen conflicting advice on this.
4. Process noise tuning: is there a systematic way to do this or is it always just trial and error? My position noise (0.05) vs velocity noise (0.025) ratio.... does that even make sense dimensionally?
5. For long outdoor runs where GPS quality varies a lot (open field vs. near buildings), is there any way to get the filter to automatically trust GPS less when it's bad, without manually tuning per-environment?

Running ROS 2 Jazzy on Ubuntu 24.04.

I got frustrated with robot_localization on my outdoor robot and ended up rewriting sensor fusion from scratch. The result is FusionCore, a 22-state UKF that fuses IMU, wheel odometry, and GPS natively in ECEF coordinates.

I benchmarked it against robot_localization on the NCLT dataset (6 outdoor sequences, GPS + IMU + wheels). FusionCore hit 4.2m average ATE RMSE. robot_localization with proper outlier gating averaged 21.8m.

https://preview.redd.it/asonhtg9w1yg1.png?width=2475&format=png&auto=webp&s=13ff8af82fb84d0c7ba16dd1428b1588fd33730f

The interesting part: when I finally got robot_localization's gating config right (the parameter is odom0_twist_rejection_threshold, not odom0_mahal_threshold which silently does nothing), it actually made RL worse on 4 out of 6 sequences. The reason: navsat_transform passes through whatever covariance the GPS receiver reports, and NCLT receivers report it way too tight. Good measurements were getting rejected. FusionCore sidesteps this by letting you set a noise floor directly.

One config file, works with any IMU and GPS, drops into a Nav2 stack. No navsat_transform needed.

https://github.com/manankharwar/fusioncore