# Why does AHRS system with Madgwick filter has so big drift?

I have an MPU9250 on my PCB and I use an AHRS system with a Madgwick filter to get yaw angles. I want to get very accurate and non-drifting yaw angles, but, for unknown reasons, I can't.

I calibrated the magnetometer, so I don't think that is the problem. The yaw angle drifts by about 10 degrees every 3 seconds. Here is the filter:

void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
float recipNorm;
float s0, s1, s2, s3;
float qDot1, qDot2, qDot3, qDot4;
float hx, hy;
float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;

// Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
MadgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az);
return;
}

// Rate of change of quaternion from gyroscope
qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz);
qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy);
qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx);
qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx);

// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {

// Normalise accelerometer measurement
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;

// Normalise magnetometer measurement
recipNorm = invSqrt(mx * mx + my * my + mz * mz);
mx *= recipNorm;
my *= recipNorm;
mz *= recipNorm;

// Auxiliary variables to avoid repeated arithmetic
_2q0mx = 2.0f * q0 * mx;
_2q0my = 2.0f * q0 * my;
_2q0mz = 2.0f * q0 * mz;
_2q1mx = 2.0f * q1 * mx;
_2q0 = 2.0f * q0;
_2q1 = 2.0f * q1;
_2q2 = 2.0f * q2;
_2q3 = 2.0f * q3;
_2q0q2 = 2.0f * q0 * q2;
_2q2q3 = 2.0f * q2 * q3;
q0q0 = q0 * q0;
q0q1 = q0 * q1;
q0q2 = q0 * q2;
q0q3 = q0 * q3;
q1q1 = q1 * q1;
q1q2 = q1 * q2;
q1q3 = q1 * q3;
q2q2 = q2 * q2;
q2q3 = q2 * q3;
q3q3 = q3 * q3;

// Reference direction of Earth's magnetic field
hx = mx * q0q0 - _2q0my * q3 + _2q0mz * q2 + mx * q1q1 + _2q1 * my * q2 + _2q1 * mz * q3 - mx * q2q2 - mx * q3q3;
hy = _2q0mx * q3 + my * q0q0 - _2q0mz * q1 + _2q1mx * q2 - my * q1q1 + my * q2q2 + _2q2 * mz * q3 - my * q3q3;
_2bx = sqrt(hx * hx + hy * hy);
_2bz = -_2q0mx * q2 + _2q0my * q1 + mz * q0q0 + _2q1mx * q3 - mz * q1q1 + _2q2 * my * q3 - mz * q2q2 + mz * q3q3;
_4bx = 2.0f * _2bx;
_4bz = 2.0f * _2bz;

// Gradient decent algorithm corrective step
s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q2 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q3 + _2bz * q1) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q2 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q3 * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q2 + _2bz * q0) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q3 - _4bz * q1) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q2 - _2bz * q0) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q1 + _2bz * q3) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q0 - _4bz * q2) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q3 + _2bz * q1) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q0 + _2bz * q2) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q1 * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
s0 *= recipNorm;
s1 *= recipNorm;
s2 *= recipNorm;
s3 *= recipNorm;

// Apply feedback step
qDot1 -= beta * s0;
qDot2 -= beta * s1;
qDot3 -= beta * s2;
qDot4 -= beta * s3;
}

// Integrate rate of change of quaternion to yield quaternion
q0 += qDot1 * (1.0f / sampleFreq);
q1 += qDot2 * (1.0f / sampleFreq);
q2 += qDot3 * (1.0f / sampleFreq);
q3 += qDot4 * (1.0f / sampleFreq);

// Normalise quaternion
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
}


This is how I use the filter:

void Process_IMU()
{
uint8_t data[14];
uint8_t reg = ACCEL_XOUT_H;

/*-------- Accel ---------*/
Accel_x = (int16_t)((int16_t)( data[0] << 8 ) | data[1]);
Accel_y = (int16_t)((int16_t)( data[2] << 8 ) | data[3]);
Accel_z = (int16_t)((int16_t)( data[4] << 8 ) | data[5]);

/*-------- Gyrometer --------*/
Gyro_x = (int16_t)((int16_t)( data[8] << 8  ) | data[9]);
Gyro_y = (int16_t)((int16_t)( data[10] << 8 ) | data[11]);
Gyro_z = (int16_t)((int16_t)( data[12] << 8 ) | data[13]);

Accel_X = 10.0*(float)((int32_t)Accel_x - Accel_x_bias)/(float)accel_sensitivity;
Accel_Y = 10.0*(float)((int32_t)Accel_y - Accel_y_bias)/(float)accel_sensitivity;
Accel_Z = 10.0*(float)((int32_t)Accel_z - Accel_z_bias)/(float)accel_sensitivity;

Gyro_X =  (float)(((int32_t)Gyro_x - Gyro_x_bias)/(float)gyro_sensitivity)*M_PI/180.0f;
Gyro_Y =  (float)(((int32_t)Gyro_y - Gyro_y_bias)/(float)gyro_sensitivity)*M_PI/180.0f;
Gyro_Z =  (float)(((int32_t)Gyro_z - Gyro_z_bias)/(float)gyro_sensitivity)*M_PI/180.0f;

// Get data of Magnetometer
Get_magnetometer();

}


