move_base strange behaviour when using earth referenced heading

edit

Hey guys,

I am performing gps waypoint navigation using move_base for the navigation side of things. My localization setup uses robot_localization i run two instances of this in order to fuse the gps, so first instance has wheel odom + IMU and second instance has wheel odom + IMU + GPS.

My IMU does not provide an earth referenced heading so i have a gnss heading receiver setup and i pipe the heading into a Pose message of the following format:

I have two pose messages setup, one for the odom frame and one for the map frame both are the exact same but with different frame_id. I fuse the earth reference heading into both robot_localization nodes in the following way:

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

  # GNSS HEADING POSE MESSAGE (ODOM)

  pose0: /gps/odometry_odom
  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

  # GNSS HEADING POSE MESSAGE (MAP)

  pose0: /gps/odometry_map
  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

The Problem Whenever i fuse in the earth referenced heading, record some GPS waypoints and attempt to navigate i get a very strange behaviour! see below:

BUT: When i do not fuse the earth reference heading. the navigation works fine????? see below:

Does anyone know why this would be happening? I remap the move_base odom topic to my ekf filtered output:

<remap from="odom" to="/bunkbot_localization/odometry/filtered_map" />

And i have checked this topic's output with both fused gnss heading and without and they seem to be fine?

UPDATE 1:

I have run the outputs of some bag files i recorded of a figure 8 movement of both robot_localization nodes and the gps odometry output of navsat_transform_node through matlab to ... (more)