numerical quaternion propagation and IMU attitude state integration

(Topic)

Numerical Quaternion Propagation and IMU Attitude State Integration

A gyroscope does not directly measure attitude. It measures angular motion. An inertial navigation or attitude system must integrate that angular motion over time to update an orientation state.

Quaternions are particularly well suited to this task because a unit quaternion can be updated by a finite incremental rotation without introducing an Euler Angle singularity.

This article develops numerical propagation for the PhysicsLibrary passive attitude quaternion

B q = qI,

(1)

which maps coordinates from an inertial or reference frame I into a body frame B.

For body resolved angular velocity,

1 B ˙q = − -ω q. 2

(2)

Over one sample interval, the exact constant rate update is

qk+1 = δqBqk,

(3)

where

( ) δqB = exp − 1-Δ 𝜃B . 2

(4)

Thus the numerical implementation must preserve three convention facts:

the body frame increment has a negative quaternion vector part;
the body resolved increment multiplies on the left;
the propagated quaternion maps reference coordinates into body coordinates.

1 Convention declaration

PhysicsLibrary uses Hamilton multiplication,

ij = k, jk = i, ki = j,

with reversed products changing sign.

Quaternion components are displayed scalar first:

q = qw + qxi + qyj + qzk.

(5)

The passive vector transformation is

Bv = q Ivq∗.

(6)

For a positive frame rotation through angle 𝜃 about unit axis u,

q = cos 𝜃-− ^u sin 𝜃. 2 2

(7)

The corresponding DCM satisfies

B B I B v = CI v, CI = C (q).

(8)

2 From gyroscope measurement to corrected angular rate

A practical body mounted gyroscope measurement can be modeled as

ωBm = B ωB ∕I + bg + ng,

(9)

where

bg

is gyroscope bias and

ng

represents measurement noise.

Given a current bias estimate b_g, form

B B ^ ^ω = ω m − bg.

(10)

For a simple zero order hold over sample interval Δt, the corresponding body resolved delta angle is

B B Δ𝜃 = ^ω Δt.

(11)

If the IMU itself supplies a delta angle measurement, that integrated quantity should normally be used directly after the appropriate bias correction and manufacturer specified compensation rather than unnecessarily converting it to a rate and integrating it again.

3 Exact exponential increment

Let

∥∥ B∥∥ Δ𝜃 = Δ 𝜃 .

(12)

Δ 𝜃 ⁄= 0,

define the unit increment axis

Δ 𝜃B ^u = -----. Δ 𝜃

(13)

The exact passive body increment is

δq = cos Δ-𝜃 − ^u sin Δ-𝜃 . B 2 2

(14)

Equivalently,

( ) 1- B δqB = exp − 2 Δ𝜃 .

(15)

In scalar first array notation,

[ cos(Δ 𝜃∕2 ) ] δq = . B − sin(Δ-𝜃∕2)Δ 𝜃B Δ𝜃

(16)

The limiting value at Δ𝜃 = 0 is the identity quaternion.

4 Why body increments left multiply

During sample k, the old attitude maps

I → Bk.

The measured body increment maps

Bk → Bk+1.

The complete passive frame chain is therefore

I → Bk → Bk+1.

PhysicsLibrary quaternion frame composition gives

B B B k+1qI = k+1qBk kqI.

(17)

Thus

qk+1 = δqB,kqk.

(18)

The side of multiplication is a consequence of the declared frame map, not a change to Hamilton multiplication.

5 Propagation flow

The following diagram summarizes the passive IMU attitude state propagation chain.

Figure. A body mounted IMU supplies angular motion and specific force. Gyroscope bias is removed, a passive incremental quaternion is formed, and the increment left multiplies the inertial to body attitude quaternion. The updated or appropriately time centered attitude is then used to transform body specific force into the navigation or inertial frame. A multiplicative estimator may carry a three component local error state while the nominal orientation remains a full unit quaternion.

6 Small angle increment

For sufficiently small Δ𝜃,

Δ 𝜃 cos --- ≈ 1 2

and

Δ 𝜃 Δ𝜃 sin --- ≈ ---. 2 2

Therefore

1 B δqB ≈ 1 − 2Δ 𝜃 .

(19)

The corresponding first order update is

( ) q ≈ 1 − 1Δ 𝜃B q . k+1 2 k

(20)

This is useful for analysis and for sufficiently small sample increments, but the exact trigonometric increment is inexpensive and generally preferable when the delta angle is already available.

7 Forward Euler integration

Starting from

˙q = − 1ωBq, 2

a forward Euler step is

qE = qk − 1ωB qkΔt. k+1 2 k

(21)

Δ 𝜃B = ωB Δt, k k

then

( ) E 1- B qk+1 = 1 − 2Δ 𝜃k qk.

(22)