The Get_magnetometer function:

void Get_magnetometer()
{
uint8_t raw_data[7];
uint8_t reg_ST1 = ST1;
uint8_t reg = XOUT_L;
if (raw_data[0] & 0x01)
{
// Read the six raw data and ST2 registers sequentially into data arra
if(!(raw_data[6] & 0x08))// Check if magnetic sensor overflow set, if not then report data
{
Mag_x = (int16_t)((raw_data[1]<<8) | raw_data[0] );
Mag_y = (int16_t)((raw_data[3]<<8) | raw_data[2] );
Mag_z = (int16_t)((raw_data[5]<<8) | raw_data[4] );
}

Mag_X_calib = (float)Mag_x * asax * mag_sensitivity - mag_offset[0];
Mag_Y_calib = (float)Mag_y * asay * mag_sensitivity - mag_offset[1];
Mag_Z_calib = (float)Mag_z * asaz * mag_sensitivity - mag_offset[2];

Mag_X_calib *=scale_x;
Mag_Y_calib *=scale_y;
Mag_Z_calib *=scale_z;

}
}


This is how I calculate the Euler angles:

 while (1)
{
Process_IMU();

q[0] = q0;
q[1] = q1;
q[2] = q2;
q[3] = q3;
a12 =   2.0f * (q[1] * q[2] + q[0] * q[3]);
a22 =   q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3];
a31 =   2.0f * (q[0] * q[1] + q[2] * q[3]);
a32 =   2.0f * (q[1] * q[3] - q[0] * q[2]);
a33 =   q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3];
float sinp = a32;
if (abs(sinp) >= 1)
pitch = copysign(M_PI/2,sinp);
else
pitch =  asin(sinp);
//pitch = -asinf(a32);
roll  = atan2f(a31, a33);
yaw   = atan2f(a12, a22);
pitch *= 180.0f / pi;
yaw = atan2f(sinf(yaw),cosf(yaw));
yaw   *= 180.0f / pi;
roll  *= 180.0f / pi;

HAL_Delay(10);
/* USER CODE END WHILE */

/* USER CODE BEGIN 3 */
}


This is how the yaw angle changes:

