# Not accurate results of yaw when fusing wheel encoders with imu using robot_localization

Hello,

I have differential drive Jaguar 4x4 wheel mobile robot. I am trying to fuse wheel odometry data with IMU sensor measurements using robot_localization. The problem is that after some time (about 1-2 minutes) /odometry/filtered starts showing uncorrect results of yaw or starts changing rotational velocity when robot is stationary.

My yaml file:

map_frame: map
odom_frame: odom
world_frame: odom

two_d_mode: true

frequency: 30

odom0: /odom
odom0_config: [false, false, false,
false, false, false,
true,  true,  false,
false, false, true,
false, false, false]
odom0_differential: false

imu0: imu/data_raw
imu0_config: [false, false, false,
false, false, false,
false, false, false,
false, false, true,
true, false, false]
imu0_differential: false
imu0_remove_gravitational_acceleration: false
imu0_relative: false

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, 1e-9, 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, 1e-6, 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, 0.1, 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, 1e-9, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9]

process_noise_covariance:     [0.001, 0,  0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0.001, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0.001, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0.001, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0.001, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0.001, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0.001, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0.001, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0.001, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0.001, 0, 0, 0, 0 ...
edit retag close merge delete

Could you please come up with a more descriptive title? "robot_localization problem" is rather generic and does not really describe anything.

( 2018-12-21 08:15:32 -0500 )edit

Sort by » oldest newest most voted

Can you clarify what you mean by "incorrect" yaw? What exactly is incorrect about it? I'm assuming you realise that when you fuse only yaw velocity data, the estimate will start to drift over time. Moving while stationary is a different issue, though.

I agree with the sentiment that you should perhaps turn off linear acceleration, though it's less of a problem when you have a velocity reference, as you do.

How is your IMU mounted? You posted sample IMU data (many thanks), but you didn't show me what the base_link -> new_imu transform is. I'm assuming it's mounted in a neutral position, given the linear acceleration values that I see, but I want to be sure.

Finally, at the point that you notice false motion in the robot when it is stationary, have you tried actually plotting the raw IMU values for Z angular velocity (or whatever axis makes sense, given the IMU's mounting)? Can you post values from all the sensors, as well as the EKF output, while you see the robot showing false rotational motion? Thanks!

more

What grade IMU are you using? If you only give the ekf angular speed around the vertical axis, it will have to integrate it to deduce yaw. Any small bias/error in angular speed is going to grow really fast since you are squaring angular speed to get the yaw. Rather than feeding angular speed to the ekf, try to give it the actual yaw from an electronic compass.

Also, unless the robot is perfectly flat, you will have issues with gravity interfering with linear acceleration measurements. Robot_localization can remove gravity, but for that it must know the pitch. In my experience, using acceleration to deduce change in position (unless using extremely high grade accelerometers) is a waste of time. You're better off relying on the wheel odometry for that.

more

It looks like you're fusing linear acceleration from your IMU. That tends to be pretty noisy and generally has non-zero bias, unless you have previously taken care of those problems or have an IMU that handles that stuff for you (in which case, tell me which one and I'll order a few :-) ).

I'd recommend fusing in the IMU's gyro data and fusing only the wheel encoders for the linear segment. However if someone else has done something different, feel free to jump in.

more

Hello, thank you for answering,

I turned off linear acceleration of IMU. And now i am fusing only gyro yaw velocity and linear velocities from wheel encoders as you suggested. However, result is the same. Can you give any other advice? Thanks in advance)

( 2018-12-23 23:46:09 -0500 )edit

your covariances are also very low for hte process covariance, I'd do more reading and look at the defaults for typical values to try.

( 2018-12-26 20:15:07 -0500 )edit