Revision history [back]

A few things to try:

1. 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.
2. 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:

1. 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.
2. 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 like garbage, then that would be the cause of your error and would need to dive into how that odometry is produced. Here is a link for the odometry source file for the ROS diff drive controller.

A few things to try:

1. 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.
2. 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:

1. 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.
2. 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 like garbage, then that would be the cause of your error and would need to dive into how that odometry is produced. Here is a link for the odometry source file for the ROS diff drive controller.

@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.

A few things to try:

1. 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.
2. 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:

1. 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.
2. 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 like garbage, then that would be the cause of your error and would need to dive into how that odometry is produced. Here is a link for the odometry source file for the ROS diff drive controller.