卡尔莫滤波 效果怎么会延迟的那么厉害，要4秒之后数据才出来

男儿勇闯天涯

这段滤波程序有没有问题啊，  反应也太慢了，，板子都稳定4秒了  串口看的数据才慢慢的稳定  比如 从0到30要用4秒的时间
[mw_shl_code=applescript,true]void Kalman_Filter(float  Gyro,float Actcel)
{
        Angle+=(Gyro - Q_bias) * dt;           //ÏèÑé1à¼Æ

       
        Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-ÏèÑé1à¼ÆÎó2îD-·½2îμÄÎ￠·Ö

        Pdot[1]= -PP[1][1];
        Pdot[2]=-PP[1][1];
        Pdot[3]=Q_gyro;
       
        PP[0][0] += Pdot[0] * dt;   // Pk-ÏèÑé1à¼ÆÎó2îD-·½2îÎ￠·ÖμÄ»y·Ö
        PP[0][1] += Pdot[1] * dt;   // =ÏèÑé1à¼ÆÎó2îD-·½2î
        PP[1][0] += Pdot[2] * dt;
        PP[1][1] += Pdot[3] * dt;
               
        Angle_err = Accel - Angle;
       
        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;                 //oóÑé1à¼ÆÎó2îD-·½2î
        PP[0][1] -= K_0 * t_1;
        PP[1][0] -= K_1 * t_0;
        PP[1][1] -= K_1 * t_1;
               
        Angle        += K_0 * Angle_err;         //oóÑé1à¼Æ
        Q_bias        += K_1 * Angle_err;         //oóÑé1à¼Æ
        Gyro_x   = Gyro - Q_bias;         //êä3öÖμ(oóÑé1à¼Æ)μÄÎ￠·Ö=½ÇËù¶è
        calculate_PWM(Gyro_x,Angle);
printf("%f\n",Angle);
}
[/mw_shl_code]

ricefat

算法问题或是性能不足。
估计是算法，STM32卡尔曼滤波做小车做飞机的很多，不会那么延迟的

男儿勇闯天涯

问题解决了    我增大定时器频率100倍  闪电般输出   实时效果非常好