Various types of filters can be used to estimate the orientation of a body based on IMUs measurements. The most commonly used filters, listed in order of increasing computational complexity, are complementary filters, Madgwick and Mahony filters, and Kalman filters.
In 2010, S. Madgwick proposed a computationally efficient orientation filter, unlike the more resource-intensive Kalman filter. This filter employs a quaternion representation, enabling the use of accelerometer and magnetometer data in an analytically derived and optimized gradient-descent algorithm. This approach computes the direction of the gyroscope measurement error as a quaternion derivative. The main advantages of this method include: (i) high efficiency at low sampling rates and (ii) performance comparable to that of Kalman filtering.
Here is the link to S. Madgwick’s work, which contains the analytical description of the filter.
The following is an implementation of the orientation filter for an IMU, written in C [1]. Specifically, the code has been optimized to minimize the required number of arithmetic operations at the expense of data memory, requiring only 109 scalar arithmetic operations and 40 bytes of data memory for global variables, along with 100 bytes of data memory for local variables during each filter update [1].
#include <math.h>
#define deltat 0.001f
#define gyroMeasError 3.14159265358979f * (5.0f / 180.0f)
#define beta sqrt(3.0f / 4.0f) * gyroMeasError
float a_x, a_y, a_z;
float w_x, w_y, w_z;
float SEq_1 = 1.0f, SEq_2 = 0.0f, SEq_3 = 0.0f, SEq_4 = 0.0f;
void filterUpdate(float w_x, float w_y, float w_z, float a_x, float a_y, float a_z)
{
float norm;
float SEqDot_omega_1, SEqDot_omega_2, SEqDot_omega_3, SEqDot_omega_4;
float f_1, f_2, f_3;
float J_11or24, J_12or23, J_13or22, J_14or21, J_32, J_33;
float SEqHatDot_1, SEqHatDot_2, SEqHatDot_3, SEqHatDot_4;
float halfSEq_1 = 0.5f * SEq_1;
float halfSEq_2 = 0.5f * SEq_2;
float halfSEq_3 = 0.5f * SEq_3;
float halfSEq_4 = 0.5f * SEq_4;
float twoSEq_1 = 2.0f * SEq_1;
float twoSEq_2 = 2.0f * SEq_2;
float twoSEq_3 = 2.0f * SEq_3;
norm = sqrt(a_x * a_x + a_y * a_y + a_z * a_z);
a_x /= norm;
a_y /= norm;
a_z /= norm;
f_1 = twoSEq_2 * SEq_4 - twoSEq_1 * SEq_3 - a_x;
f_2 = twoSEq_1 * SEq_2 + twoSEq_3 * SEq_4 - a_y;
f_3 = 1.0f - twoSEq_2 * SEq_2 - twoSEq_3 * SEq_3 - a_z;
J_11or24 = twoSEq_3;
J_12or23 = 2.0f * SEq_4;
J_13or22 = twoSEq_1;
J_14or21 = twoSEq_2;
J_32 = 2.0f * J_14or21;
J_33 = 2.0f * J_11or24;
SEqHatDot_1 = J_14or21 * f_2 - J_11or24 * f_1;
SEqHatDot_2 = J_12or23 * f_1 + J_13or22 * f_2 - J_32 * f_3;
SEqHatDot_3 = J_12or23 * f_2 - J_33 * f_3 - J_13or22 * f_1;
SEqHatDot_4 = J_14or21 * f_1 + J_11or24 * f_2;
norm = sqrt(SEqHatDot_1 * SEqHatDot_1 + SEqHatDot_2 * SEqHatDot_2 + SEqHatDot_3 * SEqHatDot_3 + SEqHatDot_4 * SEqHatDot_4);
SEqHatDot_1 /= norm;
SEqHatDot_2 /= norm;
SEqHatDot_3 /= norm;
SEqHatDot_4 /= norm;
SEqDot_omega_1 = -halfSEq_2 * w_x - halfSEq_3 * w_y - halfSEq_4 * w_z;
SEqDot_omega_2 = halfSEq_1 * w_x + halfSEq_3 * w_z - halfSEq_4 * w_y;
SEqDot_omega_3 = halfSEq_1 * w_y - halfSEq_2 * w_z + halfSEq_4 * w_x;
SEqDot_omega_4 = halfSEq_1 * w_z + halfSEq_2 * w_y - halfSEq_3 * w_x;
SEq_1 += (SEqDot_omega_1 - (beta * SEqHatDot_1)) * deltat;
SEq_2 += (SEqDot_omega_2 - (beta * SEqHatDot_2)) * deltat;
SEq_3 += (SEqDot_omega_3 - (beta * SEqHatDot_3)) * deltat;
SEq_4 += (SEqDot_omega_4 - (beta * SEqHatDot_4)) * deltat;
norm = sqrt(SEq_1 * SEq_1 + SEq_2 * SEq_2 + SEq_3 * SEq_3 + SEq_4 * SEq_4);
SEq_1 /= norm;
SEq_2 /= norm;
SEq_3 /= norm;
SEq_4 /= norm;
}[1] An efficient orientation filter for inertial and inertial/magnetic sensor arrays, Sebastian O.H. Madgwick, April 30, 2010