M6-T3 — Strapdown Mechanization

Learning objectives

After this lesson, you should be able to:

• Write the attitude, velocity and position equations of a strapdown mechanization in the local-level frame.
• Compute the Earth rate, the transport rate and the Coriolis acceleration and say when each can be ignored.
• Describe coning and why summing angle increments as a vector misses it.
• Choose between an uncorrected update and a coning-compensated one from the vibration level and the sensor grade.

The intuition

UAV M2-T7: Inertial Navigation Systems shows what an INS does in words and block diagrams. This lesson writes the three equations behind the block diagram and keeps the terms that a hover-scale treatment drops: for a vehicle that travels a hundred metres at a few tens of metres per second, most of them are negligible, and the point is to know which, and by how much.

Three steps, every cycle

Each cycle (100 to 1000 times a second) the computer does the same three things. First it updates the attitude from the gyro: the body has rotated by the measured angle since the last cycle, so the attitude quaternion is multiplied by the quaternion of that rotation. Second it rotates the measured specific force from body axes into the navigation frame with the new attitude, adds gravity (and the small Coriolis correction) to get acceleration, and integrates to velocity. Third it integrates velocity to position. The only inputs are the gyro and accelerometer outputs and a model of gravity.

The terms that depend on where you are

The navigation frame is the local level frame, tied to the Earth's surface, and both the Earth's rotation and the vehicle's motion over the curved Earth make it rotate relative to inertial space. The gyros measure rotation relative to inertial space, so the computer must subtract the rotation of the local frame: the Earth rate (15.04°/h, of which 11.5°/h is horizontal at latitude 40°) and the transport rate (the speed divided by the Earth's radius: 1.6°/h at 50 m/s, 8.1°/h at 250 m/s). For a tactical gyro with 0.3°/h bias these are larger than the sensor error and cannot be ignored. For a consumer gyro they are smaller than the bias and can be.

Why the attitude update needs care

Rotations do not commute, so adding up the angle increments as a vector is not the same as composing the rotations. The error is second order in the increment, but if the vehicle vibrates in two axes out of phase, the error builds a steady drift: coning. In the example, a coning motion of 0.01 rad at 10 Hz makes the true attitude turn about the third axis at 648°/h, while no gyro senses any rotation about it. Summing the increments as a vector misses all of it (1.8° after 10 s). Composing them as rotations, at 100 Hz, leaves 0.116°; adding a two-sample correction leaves 0.0087°.

Strapdown navigation is three updates: attitude from the gyro, velocity from the rotated specific force plus gravity and Coriolis, position from velocity. The attitude update has to compose rotations and, under vibration, correct for coning.

Definitions

Local-level frame $n$

North, east, down axes at the vehicle, tied to the Earth surface; it rotates relative to inertial space.

Earth rate $\boldsymbol{\omega}_{ie}^n$

The Earth's rotation, $7.292\times10^{-5}$ rad/s (15.04°/h), expressed in the local frame: $(\Omega\cos\varphi,\ 0,\ -\Omega\sin\varphi)$ at latitude $\varphi$.

Transport rate $\boldsymbol{\omega}_{en}^n$

The rotation of the local frame relative to the Earth, caused by motion over the curved surface: speed divided by radius.

Coning

A motion in which the body axis sweeps out a cone, produced by angular oscillation on two axes out of phase. It makes a steady rotation about the third axis that the gyros do not see as a rate.

Sculling

The velocity-update counterpart: an oscillation of the angular rate and the specific force in phase produces a steady error in the integrated velocity.

Derivation: the Coriolis term, and the coning drift rate

The velocity equation follows from Newton's law in an inertial frame and a change of frame; the coning rate from the kinematics of the rotation vector.

1. $$\ddot{\mathbf{r}}^i = \mathbf{f}^i + \mathbf{G}^i$$

   Newton's second law in the inertial frame $i$: specific force plus gravitation $\mathbf{G}$ (mass attraction, without the centrifugal part).

2. $$\dot{\mathbf{v}}^n = \mathbf{C}_i^n\ddot{\mathbf{r}}^i - (\boldsymbol{\omega}_{ie}^n + \boldsymbol{\omega}_{in}^n)\times\mathbf{v}^n - \boldsymbol{\omega}_{ie}^n\times(\boldsymbol{\omega}_{ie}^n\times\mathbf{r}^n)$$

   Express the velocity relative to the Earth in the local frame. Differentiating a vector in a rotating frame produces the frame's rotation crossed with it; the last term is the centripetal acceleration of the point on the rotating Earth.

