I am trying to implement the dual ekf navsat example on my real-world robot. The local EKF with wheel odometry and IMU works quite well and has no issues. As far as I can tell, my global EKF configuration is nearly identical to the local with the only addition being the /odometry/gps.

When I start both nodes, the map->odom transform blows up in X/Y and spins randomly when sitting stationary. I have tried adjusting process noise covariance and initial covariance estimate parameters, but these appear to not change the issue.

My GPS appears to be working well. I have RTK Fix and it reports an accuracy of ~2cm, which my testing appears to validate.

Bag file:

I recorded a bag file of the robot sitting still, then drawing a rectangle. The odometry/local appears to draw the shape relatively well. Observing the path of the GPS points also shows relatively low noise, and draws the path well. I can't upload on this site, here is a sharepoint/onedrive link.

Here is a graph showing the /odometry/global (left) vs /odometry/gps and /odometry/local (right):

Details:

• Platform: Nvidia Jetson Xavier
• ROS Version: ROS2 Humble
• Operating System: Ubuntu 22.04 (docker image: arm64v8/ros:humble-perception-jammy)
• Robot_Localization version: 3.5.1-2 (20230525)
• GPS Driver: septentrio-gnss

robot_localization config:

(Process/Initial cov matricies are identical to example)

ekf_filter_node_odom:
  ros__parameters:
    frequency: 30.0
    sensor_timeout: 0.1
    two_d_mode: true
    transform_time_offset: 0.0
    transform_timeout: 0.0
    print_diagnostics: true
    debug: false

    map_frame: map
    odom_frame: odom
    base_link_frame: base_link
    world_frame: odom

    odom0: diff_cont/odom
    odom0_config: [false, false, false,
                  false, false, false,
                  true,  true,  true,
                  false, false, true,
                  false, false, false]
    odom0_queue_size: 10
    odom0_nodelay: true
    odom0_differential: false
    odom0_relative: false

    imu0: zed2i/zed_node/imu/data
    imu0_config: [false, false, false,
                  true,  true,  false,
                  false, false, false,
                  true,  true,  true,
                  true,  true,  true]
    imu0_nodelay: false
    imu0_differential: false
    imu0_relative: false
    imu0_queue_size: 10
    imu0_remove_gravitational_acceleration: true

    use_control: false

ekf_filter_node_map:
  ros__parameters:
    frequency: 30.0
    sensor_timeout: 0.1
    two_d_mode: true
    transform_time_offset: 0.0
    transform_timeout: 0.0
    print_diagnostics: true
    debug: false

    map_frame: map
    odom_frame: odom
    base_link_frame: base_link
    world_frame: map

    odom0: diff_cont/odom
    odom0_config: [false, false, false,
                  false, false, false,
                  true,  true,  true,
                  false, false, true,
                  false, false, false]
    odom0_queue_size: 10
    odom0_nodelay: true
    odom0_differential: false
    odom0_relative: false

    imu0: zed2i/zed_node/imu/data
    imu0_config: [false, false, false,
                  true,  true,  true,
                  false, false, false,
                  false,  false,  false,
                  false,  false,  false]
    imu0_nodelay: false
    imu0_differential: false
    imu0_relative: false
    imu0_queue_size: 10
    imu0_remove_gravitational_acceleration: true

    odom1: odometry/gps
    odom1_config: [true,  true,  false,
                  false, false, false,
                  false, false, false,
                  false, false, false,
                  false, false, false]
    odom1_queue_size: 10
    odom1_nodelay: true
    odom1_differential: false
    odom1_relative: false


    use_control: false

navsat_transform:
  ros__parameters:
    frequency: 30.0
    delay: 3.0
    magnetic_declination_radians: 0.0
    yaw_offset: 0.0
    zero_altitude: false
    broadcast_utm_transform: true
    publish_filtered_gps: true
    use_odometry_yaw: false
    wait_for_datum: false

Example Sensor Messages:

diff_cont/odom:

header:
  stamp:
    sec: 1688561428
    nanosec: 748273568
  frame_id: odom
child_frame_id: base_link
pose:
  pose:
    position:
      x: 0.0
      y: 0.0
      z: 0.0
    orientation:
      x: 0.0
      y: 0.0
      z: 0.0
      w: 1.0
  covariance:
  - 0.001
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.001
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.001
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.001
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.001
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 1.0e-06
twist:
  twist:
    linear:
      x: 0.0
      y: 0.0
      z: 0.0
    angular:
      x: 0.0
      y: 0.0
      z: 0.0
  covariance:
  - 0.001
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.001
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.001
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.001
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.001
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 1.0e-06

/zed2i/zed_node/imu/data:

header:
  stamp:
    sec: 1688561477
    nanosec: 94539456
  frame_id: zed2i_imu_link
orientation:
  x: 0.04747530817985535
  y: 0.0217120498418808
  z: 0.33702659606933594
  w: 0.9400466680526733
orientation_covariance:
- 5.814558282399458e-10
- -3.2944193591010685e-10
- 3.0220212313595557e-11
- -3.294419012791137e-10
- 8.316236808529003e-10
- 2.0890566503682616e-10
- 3.022021664246969e-11
- 2.0890566503682616e-10
- 6.456938962255484e-10
angular_velocity:
  x: -0.01820158927202928
  y: 0.02127879501425481
  z: 0.000792325946237065
angular_velocity_covariance:
- 6.35957531046334e-10
- 0.0
- 0.0
- 0.0
- 6.81844497277238e-10
- 0.0
- 0.0
- -0.0
- 5.81403604702388e-10
linear_acceleration:
  x: 0.024594472721219063
  y: 1.1347548961639404
  z: 9.869213104248047
linear_acceleration_covariance:
- 0.005106136202812195
- 0.0
- 0.0
- 0.0
- 0.00590071314945817
- 0.0
- 0.0
- -0.0
- 0.006188959814608097

gps/fix:

header:
  stamp:
    sec: 1688561423
    nanosec: 145409472
  frame_id: gps_frame
status:
  status: 2
  service: 9
latitude: 40.477501743301964
longitude: -87.00108049977172
altitude: 182.53988211053635
position_covariance:
- 0.00017811922589316964
- -6.650557043030858e-05
- -1.604144199518487e-05
- -6.650557043030858e-05
- 0.0003876265836879611
- -6.546182703459635e-06
- -1.604144199518487e-05
- -6.546182703459635e-06
- 0.0009483876056037843
position_covariance_type: 3