example of numerical quaternion propagation and IMU attitude state integration

(Example)

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.

1 Convention declaration

PhysicsLibrary uses the passive inertial to body attitude quaternion

q = BqI.

(1)

Hamilton multiplication and scalar first display order are used.

For body resolved gyroscope angular velocity,

˙q = − 1ωBq. 2

(2)

A positive body frame delta angle

Δ 𝜃B

produces the passive incremental quaternion

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

(3)

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

qk+1 = δqB,kqk.

(4)

For a small passive frame error,

[ ] δq ≈ 1 . − 12δ𝜃

(5)

2 Exercises

Bias correction and delta angle.
An IMU reports

$⌊ ⌋ 0.10 ωBm = ⌈ − 0.20 ⌉rad ∕s. 1.00$
The estimated gyroscope bias is

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

$Δt = 0.01 s,$
compute the corrected angular rate and the delta angle vector.
Exact incremental quaternion.
For

$⌊ ⌋ 0 Δ 𝜃B = ⌈ 0 ⌉ rad, 0.02$
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^∘ frame rotation about +x,

$1-−-i qk = √ 2 ,$
apply the small body frame delta angle

$⌊ ⌋ 0 Δ 𝜃B = ⌈ 0 ⌉ rad. 0.01$
Write the first order update in the correct multiplication order and expand the Hamilton product.
Wrong multiplication side.
A programmer uses

$qk+1 = qkδqk$ (6)

for body resolved gyroscope data while storing

$B q = qI.$
Explain the mistake and write the correct update.
Forward Euler propagation.
Derive the forward Euler propagation formula from

$1 ˙q = − -ωBq. 2$
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

$0.1 ∘∕s$
about one axis.
Approximately how large is the attitude error after ten minutes?
Two hundred hertz constant $y$ rotation.
An IMU runs at 200 Hz and measures a constant positive body frame rotation rate of

$∘ 120 ∕s$
about the +y axis.
Compute the delta angle per sample and the exact passive incremental quaternion.
Leading coning term.
For two chronological small body frame increments Δ𝜃₁ followed by Δ𝜃₂, derive the leading cross product term in the equivalent physical rotation vector.
Numerical coning cross term.
Let

$⌊ ⌋ ⌊ ⌋ 0.01 0 Δ 𝜃1 = ⌈ 0 ⌉ , Δ 𝜃2 = ⌈0.01⌉ rad. 0 0$
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 q_k with

$T qk qk− 1 < 0.$ (7)

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

