The Problem (in short)
I am using the excellent robot_localization package to localize my robot in an outdoor environment. I'm trying to make the localization as accurate as possible, and it's currently accurate to <25cm (my GPS is accurate to ~20mm so that's what I'm aiming for). However this last 25cm is dependent on the heading of the robot. I.e. the error changes depending on the direction the robot is facing, as demonstrated if I simply rotate 360 degrees on the spot:
I am trying to find out why this is happening and how I can fix the issue so accuracy is independent of heading. This error is periodic, and consistent across several tests, so I think it is being cause by as systematic error in navsat_transform or in the way the frame transforms are being handled. All info and data is given under "System Information" at the bottom of this page.
The Problem (in full)
I have three sensor inputs: wheel odometry, IMU data, and GPS data. I am using one instance of the ekf_localisation node to fuse wheels and IMU data ("local" odometry) and a second instance to fuse everything ("global" odometry). I am also using an instance of navsat_transform to convert my GPS data to a GPS odometry stream ("gps" odometry) which is what is being used in the global EKF node. When I rotate the robot on the spot, this is the odometry reported for each odometry stream:
Keep in mind that the local odometry is reported in the local /odom reference frame while the global and GPS odometry are both reported in the global /map frame. This leads me to believe that the problem is related to the navsat_transform node or one of the frame transforms that it uses.
I think there is actually a positive feedback loop in here since the GPS odometry is fused to produce the global odometry, but the position of the GPS odometry is partially determined by the global EKF which determines the location of the /map frame that the GPS odometry is relative to.
Now I know that my GPS is not the problem, because when I directly calculate the GPS UTM coordinates of the center of the robot (/base_link) it is different to the UTM coordinates obtained via sensor fusion (I'm effectively just looking at the origin of /base_link relative to the /utm frame which is broadcast by navsat_transform). This is what I get:
So the directly calculated data indicates that the robot is turning on the spot (as expected) however the UTM coordinates of /base_link relative to /utm match the previous graph. Also, when I directly calculate the UTM coordinates of /base_link I'm using the exact same WGS84->UTM calculations used by robot_localisation and I'm using the exact same magnetic declination as what I've specified in the launch file for navsat_transform.
Now when I watch the reference frames in RViz, the /base_link and /odom do not move relative to one another - the /base_link frame only rotates (as you would expect since it turns with the robot). The /map frame however moves relative to both. So this erroneous movement is really being caused by /map moving relative to /base_link rather than the other way around.
I currently have two working theories on the cause of this problem:
- There is a bug in navsat_transform. This could distort the GPS odometry, in turn distorting the global EKF, in turn distorting the position of the /map frame, which in turn further distorts the GPS odometry. If this were the case, I think it might be due to a change between v2.2.2 and v2.2.3 because I didn't experience this issue prior to updating to v2.2.3.
- My reference frames are set up incorrectly If this were the case, it won't be navsat_transform itself that's distorting the GPS odometry but one of the transforms it's using. I've triple-checked everything and according to the /tf topic all reference frames appear to be in the correct locations.
If the error were random or otherwise evenly distributed, I would have chalked this up to natural error in the system, but since it has a fixed relationship with the heading of the robot, it must be a systematic error. If anyone else has experienced this or otherwise has any advice, I'd be very appreciative.
- OS: Ubuntu 14.04 (laptop), ROS Indigo Igloo (robot)
- Robot: Clearpath Robotics Jackal UGV (real, not simulated)
- Sensors: IMU, wheel encoders, high accuracy RTK GPS
- Localization: package used is robot_localization (v2.2.3). IMU and encoder data is fused in the /odom frame (local odometry). IMU, encoder and GPS data is fused in the /map frame (global odometry).
- Launch file: ekf_and_navsat.launch
- Frame tree: tree
- Example bagfile: spin_test_bagfile
- Example sensor inputs: wheel_encoder_input.csv, imu_input.csv, gps_input.csv
- Example odometry outputs: local_odom.csv, global_odom.csv, gps_odom.csv
At @asimay_y's suggestion, I reduced the number of sensor inputs fusing yaw velocity to just the IMU. Unfortunately it hasn't significantly affected the global output, as shown here:
In hindsight this makes sense because the only difference between the local and global fusion is the GPS data, so if the yaw velocity was causing issues, I would have expected to see similar behaviour in the local odometry output.
Regarding the movement of /map relative to /odom in RViz, my understanding is that RViz converts the odometry from each source into the reference frame you are currently viewing (for me is was /odom at the time). And if the position estimate of the robot in the /odom and /map frames is different, then the odometry will reflect that. But the thing is, the location of these frames are effectively calculated relative to the robot's current position, so rather than the robot drifting and appearing in two places at once to represent the difference, the robot is consistent and the origins of the reference frames drift relative to each other. You'll only ever see /odom and /map matching if the odometry in both frames is exactly the same.
In addition, I've also collect some more data, just by making the robot spin on the spot, move a short distance and spin again, move, spin etc. So the odometry in each frame looks something like this:
Notice how the local data at each corner (when the robot spins) doesn't deviate, but the GPS and global data does, just like when the robot spins on the spot. The UTM coordinates calculated directly and via sensor fusion mirror this. This path produces the same periodic error at each corner where the robot stops and spins, as shown:
And to make things more interesting, the relationship between error and heading seems to drift:
Again, the same periodic relationship is observed, but it seems to drift sideways over time, perhaps as the IMU drifts or error builds up in the fusion estimate.
I haven't found the cause of the problem, but I have found a work-around by effectively writing a whole new node that copies navsat_transform and uses the /utm -> /map transform it broadcasts. I can provide source code if asked, but what it effectively does is this (/auxgps_link is the reference frame for my GPS receiver):
- At launch, before the robot moves, using steps 2-4, find the UTM coordinates of /base_link, record 50 unique sets of coordinates and average them and then store them - this will be the GPS-calibrated origin of the /map frame. Also, get the current /utm -> /map frame transform and store it. Then for every new navsat fix from the auilary GPS, do steps 2-6
- Convert the current latitude/longitude to UTM coordinates (using the same equations robot_localization uses)
- Use the /utm -> /base_link and /utm -> /auxgps_link to find the difference between /base_link and /auxgps_link in UTM meters
- Subtract this difference from the UTM coordinates of /augps_link to find the UTM coordinates of /base_link
- Find the difference between the current UTM coordinates of /base_link and the GPS-calibrated UTM coordinates of the /map origin
- Get the stored /utm -> /map transform and use the rotation component to rotate this UTM difference so that it is relative to the /map frame (effectively just rotating the points by the magnetic declination). Publish this as an odometry message that can be fused in a ekf_localization node.
Its hacky, but it works more effectively than navsat_transfrom does. So if I rotate on the spot, the odometry output from my node (auxgps_odometry) correctly reflects this but navsat_transform still doesn't:
And then if we plot the distance to the /map origin (i.e. 0,0) vs magnetic heading we get:
So navsat_transform still adds this heading-related error while my node doesn't. Note that here, the global instance of ekf_localization fused the output from my node (/navsat/ausgps/odometry) instead of the output from navsat_transform (/odometry/gps) and therefore uses an updated launch file. I still don't fully understand how navsat_transform works under the hood, but from what I can tell there are a few key differences:
- Navsat_transform doesn't do any sort of calibration of the /map origin like my code does, so (unless I've missed something) it can only work out the origin of the /map frame using an existing odometry source (the output of the global EKF in this case). And if that source of odometry is dependent on the output of navsat_transform (which it is) then you have a positive feedback loop. Any inaccuracy in the global EKF will make the origin of /map inaccurate, which will make the navsat_transform's odometry calculation inaccurate, which will make the global EKF even more inaccurate. It's a circular loop. But don't think this is the sole cause of the problem because the problem is too dependent on heading.
- I minimize the use of frame transforms. Because /auxgps_link is a child frame of the /base_link frame, if there are any systematic offsets in the /utm -> /base_link transform, they will affect both the /utm -> /base_link and /utm -> /auxgps_link transform equally and cancel out when I calculate the difference between /base_link and /auxgps_link. The only other frame transform I use, and the only one that could contribute error, I store at launch and never re-calculate, so if the origin of /map drifts relative to /utm at a later date, it won't affect my calculation of GPS odometry. From what I can tell navsat_transform doesn't store any transforms, it just uses them in real time as it needs to.
So unfortunately, this doesn't solve the issue, but proves conclusively that it is a bug in navsat_transform. Note that I'm using the exact same navsat fixes in both of them, and the same WGS84 -> UTM calculations, so the only differences are the frame transforms used and how they're handled. Because I now have a work-around (as hacky and improper as it is) I likely won't spend as much time trying to solve this, but I'll revisit it as and when I can. For what it's worth, I suspect that navsat_transform is not applying the /utm -> /map transform properly, so the UTM coordinates are not being properly converted to coordinates in the /map frame.
In response to @Tom Moore's answer. The /base_link -> /auxgps_link transform is
This is set in the urdf files of the robot, rather than being broadcast by another node. The Z offset is correct, because it includes the distance between the center of the receiver and the phase center, and because I've checked the Z offset by placing the robot and receiver over a control point (the vertical error was within the natural error of the receiver). The horizontal offset should be correct as well, because the offset is the same as the rear mount on the Jackal model, and these frames appear to match in the horizontal plane in RViz. And navsat_transform should be using the correct frame thanks to this line in navsat_transform
Regarding bagfiles and data logging. The error is only up to 30cm or so, so if anyone is using a receiver that's only accurate to a meter, it will probably get lost in the natural error of the receiver. If you want more bagfiles to test, here are a few more (relatively short) ones:
Post score: 12
Comment by gvdhoorn on 2016-08-19:
I don't have an answer, but I wanted to commend you on your thorough analysis and included plots and hypotheses. Let's hope you get a good answer(s) as well.
Comment by asimay_y on 2016-08-20:
I think this problem can also solve the drift case of map&odom when integrate with r_l GPS fusion. watching this. please also include @joq know.
Comment by asimay_y on 2016-08-20:
because your question is related with integrate errors change with yaw, I know from your launch file, there are two Vyaw elements are integrated into r_l, and the covariance is not very sure, without clearly understand the r_l integrate algorithm, can you only integrate 1 exact Vyaw to verify it?
Comment by asimay_y on 2016-08-21:
good work! but about the drift between map and odom, my view is different, I think the map is start of map, odom is start of car, so the two start points is fixed and should not drift when under map frame without consider the fusion error. but of course the start point is obviously drift in r_l..
Comment by M@t on 2016-08-21:
Thanks! And yes, in theory they should be fixed. But in practice that's how RViz deals with the difference between the global and local odometry. The alternative would be to fix /odom, /map and have two visualizations of the robot - one for each odometry set (it would look like my odometry graphs)
Comment by M@t on 2016-08-21:
That's part of the reason I print out all the odometry data and plot it in Excel - because then all the odometry is relative to the same origin point. I generally find it easier to debug issues and think about reference frames that way.
Comment by gvdhoorn on 2016-09-01:\
but proves conclusively that it is a bug in navsat_transform.
It would be nice if you could report that on the proper issue tracker. Visibility of bug reports on ROS Answers is really low / non-existent, so this will likely get lost.
Comment by M@t on 2016-09-01:
Thanks @gvdhoorn, I'll do just that. Honestly, my understanding of ROS and robot_localization is still pretty rudimentary so I could be completely wrong and it could be something I've done that's causing this, which is why I wanted to post it here first.
Comment by gvdhoorn on 2016-09-02:\
[..] which is why I wanted to post it here first.
Makes sense. But in this case I think it'd be justified to -- as a sort of conclusion -- report what you saw to the issue tracker. You can just refer to this ROS Answers post. I think Tom would appreciate it very much.