What I am looking for:

A uniform way to rotate a vector (X, Y, Z) from ENU to NED and vice versa
The same uniform way to rotate a quaternion from ENU to NED and vice versa

It would be appreciated if there can be given a solution in python, but it is more about the know-how to me.

What I think I already know

If I have this vector in ENU

v_xyz_ENU = [5,2,1] 

translates to

v_xyz_NED = [2,5,-1]

ENU <-> NED
X, Y, Z <-> Y, X,-Z

If I use this technique on a vector containing RPY (if that is the correct way to put it) I get confused.

v_rpy_ENU = [0,0,90] 

translates to

v_rpy_NED = [0,0,-90]

When I write calculations down I prefer to use angles in degrees

This rotations seems incorrect, because I know the result should be 0°
ENU Yaw 0° should be NED Yaw -90°
ENU Yaw 90° should be NED Yaw 0°
ENU Yaw 180° should be NED Yaw -90°/270°
ENU Yaw -90° should be NED Yaw 180°

What I have tried so far

I did try to wrap my head about quaternions. The explanation and examples I find elsewhere online about rotations using quaternions seems straight forward but I can't seem to come up with a result I understand.

For now, I forget about RPY and start using Quaternions. I have this quaternions in ENU representing the same 90° rotation about Z

Q_ENU = pq.Quaternion(axis=[0,0,1], degrees=90)

One claimed the rotation can be made similarly to the vector rotation but leave the w component untouched. So I try:

Q_NED = pq.Quaternion(Q_ENU.q[0], Q_ENU.q[2], Q_ENU.q[1], -Q_ENU.q[3])

Quaternion.q's data order = [w,x,y,z]

Now I verify if I get a yaw of 0° in NED once I convert the Q_NED to RPY angles, but it seems the result is:

Q_ENU: 0.707 +0.000i +0.000j +0.707k
Q_NED: 0.707 +0.000i +0.000j -0.707k
rpy_NED: [0.0, 0.0, -89.99999999999999]
I expect rpy_NED [0.0, 0.0, 0.0]

Now before I start writing all other possibilities I did try, I think a save myself and you a lot of time by not doing that. I would be really thankful for some explanation on how to do it, or at least a push in the right direction.

-T

Prelude

Welcome to the magical world of quaternions! I understand your frustration with trying to learn how quaternions work; rest assured, you are not alone. Unfortunately, there exists a variety of "flavors" of quaternions, ranging from how to format/interpret them, to how quaternion multiplication works, and to what quaternion multiplication represents - needlessly complicating an already complex topic.

“Quaternions came from Hamilton after his really good work had been done, and though beautifully ingenious, have been an unmixed evil to those who have touched them in any way.” - Lord Kelvin

What is a quaternion anyway?

Many definitions exist scattered throughout the internet, but at the core, quaternions are vectors in 4-D space. A common analogy is to extend the complex domain to have 1 "real" component and 3 "imaginary" components. Below is a sample quaternion vector as a 4x1 where the first element is the "real" component, r, and the last three elements are the "imaginary" components, x, y, and z along the i, j, and k imaginary axes respectively [1,2].

hamilton_quaternion = np.array([
    [r],
    [x],  # i
    [y],  # j
    [z]   # k
])

This analogy is useful within the context of rotating vectors in 3-D space, or across two different frames of reference, and is the dominant analogy among engineering circles. However, this use is sometimes viewed with distaste by pure mathematicians for reasons explained later.

Not all quaternions are built the same

The sample quaternion provided above is known as the Hamilton convention, yet there exists a second convention: the JPL convention [1]. Using the JPL convention, the first three elements are the "imaginary" x, y, and z components along the i, j, and k imaginary axes respectively, and the last element is the "real" component, r.

jpl_quaternion = np.array([
    [x],  # i
    [y],  # j
    [z],  # k
    [r]
])

NOTE: All quaternions used henceforth will use the Hamilton convention.

Not all quaternions are multiplied the same

Quaternion multiplication, denoted as ⊗ in this explanation, follows a strict set of rules. One does not simply cross multiply two quaternions together as you do in linear-algebra. The imaginary axis of each component is critical in determining the true product of two quaternions.

Right-handed multiplication defines multiplication across the imaginary axes as:

i**2 = j**2 = k**2 = i*j*k = -1
i*j = k
j*k = i
k*i = j
j*i = -k
k*j = -i
i*k = -j

Whereas left-handed multiplication defines multiplication across the imaginary axes as:

i**2 = j**2 = k**2 = i*j*k = -1
i*j = -k
j*k = -i
k*i = -j
j*i = k
k*j = i
i*k = j

For further detail and explanation, see Section 1.2.2 of [1].

Not all quaternions represent the same thing

