M7-T4 — Attitude Filters: Complementary and Multiplicative

Learning objectives

After this lesson, you should be able to:

• Write a complementary filter for one axis and derive its two transfer functions.
• Choose the gain from the gyro bias and the vector-measurement noise and find the minimum of the error.
• Explain why a quaternion cannot be estimated with an ordinary additive Kalman update.
• Describe the multiplicative EKF with a three-component error state and a gyro bias.

The intuition

UAV M3-T5: Extended Kalman Filter gives the filter that this lesson specializes to attitude, and UAV M6-T5: PX4 state estimation and EKF2 shows how a flight stack runs one. The question here is narrower: what the filter does with the gyro and a vector measurement, and why the way the quaternion is handled matters.

Two sensors, two bandwidths

The gyro integrates to an angle that is smooth and responsive but wanders at the rate of its bias (0.5°/s here). The vector-derived angle, from the previous lesson, has no drift but carries a noise of 2° per sample. Their errors live at opposite ends of the spectrum: the gyro's at low frequency, the vector measurement's at high. A complementary filter is a pair of complementary filters: the vector measurement through a low-pass filter, the gyro through the matching high-pass filter, summed. In time-domain form it is one line: integrate the gyro, and pull the estimate toward the vector measurement with a gain $k$.

The gain is the crossover

The gain $k$ (rad/s) is the crossover frequency: below it the estimate follows the vector measurement, above it the gyro. A small $k$ lets the gyro bias accumulate: the steady error is $b/k$, so 0.5°/s with $k = 0.1$ gives 5°. A large $k$ lets the 2° noise through. The sum of the two has a minimum, here at $k \approx 1$ rad/s, where the rms pitch error is 0.66°, three times better than the vector measurement alone and far better than the gyro alone, which drifts 60° in two minutes.

Doing it with a covariance

The complementary filter has one fixed gain. A Kalman filter computes the gain from the noise levels and the current uncertainty, and estimates the gyro bias as a state. For three-axis attitude the state is a quaternion, which has four numbers and one constraint (unit norm). An additive update would move the four numbers independently and leave the constraint violated, and the covariance of four numbers with one constraint is singular. The multiplicative EKF estimates instead a three-component small rotation error $\delta\boldsymbol\theta$ (and the gyro bias), updates a reference quaternion by multiplying it with the small rotation, and resets the error to zero. The norm is preserved by construction: in the example it stays 1 to 4×10⁻¹⁶ over 120 s.

A complementary filter is a gain between gyro and vectors, set at the frequency where their errors cross. A multiplicative EKF does it with a covariance and estimates the gyro bias, and it keeps the quaternion constraint by estimating a rotation error rather than the quaternion.

Definitions

Complementary filter

An estimator whose two inputs pass through filters $L(s)$ and $1 - L(s)$ that sum to one: the low-pass for the drift-free measurement, the high-pass for the smooth one.

Crossover gain $k$

The gain of the first-order complementary filter; its crossover frequency in rad/s.

Error state $\delta\boldsymbol\theta$, $\delta\mathbf{b}$

The small rotation vector between the estimated and true attitude, and the gyro bias error.

Multiplicative update

$\mathbf{q}^+ = \mathbf{q}^-\otimes\delta\mathbf{q}(\delta\hat{\boldsymbol\theta})$: the correction is a small rotation multiplied onto the quaternion.

Derivation: the best gain, and the multiplicative error model

The complementary filter error has two parts, each with its own dependence on the gain.

1. $$\dot{e} = -k\,e + (\omega_{\mathrm{err}}) - k\,\nu$$

   Subtract the true angle from the estimate: the error $e = \hat\theta - \theta$ obeys a first-order equation driven by the gyro error and the vector noise $\nu$.

2. $$e_{\mathrm{bias}} = b/k$$

   For a constant gyro bias b the steady error is $b/k$: the gyro error is divided by the crossover.

3. $$\sigma_{\mathrm{noise}}^2 = \dfrac{k}{2}\,S_\nu$$

   For white vector noise of spectral density $S_\nu$ (variance per unit bandwidth) the error variance at the output of the first-order low-pass is $k\,S_\nu/2$, rising with the gain.

