0
$\begingroup$

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) enter image description here (In this picture yaw is also zero, but you can see, that pcb is not on the original place) enter image description here

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.

$\endgroup$

1 Answer 1

1
$\begingroup$

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.

$\endgroup$
2
  • $\begingroup$ But anyway it's quite strange, because deviation is always with negative sign and about 1.5^. I can't use magnetometer, because there are too many metal things around the pcb. "yaw (heading!) angle will drift with gyro bias." what does it mean, I calibrate gyro and accel, so I have biases for them? $\endgroup$ Feb 4, 2022 at 17:28
  • 1
    $\begingroup$ @crackanddie - You can average the sensor readings while it's known to be stationary to determine the biases, but unfortunate "bias stability" is an issue - just like the name sounds, the bias isn't constant, so you still wind up with long-term drift. $\endgroup$
    – Chuck
    Feb 6, 2022 at 15:04

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.

Not the answer you're looking for? Browse other questions tagged or ask your own question.