WARNING: Many words ahead…
I work with large rovers where following a precise predefined course is a critical success factor. Over the last year or so, we have refined our systems and tuning to the point where we can rely on averaging better than +/- 5 cm cross track error on straight sections of the planned path. This represents approximately 2% of the vehicle width. This degree of accuracy has come about due to an increasing understanding of how Ardurover works at all levels. One thing that is apparent when looking at position information at this scale, is the degree of apparent noise.
We use Ublox F9P GNSS modules in a moving base line setup on our vehicles along with our own local base station (also F9P). This arrangement gives us an heading that is not affected by magnetic interference and accurate to approximately 0.1 degrees, along with a position that is repeatable to within better than 1cm.
The Ardupilot code uses the ‘Location’ object to represent locations on the surface of our planet. This object stores latitude and longitude in decimal degrees * 1.0e07 as a signed 32bit integer. When determining the relative positions of two locations in meters, the code applies a constant multiplier based on the circumference of the Earth. In the case of longitude, a further scale factor to account for latitude is applied. This gives us a fixed resolution of ~0.011m in the N/S direction and the same in the E/W direction at the equator, but getting better the further N/S you go. This constant can be found here in the code: https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_Common/Location.h#L124 LOCATION_SCALING_FACTOR = 0.011131884502145034
Even though the Location object is limited to ~1.1cm resolution, the EKF internally uses a position represented by the NE offset in m from the origin location. This offset is represented by a float variable which should give approx 7 decimal digits of precision. This equates to a resolution of 0.0001m over a 1000km range - (better over a lesser range/ worse over a greater range).
Given the EKF internal resolution is better than the external location position by at least a few orders of magnitude, it does still rely heavily on regular location updates from the less precise GNSS module/s attached. Most of these modules in use today only supply the same 7 digits of precision that the Location object uses, with some only providing six. However, there are modules available that offer greater precision output. The Ublox F9P for instance, has the UBX HPPOSLLH message that offers the standard 7 digit lat/lon attributes but also has additional latHp and lonHp attributes that each represent additional precision. When these attributes are scaled appropriately and added to the lat/lon values, we get a measurement that is precise to 9 decimal places. This is roughly equivalent to 0.1mm. Of course this is not to say that we now have a position that is this accurate, but at least we can now be assured that the accuracy is not being eroded by the precision of the reported value - which was not the case with only 7 digits on this particular module.
To test the degree to which this lack of precision potentially affects navigation, I set about adding support for the additional precision available from the F9P. This included:
- Updating the Location class to support storing the additional latHp/lonHP attributes
- Updating all the Location methods to utilise the additional HP information in their calculations
- Adding support for processing the UBX HPPOSLLH message
- Adding the latHp/lonHP attributes to the GPS dataflash log entry
- Adding the ability to encode latHP/lonHP into the MAV_CMD_NAV_WAYPOINT param1
- Adding support for outputting the UBX HPPOSLLH message in SITL
The code for this is available here: https://github.com/jimovonz/ardupilot/tree/hp_location. This branch is based on Ardupilot master July 8, 2020. This is experimental code and has not been flight tested.
Some testing in SITL using rover default params shows some improvement with respect to the high frequency variability in cross track error as well as overall accuracy:
These graphs show the cross track error for roughly the same section of a straight path in the SITL simulator using the standard and modified code.
I have also done some limited testing using a stand alone Pixhawk Cube Black and an F9P (not attached to any vehicle platform). Looking at the reported EKF position, I can see a what subjectively appears to be an order of magnitude reduction in the variability. This would be in keeping with the simulator results.
Unfortunately I am not going to get a chance this week to test on an actual vehicle as all the available machines are currently in the process of testing various other critical systems.
I will add additional testing data as I can.
Added July 14:
I have repeated the static test and recorded data. Below are the results of setting up the Pixhawk and F9P with a local base station and recording a dataflash log for approx 5 minutes. The plots show the EKF PN vs PE in m (position North and position East) for the standard code and for my high precision code:
Note that the stock code from master resolves all location readings taken during the interval into 4 specific coordinates. The distance between these points represent the resolution limit of this code.