Numerical Quaternion Propagation and IMU Attitude State Integration: Examples, Exercises, and Solutions

This entry is the self study companion to numerical quaternion propagation and IMU attitude state integration.

All eighteen exercises are stated first. Complete worked solutions appear only after the exercise section.

Convention declaration

PhysicsLibrary uses the passive inertial to body attitude quaternion

$\displaystyle q = {}^{B}q_I.$

(1)

Hamilton multiplication and scalar first display order are used.

For body resolved gyroscope angular velocity,

$\displaystyle \dot q = -\frac12 \omega^Bq.$

(2)

A positive body frame delta angle

$\displaystyle \Delta\boldsymbol\theta^B$

produces the passive incremental quaternion

$\displaystyle \delta q_B = \exp \left( -\frac12 \Delta\boldsymbol\theta^B \right).$

(3)

Because the increment maps the old body frame into the new body frame, it multiplies on the left:

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

(4)

For a small passive frame error,

$\displaystyle \delta q \approx \begin{bmatrix} 1\ -\frac12\delta\boldsymbol\theta \end{bmatrix}.$

(5)

Exercises

Bias correction and delta angle.
An IMU reports

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

The estimated gyroscope bias is

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

For

$\displaystyle \Delta t=0.01\ \mathrm{s},$

compute the corrected angular rate and the delta angle vector.
Exact incremental quaternion.
For

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

compute the exact passive incremental quaternion.
Small angle increment.
For the increment of Exercise 2, compute the first order small angle approximation and compare its norm with one.
One exact propagation step.
Starting from the identity attitude, propagate one sample using the exact increment from Exercise 2.
Nonidentity attitude and a body increment.
Starting from the positive passive $90^\circ$ frame rotation about ,

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

apply the small body frame delta angle

$\displaystyle \Delta\boldsymbol\theta^B = \begin{bmatrix} 0\ 0\ 0.01 \end{bmatrix}\mathrm{rad}.$

Write the first order update in the correct multiplication order and expand the Hamilton product.
Wrong multiplication side.
A programmer uses

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

for body resolved gyroscope data while storing

$\displaystyle q={}^Bq_I.$

Explain the mistake and write the correct update.
Forward Euler propagation.
Derive the forward Euler propagation formula from

$\displaystyle \dot q = -\frac12\omega^Bq.$
Forward Euler norm drift.
Show that a forward Euler step does not preserve quaternion norm exactly.
Gyroscope bias accumulation.
A gyroscope has a constant uncorrected bias of

$\displaystyle 0.1^\circ/\mathrm{s}$

about one axis.

Approximately how large is the attitude error after ten minutes?
Two hundred hertz constant rotation.
An IMU runs at Hz and measures a constant positive body frame rotation rate of

$\displaystyle 120^\circ/\mathrm{s}$

about the axis.

Compute the delta angle per sample and the exact passive incremental quaternion.
Leading coning term.
For two chronological small body frame increments $\Delta\boldsymbol\theta_1$ followed by $\Delta\boldsymbol\theta_2$ , derive the leading cross product term in the equivalent physical rotation vector.
Numerical coning cross term.
Let