What could be the reason of this? What should I change or calibrate?
I took almost all the functions from here: https://github.com/sonphambk/MPU9250
EDIT 1: I rewrote a python library for MPU9250 to C (the python library: https://github.com/morgil/madgwick_py):

void mulMat(double mat1[][C1], double mat2[][C2], double mat3[R1][C2]) {
for (int i = 0; i < R1; i++) {
for (int j = 0; j < C2; j++) {
mat3[i][j] = 0;

for (int k = 0; k < R2; k++) {
mat3[i][j] += mat1[i][k] * mat2[k][j];
}
}
}
}

void MadgwickAHRSupdatePy(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
{
float norm;

// Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
MadgwickAHRSupdateIMU(gx, gy, gz, ax, ay, az);
return;
}

// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f)) && !((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f))) {
norm = sqrt(ax * ax + ay * ay + az * az);
ax /= norm;
ay /= norm;
az /= norm;

norm = sqrt(mx * mx + my * my + mz * mz);
mx /= norm;
my /= norm;
mz /= norm;

Quaternion h, qConj, qMag;
Quaternion_set(0, mx, my, mz, &qMag);
Quaternion_conjugate(&selfQ, &qConj);

Quaternion_multiply(&qMag, &qConj, &h);
Quaternion_multiply(&selfQ, &h, &h);

double b[4] = {0, sqrt(h.v[0] * h.v[0] + h.v[1] * h.v[1]), 0, h.v[2]};
double q_ins[4] = {selfQ.w, selfQ.v[0], selfQ.v[1], selfQ.v[2]};

double f[1][6] = {{
2 * (q_ins[1] * q_ins[3] - q_ins[0] * q_ins[2]) - ax,
2 * (q_ins[0] * q_ins[1] + q_ins[2] * q_ins[3]) - ay,
2 * (0.5 - q_ins[1] * q_ins[1] - q_ins[2] * q_ins[2]) - az,
2 * b[1] * (0.5 - q_ins[2] * q_ins[2] - q_ins[3] * q_ins[3]) + 2 * b[3] * (q_ins[1] * q_ins[3] - q_ins[0] * q_ins[2]) - mx,
2 * b[1] * (q_ins[1] * q_ins[2] - q_ins[0] * q_ins[3]) + 2 * b[3] * (q_ins[0] * q_ins[1] + q_ins[2] * q_ins[3]) - my,
2 * b[1] * (q_ins[0] * q_ins[2] + q_ins[1] * q_ins[3]) + 2 * b[3] * (0.5 - q_ins[1] * q_ins[1] - q_ins[2] * q_ins[2]) - mz
}};
double j[6][4] = {
{-2 * q_ins[2], 2 * q_ins[3], -2 * q_ins[0], 2 * q_ins[1]},
{2 * q_ins[1], 2 * q_ins[0], 2 * q_ins[3], 2 * q_ins[2]},
{0, -4 * q_ins[1], -4 * q_ins[2], 0},
{-2 * b[3] * q_ins[2], 2 * b[3] * q_ins[3], -4 * b[1] * q_ins[2] - 2 * b[3] * q_ins[0], -4 * b[1] * q_ins[3] + 2 * b[3] * q_ins[1]},
{-2 * b[1] * q_ins[3] + 2 * b[3] * q_ins[1], 2 * b[1] * q_ins[2] + 2 * b[3] * q_ins[0], 2 * b[1] * q_ins[1] + 2 * b[3] * q_ins[3],  -2 * b[1] * q_ins[0] + 2 * b[3] * q_ins[2]},
{2 * b[1] * q_ins[2], 2 * b[1] * q_ins[3] - 4 * b[3] * q_ins[1], 2 * b[1] * q_ins[0] - 4 * b[3] * q_ins[2],  2 * b[1] * q_ins[1]}
};
double step2d[1][4];
mulMat(f, j, step2d);

double s1 = step2d[0][0];
double s2 = step2d[0][1];
double s3 = step2d[0][2];
double s4 = step2d[0][3];
norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4);
s1 /= norm;
s2 /= norm;
s3 /= norm;
s4 /= norm;

Quaternion qGyro;
Quaternion_set(0, gx, gy, gz, &qGyro);

Quaternion_multiply(&qGyro, &selfQ, &h);
double qDot1 = (h.w * 0.5 - beta * s1) * (1 / sampleFreq);
double qDot2 = (h.v[0] * 0.5 - beta * s2) * (1 / sampleFreq);
double qDot3 = (h.v[1] * 0.5 - beta * s3) * (1 / sampleFreq);
double qDot4 = (h.v[2] * 0.5 - beta * s4) * (1 / sampleFreq);

q_ins[0] += qDot1;
q_ins[1] += qDot2;
q_ins[2] += qDot3;
q_ins[3] += qDot4;

norm = sqrt(q_ins[0] * q_ins[0] + q_ins[1] * q_ins[1] + q_ins[2] * q_ins[2] + q_ins[3] * q_ins[3]);
q_ins[0] /= norm;
q_ins[1] /= norm;
q_ins[2] /= norm;
q_ins[3] /= norm;

q0 = q_ins[0];
q1 = q_ins[1];
q2 = q_ins[2];
q3 = q_ins[3];

Quaternion_set(q_ins[0], q_ins[1], q_ins[2], q_ins[3], &selfQ);
}

}


But the result is the same. The yaw angle goes away very fast.

• Why don't you simply use the original code? The another fact difficult to understand is that you copy already existent optimized values q0, q1,...to an array, then using a hardcoded index. Did you ever try to disassemble the resultant machine code? I guess no, because you would probably avoid adding this useless overhead. Dec 2, 2021 at 10:14
• Did you take into account that embedded sensors may have different coordinate system orientation? see image here [Edited by a moderator.] Dec 2, 2021 at 10:41
• @MarkoBuršič uh, sorry. I did this because I wanted to debug the code, I know that I shouldn't do this, but it is just for debug. With the original code results are the same. Dec 2, 2021 at 10:50
• @MarkoBuršič Re: "Did you take into account that embedded sensors may have different coordinate system orientation?" Yes, as you can see here MadgwickAHRSupdate(Gyro_X,Gyro_Y,Gyro_Z,Accel_X,Accel_Y,Accel_Z,Mag_Y_calib,Mag_X_calib,-Mag_Z_calib); [Edited by a moderator.] Dec 2, 2021 at 10:50
• But you also have some mistakes: 1.) 10*(float)((int32_t)Accel_x - Accel_x_bias)/(float)accel_sensitivity; - 10.0 not 10, if you use fixed gain aka sensitvity, you should multiply there, you do spare 1 float MUL operation. The same for others: *M_PI/180.0f; 2nd mistake: the AHRS update shall be called at deterministic time....and so on. Try to run without calib values, use raw instead. This version is obsolete, there is a new version of Magdwick alsorithm. Dec 2, 2021 at 12:21