4. $$\dfrac{d}{dk}\left[\dfrac{b^2}{k^2} + \dfrac{k S_\nu}{2}\right] = 0 \;\Rightarrow\; k^* = \left(\dfrac{4b^2}{S_\nu}\right)^{1/3}$$

   The total error variance has a minimum where the two derivatives cancel. For the example, with $b = 0.0087$ rad/s and a noise of 2° at 10 Hz ($S_\nu = \sigma^2\Delta t = 1.2\times10^{-4}$ rad²·s) this gives $k^* \approx 1.4$ rad/s, consistent with the search result of 1 (the minimum is flat and the model is approximate).

The multiplicative error model

1. $$\mathbf{A} = (\mathbf{I} - [\delta\boldsymbol\theta\times])\,\hat{\mathbf{A}}$$

   Define the attitude error as a small rotation vector multiplying the estimate; it has three components and no constraint.

2. $$\delta\dot{\boldsymbol\theta} = -[\hat{\boldsymbol\omega}\times]\delta\boldsymbol\theta - \delta\mathbf{b} - \mathbf{n}_g$$

   Differentiate, using $\dot{\mathbf{A}} = -[\boldsymbol\omega\times]\mathbf{A}$ and the measured rate $\omega_{\mathrm{gyro}} = \omega + b + n_g$.

3. $$\mathbf{b}_i = \mathbf{A}\mathbf{r}_i \approx \hat{\mathbf{A}}\mathbf{r}_i + [\hat{\mathbf{A}}\mathbf{r}_i\times]\,\delta\boldsymbol\theta$$

   The measurement in terms of the error: the Jacobian is the cross-product matrix of the predicted vector.

4. $$\mathbf{q}^+ = \mathbf{q}^-\otimes\begin{bmatrix}1 \\ \tfrac12\delta\hat{\boldsymbol\theta}\end{bmatrix},\quad \delta\hat{\boldsymbol\theta}\leftarrow 0$$

   After the standard Kalman update of the error state, move the correction onto the quaternion by multiplication (and renormalize against rounding), then reset the error to zero.

The complementary gain trades the bias term b/k against the noise term that grows with k, and the optimum is near the cube root of 4b²/S. The multiplicative filter estimates a rotation error instead of the quaternion, so the unit-norm constraint holds by construction and the covariance is 3×3, not singular 4×4.

A worked example

Two numerical experiments on Hopper-sized quantities.

1. Pitch complementary filter. The gyro has a bias of 0.5°/s and an angle random walk of 0.005 rad/√s; the vector-derived pitch has 2° noise at 10 Hz. The rms pitch error after 20 s is 18.0° for k = 0.02, 5.3° for 0.1, 0.98° for 0.5, 0.664° for 1, 0.689° for 2 and 1.03° for 4.
2. Reading. The falling branch follows the bias term 0.5°/s over k (25°, 5.0°, 1.0°, 0.5°); the rising branch is the vector noise. The minimum is at k = 1 rad/s, a crossover period of 6.3 s, and is flat: 0.5 to 2 are all within 50% of the best.
3. Multiplicative EKF. Three axes, a gyro bias of (0.009, −0.005, 0.006) rad/s, gyro noise 0.002 rad/√s, two vector measurements at 10 Hz (0.5° and 1.0°), an initial attitude error of about 8°.
4. Result. The 3-axis rms attitude error is 0.62° in the first 5 s, 0.33° from 5 to 20 s and 0.34° from 20 to 120 s, against filter-predicted values of 0.79°, 0.41° and 0.32°: the covariance describes the actual error. The gyro bias estimate is correct to 0.012, 0.004 and 0.011°/s after 120 s.
5. The norm. The largest deviation of the quaternion norm from 1 is 4.4×10⁻¹⁶ over 6000 steps and 1200 updates.

import math
import random
import gnckit as g

D2R = math.pi / 180

# ---- part 1: complementary filter for pitch, gyro bias 0.5 deg/s, vector-derived pitch with 2 deg noise at 10 Hz
rnd = random.Random(1)
dt = 0.01
T = 120.0
bias = 0.5 * D2R
arw = 0.005                              # rad / sqrt(s)
sig_m = 2.0 * D2R
def truth(t): return 0.2 * math.sin(0.5 * t)
def rate(t): return 0.1 * math.cos(0.5 * t)

