# Robot Localization (Odom + IMU) - Persistent Error in Yaw

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
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 ...
edit retag close merge delete

Sort by » oldest newest most voted

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.

more

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.

( 2018-11-26 06:30:10 -0600 )edit

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/3113... Thank you in advance

( 2018-12-23 23:47:53 -0600 )edit