Madgwick Filter - Orientation Estimation

calendarDec 12, 2024 · 5 min read · filters madgwick imu orientation quaternions sensor-fusion  ·
Share on: linkedincopy

Comprehensive guide to the Madgwick filter - an efficient gradient descent algorithm for IMU orientation estimation.

Overview

The Madgwick filter is an orientation estimation algorithm that fuses:

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

Key Innovation: Uses gradient descent to find the orientation that best aligns sensor measurements with expected values.

Quaternion Representation

Orientation is represented as a unit quaternion:

$$ \mathbf{q} = \begin{bmatrix} q_0 \ q_1 \ q_2 \ q_3 \end{bmatrix} = \begin{bmatrix} q_w \ q_x \ q_y \ q_z \end{bmatrix} $$

Constraint: $|\mathbf{q}| = \sqrt{q_0^2 + q_1^2 + q_2^2 + q_3^2} = 1$

Why Quaternions?

  • No gimbal lock (unlike Euler angles)
  • Efficient (4 parameters vs 9 for rotation matrix)
  • Smooth interpolation (SLERP)
  • Easy composition (quaternion multiplication)

Algorithm Steps

Step 1: Gyroscope Integration

Predict orientation from angular velocity:

$$ \dot{\mathbf{q}}_\omega = \frac{1}{2} \mathbf{q} \otimes \begin{bmatrix} 0 \ \omega_x \ \omega_y \ \omega_z \end{bmatrix} $$

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

Step 2: Gradient Descent Correction

Find the orientation that minimizes the error between:

  • Measured accelerometer/magnetometer
  • Expected values in body frame

Objective Function (accelerometer only):

$$ f(\mathbf{q}, \mathbf{a}) = \begin{bmatrix} 2(q_1 q_3 - q_0 q_2) - a_x \ 2(q_0 q_1 + q_2 q_3) - a_y \ 2(\frac{1}{2} - q_1^2 - q_2^2) - a_z \end{bmatrix} $$

Gradient:

$$ \nabla f = J^T f $$

where $J$ is the Jacobian matrix.

Correction Step:

$$ \dot{\mathbf{q}}_\nabla = -\beta \frac{\nabla f}{|\nabla f|} $$

Step 3: Fusion

$$ \dot{\mathbf{q}} = \dot{\mathbf{q}}\omega + \dot{\mathbf{q}}\nabla $$

$$ \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 Orientation Visualization

Mathematical Details

Quaternion Kinematics

$$ \dot{\mathbf{q}} = \frac{1}{2} \mathbf{q} \otimes \boldsymbol{\omega} $$

where $\boldsymbol{\omega} = [0, \omega_x, \omega_y, \omega_z]^T$

Quaternion Multiplication

$$ \mathbf{q}1 \otimes \mathbf{q}2 = \begin{bmatrix} q{1w}q{2w} - q_{1x}q_{2x} - q_{1y}q_{2y} - q_{1z}q_{2z} \ q_{1w}q_{2x} + q_{1x}q_{2w} + q_{1y}q_{2z} - q_{1z}q_{2y} \ q_{1w}q_{2y} - q_{1x}q_{2z} + q_{1y}q_{2w} + q_{1z}q_{2x} \ q_{1w}q_{2z} + q_{1x}q_{2y} - q_{1y}q_{2x} + q_{1z}q_{2w} \end{bmatrix} $$

Gravity in Body Frame

Expected gravity direction in body frame:

$$ \mathbf{g}^b = \begin{bmatrix} 2(q_1 q_3 - q_0 q_2) \ 2(q_0 q_1 + q_2 q_3) \ 2(\frac{1}{2} - q_1^2 - q_2^2) \end{bmatrix} $$

Parameter Tuning

Beta (β)

Correction gain - controls how much to trust accelerometer vs gyroscope.

  • High β (0.5-1.0): Fast correction, more noise sensitivity
  • Medium β (0.1-0.3): Balanced (recommended)
  • Low β (0.01-0.05): Slow correction, smoother output

Rule of thumb: $\beta \approx \sqrt{3/4} \cdot \text{gyro_noise}$

Sample Rate

  • High rate (>100 Hz): More accurate integration
  • Low rate (<50 Hz): May miss fast motions

