mpu6050 卡尔曼滤波得到角度，动态情况下解算的角度异常求解

zb0918

现在：在静止状态下经kalman融合后的得到的角度与通过加速度直接转换的角度都是正常的，在转动传感器后可以发现经加速度直接转换的角度值正常，可是经过kalman滤波融合后的角度先变成很大在变到正常值。
例如，传感器初始角度为10
加速度转换的角度为10
kalman融合后的角度为10
转动传感器到50
加速度转换角度直接变为50
kalman融合后的角度变化：10 >> 20 >>40 >>60 >>70 >>60 >>50
这个是什么情况啊。。。
代码如下

/*************卡尔曼滤波***************/
void Kalman_Filter1(float Accel1,float Gyro1)               
{                       
        static const float Q_angle=0.001;  
        static const float Q_gyro=0.003;
        static const float R_angle=0.5;
        static const float dt=0.01;                                   //dt为kalman滤波器采样时间单位；秒；
        static const char  C_0 = 1;
        static float Q_bias, Angle_err;
        static float PCt_0, PCt_1, E;
        static float K_0, K_1, t_0, t_1;
        static float Pdot[4] ={0,0,0,0};
        static float PP[2][2] = { { 1, 0 },{ 0, 1 } };
               
        Angle_x+=(Gyro1 - Q_bias) * dt; //根据角速度估计这段时间内的角度变化

        Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先检验估计误差协方差微分

        Pdot[1]= -PP[1][1];
        Pdot[2]= -PP[1][1];
        Pdot[3]=Q_gyro;
               
        PP[0][0] += Pdot[0] * dt;        // Pk-先检验估计误差协方差积分
        PP[0][1] += Pdot[1] * dt;       
        PP[1][0] += Pdot[2] * dt;
        PP[1][1] += Pdot[3] * dt;
                       
        Angle_err = Accel1 - Angle_x;        //zk-检验估计值
       
        PCt_0 = C_0 * PP[0][0];
        PCt_1 = C_0 * PP[1][0];
       
        E = R_angle + C_0 * PCt_0;
               
        K_0 = PCt_0 / E;
        K_1 = PCt_1 / E;
               
        t_0 = PCt_0;
        t_1 = C_0 * PP[0][1];
       
        PP[0][0] -= K_0 * t_0;                 //最后检验估计误差协方差
        PP[0][1] -= K_0 * t_1;
        PP[1][0] -= K_1 * t_0;
        PP[1][1] -= K_1 * t_1;
                       
        Angle_x        += K_0 * Angle_err;  //传感器姿态角度
        Q_bias        += K_1 * Angle_err;  //后验估计
        Gyro_y         = Gyro1 - Q_bias;         //输出值的微分=角速度
}


        MPU6050_ReadAcc( &g_global.sensor.u16Acc[0], &g_global.sensor.u16Acc[1], &g_global.sensor.u16Acc[2]);
        MPU6050_ReadGyro( &g_global.sensor.u16Gyro[0], &g_global.sensor.u16Gyro[1], &g_global.sensor.u16Gyro[2]);
        Angle_ax = g_global.sensor.u16Acc[0]*1.55*180/(16384*3.14);   
        Gyro_y = -g_global.sensor.u16Gyro[1]/16.4;


        Kalman_Filter1(Angle_ax,Gyro_y);

zb0918

自己以解决，原来是要调参数，刚接触哈哈。。。

皮皮猪ゞ

楼主 调参怎么搞啊 我最近也再搞 老是出不来