AP_Math: rename earth_to_body to body_to_earth

[robertlong13]

This function actually converts a vector represented in the body frame to the earth frame. The previous name was wrong. This function matches the behavior of AHRS::body_to_earth. Here is a lua script and an example log of it running in SITL to demonstrate that fact: compare_ahrs_and_quat.zip

The confusion stems from the fact that the rotation that you apply to rotate frame A such that it becomes frame B is the opposite of the rotation you apply to convert a vector represented in frame A to be represented in frame B. Or, in other words, the rotation that you apply to rotate frame A such that it becomes frame B is the rotation you apply to convert a vector represented in frame B to be represented in frame A.

Interestingly, the docs for the lua binding correctly described what the function was doing (applying the rotation described by the quaternion to a vector), but the original function description described the opposite (converting a body-frame vector to earth-frame).

Let's do an example in 2D. X_E/Y_E is the Earth frame coordinate system. Our vehicle is pointing at a heading of 45 degrees. If we rotate our Earth reference frame X_E/Y_E 45 degrees to the right, we get the body reference frame X_B/Y_B.

One might call this rotation of 45 degrees to the right "a rotation from E to B". Someone might call a rotation matrix that converts a representation of a vector in frame E to one in frame B "a rotation from E to B". Those are exact opposites of each other. I'd argue the former is more correct, but because it's so easy to get confused, we need to be extremely clear what we mean. I have cleaned up some comments in the code that were ambiguous or flat-out wrong.

Unfortunately, there are many other cases remaining (stuff like get_rotation_body_to_ned, which I think should be renamed to get_body_to_ned or maybe get_body_to_ned_matrix and get a comment explaining very clearly what it does and how it's used). If people are generally on-board with cleaning those up, I'd be happy to in another PR if this one gets approved.

[robertlong13]

https://en.wikipedia.org/wiki/Active_and_passive_transformation

[rmackay9]

Thanks very much for this, we have very few users of this call so I think this should be a relatively safe change. If there are any uses of it (plane acrobatics script?) we probably have to test they still work OK.

[robertlong13]

Thanks very much for this, we have very few users of this call so I think this should be a relatively safe change. If there are any uses of it (plane acrobatics script?) we probably have to test they still work OK.

Yup, had to change the plane aerobatics script to reflect the rename. The autotest still works, here is the log of the airshow. SUPERAIRSHOW.zip

[IamPete1]

You could add your example into the math test that we run as part of CI. https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_Scripting/tests/math.lua

We could add keep the old function in the docs and mark it as deprecated and add a comment to point people to the new one.

That would be something like this:

-- Applies rotation to vector argument
---@deprecated -- function name incorrect for function use body_to_earth for the same behavior 
---@param vec Vector3f_ud
function Quaternion_ud:earth_to_body(vec) end

That gets us this in VSCode:

The language server checks pick it up:

ardupilot/libraries/AP_Scripting/examples/simple_loop.lua:16:1
        Code: deprecated
        Deprecated.(function name incorrect for function use body_to_earth for the same behavior)

However we might need some changes so that the docs don't generate a error for a unexpected definition in CI.

The binding generator also has the ability to print a one time error when a binding is first used. That would allow a "softer" deprecation where the code will still work.

[Georacer]

The quaternion class isn't defined specifically in any particular direction: Earth to body or body to Earth.

But it does contain a .from_euler constructor and Euler angles are pretty well defined. As per your link, the Euler angles are an "passive" transformation from the Earth frame into the body frame, in the yaw, pitch, roll order.

Thus, from this starting point and the method definition, (similar to Wikipedia's), we can deduce that the quaternion used in ArduPilot is in-line with the textbook definition.

A point of attention: The operation that uses a quaternion to transform an Earth-frame vector into a body-frame vector is (Stevens, B. L., Lewis, F. L., & E.N. Johnson. (2016). Aircraft Control and Simulation, Wiley, p49):

$v_{body} = q^{-1} * v_{Earth} * q$

We typically write this quaternion as $q_{b/e}$ (To body from Earth). Whereas

$v_{Earth} = q_{b/e} * v_{body} * q_{b/e}^{-1}$

Pay attention to the position of the $^{-1}$ exponent.

Now, such a multiplication between a quaternion and a vector isn't defined in C++, so we had rolled out the earth_to_body method, which uses a rotation matrix as an intermediate step.

That rotation matrix conforms to the constructions of a body to Earth rotation matrix $m$, as seen in Wikipedia, or as the transpose/inverse of the Earth to body matrix seen in (Stevens2016, p53).

Thus, the following will be true:

$v_{e}= m \cdot v_b$

Concluding, I agree with the rename.