robot_localization with GPS map frame won't stay fixed

edit

Hey guys,

I'm running a dual ekf setup of robot_localization to fuse GPS alongside navsat_transform_node which provideds the map->utm transform. I have been trying to figure this out for days now but i can't get this map frame's orientation to stay aligned with the odom frame?

I am running a gnss heading receiver which provides the true earth referenced heading which i fuse in both state estimation nodes as a pose message. My understanding is, since the only source of orientation is coming from this one message for both nodes, they should both always have the same rotation? The map frame initially is the same as odom but after some driving around and turning the robot it goes wild and very inaccurate. My gps is an RTK GPS setup i have confirmed that accuracy down to 5mm and a heading accuracy of 0.1 degree, so i know there is no issue with my GPS.

Below is my robot_localization configuration:

ekf_se_odom:
  frequency: 20
  two_d_mode: true
  sensor_timeout: 0.15
  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: /warthog_velocity_controller/odom
  odom0_config: [false, false, false,
                 false, false, false,
                 true, true, false,
                 false, false, true,
                 false, false, false]
  odom0_queue_size: 10
  odom0_nodelay: true
  odom0_differential: false
  odom0_relative: false

  pose0: /gps/odometry
  pose0_config: [false, false, false,
                 false, false, true,
                 false, false, false,
                 false, false, false,
                 false, false, false]
  pose0_queue_size: 10
  pose0_nodelay: true
  pose0_differential: false
  pose0_relative: false

  imu0: /mcu_imu/data
  imu0_config: [false, false, false,
                false, false, false,
                false, false, false,
                false, false, true,
                true, true, false]
  imu0_differential: false
  imu0_nodelay: false
  imu0_relative: false
  imu0_queue_size: 10

  use_control: false

ekf_se_map:
  frequency: 20
  sensor_timeout: 0.15
  two_d_mode: true
  transform_time_offset: 0.0
  transform_timeout: 0.0
  print_diagnostics: true
  debug: false
  debug_out_file: "/home/alec/debug_ekf.txt"

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

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

  pose0: /gps/odometry
  pose0_config: [false, false, false,
                 false, false, true,
                 false, false, false,
                 false, false, false,
                 false, false, false]
  pose0_queue_size: 10
  pose0_nodelay: true
  pose0_differential: false
  pose0_relative: false

  odom2: /bunkbot_localization/odometry/gps
  odom2_config: [true,  true,  false,
                 false, false, false,
                 false, false, false,
                 false, false, false,
                 false, false, false]
  odom2_queue_size: 10
  odom2_nodelay: true
  odom2_differential: false
  odom2_relative: false

  imu0: /mcu_imu/data
  imu0_config: [false, false, false,
                false, false, false,
                false, false, false,
                false, false, true,
                true, true, false]
  imu0_differential: false
  imu0_nodelay: true
  imu0_relative: false
  imu0_queue_size: 10

  use_control: false

Below is my navsat_transform_node configuration:

navsat_transform:
  broadcast_utm_transform: true
  delay: 3.0
  frequency: 20
  magnetic_declination_radians: 0.0 # Set this depdending on origin location in the world
  publish_filtered_gps: true
  use_odometry_yaw: true
  wait_for_datum: false
  yaw_offset: 0.0 
  zero_altitude: true

I set the use_odometry_yaw to true as my heading is fused into the odometry and not from the IMU, i also set yaw_offset to 0 as the heading is 0 when facing east.

Here are some GIF's of the current behaviour:

And below is a GIF of my ... (more)