Python Implementation

 1import numpy as np
 2
 3class MadgwickFilter:
 4    def __init__(self, beta=0.1, sample_period=0.01):
 5        self.beta = beta
 6        self.sample_period = sample_period
 7        self.q = np.array([1.0, 0.0, 0.0, 0.0])  # w, x, y, z
 8    
 9    def update(self, gyro, accel, mag=None):
10        """
11        gyro: [gx, gy, gz] in rad/s
12        accel: [ax, ay, az] in m/s^2
13        mag: [mx, my, mz] (optional)
14        """
15        q = self.q
16        
17        # Normalize accelerometer
18        accel = accel / np.linalg.norm(accel)
19        
20        # Gyroscope quaternion derivative
21        qDot_omega = 0.5 * self.quaternion_multiply(q, [0, *gyro])
22        
23        # Gradient descent correction
24        f = np.array([
25            2*(q[1]*q[3] - q[0]*q[2]) - accel[0],
26            2*(q[0]*q[1] + q[2]*q[3]) - accel[1],
27            2*(0.5 - q[1]**2 - q[2]**2) - accel[2]
28        ])
29        
30        J = np.array([
31            [-2*q[2], 2*q[3], -2*q[0], 2*q[1]],
32            [2*q[1], 2*q[0], 2*q[3], 2*q[2]],
33            [0, -4*q[1], -4*q[2], 0]
34        ])
35        
36        gradient = J.T @ f
37        gradient = gradient / np.linalg.norm(gradient)
38        
39        qDot_correction = -self.beta * gradient
40        
41        # Fusion
42        qDot = qDot_omega + qDot_correction
43        
44        # Integrate
45        self.q = self.q + qDot * self.sample_period
46        
47        # Normalize
48        self.q = self.q / np.linalg.norm(self.q)
49        
50        return self.q
51    
52    def quaternion_multiply(self, q1, q2):
53        w1, x1, y1, z1 = q1
54        w2, x2, y2, z2 = q2
55        return np.array([
56            w1*w2 - x1*x2 - y1*y2 - z1*z2,
57            w1*x2 + x1*w2 + y1*z2 - z1*y2,
58            w1*y2 - x1*z2 + y1*w2 + z1*x2,
59            w1*z2 + x1*y2 - y1*x2 + z1*w2
60        ])
61    
62    def get_euler(self):
63        """Convert quaternion to Euler angles (roll, pitch, yaw)"""
64        w, x, y, z = self.q
65        
66        # Roll
67        sinr_cosp = 2 * (w * x + y * z)
68        cosr_cosp = 1 - 2 * (x**2 + y**2)
69        roll = np.arctan2(sinr_cosp, cosr_cosp)
70        
71        # Pitch
72        sinp = 2 * (w * y - z * x)
73        pitch = np.arcsin(np.clip(sinp, -1, 1))
74        
75        # Yaw
76        siny_cosp = 2 * (w * z + x * y)
77        cosy_cosp = 1 - 2 * (y**2 + z**2)
78        yaw = np.arctan2(siny_cosp, cosy_cosp)
79        
80        return roll, pitch, yaw
81
82# Usage
83filter = MadgwickFilter(beta=0.1, sample_period=0.01)
84
85# Simulate IMU data
86gyro = np.array([0.1, 0.0, 0.0])  # rad/s
87accel = np.array([0.0, 0.0, 9.81])  # m/s^2
88
89for _ in range(1000):
90    q = filter.update(gyro, accel)
91    roll, pitch, yaw = filter.get_euler()
92    print(f"Roll: {np.degrees(roll):.1f}°, Pitch: {np.degrees(pitch):.1f}°, Yaw: {np.degrees(yaw):.1f}°")

Advantages

  1. Computationally efficient - No matrix inversions
  2. No gimbal lock - Uses quaternions
  3. Handles missing sensors - Works with gyro+accel or gyro+accel+mag
  4. Single parameter - Only β needs tuning
  5. Real-time capable - Fast enough for embedded systems

Limitations

  1. Assumes constant gravity - Doesn't work during acceleration
  2. Magnetic interference - Magnetometer can be unreliable
  3. Drift in yaw - Without magnetometer, yaw drifts over time
  4. Parameter tuning - β needs adjustment for different applications

Applications

  • Drones/Quadcopters - Attitude estimation
  • VR/AR Headsets - Head tracking
  • Smartphones - Screen rotation, gaming
  • Wearables - Activity tracking
  • Robotics - IMU-based navigation

Further Reading

Related Snippets