Thus forward Euler is equivalent to using the first order incremental quaternion without normalization.

8 Why forward Euler leaves unit norm

The exact quaternion derivative is tangent to the unit quaternion three sphere.

A forward Euler method advances along the tangent line:

E qk+1 = qk + ˙qkΔt.

For unit q_k,

qTq˙ = 0. k k

Therefore

∥∥ E ∥∥2 T qk+1 = (qk + ˙qkΔt ) (qk + q˙kΔt ) = 1 + ∥˙qk∥2Δt2.

Hence

∥∥ E ∥∥ qk+1 > 1

(23)

for a nonzero rate and nonzero step.

The norm defect is second order for one step but accumulates during repeated propagation.

9 Normalization

A common numerical correction is

q ← -q--. ∥q∥

(24)

Normalization restores the unit quaternion constraint and is generally appropriate after an approximate propagation method or after finite precision arithmetic.

However, normalization does not make an approximate integration method identical to the exact exponential update. It restores the manifold constraint but does not reconstruct higher order phase terms omitted by the integrator.

For an exact unit incremental quaternion multiplying an exactly unit state, the product is analytically unit:

∥ δqBqk∥ = 1.

(25)

Practical software may still normalize occasionally or every sample to control floating point drift.

10 A numerically stable small angle evaluation

Directly evaluating

sin-(Δ-𝜃∕2-) Δ 𝜃

can lose numerical quality when Δ𝜃 is extremely small.

Its Taylor expansion is

sin(Δ-𝜃∕2-) 1- Δ-𝜃2 -Δ𝜃4- 6 Δ 𝜃 = 2 − 48 + 3840 + O (Δ 𝜃 ).

(26)

Similarly,

Δ 𝜃 Δ𝜃2 Δ 𝜃4 cos--- = 1 − ----+ ----+ O(Δ 𝜃6). 2 8 384

(27)

A robust implementation may switch to such series below a small threshold.

11 Runge Kutta propagation

A higher order integrator can be applied directly to

1 B ˙q = f(q,t) = − -ω (t)q. 2

(28)

For classical fourth order Runge Kutta,

k1 = f(qk,tk),

(29)

( ) k2 = f qk + Δt-k1,tk + Δt- , 2 2

(30)

( ) Δt- Δt- k3 = f qk + 2 k2,tk + 2 ,

(31)

k = f (q + Δt k ,t + Δt ), 4 k 3 k

(32)

and

Δt- qk+1 = qk + 6 (k1 + 2k2 + 2k3 + k4).

(33)

The resulting quaternion should be normalized.

Runge Kutta methods are useful when an angular rate model can be evaluated inside the interval. If an IMU supplies only one integrated delta angle per sample, the exact finite incremental quaternion is often a more natural attitude update.

12 Exact update for a constant positive $z$ rate

Suppose

q0 = 1

and the body rotates at constant positive rate

ωB = Ωk.

Over one interval,

Δ 𝜃B = Ω Δt k.

The increment is

δqB = cos ΩΔt--− k sin Ω-Δt-. 2 2

(34)

After N equal intervals,

ΩN--Δt- ΩN-Δt-- qN = cos 2 − ksin 2 .

(35)

At a total positive frame rotation of 90^∘,

q = 1 −√-k. 2

(36)

This is the same passive convention diagnostic used throughout the preceding PhysicsLibrary quaternion series.

13 Using IMU supplied delta angles

Many inertial sensors report integrated angular increments directly:

B Δ 𝜃m.

(37)

Such an output may already incorporate internal high rate sampling and manufacturer specific coning processing.

If the bias estimate is expressed as an angular rate and is approximately constant over the sample, a simple correction is

B B Δ ^𝜃 = Δ 𝜃m − ^bg Δt.

(38)

The corrected delta angle is then converted directly to the passive incremental quaternion.

The device documentation must be checked to determine whether its reported increment is raw, internally compensated, expressed in a beginning sample frame, expressed in an ending sample frame, or uses another convention.

14 Gyroscope bias accumulation

If an uncompensated constant gyro bias has magnitude b, then for a simple single axis case the attitude error grows approximately as

δ𝜃(t) ≈ bt.

(39)

For example, a bias of

0.1 ∘∕s

accumulates approximately

∘ 60

of angle error in ten minutes.

This simple calculation explains why accurate bias initialization, online bias estimation, temperature modeling, and external aiding are important in inertial systems.

15 Two successive body increments and coning

Consider two small chronological body frame increments

Δ𝜃 1

followed by

Δ 𝜃2.

The passive incremental quaternions are

δq1 ≈ 1 − 1Δ 𝜃1 2

and

δq2 ≈ 1 − 1Δ 𝜃2. 2

Because body increments left multiply, the chronological combined update is

q2 = δq2δq1q0.

(40)

Expand the increment product through second order:

