1. Problem Context

In a drone video projection system, orientation data from the drone's gimbal was used to render video in a 3D scene. However, due to sensor inaccuracies, direct use of Euler angles resulted in misaligned projections. A correction interface was implemented to adjust roll, pitch, and yaw values, but testing revealed that roll adjustments produced the same visual effect as yaw adjustments, endicating a gimbal lock issue. To resolve this, a quaternion-based correction method was implemented that avoids gimbal lock by converting Euler angles to quaternions, applying the correction, and converting back to Euler angles.

2. Mathematical Foundations

2.1 Euler Angles to Quaternions

The system uses intrinsic ZXY Euler rotation order (yaw-pitch-roll). The conversion to a quaternion q = [x, y, z, w] is derived as follows:

φ = roll, θ = pitch, ψ = yaw

w = cos(φ/2) * cos(θ/2) * cos(ψ/2) - sin(φ/2) * sin(θ/2) * sin(ψ/2)
x = cos(φ/2) * sin(θ/2) * cos(ψ/2) - sin(φ/2) * cos(θ/2) * sin(ψ/2)
y = cos(φ/2) * sin(θ/2) * sin(ψ/2) + sin(φ/2) * cos(θ/2) * cos(ψ/2)
z = cos(φ/2) * cos(θ/2) * sin(ψ/2) + sin(φ/2) * sin(θ/2) * cos(ψ/2)

2.2 Quaternion Multiplication

Two quaternions q1 = [x1, y1, z1, w1] and q2 = [x2, y2, z2, w2] are multiplied as:

w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
y = w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2
z = w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2

2.3 Quaternion to Rotation Matrix

For a quaternion q = [x, y, z, w], the corresponding rotation matrix is:

R = [
  [1 - 2(y² + z²),   2(xy - wz),       2(xz + wy)],
  [2(xy + wz),       1 - 2(x² + z²),   2(yz - wx)],
  [2(xz - wy),       2(yz + wx),       1 - 2(x² + y²)]
]

2.4 Rotation Matrix to Euler Angles

Given rotation matrix elements t12, t22, t31, t32, t33, Euler angles are extracted as:

yaw   = arctan2(-t12, t22)
pitch = arcsin(t32)
roll  = arctan2(-t31, t33)

3. Implementation in Python

3.1 Quaternion Multiplication

import numpy as np

def quaternion_multiply(q1, q2):
    x1, y1, z1, w1 = q1
    x2, y2, z2, w2 = q2
    w = w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2
    x = w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2
    y = w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2
    z = w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2
    return np.array([x, y, z, w])

3.2 Visualization Function

import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
from scipy.spatial.transform import Rotation as R

def plot_rotated_axes(ax, r, name=None, offset=(0, 0, 0), scale=1):
    colors = ("#FF6666", "#005533", "#1199EE")
    loc = np.array([offset, offset])
    for i, (axis, c) in enumerate(zip((ax.xaxis, ax.yaxis, ax.zaxis), colors)):
        axis.set_label_text(axis.axis_name.upper())
        axis.label.set_color(c)
        axis.line.set_color(c)
        axis.set_tick_params(colors=c)
        line = np.zeros((2, 3))
        line[1, i] = scale
        line_rot = r.apply(line)
        line_plot = line_rot + loc
        ax.plot(line_plot[:, 0], line_plot[:, 1], line_plot[:, 2], c)
        text_loc = line[1] * 1.2
        text_loc_rot = r.apply(text_loc)
        text_plot = text_loc_rot + loc[0]
        ax.text(*text_plot, axis.axis_name.upper(), color=c, va="center", ha="center")
    ax.text(*offset, name, color="k", va="center", ha="center", bbox={"fc": "w", "alpha": 0.8, "boxstyle": "circle"})

3.3 Test Function

def test_euler_to_quaternion():
    euler = (np.pi / 4, np.pi / 2, np.pi / 2)
    r1 = R.from_euler('ZXY', euler)
    q1 = r1.as_quat()
    print("Original rotation:", q1)

    q_mod = R.from_euler('ZXY', (0, 0, np.pi / 4)).as_quat()
    q_corrected = quaternion_multiply(q_mod, q1)
    r_corrected = R.from_quat(q_corrected)
    print("Corrected rotation:", q_corrected)

    fig = plt.figure()
    ax = fig.add_subplot(111, projection='3d')
    plot_rotated_axes(ax, R.from_quat([0, 0, 0, 1]), name="Identity", offset=(0, 0, 0))
    plot_rotated_axes(ax, r1, name="Original", offset=(3, 0, 0))
    plot_rotated_axes(ax, r_corrected, name="Corrected", offset=(6, 0, 0))
    plt.show()

4. Implementation in JavaScript

4.1 Euler to Qauternion

function eulerToQuaternionZXY(euler) {
    const [yaw, pitch, roll] = euler.map(deg => deg * Math.PI / 180);
    const cy = Math.cos(yaw * 0.5);
    const sy = Math.sin(yaw * 0.5);
    const cp = Math.cos(pitch * 0.5);
    const sp = Math.sin(pitch * 0.5);
    const cr = Math.cos(roll * 0.5);
    const sr = Math.sin(roll * 0.5);

    const w = cr * cp * cy - sr * sp * sy;
    const x = cr * sp * cy - sr * cp * sy;
    const y = cr * sp * sy + sr * cp * cy;
    const z = cr * cp * sy + sr * sp * cy;

    return [x, y, z, w];
}

4.2 Quaternion to Euler

function quaternionToEulerZXY(quat) {
    const [x, y, z, w] = quat;
    const t12 = 2 * (x * y - w * z);
    const t22 = 1 - 2 * (x * x + z * z);
    const t31 = 2 * (x * z - w * y);
    const t32 = 2 * (y * z + w * x);
    const t33 = 1 - 2 * (x * x + y * y);

    const yaw = Math.atan2(-t12, t22);
    const pitch = Math.asin(t32);
    const roll = Math.atan2(-t31, t33);

    return [yaw * 180 / Math.PI, pitch * 180 / Math.PI, roll * 180 / Math.PI];
}

4.3 Correction Function

function applyQuaternionCorrection(baseEuler, modEuler) {
    const baseQuat = eulerToQuaternionZXY(baseEuler);
    const modQuat = eulerToQuaternionZXY(modEuler);
    const correctedQuat = quaternionMultiply(modQuat, baseQuat);
    return quaternionToEulerZXY(correctedQuat);
}