一阶互补滤波和卡尔曼滤波数据比较

如上图：蓝色为卡尔曼滤波，红色为一阶互补滤波。
卡尔曼滤波数据跟随较慢，一阶互补滤波跟随较快但是抗干扰能力差。

卡尔曼滤波代码：

float dt_kalman=0.02;
float angle, angle_dot;
float P[2][2] = {{ 1, 0 },{ 0, 1 }};
float Pdot[4] ={ 0,0,0,0};
float Q_angle=0.001, Q_gyro=0.005;
float R_angle=0.5 ,C_0 = 1;
float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
/*************************************************************************************
??        ????Kalman_Filter
??        ????angle_m(??y??z·??ò?????????????ù??????????,??????),gxro_m(x?á??????????????/s)
·? ?? ??????×???????????
**************************************************************************************/
float Kalman_Filter(float angle_m, float gyro_m)
{

        angle+=(gyro_m-q_bias) * dt_kalman;
        angle_err = angle_m - angle;
        Pdot[0] = Q_angle - P[0][1] - P[1][0];
        Pdot[1] = -P[1][1];
        Pdot[2] = -P[1][1];
        Pdot[3] = Q_gyro;
        P[0][0] += Pdot[0] * dt_kalman;
        P[0][1] += Pdot[1] * dt_kalman;
        P[1][0] += Pdot[2] * dt_kalman;
        P[1][1] += Pdot[3] * dt_kalman;
        PCt_0 = C_0 * P[0][0];
        PCt_1 = C_0 * P[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 * P[0][1];
        P[0][0] -= K_0 * t_0;
        P[0][1] -= K_0 * t_1;
        P[1][0] -= K_1 * t_0;
        P[1][1] -= K_1 * t_1;
        angle += K_0 * angle_err;
        q_bias += K_1 * angle_err;
        angle_dot = gyro_m-q_bias;

        return angle;
}