1 δq2δq1 ≈ 1 − --(Δ𝜃1 + Δ 𝜃2 ) 2 + 1Δ 𝜃2Δ 𝜃1. 4

The vector part of the last product is

1 1 --(Δ 𝜃2 × Δ 𝜃1) = − -(Δ 𝜃1 × Δ 𝜃2) . 4 4

Matching the passive first order form

1- 1 − 2Δ 𝜃eq

gives the leading equivalent physical rotation vector

1- Δ 𝜃eq ≈ Δ 𝜃1 + Δ 𝜃2 + 2Δ 𝜃1 × Δ 𝜃2.

(41)

The cross product term is the leading coning contribution.

16 Why coning matters

If the body rotates alternately about different axes during a sample interval, the vector sum of the measured increments is not generally the exact net rotation.

The missing information appears first through cross products of successive angular increments.

High accuracy strapdown inertial algorithms therefore use multiple subsamples or compensated delta angles to account for noncommutativity.

A simple one sample exponential update is exact for a constant rotation vector over the interval. It is not automatically exact for arbitrary high frequency motion occurring within that interval.

If an IMU already reports a coning compensated delta angle, applying an additional naive coning correction may double count the effect.

17 Attitude sign continuity

The unit quaternions

q

and

− q

represent the same physical orientation.

A numerical quaternion time history may therefore contain equivalent samples with alternating overall signs.

For plotting, interpolation, finite differencing, or optimization, choose the representative nearest the previous sample.

qTk+1qk < 0,

(42)

replace

qk+1 ← − qk+1.

(43)

This does not change the physical attitude.

Sign continuity is a representation choice. It is distinct from quaternion normalization and from selecting the principal error quaternion in a local attitude error calculation.

18 Specific force transformation

An accelerometer rigidly attached to the body reports specific force resolved in body coordinates:

Bf .

(44)

The passive attitude quaternion maps inertial coordinates into body coordinates, so the inverse DCM maps body coordinates back into inertial coordinates:

If = ICB Bf = (BCI )TBf.

(45)

Since

BCI = C (q),

we have

I TB f = C(q) f.

(46)

The equivalent quaternion sandwich is

I ∗B f = q f q.

(47)

This inversion is important: the quaternion used for attitude propagation maps I → B, while navigation mechanization often needs to rotate a measured body vector B → I.

19 Velocity and position propagation

In a simple inertial frame model,

I I I ˙v = f + g,

(48)

and

I I ˙r = v.

(49)

In a rotating navigation frame, Earth rotation, transport rate, Coriolis terms, and the chosen gravity model must also be included.

A typical strapdown propagation cycle is therefore:

read gyroscope and accelerometer information;
correct the gyroscope data for estimated bias;
form or accept a corrected body delta angle;
update the passive attitude quaternion;
obtain an attitude appropriate to the accelerometer integration time;
transform specific force from body coordinates into the navigation or inertial frame;
add gravity and required rotating frame terms;
integrate velocity and position;
propagate bias and other estimator states.

20 Why time centered attitude can matter

Using only the beginning or ending attitude to transform accelerometer data introduces an error when the body rotates appreciably during the sample.

For constant body rate, a midpoint quaternion can be formed as

qk+1∕2 = δq1∕2qk,

(50)

where

( 1 B ) δq1∕2 = exp − -Δ 𝜃 . 4

(51)

The corresponding midpoint DCM can then be used for a simple time centered specific force transformation.

High accuracy inertial algorithms develop this idea further through sculling compensation and multiple sample integration.

21 Nominal quaternion and three component error state

A navigation filter can propagate the nominal attitude as a full unit quaternion while carrying only three attitude error states.

For example, using the passive small error convention from the preceding article,

[ ] 1 δq ≈ − 1δ𝜃 . 2

(52)

The nominal quaternion supplies a globally nonsingular orientation representation, while

δ𝜃

is a local three component perturbation suitable for linear covariance propagation.

The chosen left or right error definition must remain consistent with the filter Jacobians and the side used for correction injection.

22 Direct small increment versus error state small angle

The same small angle expansion appears in two distinct roles.

In direct IMU propagation,

δqB ≈ 1 − 1Δ 𝜃B 2

approximates the actual physical incremental rotation during one sample. Its accuracy depends on the size and variation of the sample motion.

In an error state filter,

1- δq ≈ 1 − 2 δ𝜃

represents a deliberately local estimation error around a nominal full attitude quaternion.

The nominal vehicle attitude itself may be very large even when the local error state is small.

These two uses should not be conflated.

23 Normalization versus sign continuity

Two common operations have different purposes.

Normalization,

q ← -q--, ∥q∥

restores the unit norm constraint.

Sign continuity,

qk ← − qk

when needed, chooses one of two equivalent quaternion representatives.

Neither operation substitutes for an accurate attitude integration method.

