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,®,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.