def run_cf(k):
    th = 0.0
    errs = []
    for i in range(int(T / dt)):
        t = i * dt
        q = rate(t) + bias + arw / math.sqrt(dt) * rnd.gauss(0, 1)
        th += dt * (q + 0.0)
        if i % 10 == 0:                                  # 10 Hz measurement
            th += 0.0
        th_dot_corr = 0.0
        if i % 10 == 0:
            zm = truth(t) + sig_m * rnd.gauss(0, 1)
            th += 0.1 * k * (zm - th)                     # the correction k (zm - th) applied over the 0.1 s between measurements
        if t > 20.0: errs.append(th - truth(t))
    return math.sqrt(sum(e * e for e in errs) / len(errs)) / D2R
print('Complementary filter, pitch error rms (deg) after 20 s, against the correction gain k (rad/s):')
best = None
for k in (0.02, 0.05, 0.1, 0.2, 0.5, 1.0, 2.0, 4.0):
    e = run_cf(k)
    if best is None or e < best[1]: best = (k, e)
    print(f'  k = {k:5.2f}: {e:6.3f} deg   (bias term b/k = {bias / k / D2R:6.2f} deg, crossover period {2 * math.pi / k:6.1f} s)')
print(f'  smallest error at k = {best[0]}')

# ---- part 2: multiplicative EKF, 3 axes, gyro bias, two vector measurements
def skew(v): return [[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]]
def norm(v): n = math.sqrt(sum(c * c for c in v)); return [c / n for c in v]
def mv(M, v): return [sum(M[i][j] * v[j] for j in range(len(v))) for i in range(len(M))]
def A_of(q): return g.mat_T(g.quat_to_matrix(q))                    # maps reference vectors into the body frame

rnd = random.Random(4)
r_list = [norm([0, 0, 1]), norm([0.6, 0, 0.8])]
sd_v = [0.5 * D2R, 1.0 * D2R]
b_true = [0.009, -0.005, 0.006]
sig_g = 0.002                                                   # rad / sqrt(s)
sig_b = 1e-5                                                    # bias random walk, rad/s / sqrt(s)
dt = 0.02
n = int(120.0 / dt)

def omega(t): return [0.1 * math.sin(0.2 * t), 0.05 * math.cos(0.1 * t), 0.08]

qt = [1.0, 0, 0, 0]
q = g.quat_normalize([1.0, 0.05, -0.04, 0.06])                  # initial estimate off by about 8 degrees
bh = [0.0, 0.0, 0.0]
P = [[0.0] * 6 for _ in range(6)]
for i in range(3): P[i][i] = (0.15) ** 2; P[i + 3][i + 3] = (0.02) ** 2
def att_err(qa, qb):
    d = g.quat_mul(g.quat_conj(qa), qb)
    return [2 * c for c in d[1:]] if d[0] >= 0 else [-2 * c for c in d[1:]]
errs, sig, norms = [], [], []
for k in range(n):
    t = k * dt
    w = omega(t)
    qt = g.quat_integrate(qt, w, dt)
    wm = [w[i] + b_true[i] + sig_g / math.sqrt(dt) * rnd.gauss(0, 1) for i in range(3)]
    wh = [wm[i] - bh[i] for i in range(3)]
    q = g.quat_integrate(q, wh, dt)
    F = [[0.0] * 6 for _ in range(6)]
    S = skew(wh)
    for i in range(3):
        for j in range(3):
            F[i][j] = -S[i][j]
        F[i][i + 3] = -1.0
    Phi = g.mat_add(g.mat_eye(6), g.mat_scale(F, dt))
    Q = [[0.0] * 6 for _ in range(6)]
    for i in range(3): Q[i][i] = sig_g ** 2 * dt; Q[i + 3][i + 3] = sig_b ** 2 * dt
    P = g.mat_add(g.mat_mul(g.mat_mul(Phi, P), g.mat_T(Phi)), Q)
    if k % 5 == 0:                                               # 10 Hz vector measurements
        A = A_of(q); At = A_of(qt)
        H, innov, Rd = [], [], []
        for r, sd in zip(r_list, sd_v):
            z = norm([mv(At, r)[i] + sd * rnd.gauss(0, 1) for i in range(3)])
            pred = mv(A, r)
            Sp = skew(pred)
            for i in range(3):
                H.append(Sp[i] + [0.0, 0.0, 0.0]); innov.append(z[i] - pred[i]); Rd.append(sd * sd)
        m_ = len(H)
        Rm = [[Rd[i] if i == j else 0.0 for j in range(m_)] for i in range(m_)]
        Sm = g.mat_add(g.mat_mul(g.mat_mul(H, P), g.mat_T(H)), Rm)
        K = g.mat_mul(g.mat_mul(P, g.mat_T(H)), g.mat_inv(Sm))
        dx = [sum(K[j][i] * innov[i] for i in range(m_)) for j in range(6)]
        dth = dx[:3]
        q = g.quat_mul(q, [1.0, 0.5 * dth[0], 0.5 * dth[1], 0.5 * dth[2]])
        q = g.quat_normalize(q)
        bh = [bh[i] + dx[3 + i] for i in range(3)]
        IKH = g.mat_sub(g.mat_eye(6), g.mat_mul(K, H))
        P = g.mat_add(g.mat_mul(g.mat_mul(IKH, P), g.mat_T(IKH)), g.mat_mul(g.mat_mul(K, Rm), g.mat_T(K)))
        P = [[0.5 * (P[i][j] + P[j][i]) for j in range(6)] for i in range(6)]
    e = att_err(qt, q)
    errs.append(sum(c * c for c in e)); sig.append(P[0][0] + P[1][1] + P[2][2])
    norms.append(abs(math.sqrt(sum(c * c for c in q)) - 1.0))
