Rotorpy and Ardupilot Reference Frames#

This notebook describes the reference frames used in rotorpy and Ardupilot. They are summarized in the following table:

System

Body Frame

World Frame

Rotorpy

FLU (\(x\): Forward, \(y\): Left, \(z\): Up )

ENU (\(x\): East, \(y\): North, \(z\): Up)

Ardupilot

FRD (\(x\): Forward, \(y\): Right, \(z\): Down)

NED (\(x\): North, \(y\): East, \(z\): Down)

Reference Frames

[image source - px4 docs]

import numpy as np
from scipy.spatial.transform import Rotation

flu2frd = Rotation.from_euler('x', np.pi)
np.set_printoptions(precision=3) # Set print options to display 3 decimal places
print(flu2frd.as_matrix())

[[ 1.000e+00  0.000e+00  0.000e+00]
 [ 0.000e+00 -1.000e+00 -1.225e-16]
 [ 0.000e+00  1.225e-16 -1.000e+00]]

Active and passive rotations#

An active rotation is one that takes a vector expressed in a reference frame and rotates it to be in a different orientation in the same reference frame.

Let’s take, for example, the vector \(v = [1, 0, 0]\). We have a Rotation object \(R\) representing a rotation of \(\pi\) radians around the \(z\) axis. We want to understand whether \(R\) represents an active or passive rotation.

If the rotation is active, it means that performing the operation \(v'=Rv\) will result in a vector \(v'=[0,1,0]\).

If the rotation is passive, it means that the reference frame will be rotated \(\pi / 2\) radians, resulting in the new \(x\) axis being aligned with the previous \(y\) axis and the previous \(y\) axis being aligned with the negative \(x\) axis. Therefore, the vector \(v'\) will be the previous vector represented in the new reference frame \(v'=[0,-1,0]\).

R = Rotation.from_euler('z', np.pi / 2)
v = np.array([1, 0, 0])
v_prime = R.as_matrix()@ v

if np.allclose(v_prime, [0, 1, 0]):
    print("The object R represents an active rotation")
elif np.allclose(v_prime, [0, -1, 0]):
    print("The object R represents a passive rotation")
else:
    print("Something went wrong!")

The object R represents an active rotation

We see that the rotation object \(R\) represents an active rotation. We are going to need this information soon!

The physics backend used for simulating quadrotors in the Duckiematrix is rotorpy [FPK23]. This simulator provides us with the passive rotation \(R_{FLU \rightarrow NED}\) from the FLU body frame (\(x\) pointing forward, \(y\) left and \(z\) up) to the ENU inertial frame (\(x\) pointing east, \(y\) north and \(z\) up).

Therefore, if we want to express a vector \(v_B\) from the body frame in the corresponding inertial frame, \(v_I\), we have to perform the operation:

\[v_I=R_{FLU \rightarrow NED}v_B\]

Note that this can also be interpreted as the attitude of the drone in the inertial frame. To show this, let’s define:

\[R_{B\rightarrow I} = R_{FLU \rightarrow NED}\]

This is the passive rotation from the body frame to the inertial frame. The attitude is defined as the active rotation from the inertial frame to the body frame. This is equivalent to inverting the matrix once to obtain the active rotation matrix from the body frame to the inertial frame:

\[R_{B\rightarrow I}^{active}=\big(R_{B\rightarrow I}\big)^{-1}\]

and then once more to obtain the active rotation matrix from the inertial frame to the body frame:

\[R_{I\rightarrow B}^{active}= \big(R_{B\rightarrow I}^{active}\big)^{-1}=\bigg[\big(R_{B\rightarrow I}\big)^{-1}\bigg]^{-1}\]

However, because inverting a matrix twice results in the original matrix, it means that:

\[\bigg[\big(R_{B\rightarrow I}\big)^{-1}\bigg]^{-1}=R_{B\rightarrow I}=R_{I\rightarrow B}^{active}\]

and hence the attitude.

FPK23

Spencer Folk, James Paulos, and Vijay Kumar. RotorPy: a python-based multirotor simulator with aerodynamics for education and research. arXiv preprint arXiv:2306.04485, 2023.

Ardupilot - rotorpy rotation conventions#

In Ardupilot, the attitude is represented as a scalar-first quaternion, which represents the active rotation from the world frame (NED) to the body frame (FRD):

\[q_{ardupilot}=[w, x, y, z]\]

As seen in the previous section, it also represents the passive rotation from the body frame to the world frame.

In rotorpy, the attitude is represented as a scalar-last quaternion, which represents the rotation from the body frame (GLU) to the world frame (ENU):

\[q_{rotorpy}=[x, y, z, w]\]
R_flu2enu = Rotation.from_quat([0, 0, 0, 1]) # Attitude of FLU frame in ENU (i.e., the _passive_ transform _from_ FLU _to_ ENU)
M_flu2frd = R.from_euler('x', np.pi) # Active rotation from FLU to FRD
M_enu2ned = R.from_matrix([[0, 1, 0], [1, 0, 0], [0, 0, -1]]) # Active rotation from ENU to NED

# Note how the FLU2FRD rotation is equivalent to its inverse
assert np.allclose(M_flu2frd.inv().as_matrix(),M_flu2frd.as_matrix())
# Note how the ENU2NEd rotation is equivalent to its inverse
assert np.allclose(M_enu2ned.inv().as_matrix(), M_enu2ned.as_matrix())
# This is because their transposes are equivalent and the inverse of a rotation matrix is just its transpose
assert np.allclose(M_enu2ned.as_matrix().T, M_enu2ned.inv().as_matrix())

# Obtain the rotation from the body frame (FRD) to the world frame (NED)
R_frd2ned = M_enu2ned * R_flu2enu * M_flu2frd # This is the attitude of the FRD frame in the NED frame
M_frd2flu = M_flu2frd # It should actually be M_flu2frd.inv() but we have shown this to be equivalent to M_flu2frd
M_enu2ned = M_enu2ned # It should properly be the inverse of this since we defined it as a passive transformation but they are equivalent
R_frd2ned_check = M_enu2ned * R_flu2enu * M_frd2flu # This is the overall passive rotation representing the transformation from the FRD body frame to the NED inertial reference frame