3. $$\mathbf{g}^n = \mathbf{G}^n - \boldsymbol{\omega}_{ie}^n\times(\boldsymbol{\omega}_{ie}^n\times\mathbf{r}^n)$$

   Define plumb-bob gravity as gravitation minus the centripetal term. This is what gravity models (the 9.81 m/s²) give.

4. $$\dot{\mathbf{v}}^n = \mathbf{C}_b^n\mathbf{f}^b + \mathbf{g}^n - (2\boldsymbol{\omega}_{ie}^n + \boldsymbol{\omega}_{en}^n)\times\mathbf{v}^n$$

   Substitute, using $\boldsymbol{\omega}_{in} = \boldsymbol{\omega}_{ie} + \boldsymbol{\omega}_{en}$. For Hopper at 50 m/s the Coriolis term is 0.0073 m/s², 0.74 mg.

The coning drift rate

1. $$\boldsymbol{\omega}(t) = a\Omega\,[\cos\Omega t,\ -\sin\Omega t,\ 0]^{\mathsf{T}}$$

   Body rates on two axes, amplitude $a\Omega$, 90° out of phase: the coning motion with half angle a. The rate about the third axis is zero.

2. $$\dot{\boldsymbol{\Phi}} \approx \boldsymbol{\omega} + \tfrac{1}{2}\boldsymbol{\Phi}\times\boldsymbol{\omega}$$

   The rotation vector $\boldsymbol{\Phi}$ obeys this equation to second order in the angles (a standard result of the kinematics of finite rotation), where the second term is the non-commutativity.

3. $$\boldsymbol{\Phi} \approx a\,[\sin\Omega t,\ \cos\Omega t,\ 0]^{\mathsf{T}}$$

   Integrate the first-order rotation vector from the rates above.

4. $$\dot{\Phi}_z = \tfrac{1}{2}(\Phi_x\omega_y - \Phi_y\omega_x) = -\tfrac{1}{2}a^2\Omega$$

   The third component of the correction, constant in time. The magnitude of the drift is $\tfrac12 a^2\Omega$ = 3.14×10⁻³ rad/s = 648°/h for a = 0.01 rad and 10 Hz, though $\omega_z = 0$ throughout.

The velocity equation has Coriolis and transport terms because the frame it is written in rotates, and they are the size of the Earth rate times the speed. Coning is a drift that the gyro cannot sense directly, equal to half the product of the cone angle squared and the frequency.

A worked example

Hopper at latitude 40° and the coning case, computed.

1. Earth rate. 15.04°/h in total; 11.52°/h horizontal (north) and 9.67°/h vertical at 40°. Hopper's gyros must have their Earth-rate compensation if they are better than about 10°/h.
2. Transport rate. 1.62°/h at 50 m/s, 8.08°/h at 250 m/s. On a 100 m hop the term is below the bias of any gyro worse than navigation grade.
3. Coriolis. At 50 m/s: 0.0073 m/s² (0.74 mg), about the bias of a good accelerometer. Over the 60 s hop it would give 13 m of velocity-error-integrated position if ignored (0.5 × 0.0073 × 60² = 13 m), which is why it is kept.
4. Coning. A motion of 0.01 rad at 10 Hz turns the true attitude about the third axis at 648°/h, 1.8° in 10 s, with no gyro rate on that axis.
5. Three algorithms at 100 Hz. Adding the angle increments as a vector: error 1.800° (all of the drift missed). Composing them as rotations: 0.116°. Adding the two-sample correction $\Delta\boldsymbol{\theta}_k + \tfrac{1}{12}\Delta\boldsymbol{\theta}_{k-1}\times\Delta\boldsymbol{\theta}_k$: 0.0087°.
6. Small vibration. A vibration of 0.0002 rad at 10 Hz gives an uncorrected drift of 0.3°/h, below any gyro grade here: the correction is only needed when the motion is violent or the gyro excellent.

import math
import gnckit as g

Om = 7.2921150e-5                 # Earth rate, rad/s
R = 6378137.0
D2R = math.pi / 180
lat = 40 * D2R
print(f'Earth rate {Om / D2R * 3600:.2f} deg/h; at latitude 40 deg the north component is {Om * math.cos(lat) / D2R * 3600:.2f} deg/h and the vertical one {Om * math.sin(lat) / D2R * 3600:.2f} deg/h')
for v in (50.0, 250.0):
    print(f'transport rate for a speed of {v:.0f} m/s: {v / R / D2R * 3600:.2f} deg/h')
