robot_localization GPS causes large drift/covariance spike

edit

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 ...