0
$\begingroup$

Rosanswers logo

Hello everyone,

I have been trying to fuse my robot's odometry and IMU using EKF. However, I noticed that the filtered yaw was always lesser than the actual yaw of the robot. I tried rotating my robot on the spot and comparing the yaw published by both the IMU and the filtered odometry.

Image of robot rotating

In the image, the green arrows are from the EKF filtered odometry while the gold arrows are produced by directly taking the imu's yaw. When the robot has made one full rotation, the imu's arrows form a complete circle as expected but the filtered odometry's yaw stops short of a full rotation. This error slowly builds up and increases the more I rotate the robot.

This is my EKF params configuration file:

frequency: 20
sensor_timeout: 0.05
two_d_mode: true
transform_time_offset: 0.0
transform_timeout: 0.0

print_diagnostics: true
debug: false
debug_out_file: /path/to/debug/file.txt
publish_tf: true
publish_acceleration: false


map_frame: map              # Defaults to "map" if unspecified
odom_frame: odom            # Defaults to "odom" if unspecified
base_link_frame: base_footprint  # Defaults to "base_link" if unspecified
world_frame: odom           # Defaults to the value of odom_frame if unspecified


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


imu0: /imu
imu0_config: [false, false, false,
              false,  false,  false,
              false, false, false,
              false,  false,  true,
              false,  false,  false]
imu0_nodelay: false
imu0_differential: false
imu0_relative: true
imu0_queue_size: 5
imu0_remove_gravitational_acceleration: false


use_control: true
stamped_control: false
control_timeout: 0.1
control_config: [true, false, false, false, false, true]

acceleration_limits: [2.6, 0.0, 0.0, 0.0, 0.0, 6.5]
deceleration_limits: [2.6, 0.0, 0.0, 0.0, 0.0, 6.5]
acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]
deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0]

    
process_noise_covariance:
 [0.05, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
  0,    0.05, 0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
  0,    0,    0.06, 0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
  0,    0,    0,    0.03, 0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
  0,    0,    0,    0,    0.03, 0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
  0,    0,    0,    0,    0,    0.05, 0,     0,     0,    0,    0,    0,    0,    0,    0,
  0,    0,    0,    0,    0,    0,    0.350, 0,     0,    0,    0,    0,    0,    0,    0,
  0,    0,    0,    0,    0,    0,    0,     0.010, 0,    0,    0,    0,    0,    0,    0,
  0,    0,    0,    0,    0,    0,    0,     0,     0.04, 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.01, 0,    0,    0,    0,
  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0.80, 0,    0,    0,
  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,    0,    0,    0,    0.01, 0,
  0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.015]


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,    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,    100,  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,    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,     100,   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,    1e-9]

Sample IMU message

---
header: 
  seq: 32891
  stamp: 
    secs: 1420071228
    nsecs: 713141000
  frame_id: "imu"
orientation: 
  x: -0.0192271154374
  y: -0.00285466690548
  z: -0.0113646220416
  w: -0.999746441841
orientation_covariance: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]
angular_velocity: 
  x: -0.000560106534977
  y: 0.000456087000202
  z: 0.000268710689852
angular_velocity_covariance: [0.05, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.05]
linear_acceleration: 
  x: 0.0
  y: 0.0
  z: 0.0
linear_acceleration_covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
---

Sample Odom message

---
header: 
  seq: 14521
  stamp: 
    secs: 1542769157
    nsecs: 852334721
  frame_id: "odom"
child_frame_id: "base_footprint"
pose: 
  pose: 
    position: 
      x: 0.276418488316
      y: -0.258039904366
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: 0.139672427641
      w: 0.990197764569
  covariance: [0.15, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2]
twist: 
  twist: 
    linear: 
      x: 0.0
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
---

So far I have tried playing around with the process noise for both yaw and v_yaw, as well as adjusting the covariance of my odometry message, but nothing seems to be solving this problem. Is there anything that I have overlooked?

I would really appreciate any help that I can get on this. Thank you!


Update

Thank you Tom so much for the reply.

Regarding Point 1, I have checked and both my IMU and Wheel encoders produce the correct sign when turning (ACW is positive).

