Complementary vs. Mahony vs. EKF: Choosing the Right Attitude Estimator for Your Drone
Your drone’s IMU is lying to you. Not maliciously — it just has two sensors with directly opposite failure modes, and your flight controller has to reconcile them 4,000 times per second.
The accelerometer is accurate over long time windows. Point it straight up and it reads 1g — gravity — reliably. But during flight, every motor vibration, every lateral acceleration, every wind gust adds a linear component to that reading. The accelerometer cannot distinguish between “I am tilted 10 degrees” and “I am upright but accelerating sideways.” In the short term, it is an unreliable mess.
The gyroscope is the opposite. It measures angular rate with very low noise — smooth, clean, fast. But it integrates drift. A gyroscope with even a 0.01 °/s bias error accumulates 36° of attitude error per hour. On a drone, that error is visible in seconds.
The attitude estimator’s job is to fuse these two sensors into a single trustworthy orientation estimate: trust the gyroscope in the short term for its smoothness, trust the accelerometer in the long term to correct accumulated drift. How you implement that fusion determines everything about your flight controller’s accuracy, its computational load, and the hardware it requires.
Three algorithms dominate this space. They make very different tradeoffs.
Quick Reference
| Metric | Complementary Filter | Mahony Filter | Extended Kalman Filter |
|---|---|---|---|
| Computation | Ultra-low (basic algebra) | Low–Medium (quaternion math) | Very high (matrix operations) |
| Accuracy | Low — drifts in dynamic flight | High — excellent for standard flight | Highest — handles aggressive maneuvers |
| Latency | Near-zero | Very low | Variable — depends on MCU clock |
| Minimum MCU | Any 8-bit (ATmega328P) | 32-bit Cortex-M4 with FPU | 32-bit Cortex-M7 with FPU, minimum |
| Extra sensors | No | No (gyro + accel only) | Yes — GPS, baro, optical flow, mag |
| Gimbal lock risk | Yes (if using Euler angles) | No (quaternion-native) | No (quaternion-native) |
1. The Complementary Filter: The Lightweight Pioneer
How It Works
The complementary filter is three lines of code and a tuning constant. The core idea: apply a high-pass filter to the gyroscope (trust it for fast changes, reject slow drift) and a low-pass filter to the accelerometer (trust it for slow DC orientation, reject fast noise). Set the two filters to be complementary — their frequency responses sum to one across all frequencies — and you get a fused estimate.
In practice this collapses to a weighted average:
\[\theta = \alpha \cdot (\theta_{\text{prev}} + \omega \cdot dt) + (1 - \alpha) \cdot \theta_{\text{acc}}\]Where $\alpha$ is typically 0.98 (heavily trust the gyroscope), $\omega$ is gyroscope rate in rad/s, $dt$ is the timestep, and $\theta_{\text{acc}}$ is the tilt angle derived from the accelerometer via atan2.
// Complementary filter — straightforward implementation
float angle_acc = atan2f(ax, az) * RAD_TO_DEG;
angle = ALPHA * (angle + gyro_rate * dt) + (1.0f - ALPHA) * angle_acc;
Where It Works
A line-following robot, a self-balancing platform, a slow-moving ground rover. Any application where accelerations are mostly gravitational, maneuvers are gentle, and you need an attitude estimate with near-zero CPU budget. Runs cleanly on an ATmega328P at 16 MHz.
Where It Breaks
The accelerometer term assumes the only acceleration is gravity. The moment your drone banks, pitches, or throttles aggressively, linear accelerations corrupt the gravity vector estimate. atan2(ax, az) gives you the wrong answer, and the filter drags the gyroscope output toward that wrong answer at rate (1 - α). The result is attitude error that grows exactly when you need accuracy most — during dynamic flight maneuvers.
The upgrade trigger: Any aerial platform. The complementary filter is not suitable for quadcopters, fixed-wing, or any vehicle where sustained lateral acceleration is a normal flight condition.
2. The Mahony Filter: The Embedded Sweet Spot
How It Works
The Mahony filter, formalized by Robert Mahony, Talay Hamel, and Jean-Michel Pflimlin in their 2008 IEEE TAC paper, is what you use when you need real accuracy on a resource-constrained microcontroller. It operates natively in quaternion space (avoiding gimbal lock entirely) and adds an explicit PI feedback loop that actively estimates and cancels gyroscope bias.
The algorithm proceeds in three stages each loop iteration:
Stage 1 — Estimate gravity direction from the current quaternion:
Rotate the world gravity vector [0, 0, 1] into body frame using the current quaternion estimate. This gives you where gravity should be pointing given your current orientation belief.
Stage 2 — Compute the error: Cross-product the estimated gravity vector with the measured accelerometer vector. The cross product gives a rotation axis that represents the angular error between where you think gravity is and where the accelerometer says it is.
\[\mathbf{e} = \mathbf{a}_{\text{meas}} \times \mathbf{v}_{\text{est}}\]Stage 3 — Correct with PI feedback: Feed that error through a proportional-integral controller. The proportional term corrects the gyroscope rate immediately; the integral term accumulates the error over time to estimate and cancel the gyroscope bias.
\[\dot{\mathbf{b}} = -K_i \cdot \mathbf{e}\] \[\boldsymbol{\omega}_{\text{corrected}} = \boldsymbol{\omega}_{\text{meas}} - \mathbf{b} + K_p \cdot \mathbf{e}\]The corrected angular rate then integrates into the quaternion:
\[\dot{q} = \frac{1}{2} q \otimes [0,\ \omega_x,\ \omega_y,\ \omega_z]\] \[q \leftarrow \left(q + \dot{q} \cdot dt\right)_{\text{normalized}}\]// Mahony filter core — error, PI correction, quaternion integration
// vx, vy, vz: estimated gravity in body frame from current quaternion
float ex = (ay * vz - az * vy);
float ey = (az * vx - ax * vz);
float ez = (ax * vy - ay * vx);
// PI feedback
integralFBx += Ki * ex * dt;
integralFBy += Ki * ey * dt;
integralFBz += Ki * ez * dt;
// Apply corrected rates to gyro
float gx_c = gx + Kp * ex + integralFBx;
float gy_c = gy + Kp * ey + integralFBy;
float gz_c = gz + Kp * ez + integralFBz;
// Integrate into quaternion
q0 += (-q1 * gx_c - q2 * gy_c - q3 * gz_c) * (0.5f * dt);
q1 += ( q0 * gx_c + q2 * gz_c - q3 * gy_c) * (0.5f * dt);
q2 += ( q0 * gy_c - q1 * gz_c + q3 * gx_c) * (0.5f * dt);
q3 += ( q0 * gz_c + q1 * gy_c - q2 * gx_c) * (0.5f * dt);
// Normalize — mandatory every iteration
float recip_norm = inv_sqrtf(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 *= recip_norm; q1 *= recip_norm;
q2 *= recip_norm; q3 *= recip_norm;
Tuning $K_p$ and $K_i$
- $K_p$ (proportional gain): Controls how aggressively the filter corrects attitude error from the accelerometer. Too high and accelerometer noise bleeds through into the attitude estimate, making the drone jittery. Too low and the filter is slow to correct drift. Typical starting range: 2.0–10.0.
- $K_i$ (integral gain): Controls gyroscope bias estimation speed. Most real sensors have a stable bias after warm-up, so $K_i$ can be very small (0.001–0.05). Setting it too high causes the bias estimate to chase accelerometer noise.
Where It Excels
Standard quadcopter flight — manual acro, stabilize mode, mild sport flying. The Mahony filter handles sustained moderate acceleration well because the PI loop separates the slow bias correction from the fast attitude tracking. It runs efficiently on any Cortex-M4 with an FPU; a single Mahony update on an STM32F405 at 168 MHz takes under 2 µs.
Where It Falls Short
The Mahony filter cannot tell the difference between accelerometer error caused by linear acceleration and error caused by actual attitude change. Its model of sensor noise is fixed — $K_p$ and $K_i$ are constants, not adaptive. In highly dynamic flight (racing, aggressive waypoint flight, wind disturbance rejection), the filter occasionally loses track and requires several hundred milliseconds to re-converge. It also cannot natively fuse additional sensors like barometers or GPS — it is a 6-DOF algorithm only.
3. The Extended Kalman Filter: The Gold Standard
How It Works
The EKF is a fundamentally different class of algorithm. Rather than a fixed-gain fusion rule, it maintains a probabilistic model of the system state and its uncertainty, and uses Bayesian inference to optimally weight every sensor measurement based on how much it trusts that sensor at that moment.
The algorithm has two steps per iteration:
Predict step — propagate the state forward using the system model (quaternion kinematics driven by gyroscope):
\[\hat{x}_{k|k-1} = f(\hat{x}_{k-1}, \mathbf{u}_k)\] \[P_{k|k-1} = F_k P_{k-1} F_k^T + Q\]Where $P$ is the state covariance matrix (tracking uncertainty), $F$ is the Jacobian of the state transition function, and $Q$ is the process noise covariance (how much you trust the gyroscope model).
Update step — correct the prediction using a sensor measurement:
\[K_k = P_{k|k-1} H_k^T (H_k P_{k|k-1} H_k^T + R)^{-1}\] \[\hat{x}_k = \hat{x}_{k|k-1} + K_k(z_k - h(\hat{x}_{k|k-1}))\] \[P_k = (I - K_k H_k) P_{k|k-1}\]$K_k$ is the Kalman gain — the matrix that dynamically decides how much to trust the new measurement versus the prediction. $R$ is the measurement noise covariance, which encodes how noisy the sensor is. If the accelerometer is reading large non-gravitational accelerations (high dynamic motion), $R$ can be increased to deweight it automatically.
This is what the Mahony filter cannot do: the EKF’s trust in every sensor is adaptive and mathematically principled, not a fixed constant.
State Vector Scale
The size of the state vector determines complexity. For attitude-only estimation:
- 7-state EKF: Quaternion (4) + gyro bias (3). This is what TeensyPilot implements — a pure attitude estimator with bias tracking. Fast enough for 4 kHz on a Cortex-M7.
- 15-state EKF (error-state formulation, attitude as 3 error angles):
attitude error (3) + gyro bias (3) + velocity (3) + position (3)
- accel bias (3). Standard for GPS-aided navigation.
- 24-state EKF (ArduPilot EKF3): Above + wind (3) + magnetometer bias (3) + magnetic field (3). Full navigation with multi-sensor fusion, the production flight controller standard.
The Computational Reality
A 7-state EKF on the Teensy 4.0 (Cortex-M7, 600 MHz, hardware FPU) runs a full predict+update cycle in approximately 18–25 µs at the 4 kHz control loop rate — feasible, but consuming a meaningful slice of the budget. A 15-state EKF on the same hardware takes roughly 60–80 µs. ArduPilot’s EKF3 at full 24-state runs at 400 Hz, not 4 kHz — it is not the inner loop filter; it feeds the position controller which runs at a much lower rate.
On an STM32F405 at 168 MHz with a Cortex-M4 FPU, a 7-state EKF is viable at 1 kHz. A 15-state is tight. A 24-state is not practical at flight-control rates.
On an ATmega328P — forget it. A single 7×7 matrix multiply in software floating point takes several milliseconds. The flight controller would crash before it finished the first iteration.
Read: The Embedded Stack for Robotics — MCU Hierarchy →
TeensyPilot: Where This Lands in Practice
TeensyPilot uses a 7-state quaternion EKF as its primary attitude estimator, running at 4 kHz on the Teensy 4.0. The choice was not arbitrary.
The cascaded PID architecture (rate loop at 4 kHz, attitude loop at 1 kHz) needs an attitude estimate that is both fast and accurate under dynamic conditions. A Mahony filter would be sufficient for gentle flight — but the goal of TeensyPilot includes non-GPS stabilization using optical flow and stereo cameras, which introduces sustained horizontal accelerations that corrupt the Mahony filter’s gravity assumption. The EKF’s ability to adaptively deweight the accelerometer during high-acceleration phases is not optional in that context.
The 7-state choice over a full 15-state is deliberate: position and velocity come from the companion computer (Jetson/RasPi, running the stereo depth pipeline), not from the flight controller’s IMU. The flight controller’s EKF only needs to know attitude and gyro bias. Keeping the state vector small keeps the matrix operations fast enough to run at 4 kHz on the M7 without dominating the loop budget.
The ICM-20948 IMU feeds data at 8 kHz via DMA-backed SPI. The EKF predict step runs on every sample; the update step (accelerometer correction) runs at 1 kHz to avoid overcorrecting from high-rate accel noise. This separation of predict and update rates is a standard technique for keeping EKF compute costs manageable.
Which One Should You Use?
Work through these questions in order:
Is your vehicle ground-based with slow, gentle movements? Complementary filter. The math fits on a napkin, runs on anything, and is more than sufficient for the problem.
Is your vehicle airborne with standard flight dynamics — stabilize mode, moderate sport, beginner acro? Mahony filter. Accurate enough for the dynamics, runs comfortably on any Cortex-M4, and the $K_p$/$K_i$ tuning is tractable.
Does your platform need any of the following?
- Aggressive maneuvering where sustained lateral acceleration is normal
- Additional sensor fusion (barometer, GPS, optical flow, magnetometer)
- Adaptive noise weighting for varying flight conditions
- Non-GPS indoor stabilization
→ EKF. But you need the hardware to run it — minimum a Cortex-M4 with FPU for a 7-state, and a Cortex-M7 if you want it at 4 kHz alongside everything else your flight controller is doing.
Are you using ArduPilot or PX4 on a Pixhawk? EKF3 or EKF2 is already running. You tune $Q$ and $R$ matrices through parameter files. Understanding the algorithm helps you tune it correctly, but you do not implement it from scratch.
The filter is not the project. It is infrastructure. Pick the simplest one that meets your accuracy requirements at the update rate your hardware can sustain — then move on to building the thing the filter enables.