odometry/filters starts drifting a lot, how to fix it?

asked 2022-01-26 07:32:36 -0600

Delbina
109 ●47 ●73 ●94

updated 2022-01-27 05:47:34 -0600

hi everyone,

the odometry/filters starts drifting a lot, i am using the following file, how can i decrease it?

warthog_joint_publisher:
  type: "joint_state_controller/JointStateController"
  publish_rate: 50

warthog_velocity_controller:
  type: "diff_drive_controller/DiffDriveController"
  left_wheel: ['front_left_wheel_joint', 'rear_left_wheel_joint']
  right_wheel: ['front_right_wheel_joint', 'rear_right_wheel_joint']
  publish_rate: 50
  pose_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 0.03]
  twist_covariance_diagonal: [0.001, 0.001, 0.001, 1000000.0, 1000000.0, 0.03]
  cmd_vel_timeout: 0.25

  # Odometry fused with IMU is published by robot_localization, so
  # no need to publish a TF based on encoders alone.
  enable_odom_tf: false

  # Wheel separation and radius multipliers
  wheel_separation: 1.08 # Auto read will fail due to suspension link
  wheel_separation_multiplier: 1.0 # default: 1.0
  wheel_radius_multiplier    : 1.0 # default: 1.0

  # Velocity and acceleration limits
  # Whenever a min_* is unspecified, default to -max_*
  linear:
    x:
      has_velocity_limits    : true
      max_velocity           : 5.0   # m/s
      has_acceleration_limits: true
      max_acceleration       : 20.0   # m/s^2
  angular:
    z:
      has_velocity_limits    : true
      max_velocity           : 4.0   # rad/s
      has_acceleration_limits: true
      max_acceleration       : 20.0   # rad/s^2

ekf_localization:
  frequency: 50
  two_d_mode: true
  odom0: /warthog_velocity_controller/odom
  odom0_config: [false, false, false,
                 false, false, false,
                 true, true, false,
                 false, false, true,
                 false, false, false]
  odom0_differential: false
  imu0: /gx5/imu/data
  imu0_config: [false, false, false,
                false, false, false,
                false, false, false,
                false, false, true,
                true, true, false]
  imu0_differential: false
  odom_frame: odom
  base_link_frame: base_link
  world_frame: odom

  two_d_mode: true

image description

after modifying the matrices: image description

  seq: 4588
  stamp: 
    secs: 1643283813
    nsecs: 468730450
  frame_id: "odom"
child_frame_id: "base_link"
pose: 
  pose: 
    position: 
      x: 0.050336928747
      y: 0.00634622035396
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.128117776541
      w: 0.991758960299
  covariance: [108004.85516289844, -585916.9443093863, 0.0, 0.0, 0.0, -0.13113662749975435, -585916.9443093836, 3195357.383071448, 0.0, 0.0, 0.0, 0.7789360876951698, 0.0, 0.0, 4.996529035234899e-07, -6.190104952331789e-22, -1.4853369544840475e-17, 0.0, 0.0, 0.0, -6.190104952331788e-22, 4.993067684356823e-07, -2.648985345882294e-29, 0.0, 0.0, 0.0, -1.485336954484047e-17, -2.648902855234479e-29, 4.993067684356823e-07, 0.0, -0.13113662749975516, 0.7789360876951649, 0.0, 0.0, 0.0, 5.548461022681045]
twist: 
  twist: 
    linear: 
      x: 0.00294294795962
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.00349156370239
  covariance: [0.0012169734244174603, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 2603.2137443508523, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 4.994797161274615e-07, -3.806581089190225e-31, -9.048180481660797e-27, 0.0, 0.0, 0.0, -3.806581089190226e-31, 4.979317389049878e-07, -6.392718743071225e-38, 0.0, 0.0, 0.0, -9.04818048166079e-27, -6.392440735414209e-38, 4.979317389049878e-07, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001601166305818822]

thanks

Comments

@Delbina Did you end up solving your issue or are you still having problems?

404RobotNotFound ( 2022-02-01 07:28:14 -0600 )edit

I have put false the angular velocity from the wheel odometer. it seems not bad. i have accepted the answer, and if there was any other issue, I will ask here.

thanks a lot

Delbina ( 2022-02-01 07:53:32 -0600 )edit

add a comment

answered 2022-01-26 21:59:49 -0600

404RobotNotFound
331 ●19 ●8

updated 2022-01-27 07:21:54 -0600

Edit

@Delbina Something I should have noticed the first time, your kalman filter accepts the angular Z value from both the odometry ( from the diff drive controller, not the one produced by the kalman filter ) and the imu. I would check the message from both of those while running and make sure that they generally agree. If they are different, then I would choose the one you think is more correct.

It is probably best to view them with rqt_plot, where you can just add the angular z values to the plot and it will plot them side-by-side for you.

To choose which is more correct, I would try to spin in place at a constant rate input and see what values are being reported from each. You can calculate from the rate how many seconds it should take you to reach 2 Pi (a full rotation). Count the seconds it takes in real life, get the angular velocity measurement (2pi / time for full rotation), and choose the input that reports angular velocities closer to that value.

As an easier test, just remove the true from the odometry angular Z input and keep the true for the imu. Check performance. Repeat with removing the imu input altogether at that point, and just using the odometry velocity input. However, at that point, you might be able to just get away with using that odometry input directly and letting it publish to tf.

In theory, you can have both reporting the angular Z input, but in that case you really want to have your covariance values tuned correctly.

Original

A few things to try:

For the IMU data, you have the IMU reporting the acceleration in X and Y. While your IMU probably has an accelerometer, you probably don't need to use the data and might be some source of your drift. The easiest thing would be to set those values to false.
The odometry that comes from the diff drive controller only probably reports velocities in linear X and angular Z , remove the true for linear Y velocity.

Doing those two things, your ekf configuration should look like this:

ekf_localization:
  frequency: 50
  two_d_mode: true
  odom0: /warthog_velocity_controller/odom
  odom0_config: [false, false, false,
                 false, false, false,
                 true, false, false,
                 false, false, true,
                 false, false, false]
  odom0_differential: false
  imu0: /gx5/imu/data
  imu0_config: [false, false, false,
                false, false, false,
                false, false, false,
                false, false, true,
                false, false, false]
  imu0_differential: false
  odom_frame: odom
  base_link_frame: base_link
  world_frame: odom

  two_d_mode: true

If this does not fix your issue, try this:

Examine the odometry values coming from the controller, specifically from the /warthog_velocity_controller/odom. When the vehicle moves forward, the only thing that should increase that you care about is the linear X velocity increases positively. It should increase negatively when going backwards.
Repeat with examining the angular Z velocity while turning in place (assuming you have the functionality). It should increase positively when turning left, and negatively when turning right.

If any of those inputs look ... (more)

edit flag offensive delete link

Comments

hi @404RobotNotFound I did the changes, but the result becomes worse than before. I attached the pic in my question

Delbina ( 2022-01-27 05:46:17 -0600 )edit

Also, take the covariance values in display with a grain of salt. What you should care more about is the odometry arrow looking correct

404RobotNotFound ( 2022-01-27 07:20:31 -0600 )edit

add a comment

odometry/filters starts drifting a lot, how to fix it?

Comments

1 Answer

Edit

Original

Comments

Question Tools

Stats

Related questions

odometry/filters starts drifting a lot, how to fix it? edit

Comments

1 Answer

Edit

Original

Comments

Question Tools

Stats

Related questions

odometry/filters starts drifting a lot, how to fix it?