How to best integrate imprecise GPS into localization pipeline?
Hi, we've started integrating GPS into our localization pipeline using robot_localization. Reading through all of the available sources, I got the impression that it is fairly easy to set up if you have a good GPS and a good compass.
TL;DR: Neither of these is the case for our robots. We can't rely on the compass, and the GPS signal may be bad. But we want smooth Earth-referenced odometry (as much as possible given the absolute measurements quality). This is a general question where I try to figure out the approaches. Although I do have a concrete implementation and configuration at hand, I don't feel at this time it is important to share it. But of course I can if somebody finds it useful.
Magnetometer from IMU works at least a bit only after doing a 360 turn with the robot to calibrate magnetic bias. We've already resigned to doing this from time to time and save the calibrations. On some robots, they're valid for a long time, while on others not. Importantly, the unbiased mag measurements are only valid in the X-Y plane in which we calibrated the magnetometer. When the robot has a non-zero pitch/roll, the magnetometer measurements are nonsense. Doing a more proper 3D calibration does not seem reasonable, as the robots are research platforms where people add/remove parts on a daily basis.
With GPS, we do everything we can to get precise measurements, but we cannot use an RTK base placed nearby. We subscribe to NTRIP over LTE, we put the GPS antennas on a 1 m pole to get further from the robot body, we add metal ground plane under the antenna. Under clear sky, the precision gets close to a few centimeters. However, the robots are expected to drive via any outdoor location, so we also need to localize in forests, where the fix precision jumps between decimeters and hundreds of meters. What's worse, we've caught the receiever reporting a few centimeter precision while being meters off.
All of this put together, running the standard dual EKF with navsat_transform_node results in veeery jumpy odometry. On startup, the utm frame has to converge to the odometry frame, so the fused trajectory is complete nonsense for the first few meters, even if I set initial covariance in yaw to a large value. I somehow expected the compass would help with this, but even if we supply a nicely working compass measurement, the yaw still seems to be off when GPS measurements start being integrated. When yaw gets stabilized after this initial period, all the GPS fix jumps around the robot have a very bad effect on yaw estimate - I guess this is due to the holonomic motion model wired into robot_localization? I.e. the filter thinks the robot shifted 1 meter to the left, so it concludes it had to do it by changing its yaw and driving forward... I'm a bit confused this happens ...
