MPU9250做9轴数据融合后yaw会随着roll、pitch变化而变化

丹丹

MPU9250用MahonyAHRS库做9轴数据融合后yaw会随着roll、pitch变化而变化。
当roll、pitch都为0时，得到的yaw数据是准确的。各位大佬谁遇到过类似问题，怎么调参数才能解决掉这个问题呢？

#define Factor (0.030517 *0.0174533f)

// MahonyAHRS算法库参数设置
#define sampleFreq        100.0f                        // sample frequency in Hz
#define twoKpDef        (80.0f * 0.5f)        // 2 * proportional gain
#define twoKiDef        (0.02f * 1.0f)        // 2 * integral gain

float g_roll, g_pitch, g_yaw;

void Mahony_computeAngles()
{
        g_roll = atan2f(q0*q1 + q2*q3, 0.5f - q1*q1 - q2*q2);
        g_pitch = asinf(-2.0f * (q1*q3 - q0*q2));
        g_yaw = atan2f(q1*q2 + q0*q3, 0.5f - q2*q2 - q3*q3);
        anglesComputed = 1;
}
float getRoll() {
        if (!anglesComputed) Mahony_computeAngles();
        return g_roll * 57.29578f;
}
float getPitch() {
        if (!anglesComputed) Mahony_computeAngles();
        return g_pitch * 57.29578f;
}
float getYaw() {
        if (!anglesComputed) Mahony_computeAngles();
        return g_yaw * 57.29578f + 180.0f;
}

int main(){

初始化。。。。。
while(1)        {               

                     temp=MPU_Get_Temperature();        //得到温度值
                         delay_ms(2);
                         MPU_Get_Accelerometer(&aacx,&aacy,&aacz);        //得到加速度传感器数据
                         MPU_Get_Gyroscope(&gyrox,&gyrox,&gyrox);        //得到陀螺仪数据
                          delay_ms(1);
                         MPU_Get_Magnetometer(&mx, &my, &mz);  
                        MahonyAHRSupdate(gyrox*Factor,gyrox*Factor,gyrox*Factor,aacx,aacy,aacz,mx, my, mz);  //更新数据

                       if(t%20 == 0){
                           u3_printf("pitch=%0.2f   roll=%0.2f  yaw=%0.2f   \r\n",getPitch(),getRoll(),getYaw());
                        }
                         
}