Madgwick Orientation Estimation

• Madgwick-based orientation estimation is a sensor fusion technique that integrates gyroscope, accelerometer, and magnetometer data within a quaternion framework.
• The method employs gradient descent correction to mitigate gyroscopic drift and manage non-minimum-phase effects, ensuring reliable orientation estimation.
• Practical implementations demonstrate reduced error and computational efficiency, making it ideal for real-time embedded systems in dynamic control applications.

Madgwick-based orientation estimation is a computationally efficient sensor fusion technique widely employed for determining the three-dimensional orientation of a rigid body using inertial measurement units (IMUs). This method integrates gyroscope, accelerometer, and, optionally, magnetometer measurements within a quaternion-based filter, utilizing a gradient-descent correction to mitigate gyroscopic drift and leverage gravitational and magnetic cues. Madgwick-type algorithms deliver robust real-time orientation solutions in embedded systems and control contexts, particularly under moderate rotational accelerations and disturbances.

1. Quaternion-Based Orientation Parameterization

Sensor-to-world orientation is represented by a unit quaternion $q = [q_0, q_1, q_2, q_3]^{\mathsf{T}} \in S^3$ with $\|q\|_2 = 1$, avoiding singularities and redundancy inherent to Euler angles and rotation matrices (Kok et al., 2019). Quaternions map directly to three-dimensional attitude with efficient composition and normalization. The underlying quaternion product for $p = [p_0, p_1, p_2, p_3]^{\mathsf{T}}$, $r = [r_0, r_1, r_2, r_3]^{\mathsf{T}}$ is:

$$p \otimes r = \begin{bmatrix} p_0 r_0 - p_1 r_1 - p_2 r_2 - p_3 r_3 \ p_0 r_1 + p_1 r_0 + p_2 r_3 - p_3 r_2 \ p_0 r_2 - p_1 r_3 + p_2 r_0 + p_3 r_1 \ p_0 r_3 + p_1 r_2 - p_2 r_1 + p_3 r_0 \end{bmatrix}$$

This structure underpins the filter’s dynamics and correction steps.

2. Gyroscope and Accelerometer Measurement Modeling

For single-axis pendular rotation $\phi$ about the body-$x$ axis, with the IMU at distance $\ell$ from the rotation center, the body-frame accelerometer output incorporates both gravity and linear acceleration due to angular acceleration $\ddot{\phi}$ (Brändle et al., 6 Feb 2025): $$a^k_y = -\sin \phi + \ell \cos \phi \cdot \ddot{\phi}$$

$\|q\|_2 = 1$0

In the pure-gravity scenario ($\|q\|_2 = 1$1), the normalized vector yields orientation via

$\|q\|_2 = 1$2

Linearization about $\|q\|_2 = 1$3 with zero acceleration yields a unity-gain transfer: $\|q\|_2 = 1$4 Rotational acceleration ($\|q\|_2 = 1$5) introduces extra zeros in the transfer function from true roll to accelerometer-derived estimate: $\|q\|_2 = 1$6 Roots: $\|q\|_2 = 1$7 Positive real-axis zeros ($\|q\|_2 = 1$8) produce non-minimum-phase artifacts in orientation estimation.

3. Madgwick Filter Algorithmic Structure

Madgwick’s filter updates orientation via a combination of gyroscopic integration and a gradient descent correction in quaternion space (Kok et al., 2019): $\|q\|_2 = 1$9 Here, $p = [p_0, p_1, p_2, p_3]^{\mathsf{T}}$0 denotes the pure-imaginary gyroscope quaternion. The objective function

$p = [p_0, p_1, p_2, p_3]^{\mathsf{T}}$1

measures the error between estimated and measured gravity directions. The Jacobian $p = [p_0, p_1, p_2, p_3]^{\mathsf{T}}$2 yields the correction direction via $p = [p_0, p_1, p_2, p_3]^{\mathsf{T}}$3.

A fixed step-length $p = [p_0, p_1, p_2, p_3]^{\mathsf{T}}$4 modulates the trade-off between correcting accelerometer drift and following integrated gyro data. Each update concludes with quaternion normalization to preserve unit norm.

Table: Key Madgwick Filter Components

