"The gyroscope knows exactly how fast the world is turning and slowly forgets where it started. The accelerometer never forgets down but panics at every pothole. I married them, and the marriage works."
A Well-Fused AI Agent
Prerequisites
This section builds directly on the quaternion algebra of Section 24.1: you should be comfortable multiplying unit quaternions, forming the quaternion derivative from an angular rate, and reading a quaternion as a rotation. It also assumes the three inertial sensors and their frames from Chapter 23, especially that a gyroscope measures angular rate while an accelerometer at rest measures gravity. Familiarity with high-pass and low-pass filters from Chapter 6 makes the core idea land faster, and the estimation vocabulary from Chapter 4 frames why we accept a fast approximate answer here.
The Big Picture
You have two ways to know which way is up, and both are broken in complementary ways. Integrate the gyroscope and you get a smooth orientation that is correct for seconds and then wanders off, because tiny bias errors accumulate without bound. Look at the accelerometer and you get an absolute reference to gravity that never drifts, but it is contaminated by every bump, turn, and vibration the device experiences. Trust the gyroscope for the next instant and the accelerometer for forever: that one sentence is the whole idea powering nearly every drone, phone, VR headset, and fitness band on Earth. This section builds that idea three ways: the scalar complementary filter that shows it in one line, and the Mahony and Madgwick filters that make it work on the full rotation group in real time on a microcontroller.
Why one sensor is never enough
Start from the honest failure of each sensor alone. A gyroscope reports angular rate \(\omega\) in the body frame, and integrating it propagates orientation forward: in quaternion form the estimate evolves as \(\dot{q} = \tfrac{1}{2}\, q \otimes (0,\omega)\), the relation established in Section 24.1. This integration is exact in principle and smooth in practice, since a MEMS gyro is quiet and fast. The problem is bias: every gyroscope has a small, temperature-dependent offset \(b\), so you are really integrating \(\omega + b\), and that constant integrates into an angle error growing linearly with time; a few tenths of a degree per second of bias leaves your heading tens of degrees wrong within a minute. Nothing in the gyro signal itself reveals which part was drift.
The accelerometer supplies the missing anchor. When the device is not accelerating, it measures only gravity, a vector that points in a fixed navigation direction forever, giving absolute pitch and roll with zero drift. But "when the device is not accelerating" is the catch: any real motion adds linear acceleration on top of gravity, so during a drone's climb or a runner's stride the accelerometer's idea of down is momentarily a lie. A magnetometer anchors the third axis, yaw, for a reason worth stating plainly: gravity is vertical, so it constrains roll and pitch but leaves rotation about the vertical axis completely unobservable, meaning yaw drifts forever, even with a perfectly tuned gyro, unless something external fixes it. The magnetometer supplies that fix from the local magnetic field, but it is corrupted by hard-iron effects (a constant offset from nearby permanently magnetized metal) and soft-iron effects (direction-dependent distortion from nearby ferrous parts); because these distortions can dwarf gyro drift, many indoor robots and phones near steel furniture disable magnetometer correction and accept slow yaw drift rather than trust a confidently wrong heading. The pattern across all three sensors is clean: gyro is trustworthy at high frequency and untrustworthy at low frequency (drift), while accelerometer and magnetometer are trustworthy at low frequency and untrustworthy at high frequency (motion, vibration, magnetic clutter). Their error spectra are complementary, and that word is not decoration; it is the design.
Common Misconception
It is tempting to assume the accelerometer only needs protecting from short, sharp events, a pothole, a footstep, a propeller vibration, and that anything smooth is safe to trust. The opposite can be true: a car holding a steady banked turn, an elevator at constant acceleration, or a drone in a long coordinated climb all produce linear acceleration that is smooth and sustained, invisible to a jerk detector, yet it adds directly to gravity and is indistinguishable from a real tilt change over exactly the timescale the low-pass path is designed to trust. None of the three filters in this section can tell the difference; they will confidently rotate the estimated "down" toward the combined vector. Guarding against it needs an independent cue, for example a wheel-speed or GPS-derived acceleration estimate that gates the accelerometer during known maneuvers, not a smaller \(\alpha\) or \(K_p\), which only slows how fast the wrong correction is applied.
Key Insight
A complementary filter is a frequency-domain division of labor written as a single fusion rule. If \(H(s)\) is a high-pass filter and \(L(s)=1-H(s)\) its complementary low-pass, then feeding the gyro-integrated angle through \(H\) and the accelerometer angle through \(L\) and summing gives an estimate whose transfer function is \(H+L=1\): the true angle passes through undistorted, while each sensor only contributes in the band where it is reliable. You are not choosing between the sensors; you are letting each one own the frequencies it is good at. Everything Mahony and Madgwick add is a way to do this same split correctly on the curved space of rotations instead of on a single scalar angle.
The complementary filter in one line
For a single tilt angle the whole idea collapses to one update. Let \(\theta_k\) be the estimated angle at step \(k\), \(\omega_k\) the gyro rate, \(\Delta t\) the sample interval, and \(\theta^{\text{acc}}_k\) the angle computed from the accelerometer's gravity direction. Then
\[ \theta_k = \alpha\,\bigl(\theta_{k-1} + \omega_k\,\Delta t\bigr) \;+\; (1-\alpha)\,\theta^{\text{acc}}_k. \]The first term integrates the gyro (the high-pass path, dominant at short timescales); the second nudges toward the accelerometer's absolute reading (the low-pass path, dominant over long timescales). The single knob \(\alpha \in (0,1)\), usually between \(0.95\) and \(0.995\), sets the crossover: close to one trusts the gyro longer and rejects vibration well but corrects drift slowly, while lower values chase the accelerometer harder. The relation to a time constant is \(\tau = \alpha\,\Delta t /(1-\alpha)\), so at \(200\text{ Hz}\) and \(\alpha=0.99\) you correct drift with roughly a half-second memory. This is the entire algorithm, and it is why the complementary filter runs on eight-bit microcontrollers that could never host a Kalman filter (Chapter 9).
Mahony: proportional-integral feedback on SO(3)
The scalar filter breaks the moment orientation is a full three-dimensional rotation, because you cannot simply add a gyro angle to an accelerometer angle when both live on the curved manifold \(SO(3)\). Robert Mahony's insight was to keep the gyro integration on the manifold (using the quaternion derivative) and inject the accelerometer correction as a feedback torque rather than a weighted average. The device predicts where gravity should point given its current orientation estimate, \(\hat{v} = \hat{q}^{-1}\otimes g \otimes \hat{q}\), and compares it to the measured gravity direction \(a\) from the accelerometer. The mismatch is a rotation error, and on a sphere the natural measure of that error is the cross product
\[ e = a \times \hat{v}. \]This error vector points along the axis you must rotate about to bring predicted gravity onto measured gravity, and its magnitude grows with the angle between them. Mahony then feeds it back into the gyro integration through a proportional-integral controller: the proportional term \(K_p\,e\) corrects the current orientation, while the integral term \(K_i \int e\, dt\) accumulates to estimate and cancel the gyro bias \(b\), the very quantity that caused drift in the first place. The corrected rate \(\omega + K_p e + \hat{b}\) drives the quaternion update. Crucially, the integral action does not merely mask drift; it identifies the constant bias and subtracts it at the source, so a Mahony filter left running on a stationary device converges to the true gyro offset. The controller gains play the role \(\alpha\) played before: larger \(K_p\) trusts the accelerometer more, larger \(K_i\) tracks a faster-changing bias.
In Practice: keeping a racing drone level through the vibration storm
A first-person-view racing quadcopter runs its flight controller loop at \(8\text{ kHz}\) on a small ARM chip, and the single most important number it produces is attitude, updated thirty times faster than a human could react. The accelerometer on that airframe is buried in motor vibration and reads several g of noise during hard throttle, so a naive average toward it would make the craft twitch and oscillate. Betaflight and its kin use a Mahony-style filter precisely because the proportional gain can be tuned low enough to ride out the vibration while the gyro carries the fast dynamics, and the integral term quietly cancels the bias that would otherwise let the horizon tilt over a long flight. Pilots feel the difference directly: a well-tuned filter holds a locked horizon through a full-throttle punch-out, while a badly tuned one "floats." The same filter, at a tenth the loop rate, stabilizes the onboard camera gimbal.
Madgwick: gradient descent toward the reference
Sebastian Madgwick's filter reaches the same goal by a different, often cheaper route. Instead of a cross-product feedback torque, it poses the accelerometer correction as an optimization: find the small orientation change that best aligns predicted gravity with the measured accelerometer vector. That is a least-squares objective \(f(\hat{q}) = \hat{q}^{-1}\otimes g \otimes \hat{q} - a\), and its analytic gradient \(\nabla f\) points in the direction of steepest orientation error. Madgwick takes a single gradient-descent step of size \(\beta\) along the normalized gradient at each sample and blends it with the gyro integration in the same complementary spirit: the gyro sets the direction of travel, and the gradient step corrects it toward the drift-free reference. The one tuning parameter \(\beta\) has a clean physical meaning, the filter's assumed gyroscope measurement error in radians per second, so you can set it from the gyro datasheet rather than by trial and error.
Both filters solve the same problem and, well tuned, produce indistinguishable accuracy of roughly one to two degrees RMS on consumer IMUs. The differences are practical: Madgwick is a fixed, branch-light computation attractive for the tiniest targets, while Mahony's explicit integral state makes its bias estimate easy to inspect and reset, which matters when a gyro's bias jumps with temperature. Neither is a probabilistic estimator: unlike the Kalman family of Chapter 9, they carry no covariance and cannot report a calibrated uncertainty on the orientation they output, worth remembering when a downstream system needs to know how much to trust the attitude (see Chapter 18). The tradeoff is deliberate: give up the covariance to gain a filter that fits in a few dozen lines and a few microseconds.
Research Frontier
The hand-tuned constants in this section, \(\alpha\), \(K_p\)/\(K_i\), and \(\beta\), are themselves now a target for automation. The Versatile Quaternion-based Filter (VQF, Laidig and Seel, 2023) keeps the same complementary structure but makes the gains adaptive: it estimates gyro bias online, down-weights the accelerometer during detected linear acceleration, and rejects magnetic disturbances by watching how far the field magnitude strays from its resting value, all without a hand-tuned crossover. A second frontier trains the correction directly from data: temporal-convolutional and transformer attitude networks (the architectures of Chapter 14) regress orientation, or a per-sample gain, from a raw IMU window, effectively learning the schedule Mahony and Madgwick hand-tuned, and wearable foundation models (Chapter 20) are starting to fold this correction into a general-purpose IMU encoder rather than a bespoke filter. None of this changes the physics: gravity is still the only free, drift-free vertical reference; it only changes who decides how much to trust it at each instant.
import numpy as np
def madgwick_update(q, gyro, accel, beta=0.033, dt=1/200):
"""One Madgwick IMU step. q=[w,x,y,z] unit quaternion, gyro in rad/s,
accel in any consistent unit (only its direction is used)."""
q = q / np.linalg.norm(q)
a = accel / np.linalg.norm(accel) # gravity direction only
w, x, y, z = q
# Gradient of (predicted gravity - measured gravity): objective f and Jacobian J
f = np.array([2*(x*z - w*y) - a[0],
2*(w*x + y*z) - a[1],
2*(0.5 - x*x - y*y) - a[2]])
J = np.array([[-2*y, 2*z, -2*w, 2*x],
[ 2*x, 2*w, 2*z, 2*y],
[ 0.0, -4*x, -4*y, 0.0]])
grad = J.T @ f
grad = grad / (np.linalg.norm(grad) + 1e-12)
# Gyro-driven quaternion rate, corrected by a beta-sized step down the gradient
qdot = 0.5 * quat_mult(q, np.array([0.0, *gyro])) - beta * grad
q = q + qdot * dt
return q / np.linalg.norm(q)
def quat_mult(a, b):
w1, x1, y1, z1 = a; w2, x2, y2, z2 = b
return np.array([w1*w2 - x1*x2 - y1*y2 - z1*z2,
w1*x2 + x1*w2 + y1*z2 - z1*y2,
w1*y2 - x1*z2 + y1*w2 + z1*x2,
w1*z2 + x1*y2 - y1*x2 + z1*w2])qdot smoothly; the -beta * grad term is the drift-free correction that pulls predicted gravity toward the measured accelerometer direction. The single knob beta is the assumed gyro error in rad/s, set here for a typical consumer IMU at 200 Hz. Feeding this a stationary device and a slowly rotating one reproduces the smooth-yet-anchored behavior described above.The Right Tool
Listing 24.1 plus its magnetometer path, bias handling, and initialization runs to roughly 120 lines to get right, and the sign conventions in the Jacobian are exactly where bugs hide. The ahrs Python package ships tested Madgwick, Mahony, and a dozen other attitude filters that consume a whole session at once:
from ahrs.filters import Madgwick
# gyr, acc are (N, 3) arrays; returns (N, 4) quaternions, fully vectorized
Q = Madgwick(gyr=gyr, acc=acc, frequency=200.0).Qahrs, versus roughly 120 lines from scratch. The library handles quaternion initialization from the first accelerometer sample, normalization, and the magnetometer variant behind one flag.Use the library in any pipeline; keep the from-scratch version to understand what the gain is doing when the horizon drifts unexpectedly.
Exercise
Log an IMU stream (a phone app such as phyphox exposes raw gyro and accelerometer) while you set the device flat, rotate it slowly by exactly \(90^{\circ}\) about one axis, hold, and return. (1) Integrate the gyro alone and plot the estimated angle; measure how many degrees it has drifted after two minutes of stillness at the end. (2) Run the complementary update from this section with \(\alpha=0.98\) and again with \(\alpha=0.999\), and describe how the drift and the vibration rejection trade off. (3) Replace the scalar filter with ahrs's Madgwick and Mahony over the same log; report the peak difference in estimated pitch between the two, and relate its size to the one-to-two-degree figure quoted above.
Self-Check
1. In what frequency band does the gyroscope dominate the fused estimate, and in what band does the accelerometer dominate, and why is this split called "complementary"?
2. What does the integral term in a Mahony filter estimate, and why does that make its drift correction fundamentally better than simply averaging toward the accelerometer?
3. Give one concrete situation in which the accelerometer's gravity reference is temporarily wrong, and explain what the complementary structure does about it.
What's Next
These filters give you a clean, drift-free attitude from three cheap chips, but a shippable product needs more: automatic bias calibration, magnetometer soft- and hard-iron correction, and a defined behavior when a sensor drops out or saturates. In Section 24.3, we assemble the complementary filter into a full attitude and heading reference system (AHRS), the packaged orientation module that drones, phones, and headsets actually deploy.