print(f'gravity at the surface {9.81:.2f} m/s2; the Coriolis term 2 Om x v for v = 50 m/s: {2 * Om * 50.0:.5f} m/s2 = {2 * Om * 50.0 / 9.81 * 1000:.2f} mg')

# coning: gyro rates on two body axes, 90 degrees out of phase, amplitude a rad, frequency f Hz
a, f = 0.01, 10.0
W = 2 * math.pi * f
def rate(t):
    return [a * W * math.cos(W * t), -a * W * math.sin(W * t), 0.0]
def inc(t0, t1):                  # exact integral of the body rate over [t0, t1]: the angle increment a sensor delivers
    return [a * (math.sin(W * t1) - math.sin(W * t0)), a * (math.cos(W * t1) - math.cos(W * t0)), 0.0]

def cross(u, v):
    return [u[1] * v[2] - u[2] * v[1], u[2] * v[0] - u[0] * v[2], u[0] * v[1] - u[1] * v[0]]

def angle_error(q1, q2):
    d = g.quat_mul(g.quat_conj(q1), q2)
    return 2 * math.atan2(math.sqrt(d[1] ** 2 + d[2] ** 2 + d[3] ** 2), abs(d[0]))

T = 10.0
dt = 0.01
sub = 20                          # reference truth: 20 sub-steps per sample
qt = [1.0, 0, 0, 0]
qa = [1.0, 0, 0, 0]
qb = [1.0, 0, 0, 0]
sumv = [0.0, 0.0, 0.0]
prev = [0.0, 0.0, 0.0]
n = int(round(T / dt))
for k in range(n):
    t0 = k * dt
    for j in range(sub):
        ta = t0 + j * dt / sub
        tb = ta + dt / sub
        qt = g.quat_integrate(qt, [x / (dt / sub) for x in inc(ta, tb)], dt / sub)
    d = inc(t0, t0 + dt)
    qa = g.quat_integrate(qa, d, 1.0)
    c = cross(prev, d)
    dcorr = [d[i] + c[i] / 12.0 for i in range(3)]
    qb = g.quat_integrate(qb, dcorr, 1.0)
    sumv = [sumv[i] + d[i] for i in range(3)]
    prev = d
th = 0.5 * a * a * W
print(f'coning motion a = {a} rad at {f:.0f} Hz: the attitude drifts about the third axis at {th:.2e} rad/s = {th / D2R * 3600:.0f} deg/h although no gyro senses a rate about it')
print(f'after {T:.0f} s with a {1 / dt:.0f} Hz update:')
qs = g.quat_integrate([1.0, 0, 0, 0], sumv, 1.0)
print(f'  adding the increments as a vector (no composition)  attitude error {angle_error(qt, qs) / D2R:.3f} deg  (the drift {th * T / D2R:.3f} deg is missed)')
print(f'  composing the increments as rotations, one sample   attitude error {angle_error(qt, qa) / D2R:.3f} deg')
print(f'  with the two-sample coning correction            attitude error {angle_error(qt, qb) / D2R:.4f} deg')
# the correction is worth it only when the motion is that violent: no vibration
a2 = 0.0002
print(f'for a vibration of {a2} rad at {f:.0f} Hz the uncorrected drift is {0.5 * a2 * a2 * W / D2R * 3600:.1f} deg/h')

Earth rate 15.04 deg/h; at latitude 40 deg the north component is 11.52 deg/h and the vertical one 9.67 deg/h
transport rate for a speed of 50 m/s: 1.62 deg/h
transport rate for a speed of 250 m/s: 8.08 deg/h
gravity at the surface 9.81 m/s2; the Coriolis term 2 Om x v for v = 50 m/s: 0.00729 m/s2 = 0.74 mg
coning motion a = 0.01 rad at 10 Hz: the attitude drifts about the third axis at 3.14e-03 rad/s = 648 deg/h although no gyro senses a rate about it
after 10 s with a 100 Hz update:
  adding the increments as a vector (no composition)  attitude error 1.800 deg  (the drift 1.800 deg is missed)
  composing the increments as rotations, one sample   attitude error 0.116 deg
  with the two-sample coning correction            attitude error 0.0087 deg
for a vibration of 0.0002 rad at 10 Hz the uncorrected drift is 0.3 deg/h