def rms_window(a, b): 
    seg = errs[int(a / dt):int(b / dt)]; return math.sqrt(sum(seg) / len(seg)) / D2R
def sig_window(a, b):
    seg = sig[int(a / dt):int(b / dt)]; return math.sqrt(sum(seg) / len(seg)) / D2R
print('Multiplicative EKF, 3 axes, 6 error states (angle error and gyro bias), two vector measurements at 10 Hz:')
for a, b_ in ((0, 5), (5, 20), (20, 120)):
    print(f'  {a:3d}-{b_:3d} s: attitude error (3-axis rms) {rms_window(a, b_):6.3f} deg, filter-predicted {sig_window(a, b_):6.3f} deg')
print(f'  gyro bias estimate error after 120 s: ' + ', '.join(f'{abs(bh[i] - b_true[i]) / D2R:.4f}' for i in range(3)) + ' deg/s')
print(f'  largest deviation of |q| from 1 over the run: {max(norms):.1e}')

Complementary filter, pitch error rms (deg) after 20 s, against the correction gain k (rad/s):
  k =  0.02: 18.048 deg   (bias term b/k =  25.00 deg, crossover period  314.2 s)
  k =  0.05:  9.423 deg   (bias term b/k =  10.00 deg, crossover period  125.7 s)
  k =  0.10:  5.325 deg   (bias term b/k =   5.00 deg, crossover period   62.8 s)
  k =  0.20:  2.528 deg   (bias term b/k =   2.50 deg, crossover period   31.4 s)
  k =  0.50:  0.980 deg   (bias term b/k =   1.00 deg, crossover period   12.6 s)
  k =  1.00:  0.664 deg   (bias term b/k =   0.50 deg, crossover period    6.3 s)
  k =  2.00:  0.689 deg   (bias term b/k =   0.25 deg, crossover period    3.1 s)
  k =  4.00:  1.034 deg   (bias term b/k =   0.12 deg, crossover period    1.6 s)
  smallest error at k = 1.0
Multiplicative EKF, 3 axes, 6 error states (angle error and gyro bias), two vector measurements at 10 Hz:
    0-  5 s: attitude error (3-axis rms)  0.624 deg, filter-predicted  0.793 deg
    5- 20 s: attitude error (3-axis rms)  0.332 deg, filter-predicted  0.410 deg
   20-120 s: attitude error (3-axis rms)  0.343 deg, filter-predicted  0.324 deg
  gyro bias estimate error after 120 s: 0.0118, 0.0036, 0.0106 deg/s
  largest deviation of |q| from 1 over the run: 4.4e-16

In robotics

These two filters are the two ends of attitude estimation on real vehicles.

