ExampleOfNumericalQuaternionPropagationAndIMUAttitudeStateIntegration 2026-08-24 04:17:34 2026-08-24 04:17:34 Example numerical quaternion propagation and IMU attitude state integration converted to passive rotation convention % this is the default PhysicsLibrary preamble. as your knowledge % of TeX increases, you will probably want to edit this, but % it should be fine as is for beginners. % almost certainly you want these \usepackage{amssymb} \usepackage{amsmath} \usepackage{amsfonts} % used for TeXing text within eps files %\usepackage{psfrag} % need this for including graphics (\includegraphics). % If you use \includegraphics{figure} with no file extension, PhysicsLibrary % will prefer figure.png before falling back to figure.eps. \usepackage{graphicx} \DeclareGraphicsExtensions{.png,.jpg,.jpeg,.pdf,.eps} % for neatly defining theorems and propositions %\usepackage{amsthm} % making logically defined graphics %\usepackage{xypic} % there are many more packages, add them here as you need them % define commands here \section*{Numerical quaternion propagation and IMU attitude-state integration: examples, exercises, and solutions} This companion article belongs to the PhysicsLibrary entry \emph{Numerical quaternion propagation and IMU attitude-state integration}. All exercises are stated first. Complete solutions appear only after the exercise section. \section{Convention declaration} We use the attitude quaternion \begin{equation} q={}^Iq_B, \end{equation} Hamilton multiplication, scalar-first display, and body-resolved gyroscope measurements. Therefore \begin{equation} \dot q=\frac12q\omega^B \end{equation} and body-frame incremental rotations multiply on the right. \section{Exercises} \subsection*{Exercise 1} An IMU reports \begin{equation} \boldsymbol\omega_m^B= \begin{bmatrix} 0.10&-0.20&1.00 \end{bmatrix}^{T}\text{ rad/s}. \end{equation} The estimated gyro bias is \begin{equation} \widehat{\mathbf b}_g= \begin{bmatrix} 0.01&-0.02&0.03 \end{bmatrix}^{T}\text{ rad/s}. \end{equation} For $\Delta t=0.01$ s, compute the corrected angular rate and the delta-angle vector. \subsection*{Exercise 2} For a delta-angle vector \begin{equation} \Delta\boldsymbol\theta= \begin{bmatrix} 0&0&0.02 \end{bmatrix}^{T}\text{ rad}, \end{equation} compute the exact incremental quaternion. \subsection*{Exercise 3} For the increment of Exercise 2, compute the first-order small-angle approximation and compare its norm with one. \subsection*{Exercise 4} Starting from the identity attitude, propagate one sample using the exact increment from Exercise 2. \subsection*{Exercise 5} Starting from \begin{equation} q_k= \frac{1+\mathbf i}{\sqrt2}, \end{equation} apply a small body-frame delta angle $\Delta\boldsymbol\theta=[0,0,0.01]^T$ rad. Write the update in correct multiplication order. \subsection*{Exercise 6} A programmer uses \begin{equation} q_{k+1}=\delta q_k q_k \end{equation} for body-resolved gyroscope data. Explain the mistake. \subsection*{Exercise 7} Derive the forward Euler propagation formula from $\dot q=\frac12q\omega^B$. \subsection*{Exercise 8} Show that the forward Euler step does not preserve quaternion norm exactly. \subsection*{Exercise 9} A gyroscope has a constant uncorrected bias of $0.1^\circ$/s about one axis. Approximately how large is the attitude error after ten minutes? \subsection*{Exercise 10} An IMU runs at 200 Hz and measures a constant rotation rate of $120^\circ$/s about the $y$ axis. Compute the delta angle per sample and the exact incremental quaternion. \subsection*{Exercise 11} For two small body-frame increments $\Delta\boldsymbol\theta_1$ and $\Delta\boldsymbol\theta_2$, derive the leading cross-product term in the equivalent rotation vector. \subsection*{Exercise 12} Let \begin{equation} \Delta\boldsymbol\theta_1= \begin{bmatrix}0.01&0&0\end{bmatrix}^{T}, \qquad \Delta\boldsymbol\theta_2= \begin{bmatrix}0&0.01&0\end{bmatrix}^{T}. \end{equation} Compute the first coning cross term. \subsection*{Exercise 13} Explain why normalizing a forward Euler update does not make it identical to an exact exponential update. \subsection*{Exercise 14} A quaternion time history contains consecutive samples $q_{k-1}$ and $q_k$ with \begin{equation} q_k^Tq_{k-1}<0. \end{equation} What continuity operation is commonly applied, and why? \subsection*{Exercise 15} For a right-error state, use the approximation \begin{equation} \delta q_B \approx \begin{bmatrix} 1\\[1mm] \dfrac12\delta\boldsymbol\theta^B \end{bmatrix} \end{equation} to explain why a three-component attitude error can coexist with a four-component nominal quaternion. \subsection*{Exercise 16} A strapdown navigation state contains quaternion attitude, velocity, position, and gyro bias. Describe the propagation order connecting gyro measurement, quaternion update, accelerometer rotation, and velocity update. \subsection*{Exercise 17} Suppose an IMU reports delta angles directly. Why is it usually preferable to use those delta angles rather than first divide by $\Delta t$ to construct an angular rate and then multiply by $\Delta t$ again? \subsection*{Exercise 18} Compare the intended use of the small-angle approximation in direct IMU propagation with its use in a multiplicative error-state filter. \section{Solutions} \subsection*{Solution 1} The corrected rate is \begin{equation} \boldsymbol\omega_c^B = \boldsymbol\omega_m^B-\widehat{\mathbf b}_g = \begin{bmatrix} 0.09&-0.18&0.97 \end{bmatrix}^{T}\text{ rad/s}. \end{equation} Therefore \begin{equation} \Delta\boldsymbol\theta = \boldsymbol\omega_c^B\Delta t = \begin{bmatrix} 0.0009&-0.0018&0.0097 \end{bmatrix}^{T}\text{ rad}. \end{equation} \subsection*{Solution 2} The increment magnitude is $\Delta\theta=0.02$ rad. Therefore \begin{equation} \delta q = \begin{bmatrix} \cos0.01\\ 0\\ 0\\ \sin0.01 \end{bmatrix} \approx \begin{bmatrix} 0.999950\\ 0\\ 0\\ 0.010000 \end{bmatrix}. \end{equation} \subsection*{Solution 3} The first-order approximation is \begin{equation} \delta q_{\mathrm{small}} = \begin{bmatrix} 1\\ 0\\ 0\\ 0.01 \end{bmatrix}. \end{equation} Its norm is \begin{equation} \left\|\delta q_{\mathrm{small}}\right\| = \sqrt{1+0.01^2} \approx 1.000050. \end{equation} Thus it is not exactly a unit quaternion until normalized. \subsection*{Solution 4} The identity quaternion is $q_0=1$, so \begin{equation} q_1=q_0\delta q=\delta q. \end{equation} Hence the propagated attitude is exactly the increment from Exercise 2. \subsection*{Solution 5} Because the gyro increment is body-resolved, it multiplies on the right: \begin{equation} q_{k+1} = q_k\delta q_k. \end{equation} For the small increment, \begin{equation} \delta q_k \approx 1+\frac12(0.01)\mathbf k = 1+0.005\mathbf k. \end{equation} Thus \begin{equation} q_{k+1} \approx \frac{1+\mathbf i}{\sqrt2} \left(1+0.005\mathbf k\right), \end{equation} followed by normalization if the first-order increment is used. \subsection*{Solution 6} For $q={}^Iq_B$, body-resolved angular velocity satisfies \begin{equation} \dot q=\frac12q\omega^B. \end{equation} Therefore its finite increment multiplies on the right. Left multiplication corresponds to an inertial-resolved rate under this convention. \subsection*{Solution 7} Forward Euler gives \begin{equation} q_{k+1} = q_k+\dot q_k\Delta t. \end{equation} Substituting the quaternion kinematic equation gives \begin{equation} \boxed{ q_{k+1} = q_k+\frac12q_k\omega_k^B\Delta t. } \end{equation} \subsection*{Solution 8} Let \begin{equation} q_{k+1} = q_k+\frac12q_k\omega^B\Delta t. \end{equation} The added term is tangent to the unit-quaternion sphere only to first order. The squared norm therefore differs from one by terms of order $O(\Delta t^2)$. Hence repeated unnormalized Euler steps drift away from unit norm. \subsection*{Solution 9} A constant $0.1^\circ$/s bias accumulates approximately linearly. Ten minutes is 600 s, so \begin{equation} \delta\theta \approx 0.1^\circ/\text{s}\times600\text{ s} = 60^\circ. \end{equation} This illustrates why gyro-bias estimation is essential. \subsection*{Solution 10} At 200 Hz, \begin{equation} \Delta t=0.005\text{ s}. \end{equation} The sample rotation is \begin{equation} \Delta\theta = 120^\circ/\text{s}\times0.005\text{ s} = 0.6^\circ = 0.010472\text{ rad}. \end{equation} The exact increment is \begin{equation} \delta q = \cos\frac{0.010472}{2} + \mathbf j\sin\frac{0.010472}{2}, \end{equation} or approximately \begin{equation} \delta q \approx 0.999986 + 0.005236\,\mathbf j. \end{equation} \subsection*{Solution 11} Use \begin{equation} \delta q_1 \approx 1+\frac12\Delta\boldsymbol\theta_1, \qquad \delta q_2 \approx 1+\frac12\Delta\boldsymbol\theta_2. \end{equation} For body increments in time order, \begin{equation} \delta q_{12} = \delta q_1\delta q_2. \end{equation} The vector part contains \begin{equation} \frac12 \left( \Delta\boldsymbol\theta_1+ \Delta\boldsymbol\theta_2 \right) + \frac14 \Delta\boldsymbol\theta_1 \times \Delta\boldsymbol\theta_2. \end{equation} Multiplying by two to recover the equivalent small rotation vector gives \begin{equation} \boxed{ \Delta\boldsymbol\theta_{\mathrm{eq}} \approx \Delta\boldsymbol\theta_1+ \Delta\boldsymbol\theta_2+ \frac12 \Delta\boldsymbol\theta_1 \times \Delta\boldsymbol\theta_2. } \end{equation} \subsection*{Solution 12} The cross product is \begin{equation} \Delta\boldsymbol\theta_1 \times \Delta\boldsymbol\theta_2 = \begin{bmatrix} 0\\ 0\\ 0.0001 \end{bmatrix}. \end{equation} Therefore the leading coning term is \begin{equation} \frac12 \Delta\boldsymbol\theta_1 \times \Delta\boldsymbol\theta_2 = \begin{bmatrix} 0\\ 0\\ 0.00005 \end{bmatrix}\text{ rad}. \end{equation} \subsection*{Solution 13} Normalization restores the constraint $\|q\|=1$, but it does not reconstruct the higher-order phase terms omitted by the Euler discretization. The exact exponential update follows the unit-quaternion manifold for a constant sample rate, whereas normalized Euler only projects an approximate step back onto that manifold. \subsection*{Solution 14} One commonly applies \begin{equation} q_k\leftarrow -q_k. \end{equation} Because $q$ and $-q$ represent the same attitude, the physical orientation is unchanged. The sign flip simply chooses the representative closer to the previous quaternion, improving continuity for plotting, interpolation, and finite differencing. \subsection*{Solution 15} The nominal attitude needs four quaternion components to represent a global orientation without Euler-angle singularities, subject to the unit-norm constraint. A local attitude perturbation has only three independent degrees of freedom. Therefore an estimator can propagate the nominal state as a full quaternion while carrying only the small three-vector $\delta\boldsymbol\theta^B$ in its linearized error state. \subsection*{Solution 16} A typical order is: \begin{enumerate} \item read the body gyro and accelerometer measurements; \item correct the gyro measurement using the current bias estimate; \item form the delta angle and propagate the quaternion; \item rotate the measured body specific force into the navigation or inertial frame using the updated or appropriately time-centered attitude; \item combine specific force with gravity and other frame terms to propagate velocity and position; \item propagate gyro bias and other estimator states according to their process models. \end{enumerate} \subsection*{Solution 17} A delta-angle output is already the sensor's estimate of integrated angular motion over the sample. Converting it to an average rate and then integrating again adds unnecessary numerical operations and may discard information if the device applied internal high-rate integration or coning compensation. \subsection*{Solution 18} In direct propagation, the small-angle approximation may replace the exact increment only when the physical sample rotation is sufficiently small. In an error-state filter, the approximation has a different role: it represents a small local attitude error while the nominal attitude remains an exact full quaternion. The latter use is fundamental even when the nominal vehicle attitude itself is large. \section*{License} This article is an original synthesis prepared for PhysicsLibrary and intended for release under CC BY-SA 4.0.