Robot localization EKF pose estimation drift during acceleration / deceleration

edit

Hello,

I'm using robot localization's EKF on my robot and I have some issues with it. I'm using ROS kinetic version on Ubuntu 16.04 and I install robot localization pkg from apt-get install.

My inputs to the EKF are IMU (UM7), wheel encoders and fixed position from aruco tags. I'm using Optitrack motion capture system for position reference.

When the robot accelerates I noticed that the ekf estimation tends be behind the reference position and when its decelerates the ekf estimation is ahead of the referenced position. When I compared the odometry and aruco detection to the reference the error seems reasonable and significantly smaller than the ekf estimation error. My guess is that its related to the prediction phase. The bottom line is that the robot is stopping, approximately, 10 cm before its target.

I'm using the following configurations:

ekf_se_odom:
  odom_frame: odom
  base_link_frame: base_link
  world_frame: odom

  two_d_mode: true

  frequency: 40

  odom0: velocity_controller/odom
  odom0_config: [false, false, false,
                 false, false, false,
                 true, true, false,
                 false, false, true,
                 false, false, false]
  odom0_differential: false
  odom0_relative: true
  odom0_queue_size: 1

  imu0: imu/data_filtered
  imu0_config: [false, false, false,
                  false,  false,  false,
                  false, false, false,
                  false,  false,  true,
                  false,  false,  false]
  imu0_differential: false
  imu0_queue_size: 1
  imu0_remove_gravitational_acceleration: true

  debug: 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,    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]

  initial_estimate_covariance: [1e-9, 0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                0,    1e-9, 0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                0,    0,    1e-9, 0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    0,
                                0,    0,    0,    1.0,  0,    0 ...