高手帮忙看看这个卡尔曼程序。。。

happyxieqiang

  我看了一些关于卡尔曼的资料之后自己写了一个卡尔曼的程序，不知是参数设的不对还是怎么回事，滤波之后的输出完全跟随加速度计的波形 没有一点儿滤波的效果 各位大神给看看怎么回事 小弟先行谢过
附程序及示波器图

程序：
#define dt 0.02
#define Q  1
#define R  9

void KalmanFilter(void)
{
   static float GyroIntegral = 1,
                Covariance = 1,
                KalmanGain = 1;
               
    g_fGravityAngle = g_fGravityAD*GRAVITY_ANGLE_RATIO;
    g_fGyroscopeAngleSpeed = g_fGyroscopeAD*GYROSCOPE_ANGLE_RATIO-1;
   
    OutData[0] = (int)g_fGravityAngle;
    OutData[1] = (int)g_fGyroscopeAngleSpeed;
   
   GyroIntegral += g_fGyroscopeAngleSpeed*dt;
   Covariance = Covariance + Q;
   KalmanGain = Covariance/(Covariance + R);
   GyroIntegral = GyroIntegral + KalmanGain*(g_fGravityAngle - GyroIntegral);
   Covariance = (1 - KalmanGain)*Covariance;
   
   g_fCarAngle = GyroIntegral;
   g_fCarAngleSpeed = g_fGyroscopeAngleSpeed;
   
   
    OutData[2] = (int)g_fCarAngle;
}

happyxieqiang

每一个人愿意帮忙么？