# Transient odometry rotation error (robot_localization)

## TLDR:

I'm working on a differential drive base with encoders for odometry and an IMU. I'm working on fusing these data sources using robot_localization in an effort to get a more accurate odometry estimate so that our localization will perform better. Outside of some reasonable noise, all of our data tracks desired yaw velocity pretty well:

However, we are experiencing a significant transient yaw error when starting or stopping a turn. There is not a corresponding error in linear motion.

## The long version:

I'm working on a differential drive base with encoders for odometry and an IMU. I'm working on fusing these data sources using robot_localization in an effort to get a more accurate odometry estimate so that our localization will perform better. Outside of some reasonable noise, all of our data tracks desired yaw velocity pretty well:

However, we are experiencing a significant transient yaw error when starting or stopping a turn. There is not a corresponding error in linear motion.

Assuming that everything is functioning as it should be, we should be able to rotate the robot with laser scan persistence set to a couple of seconds in rviz, and see the environment remain static relative to the robot.

Ideally, when rotating, the lidar scan (with persistence) should look similar to this static lidar scan:

When rotating and at steady state, the rotating lidar scan looks like this (which is pretty close to desired):

When we start or stop rotation, however, it looks like this:

Which is significantly worse than desired. Ultimately, this leads to localization error when turning around corners and the like, which then ultimately messes up our navigation.

Here's a bagfile for your perusal:

Bag File

Topic names are namespaced by robot name (in this case, p1_001), so:

/p1_001/mobile_base_controller/cmd_vel is commanded velocity

/p1_001/scan is the lidar scan

/p1_001/mobile_base_controller/odom is the raw odometry

/p1_001/mobile_base_controller/imu is the raw imu data

/p1_001/odom is the fused output from robot_localization

I would appreciate it greatly to hear your thoughts on the matter!

EDIT:

Here are launch and config files for relevant nodes.

ROS control launchfile, launches hardware interface, Diff drive and IMU controller

ROS control config, mostly sensor covariances

Robot Localization Config file

edit retag close merge delete

Any conclusion on this?

( 2016-02-08 01:02:12 -0500 )edit

Sort by » oldest newest most voted

Can you post your launch file?

Looking at your plots, it looks as if your IMU registers the turn long before your wheel encoder odometry. Additionally, it looks as if the filtered output is tracking much closer to your wheel encoder odometry than your IMU. I'd have to look at their relative covariances (which I realize are available in the bag file) to determine why you're seeing a delay in the filter response, but my first checks would be to (a) make sure that your angular velocity in your controller is reported as soon as the robot starts turning (there's some lag between the two, if I'm reading your plot correctly), and (b) the relative covariance between your IMU angular velocity and odometry angular velocity are set correctly, i.e., I'd expect a much lower variance on yaw velocity for the IMU than for your wheel encoders. Finally, you might want to also try increasing your initial_estimate_covariance for yaw velocity. This will help it converge faster when your robot is first starting.

EDIT 1

Still looking at this. A few things:

1. Did you intentionally turn the yaw off for your IMU0 config?

imu0_config: [
false, false, false,
false, false, false,
false, false, false,
false, false, true,
true, true, true
]


Just want to make sure that was right.

2. Have you had a chance to look into the delay in your wheel encoder odometry?

3. Regardless, here's a sample odometry message:

header:
seq: 1997
stamp:
secs: 1445291702
nsecs: 645665414
frame_id: odom
pose:
pose:
position:
x: -0.00496491246256
y: 2.86013970781e-05
z: 0.0
orientation:
x: 0.0
y: 0.0
z: -0.158302775866
w: 0.987390617311
covariance: [0.001, 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.1, 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.1, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.001]
twist:
twist:
linear:
x: 0.00780897589398
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: -1.10046405246
covariance: [0.0001, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0001, 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.1, 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.001]


...and here's a sample IMU message:

header:
seq: 1965
stamp:
secs: 1445291701
nsecs: 25784241
frame_id: p1_001/imu
orientation:
x: -0.608557879925
y: -0.377054542303
z: 0.5998955369
w: -0.357228845358
orientation_covariance: [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]
angular_velocity ...
more

Relevant launch/config files uploaded. Let me know if anything else would be relevant.

( 2015-10-22 08:17:09 -0500 )edit

I'll try to check this out tonight. Sorry for the delay.

( 2015-11-10 11:30:08 -0500 )edit

Much appreciated.

( 2015-11-10 13:53:36 -0500 )edit

I have a similar problem, which is coming from the fact that although my robot is a differential robot, it does not have simply two wheels but four tracks instead (like a tank). As a result depending on the floor material we experience different amount of error in the odometry.

( 2015-11-18 07:12:16 -0500 )edit

I guess integrating IMU data is not a promising solution when you robot is slow, I tried to the exactly the same thing and it didn't worked. A better way could be trying to use laser scanner data and performing a laser match. there is a node called laser_scan_matcher but I couldn't install it...

( 2015-11-18 07:15:07 -0500 )edit

In this case, though, you can see it the MATLAB plots that the EKF output is tracking much closer to the wheel encoder odometry than the IMU. I'd like to see what happens if that is addressed (along with the other ideas I suggested).

( 2015-11-18 07:30:57 -0500 )edit

You mean that you want to determine how much your EKF output should rely on the IMU data?

( 2015-11-18 07:57:08 -0500 )edit
1

If you mean this laser_scan_matcher, you unfortunately need to build it from source. RE: IMU data: The above plots show the EKF tracks encoder data more tightly than IMU data, but the IMU data responds faster. Tom wants to see EKF data when this is swapped.

( 2015-11-18 08:27:00 -0500 )edit