24 Example propagation step

Suppose

q = 1, k

⌊ ⌋ 0.01 ωB = ⌈ − 0.02 ⌉rad ∕s, m 0.53

and

⌊ ⌋ 0.01 ^bg = ⌈− 0.02⌉ rad∕s. 0.03

The corrected rate is

⌊ 0 ⌋ B ⌈ ⌉ ^ω = 0 rad ∕s. 0.50

(53)

For

Δt = 0.1 s,

the delta angle is

⌊ ⌋ B 0 Δ 𝜃 = ⌈ 0 ⌉ rad. 0.05

(54)

Therefore

δq = cos0.025 − k sin 0.025. B

(55)

Numerically,

⌊ ⌋ 0.999687516 | 0 | δqB ≈ |⌈ 0 |⌉ . − 0.024997396

(56)

Since the initial attitude is the identity,

qk+1 = δqBqk = δqB.

(57)

25 Forward Euler comparison for the same step

The forward Euler result is

qE = 1 − 1(0.05k), k+1 2

qEk+1 = 1 − 0.025k.

(58)

Its norm is

∥ ∥ √ --------- ∥qEk+1∥ = 1.000625 ≈ 1.000312451.

(59)

After normalization,

⌊ ⌋ 0.999687646 E | 0 | ^qk+1 ≈ |⌈ 0 |⌉ . − 0.024992191

(60)

This is close to, but not identical with, the exact exponential result.

26 Implementation pseudocode

A minimal constant rate passive update can be expressed conceptually as:

omega_B = gyro_B - bias_hat_B
dtheta_B = omega_B * dt
a = norm(dtheta_B)

if a is very small:
    s = 0.5 - a*a/48
    c = 1.0 - a*a/8
else:
    s = sin(a/2) / a
    c = cos(a/2)

dq = [c, -s*dtheta_B]
q_new = dq * q_old
q_new = q_new / norm(q_new)

if dot(q_new, q_old) < 0:
    q_new = -q_new

The product in the line

qnew = dq * qold

is the Hamilton product with scalar first semantic components.

Software whose array order is scalar last may use different memory indices without changing the physical equations.

27 Common pitfalls

Using the old inverse attitude convention.
The present PhysicsLibrary state is

$B q = qI,$
not ^Iq_B.
Using a positive vector part for a positive body frame increment.
The passive increment is

$δq = cos Δ-𝜃 − ^u sin Δ-𝜃 . B 2 2$
Right multiplying body resolved increments.
For the passive inertial to body state,

$qk+1 = δqBqk.$
Assuming normalization makes Euler integration exact.
Normalization repairs norm, not truncation error.
Ignoring gyro bias.
Even a small constant rate bias integrates into a large attitude error over time.
Applying a simple vector sum to substantial multi axis motion.
Successive rotations do not commute. Coning terms matter.
Applying coning correction twice.
Determine whether the sensor supplied delta angle already contains internal compensation.
Using the inertial to body quaternion in the wrong direction for accelerometer data.
Body specific force is transformed to inertial coordinates with q^∗ on the left and q on the right, or with C(q)^T.
Interpreting quaternion sign flips as attitude jumps.
q and −q represent the same orientation.
Treating the three component filter attitude error as the nominal attitude itself.
The error vector is local; the nominal quaternion carries the global orientation.

28 Verification battery

A numerical implementation should be checked against:

zero angular rate;
constant positive rotations about +x, +y, and +z;
exact exponential propagation from the identity;
equality of C(q_k+1) with the equivalent DCM increment;
preservation of unit norm for exact increments;
expected norm drift for unnormalized forward Euler;
gyro bias accumulation;
body increment multiplication on the left;
sign continuity with q versus −q;
two noncommuting increments and the leading coning term;
specific force transformation with the inverse attitude map;
consistency with the passive error state sign from Q12.

29 Relationship to adjacent PhysicsLibrary entries

The preceding article, Relative Attitude and Error Quaternions, establishes multiplicative left and right errors and the passive local relation

1- δq ≈ 1 − 2δ 𝜃.

The present article combines that convention with the quaternion kinematics from quaternion kinematics and angular velocity to form sampled IMU attitude propagation.

A separate companion entry, Numerical Quaternion Propagation and IMU Attitude State Integration: Examples, Exercises, and Solutions, provides the Q13E self study problem bank.

30 Sources and convention notes

Quaternion propagation equations cannot be imported safely without identifying the represented frame map, the resolution of angular velocity, and the quaternion multiplication convention.

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

The equations in this entry are translated into the PhysicsLibrary passive inertial to body convention.

References

[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

[2] J. Solà, “Quaternion Kinematics for the Error State Kalman Filter,” arXiv:1711.02508, 2017. arXiv preprint

[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

[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

License

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

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

numerical quaternion propagation and IMU attitude state integration