|
| 1 | +# Quaternion |
| 2 | + |
| 3 | +## Overview & Motivation |
| 4 | + |
| 5 | +Three-dimensional attitude representation is a fundamental requirement in robotics, aerospace, |
| 6 | +and wearable sensing. Euler angles are intuitive but suffer from gimbal lock — a singularity |
| 7 | +that collapses three degrees of freedom into two whenever one angle reaches ±90°. Rotation |
| 8 | +matrices avoid this but carry nine words of state and require orthogonality re-enforcement. |
| 9 | + |
| 10 | +A unit quaternion encodes the same rotation in four words, composes orientations with sixteen |
| 11 | +multiply-adds, and is free of singularities. Every modern AHRS filter — Madgwick, Mahony, |
| 12 | +Extended Kalman — stores attitude as a unit quaternion precisely because of this combination |
| 13 | +of compactness, numerical stability, and algebraic closure. |
| 14 | + |
| 15 | +## Mathematical Theory |
| 16 | + |
| 17 | +### Core Definitions |
| 18 | + |
| 19 | +A quaternion is a hypercomplex number of the form |
| 20 | + |
| 21 | +$$q = w + x\mathbf{i} + y\mathbf{j} + z\mathbf{k}$$ |
| 22 | + |
| 23 | +where $w, x, y, z \in \mathbb{R}$ and the basis elements satisfy |
| 24 | + |
| 25 | +$$\mathbf{i}^2 = \mathbf{j}^2 = \mathbf{k}^2 = \mathbf{ijk} = -1.$$ |
| 26 | + |
| 27 | +A **unit quaternion** ($\|q\| = 1$) encodes a rotation by angle $\theta$ about unit axis $\hat{n}$ as |
| 28 | + |
| 29 | +$$q = \left(\cos\frac{\theta}{2},\; \hat{n}\sin\frac{\theta}{2}\right).$$ |
| 30 | + |
| 31 | +### Hamilton Product |
| 32 | + |
| 33 | +Composition of two rotations $q_a$ then $q_b$ is |
| 34 | + |
| 35 | +$$q_a \otimes q_b = \begin{pmatrix} |
| 36 | +w_a w_b - x_a x_b - y_a y_b - z_a z_b \\ |
| 37 | +w_a x_b + x_a w_b + y_a z_b - z_a y_b \\ |
| 38 | +w_a y_b - x_a z_b + y_a w_b + z_a x_b \\ |
| 39 | +w_a z_b + x_a y_b - y_a x_b + z_a w_b |
| 40 | +\end{pmatrix}.$$ |
| 41 | + |
| 42 | +This product is **non-commutative**: $q_a \otimes q_b \neq q_b \otimes q_a$ in general. |
| 43 | + |
| 44 | +### Vector Rotation |
| 45 | + |
| 46 | +A pure quaternion $p = (0, \mathbf{v})$ is rotated by |
| 47 | + |
| 48 | +$$\mathbf{v}' = q \otimes p \otimes q^{-1}.$$ |
| 49 | + |
| 50 | +For unit $q$ this simplifies (Rodrigues cross-product form) to |
| 51 | + |
| 52 | +$$\mathbf{v}' = \mathbf{v} + 2w\,(\mathbf{u} \times \mathbf{v}) + 2\,\mathbf{u} \times (\mathbf{u} \times \mathbf{v}),$$ |
| 53 | + |
| 54 | +where $\mathbf{u} = (x, y, z)$. This costs 15 multiply-adds vs 9 for a pre-built rotation |
| 55 | +matrix, making it preferable when rotating one vector. |
| 56 | + |
| 57 | +### Conjugate and Inverse |
| 58 | + |
| 59 | +For any quaternion $q^* = (w, -x, -y, -z)$. For a unit quaternion $q^{-1} = q^*$. |
| 60 | + |
| 61 | +### Rotation Matrix |
| 62 | + |
| 63 | +$$R(q) = \begin{pmatrix} |
| 64 | +1-2(y^2+z^2) & 2(xy-wz) & 2(xz+wy) \\ |
| 65 | +2(xy+wz) & 1-2(x^2+z^2) & 2(yz-wx) \\ |
| 66 | +2(xz-wy) & 2(yz+wx) & 1-2(x^2+y^2) |
| 67 | +\end{pmatrix}.$$ |
| 68 | + |
| 69 | +### Euler Angles (ZYX / 321 convention) |
| 70 | + |
| 71 | +Converting from unit quaternion to roll $\phi$, pitch $\theta$, yaw $\psi$: |
| 72 | + |
| 73 | +$$\phi = \operatorname{atan2}(2(wx+yz),\; 1-2(x^2+y^2))$$ |
| 74 | +$$\theta = \arcsin(2(wy-zx))$$ |
| 75 | +$$\psi = \operatorname{atan2}(2(wz+xy),\; 1-2(y^2+z^2))$$ |
| 76 | + |
| 77 | +At $\theta = \pm 90°$ the $\phi$ and $\psi$ axes align (gimbal lock); the formula still |
| 78 | +returns a bounded value but the decomposition is no longer unique. |
| 79 | + |
| 80 | +### SLERP |
| 81 | + |
| 82 | +Spherical Linear Interpolation between unit quaternions $q_0$ and $q_1$ at fraction $t \in [0,1]$: |
| 83 | + |
| 84 | +$$\operatorname{Slerp}(q_0, q_1, t) = \frac{\sin((1-t)\Omega)}{\sin\Omega}\,q_0 + \frac{\sin(t\Omega)}{\sin\Omega}\,q_1,$$ |
| 85 | + |
| 86 | +where $\cos\Omega = q_0 \cdot q_1$. When $\Omega \approx 0$ (nearly parallel quaternions) |
| 87 | +the formula degenerates; a normalized linear interpolation (nlerp) is substituted. |
| 88 | + |
| 89 | +## Complexity Analysis |
| 90 | + |
| 91 | +| Operation | Time | Space | Notes | |
| 92 | +|-----------------------|--------|-------|----------------------------------------| |
| 93 | +| Hamilton product | O(1) | O(1) | 16 multiply-adds, scalar only | |
| 94 | +| Vector rotate | O(1) | O(1) | 15 multiply-adds via cross-product | |
| 95 | +| To rotation matrix | O(1) | O(1) | 9 elements, 16 multiplications | |
| 96 | +| From rotation matrix | O(1) | O(1) | Branch on largest diagonal | |
| 97 | +| SLERP | O(1) | O(1) | 1 acos + 2 sin + scalar blends | |
| 98 | +| Euler conversion | O(1) | O(1) | 2 atan2 + 1 asin | |
| 99 | + |
| 100 | +All operations are stack-only with no heap allocation. |
| 101 | + |
| 102 | +## Step-by-Step Walkthrough |
| 103 | + |
| 104 | +Rotating $\hat{x} = (1,0,0)$ by 90° about $\hat{z}$: |
| 105 | + |
| 106 | +1. Axis-angle: $q = (\cos 45°,\, 0,\, 0,\, \sin 45°) = (\tfrac{\sqrt{2}}{2},\, 0,\, 0,\, \tfrac{\sqrt{2}}{2})$. |
| 107 | +2. $\mathbf{u} = (0, 0, \tfrac{\sqrt{2}}{2})$, $\mathbf{v} = (1, 0, 0)$. |
| 108 | +3. $\mathbf{t} = 2\,\mathbf{u} \times \mathbf{v} = 2(0 \cdot 0 - \tfrac{\sqrt{2}}{2} \cdot 0,\; \tfrac{\sqrt{2}}{2} \cdot 1 - 0,\; 0) = (0,\, \sqrt{2},\, 0)$. |
| 109 | +4. $\mathbf{u} \times \mathbf{t} = (0 \cdot 0 - \tfrac{\sqrt{2}}{2} \cdot \sqrt{2},\; \ldots) = (-1, 0, 0)$. |
| 110 | +5. $\mathbf{v}' = (1,0,0) + \tfrac{\sqrt{2}}{2}(0,\sqrt{2},0) + (-1,0,0) = (0,1,0) = \hat{y}$. Correct. |
| 111 | + |
| 112 | +## Pitfalls & Edge Cases |
| 113 | + |
| 114 | +- **Drift from unit sphere** — repeated products accumulate floating-point error; renormalize |
| 115 | + when $|\|q\|^2 - 1| > \varepsilon$ rather than every step. |
| 116 | +- **Double cover** — $q$ and $-q$ represent the same rotation. SLERP flips the sign of $q_1$ |
| 117 | + when $q_0 \cdot q_1 < 0$ to guarantee the short arc. |
| 118 | +- **Near-parallel SLERP** — when $\cos\Omega > 0.9995$, $\sin\Omega \approx 0$ causes |
| 119 | + division instability; nlerp is substituted with identical results to first order. |
| 120 | +- **Gimbal lock in ToEulerZYX** — at $\theta = \pm 90°$ the formula clamps pitch and |
| 121 | + returns an arbitrary roll/yaw decomposition; the rotation itself remains correct. |
| 122 | +- **FromRotationMatrix** — branching on the largest diagonal avoids dividing by a near-zero |
| 123 | + value when the rotation is close to 180° about a coordinate axis. |
| 124 | + |
| 125 | +## Variants & Generalizations |
| 126 | + |
| 127 | +- **Dual quaternions** — extend to rigid-body transforms (rotation + translation), used in |
| 128 | + screw-motion interpolation. |
| 129 | +- **Exponential map / log** — convert between the Lie algebra $\mathfrak{so}(3)$ and unit |
| 130 | + quaternions, enabling unbiased averaging and covariance propagation. |
| 131 | +- **nlerp** — normalized linear interpolation is faster than SLERP but does not maintain |
| 132 | + constant angular velocity; acceptable for small arcs or high frame rates. |
| 133 | + |
| 134 | +## Applications |
| 135 | + |
| 136 | +- Attitude estimation (AHRS, IMU fusion) — the canonical state representation. |
| 137 | +- 3D rigid-body simulation — compose joint rotations without gimbal lock. |
| 138 | +- Animation blending — SLERP between keyframe orientations at constant angular speed. |
| 139 | +- Computer vision — rotation parameterization in bundle adjustment and PnP solvers. |
| 140 | + |
| 141 | +## Connections to Other Algorithms |
| 142 | + |
| 143 | +- `Geometry3D` (`RotationAboutAxis`, `CrossProduct`) — provides the rotation matrix and |
| 144 | + vector primitives reused by quaternion conversions. |
| 145 | +- Madgwick / Mahony AHRS (item 33) — propagates attitude as a unit quaternion and calls |
| 146 | + `operator*` / `Normalize` on every sample. |
| 147 | +- CORDIC (item 23) — shift-add approximation of `acos`/`sin` for fixed-point axis-angle |
| 148 | + conversions on cores without an FPU. |
| 149 | + |
| 150 | +## References & Further Reading |
| 151 | + |
| 152 | +- J. B. Kuipers, *Quaternions and Rotation Sequences*, Princeton University Press, 1999. |
| 153 | +- K. Shoemake, "Animating rotation with quaternion curves," *ACM SIGGRAPH*, 1985. |
| 154 | +- J. Diebel, "Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors," Stanford Technical Report, 2006. |
0 commit comments