Fused IMU and Odom heading is wildly incorrect

edit

Platform is a Husky A200 with smooth tires, driving on a linoleum over concrete floor, IMU is a Lord Microstrain 3DM-GX5-25. I'm using ROS Melodic on Ubuntu 18.04 (bionic) on an x86 laptop (Asus U46E). The drivers for the Husky are the ones provided by Clearpath on Github, the drivers for the IMU are the ros_mscl library supplied by Lord on Github. At the moment, I think both of these components are working correctly and I have misconfigured robot_localization somehow.

My test procedure was to drive the robot in a square by teleop, and record a bag file (which is attached). I play back the bagfile while visualizing the wheel odometry from /husky_velocity_controller/odom and the fused odometry from /odometry/filtered. I'm trying to tune the process_noise_covariance and initial_estimate_covariance so that the fused odometry shows the robot driving in a square.

What I see in RViz is that the wheel odometry looks very good. The square looks square to within my ability as a driver, and the edges look straight. The fused odometry has straight-ish paths of what appears to be the right length, but the angles of each turn are too small, and the robot appears to swing in the right direction and then swing mostly back before continuing on.

I can't upload images, so a RViz screenshot is at https://imgur.com/a/MLkpB7i red is the wheel odometry, green is the fused odometry.

What I would like to see is that the fused odometry matches the wheel odometry to some degree, showing the robot turning in a square.

I don't have a covariance matrix for the IMU as provided by a datasheet, but the driver provides the ability to explicitly set one and I wrote a node to change it when the bag is playing, so I could try different values. Setting it to 0.01 or 0.001 did not appear significantly different.

I'm not using 2D mode because the eventual goal is to drive outside and do SLAM on uneven terrain, so the roll/pitch/yaw of the robot ends up mattering.

My current localization.yaml is

odom_frame: odom
base_link_frame: base_link
world_frame: odom

two_d_mode: false
frequency: 100

odom0: husky_velocity_controller/odom
odom0_config: [false, false, false, # x, y, z
               false, false, false, # roll, pitch, yaw 
               true, true, true, # x vel, y vel, z vel
               false, false, false, # roll vel, pitch vel, yaw vel
               false, false, false] # x accel, y accel, z accel
#odom0_differential: true
odom0_queue_size: 10

imu0: ned_imu #imu_converter/ned_to_enu
imu0_config: [false, false, false, # x, y, z
              true, true, true, # roll, pitch, yaw
              false, false, false, # x vel, y vel, z vel
              true, true, true, # roll vel, pitch vel, yaw vel
              false, false, false] # x accel, y accel, z accel
imu0_differential: false
imu0_queue_size: 10
imu0_remove_gravitational_acceleration: true

                           #x    y     z     R     P     Y     x_vel  y_vel  z_vel R_vel P_vel Y_vel x_acc y_acc z_acc
process_noise_covariance: [0.05, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0.05 ...