$\displaystyle \Delta\boldsymbol\theta_1 = \begin{bmatrix} 0.01\ 0\ 0 \end{b... ...boldsymbol\theta_2 = \begin{bmatrix} 0\ 0.01\ 0 \end{bmatrix}\mathrm{rad}.$

Compute the leading coning cross term.
Normalization versus exact propagation.
Explain why normalizing a forward Euler update does not make it identical to an exact exponential update.
Quaternion sign continuity.
A quaternion time history contains consecutive samples $q_{k-1}$ and with

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

What continuity operation is commonly applied, and why?
Three component error state.
For a passive multiplicative error state, use

$\displaystyle \delta q \approx \begin{bmatrix} 1\ -\frac12\delta\boldsymbol\theta \end{bmatrix}$ (8)

to explain why a three component attitude error can coexist with a four component nominal quaternion.
Strapdown state propagation order.
A strapdown navigation state contains quaternion attitude, velocity, position, and gyroscope bias.

Describe the propagation order connecting gyroscope measurement, quaternion update, accelerometer transformation, and velocity update.
Using IMU supplied delta angles.
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 average angular rate and then multiply by $\Delta t$ again?
Direct small increment versus filter small error.
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: bias correction and delta angle

The corrected rate is

$\begin{equation*} \begin{aligned} \boldsymbol\omega_c^B &= \boldsymbol\omega_m^B... ... 0.09\ -0.18\ 0.97 \end{bmatrix}\mathrm{rad/s}. \end{aligned}\end{equation*}$

Therefore

$\displaystyle \boldsymbol\omega_c^B = \begin{bmatrix} 0.09\ -0.18\ 0.97 \end{bmatrix}\mathrm{rad/s}.$

(9)

The delta angle is

$\displaystyle \Delta\boldsymbol\theta^B = \boldsymbol\omega_c^B\Delta t.$

Thus

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

(10)

Solution 2: exact incremental quaternion

The increment magnitude is

$\displaystyle \Delta\theta = 0.02\ \mathrm{rad}.$

The positive physical frame axis is $+\widehat{\mathbf z}$ .

Under the PhysicsLibrary passive convention,

$\displaystyle \delta q_B = \cos\frac{\Delta\theta}{2} - \mathbf k \sin\frac{\Delta\theta}{2}.$

Hence

$\displaystyle \delta q_B = \cos0.01 - \mathbf k\sin0.01.$

(11)

Numerically,

$\displaystyle \delta\mathbf q_B \approx \begin{bmatrix} 0.9999500004\ 0\ 0\ -0.0099998333 \end{bmatrix}.$

(12)

The negative vector component is the passive sign for a positive frame rotation.

Solution 3: small angle increment

For a small passive body frame increment,

$\displaystyle \delta q_B \approx 1 - \frac12 \Delta\boldsymbol\theta^B.$

Therefore

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

(13)

Its norm is

$\displaystyle \left\lVert \delta q_{B,\mathrm{small}} \right\rVert = \sqrt{1+0.01^2}.$

Thus

$\displaystyle \left\lVert \delta q_{B,\mathrm{small}} \right\rVert \approx 1.000049999.$

(14)

The first order increment is therefore not exactly unit until normalized.

Solution 4: one exact propagation step

The initial attitude is

$\displaystyle q_0=1.$

Body resolved increments left multiply:

$\displaystyle q_1 = \delta q_Bq_0.$

Since $q_0=1$ ,

$\displaystyle q_1 = \delta q_B.$

(15)

Therefore

$\displaystyle q_1 = \cos0.01 - \mathbf k\sin0.01.$

(16)

Solution 5: nonidentity attitude and a body increment

The first order passive increment is

$\displaystyle \delta q_B \approx 1 - 0.005\mathbf k.$

The correct body increment update is

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

Therefore

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

Expand:

$\begin{equation*} \begin{aligned} q_{k+1} &\approx \frac{1}{\sqrt2} \left( 1-\ma... ... i-0.005\mathbf k +0.005\mathbf k\mathbf i \right). \end{aligned}\end{equation*}$

Since

$\displaystyle \mathbf k\mathbf i=\mathbf j,$

we obtain

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

(17)

If this first order increment is used numerically, the result should be normalized.

Solution 6: wrong multiplication side

For

$\displaystyle q={}^Bq_I,$

body resolved angular velocity satisfies

$\displaystyle \dot q = -\frac12\omega^Bq.$

The finite body increment maps

$\displaystyle B_k\rightarrow B_{k+1}$

and therefore appears on the left of the existing

$\displaystyle I\rightarrow B_k$

map.

Thus the correct update is

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

(18)

Right multiplication would correspond to expressing the increment on the reference side under this passive convention.

Solution 7: forward Euler propagation

Forward Euler gives

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

Substitute

$\displaystyle \dot q_k = -\frac12 \omega_k^Bq_k.$

Then

$\displaystyle q_{k+1} = q_k - \frac12 \omega_k^Bq_k \Delta t.$

(19)

With

$\displaystyle \Delta\boldsymbol\theta_k^B = \boldsymbol\omega_k^B\Delta t,$

this can also be written

$\displaystyle q_{k+1} = \left( 1 - \frac12 \Delta\boldsymbol\theta_k^B \right) q_k.$

(20)

Solution 8: forward Euler norm drift

Let

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

For the exact quaternion differential equation,

$\displaystyle q_k^T\dot q_k=0,$

because the derivative is tangent to the unit quaternion three sphere.

Thus

$\begin{equation*} \begin{aligned} \lVert q_{k+1}\rVert^2 &= (q_k+\dot q_k\Delta ... ...lta t)\ &= 1 + \lVert\dot q_k\rVert^2 \Delta t^2. \end{aligned}\end{equation*}$

For nonzero angular rate,

$\displaystyle \lVert q_{k+1}\rVert^2>1.$

(21)

The error is second order per step, so repeated unnormalized Euler propagation drifts from unit norm.

Solution 9: gyroscope bias accumulation

Ten minutes is

$\displaystyle 600\ \mathrm{s}.$

A constant bias of

$\displaystyle 0.1^\circ/\mathrm{s}$

therefore accumulates approximately

$\displaystyle \delta\theta = 0.1^\circ/\mathrm{s} \times 600\ \mathrm{s}.$

Hence

$\displaystyle \delta\theta \approx 60^\circ.$

(22)

This illustrates why gyroscope bias estimation and correction are essential.

Solution 10: two hundred hertz constant rotation

At $200$ Hz,

$\displaystyle \Delta t = \frac{1}{200} = 0.005\ \mathrm{s}.$

The sample rotation is

$\displaystyle \Delta\theta = 120^\circ/\mathrm{s} \times 0.005\ \mathrm{s} = 0.6^\circ.$

In radians,

$\displaystyle \Delta\theta \approx 0.01047198\ \mathrm{rad}.$

(23)

The half angle is approximately

$\displaystyle 0.00523599\ \mathrm{rad}.$

For a positive body frame rotation about $+y$ , the passive increment is

$\displaystyle \delta q_B = \cos0.00523599 - \mathbf j\sin0.00523599.$

(24)

Numerically,

$\displaystyle \delta q_B \approx 0.999986292 - 0.005235964\,\mathbf j.$

(25)

Solution 11: leading coning term

The first order passive increments are

$\displaystyle \delta q_1 \approx 1 - \frac12 \Delta\boldsymbol\theta_1$

and

$\displaystyle \delta q_2 \approx 1 - \frac12 \Delta\boldsymbol\theta_2.$

Because the first increment occurs before the second, body frame multiplication gives

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

Expand:

$\begin{equation*} \begin{aligned} \delta q_{21} \approx{}& 1 - \frac12 \left( \D... ...Delta\boldsymbol\theta_2 \Delta\boldsymbol\theta_1. \end{aligned}\end{equation*}$

For pure vector quaternions,

$\displaystyle \Delta\boldsymbol\theta_2 \Delta\boldsymbol\theta_1 = - \Delta\bo... ...ldsymbol\theta_1 + \Delta\boldsymbol\theta_2 \times \Delta\boldsymbol\theta_1.$

The vector contribution is therefore

$\displaystyle -\frac14 \Delta\boldsymbol\theta_1 \times \Delta\boldsymbol\theta_2.$

Match this with the passive form

$\displaystyle 1 - \frac12 \Delta\boldsymbol\theta_{\mathrm{eq}}.$

The resulting equivalent physical rotation vector is

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

(26)

The physical coning cross term has the same positive cross product form, although the quaternion vector part itself carries the passive negative sign.

Solution 12: numerical coning cross term

The cross product is

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

Therefore the leading coning contribution is

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

(27)

Solution 13: normalization versus exact propagation

Normalization restores

$\displaystyle \lVert q\rVert=1.$

It does not reconstruct higher order rotational information omitted by a forward Euler step.

The exact exponential update follows the correct finite rotation on the unit quaternion manifold for a constant sample rotation vector.

Normalized Euler first takes a tangent line approximation and then projects that approximate point back onto the unit sphere.

The projected point is generally close to, but not identical with, the exact exponential result.

Solution 14: quaternion sign continuity

One commonly applies

$\displaystyle q_k \leftarrow -q_k.$

(28)

Because $q$ and $-q$ represent the same physical orientation, this operation does not alter attitude.

It simply chooses the representative closer to the preceding quaternion. This improves continuity for plotting, interpolation, finite differencing, and optimization.

Solution 15: three component error state

The nominal quaternion is a global attitude representation with four stored components constrained by

$\displaystyle q^Tq=1.$

Therefore it has three independent degrees of freedom.

A local passive multiplicative error can be written

$\displaystyle \delta q \approx \begin{bmatrix} 1\ -\frac12 \delta\boldsymbol\theta \end{bmatrix}.$

The three vector components of

$\displaystyle \delta\boldsymbol\theta$

are therefore sufficient to represent the local orientation perturbation.

An estimator can consequently propagate the nominal attitude as a full unit quaternion while carrying only three attitude error variables in the linearized covariance state.

Solution 16: strapdown state propagation order

A typical passive quaternion strapdown propagation order is:

read body gyroscope and accelerometer measurements;
correct the gyroscope measurement or delta angle using the current bias estimate;
form the passive body incremental quaternion;
update attitude using

$\displaystyle q_{k+1} = \delta q_Bq_k;$
obtain the updated or appropriately time centered attitude for accelerometer processing;
transform measured body specific force into the navigation or inertial frame using the inverse attitude map;
combine specific force with gravity and any required rotating frame terms;
integrate velocity and position;
propagate gyroscope bias and other estimator states according to their process models.

For

$\displaystyle q={}^Bq_I,$

the body to inertial specific force transformation is

$\displaystyle {}^I\mathbf f = C(q)^T {}^B\mathbf f.$

Solution 17: using IMU supplied delta angles

A delta angle output is already the sensor's estimate of integrated angular motion over the sample.

Converting it to an average rate by dividing by $\Delta t$ and then multiplying by $\Delta t$ again adds unnecessary operations.

More importantly, the device may have formed the delta angle using internal high rate samples and may already include coning or other compensation.

Reducing that output to a single average rate can hide how the device formed the integrated quantity and can invite accidental duplicate processing.

Therefore the sensor supplied delta angle is normally the more natural input to the finite quaternion update, subject to the device documentation and bias correction model.

Solution 18: direct small increment versus filter small error

In direct IMU propagation,

$\displaystyle \delta q_B \approx 1 - \frac12 \Delta\boldsymbol\theta^B$

is an approximation to the actual physical rotation occurring during one sample.

Its usefulness depends on the sample rotation being sufficiently small.

In a multiplicative error state filter,

$\displaystyle \delta q \approx 1 - \frac12 \delta\boldsymbol\theta$

has a different role.

The nominal attitude remains a full unit quaternion representing the global orientation, while the three component vector $\delta\boldsymbol\theta$ represents only a local estimation error around that nominal attitude.

The nominal vehicle attitude may therefore be arbitrarily large even though the error state is intentionally kept small.

Compact passive propagation checks

Quantity	PhysicsLibrary passive result
Attitude state	$q={}^Bq_I$
Body rate kinematics	$\dot q=-\frac12\omega^Bq$
Positive body increment	$\delta q_B=\exp(-\Delta\boldsymbol\theta^B/2)$
Body increment update	$q_{k+1}=\delta q_Bq_k$
Small body increment	$\delta q_B\approx[1,-\Delta\boldsymbol\theta^B/2]^T$
Passive error state	$\delta q\approx[1,-\delta\boldsymbol\theta/2]^T$
Sign continuity	$q_k\leftarrow-q_k$ when $q_k^Tq_{k-1}<0$

Sources and exercise provenance

This companion follows the eighteen problem progression of the earlier PhysicsLibrary example article but rewrites every convention sensitive formula and worked result for the passive inertial to body convention.

Sommer and coauthors provide a modern discussion of quaternion convention management. Solà provides a detailed treatment of quaternion kinematics and error state filtering. Titterton and Weston provide a broad engineering treatment of strapdown inertial navigation. Savage develops classical strapdown attitude integration and coning compensation.

Bibliography

1: H. Sommer, I. Gilitschenski, M. Bloesch, S. Weiss, R. Siegwart, and J. Nieto, “Why and How to Avoid the Flipped Quaternion Multiplication,” Aerospace, vol. 5, no. 3, article 72, 2018. Published under CC BY 4.0. Publisher article https://www.mdpi.com/2226-4310/5/3/72
2: J. Solà, “Quaternion Kinematics for the Error State Kalman Filter,” arXiv:1711.02508, 2017. arXiv preprint https://arxiv.org/abs/1711.02508
3: D. H. Titterton and J. L. Weston, Strapdown Inertial Navigation Technology, 2nd ed., The Institution of Engineering and Technology, 2004/2005. Engineering reference. IET book page https://shop.theiet.org/strapdn-inertial-navig-t-2ed
4: P. G. Savage, “Strapdown Inertial Navigation Integration Algorithm Design Part 1: Attitude Algorithms,” Journal of Guidance, Control, and Dynamics, vol. 21, no. 1, pp. 19–28, 1998. DOI record https://doi.org/10.2514/2.4228

License

Unless otherwise noted, this PhysicsLibrary entry is intended for release under the Creative Commons Attribution ShareAlike 4.0 International license.

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