robot_localization drifts when gps is missing (with RTK GPS, heading and IMU)
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:
- GPS RTK:
- topic:
/piksi_receiver/navsatfix_rtk_fix - msg_type: sensor_msgs/NavSatFix
- topic:
- 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
- topic:
- 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 theorientation.yawwith the known/heading, so it complies with the required ENU orientation). - msg_type: sensor_msgs/Imu
- topic:
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
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).
ekf_imu_heading_localization.yaml
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 ...
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.
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.