robot_localization (ekf node) output is not stable for a long time

asked 2021-04-30 05:31:25 -0500

Fuat Akca
3 ●1 ●2 ●3

updated 2021-05-09 04:40:35 -0500

Hi. I am using ekf_localization_node for fusing imu, wheel odometry and amcl_pose ( My config is as follow ) The reason why i am using amcl_pose; When i use odom0_differential: true the filter output odometry_filtered/odom covariance grows quickly. It doesn't grow with adding amcl_pose. Aynway the main problem is everything is working fine for a few hours (in my case 5 hours) then the output odometry_filtered/odom is getting wrong. The robot seems to be spinning around itself when robot try to move (So the localization is not usable for navigation after this behaviour). Hence amcl could not match laser scan with map. Has anyone encountered similar problem? What should be problem in my setup?

my config:

frequency: 20
sensor_timeout: 0.1
two_d_mode: true
transform_time_offset: 0.0
transform_timeout: 0.05
print_diagnostics: true
debug: false
debug_out_file: /path/to/debug/file.txt
publish_tf: true
publish_acceleration: false

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


odom0: mobile_base_controller/odom

odom0_config: [true,  true,  false,
               false, false, true,
               false, false, false,
               false, false, false,
               false, false, false]

odom0_queue_size: 40
odom0_nodelay: false
odom0_differential: true
odom0_relative: true
odom0_pose_rejection_threshold: 50
odom0_twist_rejection_threshold: 10


pose0: amcl_pose
pose0_config: [true,  true,  false,
               false, false, true,
               false, false, false,
               false, false, false,
               false, false, false]
pose0_differential: false
pose0_relative: false
pose0_queue_size: 5
pose0_rejection_threshold: 2
pose0_nodelay: false


imu0: imu_data
imu0_config: [false, false, false,
              false, false, true,
              false, false, false,
              false, false, true,
              false, false, false]
imu0_nodelay: false
imu0_differential: false
imu0_relative: true
imu0_queue_size: 50
imu0_pose_rejection_threshold: 0.8
imu0_twist_rejection_threshold: 0.8
imu0_linear_acceleration_rejection_threshold: 0.8
imu0_remove_gravitational_acceleration: true

mobile_base_controller/odom sample:

header: 
  seq: 400542
  stamp: 
    secs: 1619769333
    nsecs: 569522981
  frame_id: "odom"
child_frame_id: "base_link"
pose: 
  pose: 
    position: 
      x: -2.84049144363
      y: -8.9643127804
      z: 0.0
    orientation: 
      x: 0.0
      y: -0.0
      z: -0.73369743811
      w: -0.679476319905
  covariance: [0.02, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2]
twist: 
  twist: 
    linear: 
      x: 1.04163639519e-14
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 4.58834359396e-15
  covariance: [0.02, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.02, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1000000.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2]
---

imu_data:

header: 
  seq: 1328
  stamp: 
    secs: 1619771417
    nsecs: 729498740
  frame_id: "imu_link"
orientation: 
  x: -0.0122008586339
  y: 0.0218509162555
  z: -0.484715149522
  w: 0.874313959817
orientation_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
angular_velocity ...

(more)

edit retag flag offensive close merge delete

add a comment

answered 2021-06-09 04:38:47 -0500

Tom Moore

flag of United Kingdom of Great Britain and Northern Ireland

13689 ●74 ●240 ●232 http://github.com/ayrton04

What's the need for fusing amcl's output with odometry and IMU? I would think using amcl alone would be pretty stable.

First of all, if you are ever having trouble, get rid of the advanced parameters. In this case, remove the rejection_threshold parameters.

As for your questions:

When i use odom0_differential: true the filter output odometry_filtered/odom covariance grows quickly

That's what I'd expect to see. If you only fuse sources that are subject to drift, your covariance will grow without bound. What you typically see with a two-tier setup is that your odom-frame state estimate has an error that grows without bound (which is correct), and either amcl or a second map-frame EKF produces a state estimate in the map frame that is _not_ subject to drift. See REP-105 for more details.

Regardless, if I were you, I'd stop fusing absolute pose from wheel odometry. Just fuse the velocities. differential mode is meant for sensors that _only_ produce pose data. It differentiates subsequent poses and generates velocities from them. Your wheel encoders generate velocity data directly, so just fuse X velocity, Y velocity, and yaw velocity from your wheel encoders, and set X, Y, and yaw to false. You can then also remove the differential setting.

As far as your other issue goes, there are a couple things I see:

Your amcl pose is in the map frame, but your EKF is in the odom frame. Since amcl is the very node that is producing the map -> odom transform (I believe), when you fuse that measurement into the EKF, which is in the odom frame, you are effectively just feeding the EKF back its own pose as a measurement.
You are fusing absolute yaw from your IMU and amcl. While you correctly turned on relative mode for the IMU, I am wondering if the IMU's orientation is drifting. Magnetometers are notoriously terrible.

In the end, the key takeaways are:

You should use amcl's pose as your estimate of where your robot is in the world. That can be used for global planning.
Your odom frame state estimate from the EKF _will_ drift and become globally erroneous, and it will also undergo unbounded covariance growth. This is 100% correct and fine. You're just integrating a bunch of velocity measurements, each of which has errors, so you're also integrating the error. Even with that drift and error, the odom frame estimate is usable for local planning.

Comments

Thank you for your detailed explanation. I had question marks in my mind about the covariance growth. But it's ok now everything is cleaner.

Fuat Akca ( 2021-06-21 14:05:14 -0500 )edit

add a comment

robot_localization (ekf node) output is not stable for a long time

1 Answer

Comments

Question Tools

Stats

Related questions

robot_localization (ekf node) output is not stable for a long time edit

1 Answer

Comments

Question Tools

Stats

Related questions

robot_localization (ekf node) output is not stable for a long time