Numerical quaternion propagation and IMU attitude-state integration: examples, exercises, and solutions

This companion article belongs to the PhysicsLibrary entry Numerical quaternion propagation and IMU attitude-state integration. All exercises are stated first. Complete solutions appear only after the exercise section.

Convention declaration

We use the attitude quaternion

$\displaystyle q={}^Iq_B,$

(1)

Hamilton multiplication, scalar-first display, and body-resolved gyroscope measurements. Therefore

$\displaystyle \dot q=\frac12q\omega^B$

(2)

and body-frame incremental rotations multiply on the right.

Exercises

Exercise 1

An IMU reports

$\displaystyle \boldsymbol\omega_m^B= \begin{bmatrix} 0.10&-0.20&1.00 \end{bmatrix}^{T}$ rad/s $\displaystyle .$

(3)

The estimated gyro bias is

$\displaystyle \widehat{\mathbf b}_g= \begin{bmatrix} 0.01&-0.02&0.03 \end{bmatrix}^{T}$ rad/s $\displaystyle .$

(4)

For $\Delta t=0.01$ s, compute the corrected angular rate and the delta-angle vector.

Exercise 2

For a delta-angle vector

$\displaystyle \Delta\boldsymbol\theta= \begin{bmatrix} 0&0&0.02 \end{bmatrix}^{T}$ rad $\displaystyle ,$

(5)

compute the exact incremental quaternion.

Exercise 3

For the increment of Exercise 2, compute the first-order small-angle approximation and compare its norm with one.

Exercise 4

Starting from the identity attitude, propagate one sample using the exact increment from Exercise 2.

Exercise 5

Starting from

$\displaystyle q_k= \frac{1+\mathbf i}{\sqrt2},$

(6)

apply a small body-frame delta angle $\Delta\boldsymbol\theta=[0,0,0.01]^T$ rad. Write the update in correct multiplication order.

Exercise 6

A programmer uses

$\displaystyle q_{k+1}=\delta q_k q_k$

(7)

for body-resolved gyroscope data. Explain the mistake.

Exercise 7

Derive the forward Euler propagation formula from $\dot q=\frac12q\omega^B$ .

Exercise 8

Show that the forward Euler step does not preserve quaternion norm exactly.

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?

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.

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.

Exercise 12

Let

$\displaystyle \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}.$

(8)

Compute the first coning cross term.

Exercise 13

Explain why normalizing a forward Euler update does not make it identical to an exact exponential update.

Exercise 14

A quaternion time history contains consecutive samples $q_{k-1}$ and $q_k$

with

$\displaystyle q_k^Tq_{k-1}<0.$

(9)

What continuity operation is commonly applied, and why?

Exercise 15

For a right-error state, use the approximation

$\displaystyle \delta q_B \approx \begin{bmatrix} 1\\ [1mm] \dfrac12\delta\boldsymbol\theta^B \end{bmatrix}$

(10)

to explain why a three-component attitude error can coexist with a four-component nominal quaternion.

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.

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?

Exercise 18

Compare the intended use of the small-angle approximation in direct IMU propagation with its use in a multiplicative error-state filter.

Solutions

Solution 1

The corrected rate is

$\displaystyle \boldsymbol\omega_c^B = \boldsymbol\omega_m^B-\widehat{\mathbf b}_g = \begin{bmatrix} 0.09&-0.18&0.97 \end{bmatrix}^{T}$ rad/s $\displaystyle .$

(11)

Therefore

$\displaystyle \Delta\boldsymbol\theta = \boldsymbol\omega_c^B\Delta t = \begin{bmatrix} 0.0009&-0.0018&0.0097 \end{bmatrix}^{T}$ rad $\displaystyle .$

(12)

Solution 2

The increment magnitude is $\Delta\theta=0.02$ rad. Therefore

$\displaystyle \delta q = \begin{bmatrix} \cos0.01\ 0\ 0\ \sin0.01 \end{bmatrix}\approx \begin{bmatrix} 0.999950\ 0\ 0\ 0.010000 \end{bmatrix}.$

(13)

Solution 3

The first-order approximation is

$\displaystyle \delta q_{\mathrm{small}} = \begin{bmatrix} 1\ 0\ 0\ 0.01 \end{bmatrix}.$

(14)

Its norm is

$\displaystyle \left\Vert\delta q_{\mathrm{small}}\right\Vert = \sqrt{1+0.01^2} \approx 1.000050.$

(15)

Thus it is not exactly a unit quaternion until normalized.

Solution 4

The identity quaternion is $q_0=1$

, so