In robotics

Every strapdown computer implements these steps; the choices are the rate and the compensation.

• Update rate. Attitude is updated at the highest rate the processor allows (often 1 kHz or more with a sub-sampled coning correction), velocity and position at a lower rate. A 100 Hz attitude update with no correction is adequate for a quiet vehicle like Hopper on a short hop.
• Vibration. A rocket engine, a rotor or a propeller makes vibration at tens to hundreds of hertz, and the sensor delivers integrated increments (delta-angles) for exactly this reason: the increments are exact integrals of the rate, and the coning algorithm works on them.
• Which terms to keep. A consumer-grade multirotor ignores Earth rate and Coriolis entirely. A tactical system keeps Earth rate. A navigation-grade system keeps all of them, and a good gravity model with latitude and height.
• Quaternion or matrix. Both are used; the quaternion is cheaper to keep normalized. The quaternion update with the exact exponential, as in the example, preserves the norm to rounding.
• Test. A mechanization is tested by driving it with synthetic sensor data from a known trajectory, including a coning motion and an Earth-fixed stationary case, which should show no drift beyond the known Earth-rate effects.

The lab M6-L2: Mechanize and Watch It Drift writes a planar version and injects biases. The attitude and velocity errors that result are derived in M6-T4: INS Error Dynamics and the Schuler Oscillation.

Common misconceptions

Key takeaways

1. Strapdown navigation integrates three things: attitude (quaternion from the gyro), velocity $\dot{\mathbf{v}} = \mathbf{C}\mathbf{f} + \mathbf{g} - (2\boldsymbol{\omega}_{ie} + \boldsymbol{\omega}_{en})\times\mathbf{v}$ and position.
2. The gyro rate has the Earth rate and the transport rate subtracted before the attitude is integrated.
3. Earth rate is 15.04°/h (11.5°/h horizontal at 40°); transport rate is speed over radius (1.6°/h at 50 m/s); Coriolis at 50 m/s is 0.74 mg.
4. Coning gives a drift $\tfrac12 a^2\Omega$ about an axis with no rate: 648°/h for 0.01 rad at 10 Hz.
5. Composing rotations at 100 Hz cuts the 1.8° error to 0.116°; a two-sample correction gives 0.0087°.
6. The corrections are chosen by comparing their size with the sensor errors, not by habit.

References and further reading

Each source is marked free or paid. Free to read does not mean free to reuse: the course resources page lists every source with its licence.

• P. D. Groves, Principles of GNSS, Inertial, and Multisensor Integrated Navigation Systems, 2nd edition, Artech House, 2013, chapters 5 and 6 on the frames and the navigation equations. Paid.
• P. G. Savage, "Strapdown inertial navigation integration algorithm design", Journal of Guidance, Control, and Dynamics 21(1) and 21(2), 1998, the standard treatment of coning and sculling. Paid (AIAA).
• D. H. Titterton and J. L. Weston, Strapdown Inertial Navigation Technology, 2nd edition, IET, 2004. Paid.
• MIT 16.07 Dynamics (OpenCourseWare) , for rotating frames and the Coriolis theorem used in the derivation. Free; licence CC BY-NC-SA 4.0.

Knowledge check

Three questions. Write an answer, then reveal the explanation. Your answers stay in your browser.

What’s next

Related content

Nothing below is required to finish this lesson.

• Before
  M6-T2: Allan Variance: Reading a Sensor from Its Noise

  Compute an Allan deviation curve from a static record and read angle random walk, bias instability and rate random walk from its slopes.

• Lab
  M6-L2: Mechanize and Watch It Drift

  Write a planar strapdown mechanization, inject a known accelerometer bias and gyro bias, and match the growth of the position error to the error equations.

Other courses on this site treat neighbouring topics from their own angle:

• UAV M2-T7: Inertial Navigation Systems
• UAV M2-T8: Dead reckoning and inertial drift
• UAV M3-T7: IMU noise and bias modelling
• UAV M3-T8: IMU preintegration
• UAV M1-T8: IMU, barometer, magnetometer and GNSS
• Probability M2-T4: Distributions of Real Sensor Noise
• Probability M5-T4: Identifying Noise Parameters from Data
• Linear Algebra M10-T6: Small-Angle Approximations

This course leaves the following to the pages that already teach them:

• What an INS is and why it drifts, without the equations
• IMU preintegration for factor-graph estimators
• Wheel odometry and its motion models