Why doesn't yaw angle return to its 0 state after rotations?

Question

I'm using STM32F4 mcu with MPU6050 on it. I want to get very accurate yaw angle using gyroscope with Madgwick filter. I tried lots of things: with and without gyro thresholds, with and without acceletometer, with and without other gyro axes. I also tried all gyro sensitivities, but nothing of this help. So about the problem - When I move and rotate controller from specific place and get it back after few seconds - yaw angle doesn't come back to zero:
(Start placement with yaw==0) (In this picture yaw is also zero, but you can see, that pcb is not on the original place)

The Madgwick filter:

void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az) {
    float recipNorm;
    float recipNormAccel;
    float recipNormS;
    float s0, s1, s2, s3;
    float s0_view, s1_view, s2_view, s3_view;
    float qDot1, qDot2, qDot3, qDot4;
    float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;

    // 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
        recipNormAccel = invSqrt(ax * ax + ay * ay + az * az);
        ax *= recipNormAccel;
        ay *= recipNormAccel;
        az *= recipNormAccel;

        // Auxiliary variables to avoid repeated arithmetic
        _2q0 = 2.0f * q0;
        _2q1 = 2.0f * q1;
        _2q2 = 2.0f * q2;
        _2q3 = 2.0f * q3;
        _4q0 = 4.0f * q0;
        _4q1 = 4.0f * q1;
        _4q2 = 4.0f * q2;
        _8q1 = 8.0f * q1;
        _8q2 = 8.0f * q2;
        q0q0 = q0 * q0;
        q1q1 = q1 * q1;
        q2q2 = q2 * q2;
        q3q3 = q3 * q3;

        // Gradient decent algorithm corrective step
        s0_view = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
        s1_view = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
        s2_view = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
        s3_view = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay;
        recipNormS = invSqrt(s0_view * s0_view + s1_view * s1_view + s2_view * s2_view + s3_view * s3_view); // normalise step magnitude
        s0 = s0_view * recipNormS;
        s1 = s1_view * recipNormS;
        s2 = s2_view * recipNormS;
        s3 = s3_view * recipNormS;

        // 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;
}

float invSqrt(float x) {
    if (x != 0)
        return 1.0f / sqrtf(x);
    return 1;
}

This is how I use the filter:

#define GYRO_Y_THRESH 0.0045

void Process_IMU()
{
    //read raw data
    uint8_t data[14];
    uint8_t reg = ACCEL_XOUT_H;
    uint8_t mpu_address = MPU6050_ADDRESS_DEFAULT;


    while(HAL_I2C_Master_Transmit(_MPU6050_I2C,(uint16_t)mpu_address,&reg,1,1000) != HAL_OK);
    while(HAL_I2C_Master_Receive(_MPU6050_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 = (float)((int32_t)Accel_x - Accel_x_bias)/(float)accel_sensitivity;
    Accel_Y = (float)((int32_t)Accel_y - Accel_y_bias)/(float)accel_sensitivity;
    Accel_Z = (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.0 * STUPID_PRES;
    Gyro_Y =  (float)(((int32_t)Gyro_y - Gyro_y_bias)/(float)gyro_sensitivity)*M_PI/180.0 * STUPID_PRES;
    Gyro_Z =  (float)(((int32_t)Gyro_z - Gyro_z_bias)/(float)gyro_sensitivity)*M_PI/180.0 * STUPID_PRES;

    if (fabs(Gyro_Y ) < GYRO_Y_THRESH)
        Gyro_Y = 0;

    MadgwickAHRSupdateIMU(0,0,-Gyro_Y ,0,0,-accel_sensitivity);

}

(I don't want to use accelerometer values, because when I use them yaw angle starts changing when I move pcb without rotating it. And also roll and pitch start increasing even if I do not rotate pcb in these axes)
Now I use ACCEL_FS_4 sensitivity for acceletometer and GYRO_FS_2000 for gyro.

I also calibrate gyro and accelerometer at the begin.

What should I do to prevent this? I want yaw angle to come back to zero after some rotations.

Answer 1

You can't. The down vector is fixed because gravity always points down, and you can measure accelerations (gravity) with the accelerometers.

You can't fix heading without a magnetometer at least, to measure magnetic North, and even with a magnetometer you can have issues with nearby ferrous objects, electric fields, etc.

But with no magnetometer you can't fix your yaw (heading!) angle. Remember your gyroscopes are measuring angular velocity, not angular position, so your yaw (heading!) angle will drift with gyro bias.