$\displaystyle q_1=q_0\delta q=\delta q.$

(16)

Hence the propagated attitude is exactly the increment from Exercise 2.

Solution 5

Because the gyro increment is body-resolved, it multiplies on the right:

$\displaystyle q_{k+1} = q_k\delta q_k.$

(17)

For the small increment,

$\displaystyle \delta q_k \approx 1+\frac12(0.01)\mathbf k = 1+0.005\mathbf k.$

(18)

Thus

$\displaystyle q_{k+1} \approx \frac{1+\mathbf i}{\sqrt2} \left(1+0.005\mathbf k\right),$

(19)

followed by normalization if the first-order increment is used.

Solution 6

For $q={}^Iq_B$ , body-resolved angular velocity satisfies

$\displaystyle \dot q=\frac12q\omega^B.$

(20)

Therefore its finite increment multiplies on the right. Left multiplication corresponds to an inertial-resolved rate under this convention.

Solution 7

Forward Euler gives

$\displaystyle q_{k+1} = q_k+\dot q_k\Delta t.$

(21)

Substituting the quaternion kinematic equation gives

$\displaystyle \boxed{ q_{k+1} = q_k+\frac12q_k\omega_k^B\Delta t. }$

(22)

Solution 8

Let

$\displaystyle q_{k+1} = q_k+\frac12q_k\omega^B\Delta t.$

(23)

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.

Solution 9

A constant $0.1^\circ$ /s bias accumulates approximately linearly. Ten minutes is 600 s, so

$\displaystyle \delta\theta \approx 0.1^\circ/$ s $\displaystyle \times600$ s $\displaystyle = 60^\circ.$

(24)

This illustrates why gyro-bias estimation is essential.

Solution 10

At 200 Hz,

$\displaystyle \Delta t=0.005$ s $\displaystyle .$

(25)

The sample rotation is

$\displaystyle \Delta\theta = 120^\circ/$ s $\displaystyle \times0.005$ s $\displaystyle = 0.6^\circ = 0.010472$ rad $\displaystyle .$

(26)

The exact increment is

$\displaystyle \delta q = \cos\frac{0.010472}{2} + \mathbf j\sin\frac{0.010472}{2},$

(27)

or approximately

$\displaystyle \delta q \approx 0.999986 + 0.005236\,\mathbf j.$

(28)

Solution 11

Use

$\displaystyle \delta q_1 \approx 1+\frac12\Delta\boldsymbol\theta_1, \qquad \delta q_2 \approx 1+\frac12\Delta\boldsymbol\theta_2.$

(29)

For body increments in time order,

$\displaystyle \delta q_{12} = \delta q_1\delta q_2.$

(30)

The vector part contains

$\displaystyle \frac12 \left( \Delta\boldsymbol\theta_1+ \Delta\boldsymbol\theta_2 \right) + \frac14 \Delta\boldsymbol\theta_1 \times \Delta\boldsymbol\theta_2.$

(31)

Multiplying by two to recover the equivalent small rotation vector gives

$\displaystyle \boxed{ \Delta\boldsymbol\theta_{\mathrm{eq}} \approx \Delta\bold... ...\theta_2+ \frac12 \Delta\boldsymbol\theta_1 \times \Delta\boldsymbol\theta_2. }$

(32)

Solution 12

The cross product is

$\displaystyle \Delta\boldsymbol\theta_1 \times \Delta\boldsymbol\theta_2 = \begin{bmatrix} 0\ 0\ 0.0001 \end{bmatrix}.$

(33)

Therefore the leading coning term is

$\displaystyle \frac12 \Delta\boldsymbol\theta_1 \times \Delta\boldsymbol\theta_2 = \begin{bmatrix} 0\ 0\ 0.00005 \end{bmatrix}$ rad $\displaystyle .$

(34)

Solution 13

Normalization restores the constraint $\Vert q\Vert=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.

Solution 14

One commonly applies

$\displaystyle q_k\leftarrow -q_k.$

(35)

Because

and

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.

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.

Solution 16

A typical order is:

read the body gyro and accelerometer measurements;
correct the gyro measurement using the current bias estimate;
form the delta angle and propagate the quaternion;
rotate the measured body specific force into the navigation or inertial frame using the updated or appropriately time-centered attitude;
combine specific force with gravity and other frame terms to propagate velocity and position;
propagate gyro bias and other estimator states according to their process models.

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.

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.

License

This article is an original synthesis prepared for PhysicsLibrary and intended for release under CC BY-SA 4.0.

"example of numerical quaternion propagation and IMU attitude state integration" is owned by bloftin.
(view preamble)