Robot pose is off when using robot localization with fused IMU and Odometry

edit

Hello All,

I am working on a 3-wheeled omnidirectional robot and trying to fuse IMU and Odometry using robot localization for better robot pose. My odometry is working well and I verified the axis of the IMUs align physically with the robot odometry, x forward and checked that when the the robot rotates anticlockwise yaw from my topic /imu/rpy is positive. However I am still having problems getting good robot pose while robot is navigating.

The minute the robot rotates along the Z axis, it begins to loose sense of its pose/orientation. In fact oddly enough the robot pose seems better without fusing IMU. Because I want the robot to navigate small areas I really want to get the benefit of better pose by fusing IMU and Odometry. Any thoughts on what I am doing wrong? Also I supposed to stop my odometry nod from publishing base---->odom TF? See my IMU topic, odom topic EKF and AMCL config below:

EKF Config

//silent_tf_failure: false
sensor_timeout: 0.1
two_d_mode: true

transform_time_offset: 0.0

transform_timeout: 0.5

print_diagnostics: true

debug: false

debug_out_file: /home/ubuntu/robot_localizationfile.txt

publish_tf: true

publish_acceleration: false

initial_state: [ 1.0, -4.0, 0.0,
                 0.0,  0.0, 0.0,
                 0.0,  0.0, 0.0,
                 0.0,  0.0, 0.0,
                 0.0,  0.0  0.0]


map_frame: map              # Defaults to "map" if unspecified
odom_frame: odom            # Defaults to "odom" if unspecified
base_link_frame: base_link  # Defaults to "base_link" if unspecified
world_frame: odom           # Defaults to the value of odom_frame if unspecified but if map specified USE MAP (Check AMCL)

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

odom0_pose_rejection_threshold: 5
odom0_twist_rejection_threshold: 1


imu0: /imu
imu0_config: [false, false, false,
              false, false, false,
              false, false, false,
              false, false,  true,
              true,  true,  false]

imu0_differential: false


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.06, 0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0.025, 0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0,    0,    0,    0,    0,    0,     0.025, 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.02, 0 ...