EKF2: ellipsoidal earth navigation

[bresch]

Solved Problem

The current EKF evolves in a fixed local NED tangent frame with an origin set at boot. This works well for short and moderate distances but degrades when flying over long distances (>100km) as the tangent frame gets significantly further from the ellipsoid surface.

Solution

Mechanize the navigation equations using a local-navigation-frame which is NED-oriented and with its origin attached at the CG of the vehicle. The position state now defines the global position of the drone (geodetic Latitude-Longitude-Altitude) and the local position is constructed by projecting the global position using an arbitrary origin (initialized at boot or manually by the user).

The error-state is defined in a fixed local-navigation-frame to ignore all the -really small- pseudo-forces due to the frame rotation. This allows us to keep the covariance prediction really simple and unchanged from the previous local tangent frame approximation. The position uncertainty is also defined in the local NED which makes it more intuitive than latitude/longitude errors.

Note that the global position state is not directly included in the state vector from the derivation as lat/lon require double precision. The global position is stored in a new class LatLonAlt which contains operators to combine ellipsoidal position and Cartesian position errors (to compute position innovations and apply corrections).

When no latitude/longitude/altitude is available, the EKF assumes to be started at 0/0/0. This avoids having to handle multiple conditions everywhere in the code and the incorrect radii of curvatures is insignificant when navigating close to the starting location. It is then still possible to manually set the current position of the drone to automatically initialize the EKF at the correct location and continue dead-reckoning from there.

The position of the output predictor is considered as an error relative to the main (delayed-time EKF) and thus integrates the velocity and position directly on the Cartesian local-navigation-frame (origin at the LLA of the delayed-time EKF).

Changelog Entry

For release notes:

TBD
New parameter: -
Documentation: -

Alternatives

Test coverage

Context

[Jaeyoung-Lim]

Nice!

The height definitions in mavlink is based on AMSL, which are defined by a geoid.

Wouldn't this have implications in the current navigation stack where it doesn't distinguish ellipsoidal or geoid heights?
The GPS returns both, but I couldn't find where this would be explicitly distinguished.

[bresch]

Wouldn't this have implications in the current navigation stack where it doesn't distinguish ellipsoidal or geoid heights?

@Jaeyoung-Lim You're right, it's not ideal but we still use AMSL everywhere so it's at least consistent on the altitude reporting side. A current issue is that we use AMSL instead of ellipsoid height in the calculations inside the LatLonAlt class.
My goal is to only use ellipsoid altitude inside EKF2 and then convert the output to AMSL. We currently don't have a geoid model, so either we add one or we continue to steal it from the GNSS receiver (i.e.: compute the difference between reported AMSL and ellipsoid alt).
This PR is however large enough so I'll do the changes in a follow-up one.

[Jaeyoung-Lim]

@bresch Thanks! I think this would be anyway a huge improvement in navigation performance, especially when you care about altitude above terrain.

Last thing I would like to comment (For myself really) is that this would probably break the gazebo gps plugin setup, since gazebo assumes a cartesian world. I have a separate gps sensor plugin that considers the simulated world as the projected surface on the geoid to simulate flying in a CH1903/LV03 digital elevation map in Switzerland. We can probably adapt this to UTM coordinates for more general locations.