In the context of general engineering and computer science, quaternions are used to represent the rotation of a vector from one frame of reference to another. However, this rotation can either represent the rotation of a vector...

• from a LOCAL frame to a global frame, or
• from a GLOBAL frame to a *local frame

How does a quaternion rotate a vector anyway?

The "formal" way of obtaining the rotated vector, p', from a quaternion expressing the rotation between two frames of reference, q, and the original vector, p is done as follows:

Here q* is called the quaternion conjugate of q. The quaternion conjugate is simply q but where the "imaginary" x, y, and z components are negative, but the "real" component is unchanged as demonstrated in Equations 1 and 2.

For an intuitive understanding of how this actually works, see [2].

!!! ASSUMPTIONS !!!

• p is a "pure" quaternion, where the "real" component is 0
• p has a magnitude (L2 norm) of 1
• q has a magnitude (L2 norm) of 1
• The magnitude of a quaternion is the Euclidean distance, or L2 norm, of the quaternion elements, calculated as sqrt(q_r^2 + q_x^2 + q_y^2 + q_z^2)

If any of these assumptions is false, p' will NOT give you the rotated vector you are expecting. As one can imagine, the above assumptions strictly limit the capability of quaternion algebra.

As may be noted, the resulting p' is a quaternion - but because of the assumptions made above, it should be a "pure" quaternion, meaning the rotated 3D vector can be extracted as the x, y, and z components along the imaginary i, j, and k axes. Here this will be the last 3 elements of p', or p'[1:].

So far, nothing presented is (should be?) blasphemous among the mathematician circles... until I present you the "engineering" equivalent notation of quaternion multiplication where a quaternion is decomposed into the "real" components (left of |) and the "vector" components (right of |). For more information, see Section 1.2.2 [1]:

a ⊗ b = [a0*b0 - np.dot(a, b) | a0*b + b0*a + np.cross(a,b)]

Wait, what are quaternions doing in my rotation matrices?

All of the heavy lifting, really. Turns out using angles to represent rotations in 3D space is a horrible idea because of trigonometric singularities. Physically, these singularities express themselves as gimbal lock: https://en.wikipedia.org/wiki/Gimbal_lock.

A rotation matrix can be constructed such that we define Equation 4 to be an equivalent mathematical representation of Equation 3 [1]. A small bonus is that the rotation matrix can be written as a 3x3, and both p and p' do not need to be converted to quaternions and can stay as good old 3x1 vectors representing x, y, and z values.

def build_rotation_matrix(rotation_quaternion: np.ndarray):
    """Construct a 3x3 rotation matrix given a rotation quaternion"""
    q0 = rotation_quaternion[0][0]
    q1 = rotation_quaternion[1][0]
    q2 = rotation_quaternion[2][0]
    q3 = rotation_quaternion[3][0]
    return np.array([
        [2*q0**2 + 2*q1**2 - 1, 2*(q1*q2 - q3*q0),      2*(q1*q3 + q2*q0)],
        [2*(q1*q2 + q3*q0),     2*q0**2 + 2*q2**2 - 1,  2*(q2*q3 - q1*q0)],
        [2*(q1*q3 - q2*q0),     2*(q2*q3 + q1*q0),      2*q0**2 + 2*q3**2 - 1]
    ])

Where q0 = q_r, q1 = q_x, q2 = q_y, and q3 = q_z as used previously.

Useful quaternion properties/definitions

The relationship between a vector, r = [r_x; r_y; r_z] and rotation angle, theta and a rotation quaternion is useful as an intuitive understanding of what the quaternion is doing. Because, and only because, of the assumptions made previously, the following can be found [1]:

Constructing the ENU and NED rotation quaternions

This is by far the least mathematically rigorous section in this answer, I have yet to find a mathematical proof to help me find the rotation quaternion between two FoR, although I haven't looked too hard yet. I just use [2] https://eater.net/quaternions/video/rotation and rotate the sphere by individually dragging the i, j, and k components of the quaternion until I got the sphere's ENU axes (i, j, and k) to rotate to NED (i, j, and k).

enu2ned_quaternion = np.array([
    [0],
    [sqrt(2)/2],
    [sqrt(2)/2],
    [0]
])
enu2ned_rotation_matrix = build_rotation_matrix(enu2ned_quaternion)

It is intuitive that the rotation from a LOCAL to a global FoR is just rotating in the opposite direction, but along the same rotation plane/vector. Where theta=180 in enu2ned_quaternion, we now set theta=-180, which with Equation 5 shows us is:

ned2enu_quaternion = np.array([
    [0],
    [-sqrt(2)/2],
    [-sqrt(2)/2],
    [0]
])
ned2enu_rotation_matrix = build_rotation_matrix(ned2enu_quaternion)

Fun fact, this is also the conjugate of the first rotation quaternion, or ned2enu_quaternion = enu2ned_quaternion*