$[ ] 1 δq ≈ − 1 δ𝜃 2$ (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 Δt to construct an average angular rate and then multiply by Δ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.

3 Solutions

Solution 1: bias correction and delta angle

The corrected rate is

ωBc = ωBm − ^bg ⌊ ⌋ ⌊ ⌋ 0.10 0.01 = ⌈ − 0.20⌉ − ⌈ − 0.02⌉ 1.00 0.03 ⌊ ⌋ 0.09 = ⌈ − 0.18⌉ rad∕s. 0.97

Therefore

⌊ ⌋ 0.09 ωB = ⌈ − 0.18⌉ rad ∕s. c 0.97

(9)

The delta angle is

B B Δ𝜃 = ω c Δt.

Thus

⌊ ⌋ 0.0009 Δ 𝜃B = ⌈− 0.0018⌉ rad. 0.0097

(10)

Solution 2: exact incremental quaternion

The increment magnitude is

Δ 𝜃 = 0.02 rad.

The positive physical frame axis is +z.

Under the PhysicsLibrary passive convention,

Δ-𝜃 Δ-𝜃 δqB = cos 2 − k sin 2 .

Hence

δqB = cos0.01 − k sin 0.01.

(11)

Numerically,

⌊ ⌋ 0.9999500004 | 0 | δqB ≈ |⌈ 0 |⌉ . − 0.0099998333

(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,

δq ≈ 1 − 1Δ 𝜃B. B 2

Therefore

⌊ ⌋ 1 | 0 | δqB,small = |⌈ 0 |⌉ . − 0.01

(13)

Its norm is

√ --------- ∥δqB,small∥ = 1 + 0.012.

Thus

∥δqB,small∥ ≈ 1.000049999.

(14)

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

Solution 4: one exact propagation step

The initial attitude is

q0 = 1.

Body resolved increments left multiply:

q1 = δqBq0.

Since q₀ = 1,

q = δq . 1 B

(15)

Therefore

q1 = cos 0.01 − k sin0.01.

(16)

Solution 5: nonidentity attitude and a body increment

The first order passive increment is

δqB ≈ 1 − 0.005k.

The correct body increment update is

qk+1 = δqBqk.

Therefore

1 − i qk+1 ≈ (1 − 0.005k) -√---. 2

Expand:

1 qk+1 ≈ √---(1 − i − 0.005k + 0.005ki ). 2

Since

ki = j,

we obtain

q ≈ √1-(1 − i + 0.005j − 0.005k ). k+1 2

(17)

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

Solution 6: wrong multiplication side

For

B q = qI,

body resolved angular velocity satisfies

1- B ˙q = − 2ω q.

The finite body increment maps

Bk → Bk+1

and therefore appears on the left of the existing

I → Bk

map.

Thus the correct update is

qk+1 = δqB,kqk.

(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

qk+1 = qk + ˙qkΔt.

Substitute

˙qk = − 1ωB qk. 2 k

Then

qk+1 = qk − 1ωBk qkΔt. 2

(19)

With

B B Δ𝜃 k = ω k Δt,

this can also be written

( ) qk+1 = 1 − 1Δ 𝜃Bk qk. 2

(20)

Solution 8: forward Euler norm drift

Let

qk+1 = qk + ˙qkΔt.

For the exact quaternion differential equation,

qTq˙ = 0, k k

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

Thus

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

For nonzero angular rate,

∥qk+1∥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

600 s.

A constant bias of

0.1 ∘∕s

therefore accumulates approximately

∘ δ𝜃 = 0.1 ∕s × 600 s.

Hence

δ𝜃 ≈ 60 ∘.

(22)

This illustrates why gyroscope bias estimation and correction are essential.

Solution 10: two hundred hertz constant $y$ rotation

At 200 Hz,

1 Δt = 200-= 0.005 s.

The sample rotation is

Δ 𝜃 = 120∘∕s × 0.005 s = 0.6∘.

In radians,

Δ 𝜃 ≈ 0.01047198 rad.

(23)

The half angle is approximately

0.00523599 rad.

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

δqB = cos0.00523599 − j sin 0.00523599.

(24)

Numerically,

δqB ≈ 0.999986292 − 0.005235964 j.

(25)

Solution 11: leading coning term

The first order passive increments are

δq ≈ 1 − 1Δ 𝜃 1 2 1

and

δq2 ≈ 1 − 1Δ 𝜃2. 2

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

δq21 = δq2δq1.

Expand:

1 δq21 ≈ 1 − --(Δ 𝜃1 + Δ 𝜃2) 12 + -Δ 𝜃2Δ 𝜃1. 4

For pure vector quaternions,

Δ 𝜃2Δ 𝜃1 = − Δ 𝜃2 ⋅ Δ 𝜃1 + Δ𝜃2 × Δ𝜃1.

The vector contribution is therefore

− 1Δ 𝜃1 × Δ 𝜃2. 4

Match this with the passive form

1- 1 − 2 Δ𝜃eq.

The resulting equivalent physical rotation vector is

1- Δ 𝜃eq ≈ Δ 𝜃1 + Δ 𝜃2 + 2Δ 𝜃1 × Δ 𝜃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

⌊ ⌋ 0 Δ 𝜃 × Δ 𝜃 = ⌈ 0 ⌉ . 1 2 0.0001

Therefore the leading coning contribution is

⌊ ⌋ 1 0 -Δ 𝜃1 × Δ 𝜃2 = ⌈ 0 ⌉ rad. 2 0.00005

(27)

Solution 13: normalization versus exact propagation

Normalization restores

∥q∥ = 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

qk ← − qk.

(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

T q q = 1.

Therefore it has three independent degrees of freedom.

A local passive multiplicative error can be written

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

The three vector components of

δ𝜃

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

$qk+1 = δqBqk;$
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

B q = qI,

the body to inertial specific force transformation is

I TB f = C(q) 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 Δt and then multiplying by Δ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,

1 B δqB ≈ 1 − -Δ 𝜃 2

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,

1 δq ≈ 1 − --δ𝜃 2

has a different role.

The nominal attitude remains a full unit quaternion representing the global orientation, while the three component vector δ𝜃 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.

4 Compact passive propagation checks


Quantity	PhysicsLibrary passive result

Attitude state	q = ^Bq_I

Body rate kinematics	q = −ω^Bq

Positive body increment	δq_B = exp(−Δ𝜃^B∕2)

Body increment update	q_k+1 = δq_Bq_k

Small body increment	δq_B ≈ [1,−Δ𝜃^B∕2]^T

Passive error state	δq ≈ [1,−δ𝜃∕2]^T

Sign continuity	q_k ←−q_k when q_k^Tq_k−1 < 0

5 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.

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.

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

example of numerical quaternion propagation and IMU attitude state integration

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

1 Convention declaration

2 Exercises

3 Solutions

Solution 1: bias correction and delta angle

Solution 2: exact incremental quaternion

Solution 3: small angle increment

Solution 4: one exact propagation step

Solution 5: nonidentity attitude and a body increment

Solution 6: wrong multiplication side

Solution 7: forward Euler propagation

Solution 8: forward Euler norm drift

Solution 9: gyroscope bias accumulation

Solution 10: two hundred hertz constant $y$ rotation

Solution 11: leading coning term

Solution 12: numerical coning cross term

Solution 13: normalization versus exact propagation

Solution 14: quaternion sign continuity

Solution 15: three component error state

Solution 16: strapdown state propagation order

Solution 17: using IMU supplied delta angles

Solution 18: direct small increment versus filter small error

4 Compact passive propagation checks

5 Sources and exercise provenance

References

License

Pending Errata and Addenda

Discussion

Interact

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

1 Convention declaration

2 Exercises

3 Solutions

Solution 1: bias correction and delta angle

Solution 2: exact incremental quaternion

Solution 3: small angle increment

Solution 4: one exact propagation step

Solution 5: nonidentity attitude and a body increment

Solution 6: wrong multiplication side

Solution 7: forward Euler propagation

Solution 8: forward Euler norm drift

Solution 9: gyroscope bias accumulation

Solution 10: two hundred hertz constant rotation

Solution 11: leading coning term

Solution 12: numerical coning cross term

Solution 13: normalization versus exact propagation

Solution 14: quaternion sign continuity

Solution 15: three component error state

Solution 16: strapdown state propagation order

Solution 17: using IMU supplied delta angles

Solution 18: direct small increment versus filter small error

4 Compact passive propagation checks

5 Sources and exercise provenance

References

License

Solution 10: two hundred hertz constant $y$ rotation