Robotics StackExchange | Archived questions

Robot localization EKF pose estimation drift during acceleration / deceleration

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,    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,    1e-9, 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,    1.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,    1.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,     1.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,    1.0,  0,
                                0,    0,    0,    0,    0,    0,    0,    0,    0,    0,     0,     0,     0,    0,    1.0]

ekf_se_map:

  frequency: 40

  sensor_timeout: 0.1
  two_d_mode: true
  transform_time_offset: 0.0
  transform_timeout: 0.0
  print_diagnostics: false
  debug: false

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

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


  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

  use_control: false
  control_config: [true,  false, false,
                   false, false, true]
  acceleration_limits: [0.8,  0.0, 0.0,
                        0.0,  0.0, 3.0]
  deceleration_limits: [0.8,  0.0, 0.0,
                        0.0,  0.0, 3.0]

  pose0: /aruco_single/pose
  pose0_config: [true, true, false,
                  false,  false,  true,
                  false, false, false,
                  false,  false,  false,
                  false,  false,  false]
  pose0_differential: false
  pose0_queue_size: 1
  pose0_relative: false

  pose1: /set_pose/pose
  pose1_config: [true,  true,  false,
                 false, false, true,
                 false, false, false,
                 false, false, false,
                 false, false, false]
  pose1_differential: false
  pose1_queue_size: 1
  pose1_relative: false

  smooth_lagged_data: true
  history_length: 0.2

  process_noise_covariance: [1.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,    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: [1.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,    1e-9, 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,    1.0,  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,    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,    1.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,     1.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,     1.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,    1.0]

Sample msgs: Imu:

header: 
  seq: 312
  stamp: 
    secs: 1556111895
    nsecs:  57998392
  frame_id: "base_imu_link"
orientation: 
  x: -0.0259826382
  y: 0.0006378167
  z: -0.8054282149
  w: -0.5920617441
orientation_covariance: [0.002741552146694444, 0.0, 0.0, 0.0, 0.002741552146694444, 0.0, 0.0, 0.0, 0.007615422629706791]
angular_velocity: 
  x: -0.000720391186613
  y: 0.00792870233999
  z: -0.0120081267761
angular_velocity_covariance: [1.0966208586777776e-06, 0.0, 0.0, 0.0, 1.0966208586777776e-06, 0.0, 0.0, 0.0, 1.0966208586777776e-06]
linear_acceleration: 
  x: -0.558524621056
  y: -0.306901720343
  z: -9.71105086042
linear_acceleration_covariance: [0.0015387262937311438, 0.0, 0.0, 0.0, 0.0015387262937311438, 0.0, 0.0, 0.0, 0.0015387262937311438]

Odom:

header: 
  seq: 940
  stamp: 
    secs: 1556111896
    nsecs:  73360397
  frame_id: "odom"
child_frame_id: "base_link"
pose: 
  pose: 
    position: 
      x: 4.08163286585
      y: -0.0399870891138
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.0159623096425
      w: 0.999872594219
  covariance: [0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.03]
twist: 
  twist: 
    linear: 
      x: -0.00385667669819
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.00110043238333
  covariance: [0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.03]

Aruco:

header: 
  seq: 100
  stamp: 
    secs: 1556111891
    nsecs:  38134000
  frame_id: "map"
pose: 
  pose: 
    position: 
      x: 2.42997637311
      y: 6.86889425999
      z: 0.0712525943425
    orientation: 
      x: 0.00531628989327
      y: 0.0096704682779
      z: 0.701787258129
      w: 0.712301104473
  covariance: [0.01, 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.01, 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.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0003]

Asked by asafeniger on 2019-05-08 14:57:32 UTC

Comments

For future questions/edits, to enable syntax highlighting please don't wrap code/terminal out/etc. in ''', you can use the preformatted text (101010) button. To use it, highlight the text and then click on the preformatted text (101010) button.

Asked by jayess on 2019-05-08 17:36:27 UTC

Answers

My guess is that it's more a function of covariance values. They don't look terrible at first glance, but I'd have to see it in action. Some lag is to be expected - it is a filter, after all - but it shouldn't impact overall accuracy, and certainly should result in overshooting to that level (assuming that's the cause of the overshoot).

Can we step back and simplify the setup? Can you just run the first-tier (odom) EKF and compare the pose to the OptiTrack pose? I realize the frames won't agree, but you're more concerned with lag in starting and stopping.

BTW, I assume you have a base_link -> base_imu_link specified somewhere?

Asked by Tom Moore on 2019-05-15 04:51:30 UTC

Comments