robot_localization drifts when gps is missing (with RTK GPS, heading and IMU)

asked 2019-09-05 08:48:50 -0500

davidllp gravatar image

updated 2019-09-06 00:56:25 -0500

Hi everyone,

After searching on the forum, I haven't found any answer that solved this issue so I hope it is not a duplicate. The system we are using is: an outdoors robot that performs a high precision mapping of objects detected by a camera. To this objective the robot computes the localization using the following sensors:

  1. GPS RTK:
    • topic: /piksi_receiver/navsatfix_rtk_fix
    • msg_type: sensor_msgs/NavSatFix
  2. Heading RTK (in fact, there are 2 GPS antennas, and their drivers compute a high accurate heading using their position)
    • topic: /heading (the driver sends a custom message in degrees and True north referenced, then, we transform it to ENU, so it complies with the required orientation specified by the robot_localization).
    • msg_type: sensor_msgs/Imu
  3. Imu
    • topic: /imu0 (the imu has a magnetometer, but it's magnetic field is highly affected by the system, so we are using it as a VRU device and we are resetting the orientation.yaw with the known /heading, so it complies with the required ENU orientation).
    • msg_type: sensor_msgs/Imu

Covariances of all sensors are set accordingly to their datasheets.

Problem explanation

While there is RTK quality of the gps everything works smoothly, however, when there's a loss in the quality of the gps, the position it provides has an important offset. Hence, we decided to only use the gps position when the signal has RTK quality. And we though that the ekf would fill in the missing data with the heading and the imu. However, as it can be seen in the images, while the heading (light blue arrow) keeps the correct orientation, the base_link has a drifting to the right over time. We tried different configurations of the robot_localization but all results were similar.

Has anyone an idea of what could be happening? Is there something wrong in the configuration of the robot localization? Are we missing something?

Thanks in advanced for your help!


Images of the results commented hereafter (link to image as I don't have rights to upload them). Legend:

  • Bright green points: rtk gps points (in WSG84)
  • Grey points: bad gps quality (in WSG84)
  • Green, Red and Blue lines: are the frames tracking for Reference (gps1), Attitude (gps2 for heading computing) and Robot base_link (odometry of frames in /odom)
  • Light blue arrow: is the odometry of the robot with heading.

robot_localization configuration

All transforms are defined in a URDF except by the transform from map to odom that is specified as a static transform in the roslaunch of the localization node (see below).


frequency: 60
sensor_timeout: 0.1
two_d_mode: false
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
base_link_frame: base_link  # Defaults to "base_link" if unspecified
world_frame: odom           # Defaults to the value of odom_frame if unspecified

predict_to_current_time: true

odom0: /odometry/gps
odom0_config: [true,  true,  true,
               false, false, false,
               false, false, false,
               false, false, false,
               false ...
edit retag flag offensive close merge delete


IMUs accumulate a lot of position error over time. Using the imu feed as your only source of odometry is going to give you a lot of drift - because you are double integrating that noisy acceleration measurement.

Also if your imu orientation is incorrect, and your imu reports gravity in its accelerometer, your system will be interpreting gravity as some acceleration in all axes.

Hopefully Tom Moore comments on this, I am only a localizing beginner.

johnconn gravatar imagejohnconn ( 2019-09-06 10:05:10 -0500 )edit

Actually, we did a test using the robot_localization only using the IMUs (configured in the same as the code provided) to observe the odometry it was producing. While the absolute positioning was obviously off, the odometry was a quite smooth straigh line. So, in this case, I'm not sure it's a IMU drifting issue. Additionally, the IMU orientation has been triple check because we also though it could be that! (However, it still remains a little bit unclear for me, how the orientation of each field must be expressed. Though, I'm pretty sure it is correct right now). With respect to the gravity, there is a parameter in the configuration of the robot_localization that eliminates the gravity if the IMU doesn't do it (imu0_remove_gravitational_acceleration). I'll try to provide a bag this afternoon to reproduce the system.

davidllp gravatar imagedavidllp ( 2019-09-09 02:15:33 -0500 )edit