# 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 angle. I want to get very accurate and non-drifting yaw angle, 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. I use CubeIDE to write firmware for the PCB. And I took the sources from here: https://github.com/sonphambk/MPU9250/tree/master/Src

This is how the yaw angle changes:

I also 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.
What could be the reason of this? What should I change or calibrate?

• Have you tried debugging (printing lol) the magnetometer readings to ensure you're getting non-zero values off the IMU? Looks like your code defaults to the accelerometer+gyro version if the magnetometer readings are zero, and then I'd certainly expect to see drift.
– Chuck
Dec 6, 2021 at 13:34
• @Chuck thanks for reply. Yes, I tried. I debugged all of them - accel, gyro and magn. All of them seem to work well. Dec 7, 2021 at 4:43
• This may be simply an issue with that particular mpu..the 9250s are bit notorious for being bad. However in my own implementation in c++ i had terrible drift when i didn’t use tinyg’s manufactures magnetic cal tool, and if i poorly guessed gyroscope offsets Dec 18, 2021 at 19:48
• Keep also in mind the mag axis directions arn’t the same as the gyro and accell, you need to rotate it’s frame before you send it to the filter…if you didn’t, everything will be wonky no matter how well you calibrate Dec 18, 2021 at 19:50