Navsat_transform diverges from GPS points

edit

Hi, I'm trying to use the navsat_transform_node as per the example provided in the robot_localization package. However, the output of the node always diverges (quite significantly) from the original GPS points (see image below - odom.csv is the odometry without GPS, odom_fused.csv incorporates the GPS, odom_gps.csv is the output of the navsat_transform_node, gps.csv is the input GPS points, and gps_filtered.csv is the filtered GPS output from navsat_transform_node). I used the Haversine formula to convert the GPS coordinates into the local odom frame (assuming that the first GPS point is the true start position of the robot). The odometry output (from ekf_se_odom) lines up nicely with the GPS data, so the heading information from the IMU seems to be correct. However, the output from navsat_transform is slightly rotated from the GPS data, and consequently the ekf_se_map output does not line up with the GPS data. Interestingly, the filtered GPS output is still quite close to the original GPS.

I'm pretty sure that the cause of the error is that the navsat_transform_node is using a heading that is slightly off to initialise itself. However, I have no idea how to go about fixing this. Any suggestions? I have uploaded the bagfile, launch and parameter files here - https://drive.google.com/drive/folder...

Thanks in advance for your help!

EDIT:

Configuration data:

ekf_se_odom:
  frequency: 30
  sensor_timeout: 0.1
  two_d_mode: true
  transform_time_offset: 0.0
  print_diagnostics: true
  debug: false

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

  twist0: radar
  twist0_config: [false, false, false,
                 false, false, false,
                 true,  true,  false,
                 false, false, false,
                 false, false, false]
  twist0_queue_size: 10
  twist0_nodelay: false
  twist0_differential: false
  twist0_relative: false

  imu0: imu
  imu0_config: [false, false, false,
                false,  false,  true,
                false, false, false,
                false,  false,  true,
                true,  false,  false]
  imu0_nodelay: false
  imu0_differential: false
  imu0_relative: false
  imu0_queue_size: 10
  imu0_remove_gravitational_acceleration: true

  use_control: false

  process_noise_covariance: [1e-3, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                             0,    1e-3, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                             0,    0,    1e-3, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                             0,    0,    0,    0.3,  0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                             0,    0,    0,    0,    0.3,  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.5,   0,     0,    0,    0,    0,    0,    0,    0,
                             0,    0,    0,    0,    0,    0,    0,     0.5,   0,    0,    0,    0,    0,    0,    0,
                             0,    0,    0,    0,    0,    0,    0,     0,     0.1,  0,    0,    0,    0,    0,    0,
                             0,    0,    0,    0,    0,    0,    0,     0,     0,    0.3,  0,    0,    0,    0,    0,
                             0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0.3,  0,    0,    0,    0,
                             0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.3,  0,    0,    0,
                             0,    0,    0,    0,    0,    0 ...