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

Question

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()
{
    //read raw data
    uint8_t data[14];
    uint8_t reg = ACCEL_XOUT_H;
    uint8_t mpu_address = MPU9250_ADDRESS_DEFAULT;


    while(HAL_I2C_Master_Transmit(_MPU9250_I2C,(uint16_t)mpu_address,&reg,1,1000) != HAL_OK);
    while(HAL_I2C_Master_Receive(_MPU9250_I2C, (uint16_t)mpu_address, data, 14, 1000) != HAL_OK);

    /*-------- 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();

    MadgwickAHRSupdate(Gyro_X,Gyro_Y,Gyro_Z,Accel_X,Accel_Y,Accel_Z,Mag_Y_calib,Mag_X_calib,-Mag_Z_calib);
}

The Get_magnetometer function:

void Get_magnetometer()
{
    uint8_t raw_data[7];
    uint8_t reg_ST1 = ST1;
    uint8_t mag_address = MAG_ADDRESS_DEFAULT;
    uint8_t reg = XOUT_L;
    while(HAL_I2C_Master_Transmit(_MPU9250_I2C,(uint16_t)mag_address,&reg_ST1,1,1000) != HAL_OK);
    while(HAL_I2C_Master_Receive(_MPU9250_I2C,(uint16_t)mag_address,raw_data,1,1000) != HAL_OK);
    if (raw_data[0] & 0x01)
    {
        while(HAL_I2C_Master_Transmit(_MPU9250_I2C,(uint16_t)mag_address,&reg,1,1000) != HAL_OK);
        while(HAL_I2C_Master_Receive(_MPU9250_I2C,(uint16_t)mag_address,raw_data,7,1000) != HAL_OK);
        // 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.