• Complementary filters. Cheap and robust, they run on microcontrollers (the Mahony and Madgwick filters are complementary filters with a quaternion state). They need a gain and no covariance, and they are the reference to compare a Kalman filter with: if the extra complexity does not beat a well-tuned complementary filter, it is not worth it.
• The accelerometer trap. The vector measurement of the gravity direction is the specific force, which in powered flight is thrust plus gravity, not gravity. A complementary filter that trusts the accelerometer during a burn tilts the estimate toward the thrust axis. Flight filters reduce the gain during acceleration, or take the vector from another source (the beacon-based velocity direction).
• Multiplicative EKFs. Spacecraft attitude filters (with star trackers and gyros) are multiplicative EKFs, as are the attitude parts of most flight-stack EKFs. The error state is small by design, so the linearization is accurate and the singularities of Euler angles do not occur.
• Bias estimation. Estimating the gyro bias is what turns the Kalman filter into more than a gain: it learns the 0.009 rad/s here to 0.012°/s, which is what lets a spacecraft coast through an eclipse when no vectors are available.
• Tuning. The process noises come from the Allan variance of M6-T2: Allan Variance: Reading a Sensor from Its Noise: the angle random walk for the gyro noise, and the rate random walk or bias instability for the bias.

The aiding sensors of the next lesson provide the measurements; a filter's consistency is then checked with the innovation tests of M7-T6: Navigation Integrity: Fault Detection and Protection Levels.

Common misconceptions

Key takeaways

1. A complementary filter integrates the gyro and pulls toward the vector-derived angle with gain k, the crossover frequency.
2. The error is a bias term $b/k$ plus a noise term rising with k; the optimum is near $(4b^2/S_\nu)^{1/3}$ and is flat.
3. For the example the best gain is 1 rad/s, with 0.66° rms against 5.3° at k = 0.1 and 1.03° at k = 4.
4. A quaternion has a constraint, so a Kalman filter estimates a three-component rotation error and updates by multiplication.
5. The multiplicative EKF reached 0.34° rms (predicted 0.32°), estimated the gyro bias to 0.012°/s and held the norm to 4×10⁻¹⁶.
6. The accelerometer is a gravity vector only when the vehicle is not accelerating.

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.

• R. Mahony, T. Hamel and J.-M. Pflimlin, "Nonlinear complementary filters on the special orthogonal group", IEEE Transactions on Automatic Control 53(5), 2008. Paid.
• F. L. Markley, "Attitude error representations for Kalman filtering", Journal of Guidance, Control, and Dynamics 26(2), 2003, the multiplicative EKF. Paid (AIAA).
• J. Solà, "Quaternion kinematics for the error-state Kalman filter", 2017, a free tutorial with all the equations. Free to read on arXiv (arXiv:1711.02508); licence stated there.
• MIT 16.322 Stochastic Estimation and Control (OpenCourseWare) , for the Kalman filter and its steady-state gain. 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
  M7-T3: Attitude from Vector Observations: TRIAD and Wahba's Problem

  Compute attitude from two vector pairs with TRIAD, and from any number with the singular-value solution of Wahba's problem.

• Lab
  M7-L2: Two Vectors, One Attitude

  A satellite measures the Sun and the magnetic field with different accuracies. Implement TRIAD and the Wahba solution, and compare their errors as the two vectors approach parallel.

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

• UAV M2-T1: GNSS fundamentals
• UAV M2-T3: GNSS receivers and positioning
• UAV M2-T4: GNSS errors and multipath
• UAV M2-T6: GNSS jamming and spoofing
• UAV M3-T6: Error-State Kalman Filter
• UAV M3-T5: Extended Kalman Filter
• UAV M3-T11: Multi-sensor fusion architectures
• UAV M3-T13: Innovation monitoring
• UAV M3-T16: Sensor fault detection and isolation
• UAV M6-T5: PX4 state estimation and EKF2
• UAV M2-T11: Visual-Inertial Odometry
• SLAM M8-T5: GPS-Denied Localization
• Linear Algebra M5-T6: Weighted Least Squares
• Linear Algebra M6-T5: Singular Value Decomposition
• Probability for Robotics (modules 7, 8 and 12: Kalman, extended and error-state filters)

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

• Constellations, signals, error sources, multipath, jamming and spoofing
• The Kalman, extended and error-state filter derivations
• Visual, LiDAR and radar odometry, VIO and SLAM as aiding sources
• Innovation tests (NIS, NEES) as such