Robot Localization (Odom + IMU) - Persistent Error in Yaw

edit

Hello everyone,

I have been trying to fuse my robot's odometry and IMU using EKF. However, I noticed that the filtered yaw was always lesser than the actual yaw of the robot. I tried rotating my robot on the spot and comparing the yaw published by both the IMU and the filtered odometry.

Image of robot rotating

In the image, the green arrows are from the EKF filtered odometry while the gold arrows are produced by directly taking the imu's yaw. When the robot has made one full rotation, the imu's arrows form a complete circle as expected but the filtered odometry's yaw stops short of a full rotation. This error slowly builds up and increases the more I rotate the robot.

This is my EKF params configuration file:

frequency: 20
sensor_timeout: 0.05
two_d_mode: true
transform_time_offset: 0.0
transform_timeout: 0.0

print_diagnostics: true
debug: false
debug_out_file: /path/to/debug/file.txt
publish_tf: true
publish_acceleration: false


map_frame: map              # Defaults to "map" if unspecified
odom_frame: odom            # Defaults to "odom" if unspecified
base_link_frame: base_footprint  # Defaults to "base_link" if unspecified
world_frame: odom           # Defaults to the value of odom_frame if unspecified


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


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


use_control: true
stamped_control: false
control_timeout: 0.1
control_config: [true, false, false, false, false, true]

acceleration_limits: [2.6, 0.0, 0.0, 0.0, 0.0, 6.5]
deceleration_limits: [2.6, 0.0, 0.0, 0.0, 0.0, 6.5]
acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]
deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0]


process_noise_covariance:
 [0.05, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
  0,    0.05, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
  0,    0,    0.06, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
  0,    0,    0,    0.03, 0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
  0,    0,    0,    0,    0.03, 0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
  0,    0,    0,    0,    0,    0.05, 0,     0,     0,    0,    0,    0,    0,    0,    0,
  0,    0,    0,    0,    0,    0,    0.350, 0,     0,    0,    0,    0,    0,    0,    0,
  0,    0,    0,    0,    0,    0,    0,     0.010, 0,    0,    0,    0,    0,    0,    0,
  0,    0,    0,    0,    0,    0,    0,     0,     0.04, 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.01, 0,    0,    0,    0,
  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0 ...