Ask Your Question

Fused IMU and Odom heading is wildly incorrect

asked 2020-11-04 17:18:55 -0500

abend gravatar image

updated 2022-01-22 16:10:51 -0500

Evgeny gravatar image

Platform is a Husky A200 with smooth tires, driving on a linoleum over concrete floor, IMU is a Lord Microstrain 3DM-GX5-25. I'm using ROS Melodic on Ubuntu 18.04 (bionic) on an x86 laptop (Asus U46E). The drivers for the Husky are the ones provided by Clearpath on Github, the drivers for the IMU are the ros_mscl library supplied by Lord on Github. At the moment, I think both of these components are working correctly and I have misconfigured robot_localization somehow.

My test procedure was to drive the robot in a square by teleop, and record a bag file (which is attached). I play back the bagfile while visualizing the wheel odometry from /husky_velocity_controller/odom and the fused odometry from /odometry/filtered. I'm trying to tune the process_noise_covariance and initial_estimate_covariance so that the fused odometry shows the robot driving in a square.

What I see in RViz is that the wheel odometry looks very good. The square looks square to within my ability as a driver, and the edges look straight. The fused odometry has straight-ish paths of what appears to be the right length, but the angles of each turn are too small, and the robot appears to swing in the right direction and then swing mostly back before continuing on.

I can't upload images, so a RViz screenshot is at red is the wheel odometry, green is the fused odometry.

What I would like to see is that the fused odometry matches the wheel odometry to some degree, showing the robot turning in a square.

I don't have a covariance matrix for the IMU as provided by a datasheet, but the driver provides the ability to explicitly set one and I wrote a node to change it when the bag is playing, so I could try different values. Setting it to 0.01 or 0.001 did not appear significantly different.

I'm not using 2D mode because the eventual goal is to drive outside and do SLAM on uneven terrain, so the roll/pitch/yaw of the robot ends up mattering.

My current localization.yaml is

odom_frame: odom
base_link_frame: base_link
world_frame: odom

two_d_mode: false
frequency: 100

odom0: husky_velocity_controller/odom
odom0_config: [false, false, false, # x, y, z
               false, false, false, # roll, pitch, yaw 
               true, true, true, # x vel, y vel, z vel
               false, false, false, # roll vel, pitch vel, yaw vel
               false, false, false] # x accel, y accel, z accel
#odom0_differential: true
odom0_queue_size: 10

imu0: ned_imu #imu_converter/ned_to_enu
imu0_config: [false, false, false, # x, y, z
              true, true, true, # roll, pitch, yaw
              false, false, false, # x vel, y vel, z vel
              true, true, true, # roll vel, pitch vel, yaw vel
              false, false, false] # x accel, y accel, z accel
imu0_differential: false
imu0_queue_size: 10
imu0_remove_gravitational_acceleration: true

                           #x    y     z     R     P     Y     x_vel  y_vel  z_vel R_vel P_vel Y_vel x_acc y_acc z_acc
process_noise_covariance: [0.05, 0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0,
                           0,    0.05 ...
edit retag flag offensive close merge delete


When just inputting velocity/accel measurements, your covariance will always increase.

You are inputting orientation from your imu, so it's expected that the kalman filter maintains low covariance on it's orientation output.

Are you sure your imu is working correctly? If your wheel odom is giving you distance info, and your imu is giving you orientation info, and you trust the wheel odom, the imu should be suspected.

johnconn gravatar image johnconn  ( 2020-11-05 20:29:36 -0500 )edit

I think it's working correctly.

I did have it getting sampled 20x more frequently than the wheel odom, but even with them both at the same frequency (10Hz), I get the same results. I tried graphing the IMU and wheel odom reported yaw velocity, and they agreed well in magnitude and direction, although with the IMU very slightly lagging the wheel odom.

My IMU doesn't report its own covariance matrices for the outputs, but I can specify one to the driver that gets repeated in each message. I haven't had a lot of luck getting different results by changing that, though.

abend gravatar image abend  ( 2020-11-05 21:01:18 -0500 )edit

What about the orientation values themselves, not the velocities? You are also adding those absolute measurements from the imu. If the velocities looked good, I would remove those absolute orientation inputs from your kalman filter and see what happens.

johnconn gravatar image johnconn  ( 2020-11-06 09:34:06 -0500 )edit

That was it, if you want to make that an answer instead of a comment, I can accept it.

abend gravatar image abend  ( 2020-11-06 10:28:41 -0500 )edit

sure I can do that :)

johnconn gravatar image johnconn  ( 2020-11-06 11:07:41 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted

answered 2020-11-06 11:11:21 -0500

johnconn gravatar image

The problem is that you are inputting absolute orientation from your imu into your Kalman filter. The imu being used either isn't meant to send these values or has an error in its orientation calculation.

The imu orientation velocities are correct though, so inputting only those values into your filter will give you better results.

With only velocity measurements input to your filter, your covariance will continue to rise forever as the system accumulates more uncertainty about it's position/orientation.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower


Asked: 2020-11-04 17:18:55 -0500

Seen: 203 times

Last updated: Nov 06 '20