I actually solved this problem a couple of hours ago too. Indeed, the use_control parameter was causing the issue with this. For my robot, cmd_vel is only published once, but the robot will continue moving until it receives another zero command. This causes the control to timeout while the robot is still moving, which resulted in the yaw error. I tried setting a very large timeout (100s) as well as turning off control and both methods immediately solved this issue.

However, upon doing so, the filtered odometry started following the exact same pose as the wheel odometry and no matter how I tweaked the covariances, it still remained the same. This was immediately solved by publishing the base_footprint->imu transform that you have mentioned (which was previously non-existent). The output filtered odometry now returns much more favorable results.

*Green arrow --> Filtered Odom, Red arrow --> Wheel Odom

The weird thing is that even though the previous absence of a base_footprint->imu transform causes my IMU data to not be fused, the output filtered odometry (despite the yaw error) still managed to give me rather acceptable results, leading me to believe that the IMU message was indeed being fused in. But when i turn off the use_control parameter (or give it a very high timeout), the output filtered odometry becomes the same as wheel odometry.

I am still rather confused about the base_footprint->imu transform though. May I ask why this transform is necessary and just the /imu topic itself is insufficient? Also, if there is to be more than one imu, then wouldn't there be multiple imu frames needed? I do not recall seeing any options to set the imu frame.


Originally posted by wiireless on ROS Answers with karma: 3 on 2018-11-21

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

Thank you for providing sample input messages. It might be helpful if you could capture two messages from when the robot is actually being rotated.

A few possibilities spring to mind.

  1. Given the much smaller covariance for the IMU (which makes sense), I wonder if your wheel encoder odometry is producing the wrong sign when turning. One thing you didn't show in your image was the rotation for raw odometry.
  2. What is providing the base_footprint->imu transform? Without it, your IMU data is not going to get fused.
  3. You should turn off the use_control parameter while you sort this out.

This error slowly builds up and increases the more I rotate the robot.

This is to be expected, regardless of what you change. Your IMU's yaw is coming from a magnetometer (I assume), so it won't be subject to drift. In the EKF, you are only fusing yaw velocity, and the errors in that velocity are going to integrate over time.

EDIT in response to updated question:

I am still rather confused about the base_footprint->imu transform though. May I ask why this transform is necessary and just the /imu topic itself is insufficient? Also, if there is to be more than one imu, then wouldn't there be multiple imu frames needed? I do not recall seeing any options to set the imu frame.

I'd recommend that you have a look at the tf2 wiki, but just as a short example, consider a system where you have two identical IMUs mounted on your robot. One is mounted upside-down, and the other is mounted right-side-up. If we just fused their data in the EKF, then when we rotate our robot clockwise, one sensor would read +Z angular velocity, and the other would read -Z angular velocity. That obviously doesn't make sense, but how can the EKF know that one of the sensors is upside-down?

The answer is that you need to use transforms to tell the EKF how that sensor is mounted. So you define a transform from base_link to imu_rightsideup, and it might be just the identity transform (which means the IMU is mounted at your robot's body origin). In that IMU's mesages, you would want to make sure that the frame_id was set to imu_rightsideup. Then the EKF knows to look for a transform (via tf2) from that frame_id to the frame_ids that it knows about. For the other IMU, you would define a transform from base_link to imu_upsidedown. That transform would have a roll angle of pi radians. Then you would configure the IMU node to change the frame_id in the message to imu_upsidedown. When the EKF receives that message, it can again use tf2 to look up how to transform that data into a frame that it can use.


Originally posted by Tom Moore with karma: 13689 on 2018-11-26

This answer was ACCEPTED on the original site

Post score: 2


Original comments

Comment by wiireless on 2018-11-26:
Thank you Tom, I have updated my question with some results and thoughts. To summarize: removing use_control solved the yaw error problem, which then exposed the problem of IMU data not being fused. This is then solved by introducing a static transform from base_footprint -> imu.

Comment by ar4angel on 2018-12-24:
Hello Tom @Tom Moore, can you have a look at this question, please. It is very similar to this one (problem is the same) https://answers.ros.org/question/311304/not-accurate-results-of-yaw-when-fusing-wheel-encoders-with-imu-using-robot_localization/ Thank you in advance

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.