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 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?