# robot_localization lag

I have robot_localization working pretty well, but I am noticing that it introduces a lag into the pose update, particularly when turning. On rviz, you see the laser hit pattern move first, then the robot pose updates and "catches up" to the laser pattern. Going to goals and avoiding obstacles are working pretty well and the accuracy of the overall pose is pretty good when the robot stops moving. In general, simple translations do not show this effect.

I am only using vx and vYaw as inputs to robot_localization, from wheel odometry and a gyro, respectively. I tried varying the covariance of vYaw, but the problem persists over a wide range of covariances.

Suggestions?

edit retag close merge delete

( 2016-01-04 03:27:50 -0500 )edit

Sort by » oldest newest most voted

I'd have to see input messages and your launch file. Also, see this question. In addition, try increasing the process_noise_covariance parameter for yaw velocity (not the measurement covariance).

EDIT

Looking at your bag and launch files, a few things jump out at me. There are things that could be causing your issue, and things that are probably unrelated, but should be fixed.

Potential Causes for Delay

1. The EKF and UKF in robot_localization are typically subject to a two-spin-cycle delay. Part of this is due to the use of TF message filters. I've managed to get this down to one cycle in the latest indigo-devel source. However, part of your problem is that you have your frequency set to 20 Hz. If every measurement is delayed by two spin cycles, then you're looking at a lag of 0.1 seconds, which is quite a long time. This also appears to align with what I'm seeing when I plot the IMU input for yaw velocity and the filter output. Grab the latest source and see if that brings it down a bit. Then up the frequency parameter, or decrease the sensor_timeout parameter (or both).
2. Maybe up the process_noise_covariance a bit more for yaw velocity.
3. You have yaw set to true for your IMU data, which means you are (presumably) using a magnetometer. I personally find magnetometers to be nearly useless indoors, but YMMV.

Other Things That You Should Correct

1. Despite the use of yaw information from your IMU, you have your initial_estimate_covariance yaw value set to 0.01. That will make the filter slow to integrate the yaw measurements initially, which may or may not have been your intention. Making matters worse is that your yaw covariance value in your measurement is 1000000.0, which will effectively make the filter ignore those measurements.

2. Your IMU is reporting at a rate of 9.89 Hz, which seems low for an IMU, and makes me wonder if it's supposed to be 10, and there's a delay in the handling of the raw data. If you can up the frequency on your IMU (and the EKF), then it might be helpful.

3. You have nothing measuring Y velocity. If your robot cannot move laterally, then go ahead and set Y velocity to true for your odometry, and make sure the odometry data has a small covariance value for Y velocity.

more

Here are bag files, with and without robot_localization running: https://www.dropbox.com/s/5io70piu98w...https://www.dropbox.com/s/dgna06kw5pq... Launch file: https://www.dropbox.com/s/fwsf5hc6vgd...

( 2015-12-08 15:09:15 -0500 )edit

In the case without robot_localization, we are just publishing the raw wheel odometry and gyro yaw angle without filtering. You can see that with robot_localization, translation is fine but rotation seems to momentarily go the wrong way, as revealed by the laser scan then "catches up" but with lag

( 2015-12-08 15:13:06 -0500 )edit

Thanks for the feedback. I tried all of your suggestions and the lag has improved, but not resolved. Using the dev version of code now. The parameter that made the most difference was the update rate of the filter (to 50 Hz). I increased the IMU rate as well (also to 50 Hz).

( 2015-12-26 22:36:59 -0500 )edit

I increased the process_noise_covariance and the initial_estimate_covariance and he sensor_timeout. I set Y velocity to true and made its covariance small. These things did not seem to make much difference.

( 2015-12-26 22:42:10 -0500 )edit

Here are the bag files with and without robot_localization running. When robot_localization is not running, the raw odometry is used without any filtering. amcl seems to track the pose better with the raw data. https://www.dropbox.com/sh/octco12yqs...

( 2015-12-26 22:45:33 -0500 )edit

There will never be non-zero lag. You'll always be behind by ~1 spin cycle + the time required to integrate your measurement (the latter is why I am having you play with covariances). Not much to tell you at this point, other than to make sure your IMU driver itself isn't lagged.

( 2015-12-28 07:32:12 -0500 )edit

Sounds good. Once again, thanks for the help.

( 2016-01-02 11:51:02 -0500 )edit

We are having a similar issue. Did you ever get this resolved?

( 2016-06-02 17:42:14 -0500 )edit