And now to build out the data for our tests as well as a couple handy functions:

# Global FoR : ENU
global_for = {
    'East': np.array([[1], [0], [0]]),
    'West': np.array([[-1], [0], [0]]),
    'North': np.array([[0], [1], [0]]),
    'South': np.array([[0], [-1], [0]]),
    'Up': np.array([[0], [0], [1]]),
    'Down': np.array([[0], [0], [-1]])
}

# Local FoR : NED
local_for = {
    'North': np.array([[1], [0], [0]]),
    'South': np.array([[-1], [0], [0]]),
    'East': np.array([[0], [1], [0]]),
    'West': np.array([[0], [-1], [0]]),
    'Up': np.array([[0], [0], [-1]]),
    'Down': np.array([[0], [0], [1]]),
}


def l2norm(vec: np.ndarray):
    """Calculate the L2 norm, Euclidean distance, of a nx1 vector"""
    return sqrt(sum([_**2 for _ in vec]))


def str_vec(vec: np.ndarray):
    return f'[{round(vec[0][0],2)},' \
           f' {round(vec[1][0],2)},' \
           f' {round(vec[2][0],2)}]'

We can now test out what our conversions give us (with some ugly code for the time being - maybe I'll rework it to look nicer when it's not 1AM):

if __name__ == '__main__':
    # Verifying ENU -> NED
    for direction_enu, vector_enu in global_for.items():
        vector_ned = np.matmul(enu2ned_rotation_matrix, vector_enu)
        direction_ned = [_ for _ in local_for.keys()
                         if l2norm(vector_ned - local_for[_]) < 0.001]
        if len(direction_ned) > 1:
            print('ERROR: more than one solution found? IMPOSSIBLE!!!')
            break
        direction_ned = direction_ned[0]  # cause it's a list
        print(f'ENU {direction_enu} {str_vec(vector_enu)} => '
              f'NED {direction_ned} {str_vec(vector_ned)}')

    # Verifying NED -> ENU
    for direction_ned, vector_ned in local_for.items():
        vector_enu = np.matmul(ned2enu_rotation_matrix, vector_ned)
        direction_enu = [_ for _ in global_for.keys()
                         if l2norm(vector_enu - global_for[_]) < 0.001]
        if len(direction_enu) > 1:
            print('ERROR: more than one solution found? IMPOSSIBLE!!!')
            break
        direction_enu = direction_enu[0]  # cause it's a list
        print(f'NED {direction_ned} {str_vec(vector_ned)} => '
              f'ENU {direction_enu} {str_vec(vector_enu)}')

And the results...

ENU East [1, 0, 0] => NED East [0.0, 1.0, 0.0]
ENU West [-1, 0, 0] => NED West [-0.0, -1.0, 0.0]
ENU North [0, 1, 0] => NED North [1.0, 0.0, 0.0]
ENU South [0, -1, 0] => NED South [-1.0, -0.0, 0.0]
ENU Up [0, 0, 1] => NED Up [0.0, 0.0, -1.0]
ENU Down [0, 0, -1] => NED Down [0.0, 0.0, 1.0]
NED North [1, 0, 0] => ENU North [0.0, 1.0, 0.0]
NED South [-1, 0, 0] => ENU South [-0.0, -1.0, 0.0]
NED East [0, 1, 0] => ENU East [1.0, 0.0, 0.0]
NED West [0, -1, 0] => ENU West [-1.0, -0.0, 0.0]
NED Up [0, 0, -1] => ENU Up [0.0, 0.0, 1.0]
NED Down [0, 0, 1] => ENU Down [0.0, 0.0, -1.0]

Misconceptions

A rotation quaternion, q, is NEITHER the orientation NOR attitude of one frame of reference relative to another. Instead, it represents a "midpoint" vector normal to the rotation plane about which vectors from one frame of reference are rotated to the other. This is why defining quaternions to rotate from a GLOBAL frame to a local frame (or vice-versa) is incredibly important. The ENU to NED rotation is a perfect example, where the rotation quaternion is [0; sqrt(2)/2; sqrt(2)/2; 0], a "midpoint" between the two abscissa (X) axes (in both the global and local frames of reference). If you do the "right hand rule" with your three fingers pointing along the ENU orientation, and rapidly switch back and forth from the NED orientation, you'll see that the rotation from both FoR's is simply a rotation about [1; 1; 0] in the Global FoR.

References

I cannot recommend the following open-source reference highly enough:

[1] "Quaternion kinematics for the error-state Kalman filter" by Joan Solà. https://hal.archives-ouvertes.fr/hal-01122406v5

For a "playground" to experiment with, and gain a "hands-on" understanding of quaternions:

[2] Visualizing quaternions, An explorable video series. Lessons by Grant Sanderson. Technology by Ben Eater https://eater.net/quaternions