Component	Role	Details/Remarks
Quaternion $p = [p_0, p_1, p_2, p_3]^{\mathsf{T}}$5	Orientation parameterization	4D unit vector
Gyro $p = [p_0, p_1, p_2, p_3]^{\mathsf{T}}$6	Rate input	Pure-imaginary quaternion
Gradient of $p = [p_0, p_1, p_2, p_3]^{\mathsf{T}}$7	Correction direction	Jacobian of gravity error
Step length $p = [p_0, p_1, p_2, p_3]^{\mathsf{T}}$8	Fusion aggressiveness	Low $p = [p_0, p_1, p_2, p_3]^{\mathsf{T}}$9: slow, robust

4. Transfer Function Effects and Bandwidth Considerations

Rotational acceleration introduces zeros in $r = [r_0, r_1, r_2, r_3]^{\mathsf{T}}$0, yielding non-minimum-phase estimator behavior (Brändle et al., 6 Feb 2025). Attempting to “invert” these zeros with controller poles destabilizes closed-loop response. The Madgwick filter mitigates these effects through complementary filtering: $r = [r_0, r_1, r_2, r_3]^{\mathsf{T}}$1 $r = [r_0, r_1, r_2, r_3]^{\mathsf{T}}$2 forms a first-order low-pass on the accelerometer correction. For frequencies $r = [r_0, r_1, r_2, r_3]^{\mathsf{T}}$3, gyroscope integration dominates and high-frequency acceleration-induced errors in accelerometer data are rejected. Lower $r = [r_0, r_1, r_2, r_3]^{\mathsf{T}}$4 values narrow the filter’s bandwidth, enhancing robustness to rotational acceleration but at the expense of convergence rate.

5. Experimental Validation and Practical Tuning

Experiments involving an autonomous e-scooter with reaction wheel roll stabilization demonstrated the theoretical impact of non-minimum-phase zeros (Brändle et al., 6 Feb 2025). With two IMUs ($r = [r_0, r_1, r_2, r_3]^{\mathsf{T}}$5 m, $r = [r_0, r_1, r_2, r_3]^{\mathsf{T}}$6 m), tuning $r = [r_0, r_1, r_2, r_3]^{\mathsf{T}}$7 from $r = [r_0, r_1, r_2, r_3]^{\mathsf{T}}$8 to $r = [r_0, r_1, r_2, r_3]^{\mathsf{T}}$9:

• Eliminated oscillations with high-$$p \otimes r = \begin{bmatrix} p_0 r_0 - p_1 r_1 - p_2 r_2 - p_3 r_3 \ p_0 r_1 + p_1 r_0 + p_2 r_3 - p_3 r_2 \ p_0 r_2 - p_1 r_3 + p_2 r_0 + p_3 r_1 \ p_0 r_3 + p_1 r_2 - p_2 r_1 + p_3 r_0 \end{bmatrix}$$0 mounting
• Decreased steady-state roll error from $$p \otimes r = \begin{bmatrix} p_0 r_0 - p_1 r_1 - p_2 r_2 - p_3 r_3 \ p_0 r_1 + p_1 r_0 + p_2 r_3 - p_3 r_2 \ p_0 r_2 - p_1 r_3 + p_2 r_0 + p_3 r_1 \ p_0 r_3 + p_1 r_2 - p_2 r_1 + p_3 r_0 \end{bmatrix}$$1 to $$p \otimes r = \begin{bmatrix} p_0 r_0 - p_1 r_1 - p_2 r_2 - p_3 r_3 \ p_0 r_1 + p_1 r_0 + p_2 r_3 - p_3 r_2 \ p_0 r_2 - p_1 r_3 + p_2 r_0 + p_3 r_1 \ p_0 r_3 + p_1 r_2 - p_2 r_1 + p_3 r_0 \end{bmatrix}$$2
• Reduced estimator bandwidth from $$p \otimes r = \begin{bmatrix} p_0 r_0 - p_1 r_1 - p_2 r_2 - p_3 r_3 \ p_0 r_1 + p_1 r_0 + p_2 r_3 - p_3 r_2 \ p_0 r_2 - p_1 r_3 + p_2 r_0 + p_3 r_1 \ p_0 r_3 + p_1 r_2 - p_2 r_1 + p_3 r_0 \end{bmatrix}$$3 Hz to $$p \otimes r = \begin{bmatrix} p_0 r_0 - p_1 r_1 - p_2 r_2 - p_3 r_3 \ p_0 r_1 + p_1 r_0 + p_2 r_3 - p_3 r_2 \ p_0 r_2 - p_1 r_3 + p_2 r_0 + p_3 r_1 \ p_0 r_3 + p_1 r_2 - p_2 r_1 + p_3 r_0 \end{bmatrix}$$4 Hz
• Slowed convergence by a factor of five

