Mahony Filter - Orientation Estimation

Dec 12, 2024 · 6 min read · filters mahony imu orientation quaternions sensor-fusion pi-controller ·

Share on:

Comprehensive guide to the Mahony filter - a complementary filter with PI feedback for IMU orientation estimation.

Overview

The Mahony filter is an orientation estimation algorithm that uses a complementary filter approach with PI (Proportional-Integral) feedback to fuse:

Gyroscope (angular velocity)
Accelerometer (gravity direction)
Magnetometer (magnetic north) - optional

Key Innovation: Uses cross-product error between measured and estimated directions, corrected with PI controller.

Core Concept

Complementary Filter

High-pass filter gyroscope (no drift accumulation) + Low-pass filter accelerometer (smooth out noise):

$$ \text{Orientation} = \alpha \cdot \text{Gyro} + (1-\alpha) \cdot \text{Accel/Mag} $$

PI Controller

Proportional term: Immediate correction Integral term: Compensates for gyroscope bias over time

$$ \omega_{corrected} = \omega_{gyro} + K_p \cdot e + K_i \cdot \int e , dt $$

Algorithm Steps

Step 1: Calculate Error

Compute cross-product between measured and estimated directions:

Accelerometer Error:

$$ \mathbf{e}a = \hat{\mathbf{g}}^b \times \mathbf{a}{measured} $$

where $\hat{\mathbf{g}}^b$ is the estimated gravity direction in body frame.

Magnetometer Error (if available):

$$ \mathbf{e}m = \hat{\mathbf{b}}^b \times \mathbf{m}{measured} $$

Total Error:

$$ \mathbf{e} = \mathbf{e}_a + \mathbf{e}_m $$

Step 2: PI Correction

Proportional:

$$ \omega_p = K_p \cdot \mathbf{e} $$

Integral (bias compensation):

$$ \mathbf{b}_{k+1} = \mathbf{b}_k + K_i \cdot \mathbf{e} \cdot \Delta t $$

$$ \omega_i = \mathbf{b} $$

Corrected Angular Velocity:

$$ \omega_{corrected} = \omega_{gyro} + \omega_p + \omega_i $$

Step 3: Quaternion Integration

$$ \dot{\mathbf{q}} = \frac{1}{2} \mathbf{q} \otimes \begin{bmatrix} 0 \ \omega_{corrected} \end{bmatrix} $$

$$ \mathbf{q}_{k+1} = \mathbf{q}_k + \dot{\mathbf{q}} \Delta t $$

Normalize: $\mathbf{q}{k+1} = \frac{\mathbf{q}{k+1}}{|\mathbf{q}_{k+1}|}$

Visual Explanation

Interactive 3D Visualization

Mathematical Details

Error Calculation

The cross product between estimated and measured directions gives the rotation axis:

$$ \mathbf{e} = \hat{\mathbf{v}} \times \mathbf{v}_{measured} $$

Magnitude: Proportional to sine of angle between vectors Direction: Axis of rotation needed to align them

PI Controller

Proportional Term ($K_p$):

$$ \omega_p = K_p \cdot \mathbf{e} $$

Immediate correction
Higher $K_p$ = faster response, more noise

Integral Term ($K_i$):

$$ \mathbf{b}_{k+1} = \mathbf{b}_k + K_i \cdot \mathbf{e} \cdot \Delta t $$

Accumulates error over time
Compensates for gyroscope bias
Higher $K_i$ = better bias compensation, slower convergence

Corrected Angular Velocity

$$ \boldsymbol{\omega}{corrected} = \boldsymbol{\omega}{gyro} + K_p \cdot \mathbf{e} + \mathbf{b} $$

Parameter Tuning

Kp (Proportional Gain)

Effect: Controls response speed to orientation errors

High Kp (5-10): Fast correction, sensitive to noise
Medium Kp (1-3): Balanced (recommended)
Low Kp (0.1-0.5): Slow correction, smooth output

Rule of thumb: $K_p \approx 2 \times \text{expected_angular_velocity}$

Ki (Integral Gain)

Effect: Controls gyroscope bias compensation

High Ki (0.1-1): Fast bias compensation, can overshoot
Medium Ki (0.01-0.05): Balanced (recommended)
Low Ki (0.001-0.01): Slow bias compensation, stable
Zero Ki: No bias compensation (like complementary filter)

Rule of thumb: $K_i \approx 0.1 \times K_p$

Python Implementation

 1import numpy as np
 2
 3class MahonyFilter:
 4    def __init__(self, Kp=2.0, Ki=0.01, sample_period=0.01):
 5        self.Kp = Kp
 6        self.Ki = Ki
 7        self.sample_period = sample_period
 8        self.q = np.array([1.0, 0.0, 0.0, 0.0])  # w, x, y, z
 9        self.integral_error = np.array([0.0, 0.0, 0.0])
