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

Improve this question
$\endgroup$

1 Answer 1

Reset to default
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

Improve this answer
$\endgroup$