Add support for Covariance for GPS Accuracy

[Ryanf55]

Feature request

In order to support the ROS2 NavSatFix message, which Ardupilot can publish over DDS, the covariance field for accuracy needs to be filled in. Currently, AP_GPS does not have any support for covariance; only hdop and vdop.

Describe the solution you'd like

1. Add a generic method to AP_GPS to retrieve the covariance of the current GPS position for each sensor in the front end
2. If a backend GPS cannot supply any accuracy, then there share be an enumeration to say that.
3. If the GPS supports reporting of horizontal and vertical accuracy, use it to fill in the diagonals of the covariance.
4. If a backend GPS supports covariance, use it to fully fill in the matrix.

Describe alternatives you've considered

I initially implemented this logic in the DDS layer of Ardupilot, but calculating covariance is something the GPS library could be doing.

Platform
[ x] All
[ ] AntennaTracker
[ ] Copter
[ ] Plane
[ ] Rover
[ ] Submarine

Additional context

• ROS nmea_navsat_driver covariance support: Driver computes covariance incorrectly ros-drivers/nmea_navsat_driver#17
• Formulas: https://core.ac.uk/reader/236623780
• Ublox M8 API that has covariance https://content.u-blox.com/sites/default/files/products/documents/u-blox8-M8_ReceiverDescrProtSpec_UBX-13003221.pdf

[tridge]

I think horizontal_accuracy() and vertical_accuracy() would likely be more appropriate, and less maths

[Ryanf55]

The notes are how to implement this are discussed here. Ready for implementation.
https://docs.google.com/document/d/1MNCh-qiqblKFaB3a_ZDKPgmsTwBaopdHQuyWSKnw1Zc/edit#heading=h.kmo72m9ssosu

[Ryanf55]

Thoughts on the following?

In AP_GPS.h add the following enum

    // Match https://docs.ros2.org/foxy/api/sensor_msgs/msg/NavSatFix.html
    enum class GPS_CovarianceType {
         UNKNOWN = 0, ///< The GPS does not support any accuracy metrics
        APPROXIMATED = 1,  ///< The accuracy is approximated through metrics such as HDOP/VDOP
        DIAGONAL_KNOWN = 2, ///< The diagonal (east, north, up) components of covariance are reported by the GPS
        KNOWN = 3, ///< The full covariance array is reported by the GPS
    };

Then, add the following generic method to populate a covariance array in AP_GPS class. Use either a raw array, or std::array, since size is known at compile time, or the nice Matrix3 class in ArduPilot. Since covariance in SI units in this order is pretty standard, it can be in AP_GPS rather than AP_DDS.

    GPS_CovarianceType covariance(const uint8_t instance, double cov[]) const WARN_IF_UNUSED

OR

GPS_CovarianceType covariance(const uint8_t instance, std::array<double, 9>& cov) const WARN_IF_UNUSED

OR

GPS_CovarianceType covariance(const uint8_t instance, Matrix3d& cov) const WARN_IF_UNUSED

[WickedShell]

Septentrio provides a full covariance matrix for us, we actually derive the horizontal/vertical accuracy numbers from it.

[Ryanf55]

Septentrio provides a full covariance matrix for us, we actually derive the horizontal/vertical accuracy numbers from it.

Great to know! I'd love to see some of the nice GPS backends override this method to populate the full covariance matrices when possible, however it's out of scope for the initial PR. Ublox M8 also can do it according to their API docs, and that's a pretty widely used device from what I understand.

[WickedShell]

@Ryanf55 fair enough. For future reference it's here: https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_GPS/AP_GPS_SBF.cpp#L494 and the struct is here: https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_GPS/AP_GPS_SBF.h#L202

[Ryanf55]

The usage of this new feature will be covered in this ticket:
#23277

It's next up. I'm proposing to merge this as-is without a consumer to keep PR scope small.

EDIT: No dead code allowed. It's going in.