10    
11    def update(self, gyro, accel, mag=None):
12        """
13        gyro: [gx, gy, gz] in rad/s
14        accel: [ax, ay, az] in m/s^2
15        mag: [mx, my, mz] (optional)
16        """
17        q = self.q
18        
19        # Normalize accelerometer
20        accel_norm = np.linalg.norm(accel)
21        if accel_norm == 0:
22            return self.q
23        accel = accel / accel_norm
24        
25        # Estimated gravity direction in body frame
26        vx = 2 * (q[1]*q[3] - q[0]*q[2])
27        vy = 2 * (q[0]*q[1] + q[2]*q[3])
28        vz = q[0]**2 - q[1]**2 - q[2]**2 + q[3]**2
29        
30        # Error: cross product
31        ex = vy * accel[2] - vz * accel[1]
32        ey = vz * accel[0] - vx * accel[2]
33        ez = vx * accel[1] - vy * accel[0]
34        
35        # Integral feedback
36        if self.Ki > 0:
37            self.integral_error += np.array([ex, ey, ez]) * self.sample_period
38        
39        # Corrected gyroscope
40        gyro_corrected = gyro + self.Kp * np.array([ex, ey, ez]) + self.integral_error
41        
42        # Quaternion derivative
43        qDot = 0.5 * self.quaternion_multiply(q, [0, *gyro_corrected])
44        
45        # Integrate
46        self.q = self.q + qDot * self.sample_period
47        
48        # Normalize
49        self.q = self.q / np.linalg.norm(self.q)
50        
51        return self.q
52    
53    def quaternion_multiply(self, q1, q2):
54        w1, x1, y1, z1 = q1
55        w2, x2, y2, z2 = q2
56        return np.array([
57            w1*w2 - x1*x2 - y1*y2 - z1*z2,
58            w1*x2 + x1*w2 + y1*z2 - z1*y2,
59            w1*y2 - x1*z2 + y1*w2 + z1*x2,
60            w1*z2 + x1*y2 - y1*x2 + z1*w2
61        ])
62    
63    def get_euler(self):
64        """Convert quaternion to Euler angles"""
65        w, x, y, z = self.q
66        
67        # Roll
68        sinr_cosp = 2 * (w * x + y * z)
69        cosr_cosp = 1 - 2 * (x**2 + y**2)
70        roll = np.arctan2(sinr_cosp, cosr_cosp)
71        
72        # Pitch
73        sinp = 2 * (w * y - z * x)
74        pitch = np.arcsin(np.clip(sinp, -1, 1))
75        
76        # Yaw
77        siny_cosp = 2 * (w * z + x * y)
78        cosy_cosp = 1 - 2 * (y**2 + z**2)
79        yaw = np.arctan2(siny_cosp, cosy_cosp)
80        
81        return roll, pitch, yaw
82    
83    def reset_integral(self):
84        """Reset integral error (useful after large disturbances)"""
85        self.integral_error = np.array([0.0, 0.0, 0.0])
86
87# Usage
88filter = MahonyFilter(Kp=2.0, Ki=0.01, sample_period=0.01)
89
90# Simulate IMU data
91gyro = np.array([0.1, 0.0, 0.0])  # rad/s
92accel = np.array([0.0, 0.0, 9.81])  # m/s^2
93
94for _ in range(1000):
95    q = filter.update(gyro, accel)
96    roll, pitch, yaw = filter.get_euler()
97    print(f"Roll: {np.degrees(roll):.1f}°, Pitch: {np.degrees(pitch):.1f}°, Yaw: {np.degrees(yaw):.1f}°")

Comparison: Mahony vs Madgwick

Feature	Mahony	Madgwick
Approach	Complementary filter + PI	Gradient descent
Parameters	Kp, Ki (2 params)	β (1 param)
Bias Compensation	Yes (integral term)	No
Computational Cost	Lower	Slightly higher
Tuning Complexity	Moderate	Easier
Convergence	Faster with good Ki	Depends on β
Stability	Very stable	Stable

Advantages

Gyroscope bias compensation - Integral term removes drift
Computationally efficient - No matrix operations
Stable - PI controller is well-understood
Flexible - Can disable Ki for pure complementary filter
Real-time capable - Fast enough for embedded systems

Limitations

Two parameters - Kp and Ki need tuning (vs Madgwick's single β)
Assumes constant gravity - Doesn't work during acceleration
Integral windup - Ki can accumulate error during prolonged disturbances
Yaw drift - Without magnetometer, yaw drifts over time

Applications

Drones/Quadcopters - Attitude control with bias compensation
Robotics - IMU-based navigation
VR/AR - Head tracking with drift correction
Wearables - Activity and motion tracking
Stabilization - Camera gimbals, platforms

Best Practices

Start with Kp only - Set Ki=0, tune Kp for desired response
Add Ki gradually - Increase Ki until bias is compensated
Monitor integral error - Reset if it grows too large
Calibrate sensors - Remove static bias before filtering
Handle saturation - Limit integral error accumulation
Test with motion - Verify performance during actual use

Related Snippets

Filter Design Principles
Choosing and designing digital filters
FIR Filters
Finite Impulse Response digital filters
IIR Filters
Infinite Impulse Response digital filters
Kalman Filter
Optimal state estimation for linear systems
Madgwick Filter - Orientation Estimation
Gradient descent orientation filter for IMU sensor fusion
Median Filter
Non-linear filtering for noise reduction
Sensor Fusion with Kalman Filters
Combining multiple sensors using Kalman filtering

Mahony Filter - Orientation Estimation

Overview

Core Concept

Complementary Filter

PI Controller

Algorithm Steps

Step 1: Calculate Error

Step 2: PI Correction

Step 3: Quaternion Integration

Visual Explanation

Interactive 3D Visualization

Mathematical Details

Error Calculation

PI Controller

Corrected Angular Velocity

Parameter Tuning

Kp (Proportional Gain)

Ki (Integral Gain)

Python Implementation

Comparison: Mahony vs Madgwick

Advantages

Limitations

Applications

Best Practices

Further Reading

Related Snippets