Guidance in NED frame does not work for long distances

[EwoudSmeur]

The problem

We are used to doing the guidance (in paparazzi also called navigation) in the NED frame. This causes issues when flying long distance due to the fact that the earth is not flat:

• When flying far away from the NED origin, the NED frame is tilted with respect to the ground, which means that down is no longer really down. This leads to horizontal paths in NED becoming inclined in the local tangent plane of the drone's location.
• Due to the above issue, the attitude coming from stateGetNedToBodyQuat is either not what the name says it is, or it does not give the attitude wrt. the local tangent plane.
• The waypoints that are defined in LLA in the flight plan, are correctly converted to the NED frame. However, this means that if a drone is sent to a waypoint far away at the same altitude, it is first going to descend to the right z coordinate in the NED frame, before it covers any significant distance horizontally, and will climb slowly to the right altitude, keeping the same NED z coordinate.
• Guidance in a NED frame far away leads to other problems as well: two points perfectly north of each other, have both a north and east difference in a NED frame at a different longitude. Due to this, I think a drone will not fly the shortest path between two points.

Note that additionally, there is a mismatch with some of the INS systems: EKF2 takes hmsl from the GPS and puts it in the z coordinate, so then the state position is actually not in proper NED.

The proposed solution

Though the NED coordinates can be intuitive when flying in close proximity to the takeoff location, it doesn't make sense to use these for guidance on a larger scale. I propose to modify the way the guidance works. Instead of strictly adhering to the NED frame, we should calculate North and East errors based on the local tangent plane of the current position of the drone. The vertical error should be based on height above the geoid. This way, you can fly to a waypoint far away with the same geoid altitude, and nicely follow the earth curvature.
Instead of using the NED coordinates of waypoints to compute errors for the guidance, we can do the following procedure:

• Obtain the LLA coordinate of a waypoint.
• Obtain the LTP of the current drone position.
• Convert the waypoint LLA to this LTP frame.
• Use the North and East position directly as error for the guidance.

Required changes

I quickly hacked this in and it works in simulation. But I found there are a lot of things to change, as the NED coordinates are used everywhere, most notably in how waypoint coordinates are shared, in the guidance reference system and in navigation (which should be called guidance) functions.

Though a big change, I think it is better to solve the issue in a proper way than to put some bandage (e.g. moving the NED origin). We can then also make it a bit more clear what altitude definitions we use e.g. in the flight plan.

Please let me know what you think! I would like to work on it, but I would like to know if you agree on the direction first.

[noether]

Hi Ewoud,

We will start flying BVLOS soon. Definitively, let us know if we can assist you with anything in this matter.