Monte Carlo simulations and Vicon-groundtruth testing indicate roll/pitch/yaw RMSEs $$p \otimes r = \begin{bmatrix} p_0 r_0 - p_1 r_1 - p_2 r_2 - p_3 r_3 \ p_0 r_1 + p_1 r_0 + p_2 r_3 - p_3 r_2 \ p_0 r_2 - p_1 r_3 + p_2 r_0 + p_3 r_1 \ p_0 r_3 + p_1 r_2 - p_2 r_1 + p_3 r_0 \end{bmatrix}$$5 across realistic inertial signal conditions (Kok et al., 2019).

6. Implementation and Computational Efficiency

Madgwick’s filter, in its original form, estimates the full quaternion update (218 arithmetic operations per step) (Kok et al., 2019). Improved implementations estimate only the 3D corrective angular velocity, reducing computational load to $$p \otimes r = \begin{bmatrix} p_0 r_0 - p_1 r_1 - p_2 r_2 - p_3 r_3 \ p_0 r_1 + p_1 r_0 + p_2 r_3 - p_3 r_2 \ p_0 r_2 - p_1 r_3 + p_2 r_0 + p_3 r_1 \ p_0 r_3 + p_1 r_2 - p_2 r_1 + p_3 r_0 \end{bmatrix}$$6 operations—reportedly a $$p \otimes r = \begin{bmatrix} p_0 r_0 - p_1 r_1 - p_2 r_2 - p_3 r_3 \ p_0 r_1 + p_1 r_0 + p_2 r_3 - p_3 r_2 \ p_0 r_2 - p_1 r_3 + p_2 r_0 + p_3 r_1 \ p_0 r_3 + p_1 r_2 - p_2 r_1 + p_3 r_0 \end{bmatrix}$$7 decrease—particularly beneficial for low-power embedded targets.

Pseudocode for one-step gradient descent update:

$\phi$8

All tuning is subsumed within $$p \otimes r = \begin{bmatrix} p_0 r_0 - p_1 r_1 - p_2 r_2 - p_3 r_3 \ p_0 r_1 + p_1 r_0 + p_2 r_3 - p_3 r_2 \ p_0 r_2 - p_1 r_3 + p_2 r_0 + p_3 r_1 \ p_0 r_3 + p_1 r_2 - p_2 r_1 + p_3 r_0 \end{bmatrix}$$8 and the reference vectors $$p \otimes r = \begin{bmatrix} p_0 r_0 - p_1 r_1 - p_2 r_2 - p_3 r_3 \ p_0 r_1 + p_1 r_0 + p_2 r_3 - p_3 r_2 \ p_0 r_2 - p_1 r_3 + p_2 r_0 + p_3 r_1 \ p_0 r_3 + p_1 r_2 - p_2 r_1 + p_3 r_0 \end{bmatrix}$$9.

7. Guidelines and Best Practices

Recommendations for practitioners (Brändle et al., 6 Feb 2025):

1. Place accelerometers as close to the rotation axis as possible to minimize $\phi$0 and thus push problematic zeros out of the relevant frequency band.
2. Calculate dominant operating point $\phi$1; compute zeros $\phi$2. Choose $\phi$3 such that the filter cutoff is well below $\phi$4.
3. Gradually decrease $\phi$5 to suppress closed-loop oscillations, but avoid excessive reduction that unduly degrades transient response.
4. For demanding dynamic regimes, use model-based compensation leveraging multiple IMUs, knowledge of the lever arm $\phi$6, and direct $\phi$7 estimation.
5. Account for combined estimator–controller dynamics; “separation principle” does not reliably apply due to attitude-dependent accelerometer measurements.

Madgwick-style orientation filters provide effective orientation estimation when tuned in accordance with underlying system dynamics and sensor montage. Their simplicity and real-time tractability can be preserved under substantial motion acceleration by bandwidth management and model-based compensation strategies.