From the AHRS sensor fusion orientation filter to a 3d graphical rotating cube

In a comment from Ciskje on my sensor fusion implementation, I've been asked to explain how the Processing graphical uses rotations to visualize the orientation informations coming from the sensor fusion algorithm.

The question was basically, what's the reason about using the Yaw, Pitch and Roll in a "strange" order in the following lines of code in my Processing graphical cube example:

rotateZ(-Euler[2]); // phi - roll
rotateX(-Euler[1]); // theta - pitch
rotateY(-Euler[0]); // psi - yaw

The following explanation assumes some basic knowledge of rotation matrices and 3d coordinate systems. For a good introduction on the topics, I strongly suggest reading Chapter 2 of "Robot Modeling and Control" by Mark W. Spong.

Demonstration

The first thing you have to understand is that we have 3 coordinate systems here.

• O_sX_sY_sZ_s attached to the sensor array
• O_wX_wY_wZ_w attached to the real world, with X_w pointing to the Hearth North and Z_w pointing to the sky
• O_mX_mY_mZ_m the coordinate system of the graphics program, usually it is attached on the top left corner of the monitor with X_m pointing right, Y_m pointing down and Z_m pointing forward to the monitor.

Note that when X_w and Z_m are aligned (you point the monitor to the Earth north in your location) there is a simple relationship between O_wX_wY_wZ_w and O_mX_mY_mZ_m: a point p_w defined in the coordinate system W having coordinates p_w = [p_{w_x}, p_{w_y}, p_{w_z}] can be expressed in the coordinate system M doing p_m = [-p_{w_y}, -p_{w_z}, p_{w_x}]

The sensor fusion algorithm running on Arduino computes a quaternion representing the orientation of O_sX_sY_sZ_s with respect to O_wX_wY_wZ_w, and from the quaternionToEuler function in the Processing code we can get the Euler angles expressed in the aerospace sequence, so they are the yaw (ψ - psi), pitch (θ - theta) and roll (φ - phi) angles.

We can use them in the following way: supposing that O_sX_sY_sZ_s and O_wX_wY_wZ_w are aligned in the beginning we can rotate O_sX_sY_sZ_s so that it would assume the orientation described in the quaternion or yaw, pitch, roll angles by:

1. rotate by ψ around Z_s
2. rotate by θ around Y_s
3. rotate by φ around X_s

After doing the above we will obtain that O_sX_sY_sZ_s is oriented with respect to O_wX_wY_wZ_w according to the orientation described by the output quaternion from the sensor fusion algorithm.

The same result could have been produced by rotations relative to the W coordinate system, but using the reverse order:

1. rotate by φ around X_w
2. rotate by θ around Y_w
3. rotate by ψ around Z_w

However, we are interested in rotating the graphical cube which is defined in the O_mX_mY_mZ_m coordinate system so a slightly different approach is needed. From the relationship between O_wX_wY_wZ_w and O_mX_mY_mZ_m described above we can see that there is a relationship between the rotations made in the W and M coordinate system:

• a rotation φ around X_w corresponds to a rotation of -φ around Z_m
• a rotation θ around Y_w corresponds to a rotation of -θ around X_m
• a rotation ψ around Z_w corresponds to a rotation of -ψ around Y_m

Given the above and remembering that in Processing the rotations are made in respect to the monitor frame we can conclude that, in order to align a graphical cube defined in the O_mX_mY_mZ_m to the orientation described by the quaternion and then Euler angles coming from the sensor fusion algorithm of the sensors coordinate system O_sX_sY_sZ_s, we have to do the following rotations:

1. a rotation of -φ around Z_m
2. a rotation of -θ around X_m
3. a rotation of -ψ around Y_m

as we wanted to demonstrate.