A Fast Orientation Estimation Filter: Madgwick

I'm building a custom quadrotor flight controller, and one of the challenges is fast state estimation with limited onboard compute. The flight controller is a Teensy 4.0 (600 MHz ARM Cortex M7 with hardware FPU), and I need a fast orientation estimator that I can later fuse with other sensors (barometer, magnetometer, GPS) for state estimation, which will be used by an MPC or RL based controller (more on that in a future post) and for trajectory following. The IMU is an MPU6050, providing a 3-axis gyroscope and a 3-axis accelerometer.

Breadboard setup with a Teensy 4.0 and MPU6050

This post covers the Madgwick filter, which I used for orientation estimation. It's a short read focused on the algorithm and the math behind it.

Madgwick represents orientation throughout as a unit quaternion $q = (q_{0}, q_{1}, q_{2}, q_{3})$ .

Problem

Integrating gyro readings alone drifts over time due to bias and noise accumulation every sample, and orientation error grows unbounded even when the drone is at rest.

The accelerometer measures gravity, which can be used as an absolute reference for roll and pitch with no drift. The reading is noisy sample to sample, though, and gets corrupted the moment there is sudden acceleration such as thrust changes or motor vibration.

Madgwick's filter fuses the two. The gyro gives a fast, drift prone estimate, and the accelerometer continuously corrects it with a slow, absolute, but noisy reference.

IMU data flows through the Madgwick filter to a quaternion, then to attitude and linear acceleration

1. Gyroscope prediction

The angular velocity $ω = (ω_{x}, ω_{y}, ω_{z})$ , measured in rad/s, integrates into a quaternion rate using the standard quaternion kinematic equation.

\overset{q}{˙}_{ω} = \frac{1}{2} q_{t - 1} \otimes [0, ω_{x}, ω_{y}, ω_{z}]

This relates the time derivative of a rotation quaternion to the angular velocity. Expanded into scalar form, it can be expressed as the following four terms.

\overset{q}{˙}_{0} \overset{q}{˙}_{1} \overset{q}{˙}_{2} \overset{q}{˙}_{3} = 0.5 (- q_{1} ω_{x} - q_{2} ω_{y} - q_{3} ω_{z}) = 0.5 (q_{0} ω_{x} + q_{2} ω_{z} - q_{3} ω_{y}) = 0.5 (q_{0} ω_{y} - q_{1} ω_{z} + q_{3} ω_{x}) = 0.5 (q_{0} ω_{z} + q_{1} ω_{y} - q_{2} ω_{x})

2. Accelerometer correction

The orientation $q$ is found by aligning the reference gravity direction in the inertial frame, rotated into the sensor frame, with the measured and normalized accelerometer reading $a = (a_{x}, a_{y}, a_{z})$ . This alignment is expressed as an error function $f (q, a)$ , which is zero when the current orientation guess exactly matches what the accelerometer sees.

f (q, a) = q^{*} \otimes [0, 0, 0, 1] \otimes q - a

f (q, a) = 2 (q_{1} q_{3} - q_{0} q_{2}) - a_{x} 2 (q_{0} q_{1} + q_{2} q_{3}) - a_{y} q_{0}^{2} - q_{1}^{2} - q_{2}^{2} + q_{3}^{2} - a_{z}

This error function is minimized using gradient descent. Since $f$ has three components and $q$ has four, the sensitivity of each error component to each quaternion component is captured by the Jacobian of $f$ with respect to $q$ , shown below.

J (q) = - 2 q_{2} - 2 q_{1} - 2 q_{0} 2 q_{3} 2 q_{0} - 2 q_{1} - 2 q_{0} - 2 q_{3} - 2 q_{2} 2 q_{1} 2 q_{2} 2 q_{3}

The gradient is $\nabla f = J (q)^{T} f (q, a)$ , and it points in the direction that increases error the fastest. Expanding this product algebraically gives the four terms actually used in the update.

s_{0} s_{1} s_{2} s_{3} = 4 q_{0} q_{2}^{2} + 2 q_{2} a_{x} + 4 q_{0} q_{1}^{2} - 2 q_{1} a_{y} = 4 q_{1} q_{3}^{2} - 2 q_{3} a_{x} + 4 q_{0}^{2} q_{1} - 2 q_{0} a_{y} - 4 q_{1} + 8 q_{1}^{3} + 8 q_{1} q_{2}^{2} + 4 q_{1} a_{z} = 4 q_{0}^{2} q_{2} + 2 q_{0} a_{x} + 4 q_{2} q_{3}^{2} - 2 q_{3} a_{y} - 4 q_{2} + 8 q_{1}^{2} q_{2} + 8 q_{2}^{3} + 4 q_{2} a_{z} = 4 q_{1}^{2} q_{3} - 2 q_{1} a_{x} + 4 q_{2}^{2} q_{3} - 2 q_{2} a_{y}

The vector $(s_{0}, s_{1}, s_{2}, s_{3})$ is this gradient normalized to unit length, so it represents a pure correction direction rather than a magnitude tied to how large the current error happens to be.

Predicted gravity from the current quaternion versus measured accelerometer down; the difference is the error that drives the correction

3. Fusion

\overset{q}{˙} = \overset{q}{˙}_{ω} - β \frac{s}{∣ s ∣}

Since the gradient points toward increasing error, subtracting it moves the estimate toward decreasing error. $β$ is the only tuning parameter in the filter. It sets how much the accelerometer correction is trusted relative to the gyro prediction.

4. Integration and normalization

q_{t} q_{t} = q_{t - 1} + \overset{q}{˙} Δ t = \frac{q _{t}}{∣ q _{t} ∣}

Normalization runs every step because adding $\overset{q}{˙} Δ t$ does not preserve $∣ q ∣ = 1$ on its own. This predict, correct, fuse, integrate, and normalize sequence runs once for every IMU sample.

Roll, pitch, yaw

roll pitch yaw = atan2 (2 (q_{0} q_{1} + q_{2} q_{3}), 1 - 2 (q_{1}^{2} + q_{2}^{2})) = arcsin (2 (q_{0} q_{2} - q_{3} q_{1})) = atan2 (2 (q_{0} q_{3} + q_{1} q_{2}), 1 - 2 (q_{2}^{2} + q_{3}^{2}))

Clamp the $arcsin$ argument to $[- 1, 1]$ , since floating point rounding can occasionally push it slightly outside that range. Yaw has no absolute reference without a magnetometer, so it drifts slowly over time. Roll and pitch stay anchored by gravity and do not drift.

Linear acceleration

The gravity direction in the sensor frame, computed from $q$ , is given by the following three components.

g_{x} g_{y} g_{z} = 2 (q_{1} q_{3} - q_{0} q_{2}) = 2 (q_{0} q_{1} + q_{2} q_{3}) = q_{0}^{2} - q_{1}^{2} - q_{2}^{2} + q_{3}^{2}

The linear acceleration is then $a_{lin} = a - g \cdot G$ , where $G$ is the gravity constant, and $(g_{x}, g_{y}, g_{z})$ is treated as a unit vector pointing in the direction gravity currently occupies in the sensor frame.

Choosing $β$

A low $β$ gives smooth output, but roll and pitch visibly drift over tens of seconds. A high $β$ corrects drift quickly, but motor vibration and prop wash inject noise directly into the attitude estimate.