求高人给看一下 我的角度融合波形

happyxieqiang

我参考网上资料写了一个Kalman的程序 程序如下
#define dt 0.004
#define Q  0.010
#define R  120

void KalmanFilter(void)
{
   static float GyroIntegral = 1,
                Covariance = 1,
                KalmanGain = 1;
               
   g_fGravityAngle = (GRAVITY_OFFSET - g_fGravityAD)*GRAVITY_ANGLE_RATIO;
    g_fGyroscopeAngleSpeed = (g_fGyroscopeAD - GYROSCOPE_OFFSET)*GYROSCOPE_ANGLE_RATIO;
   
    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-20;
}
调了一天了 波形是这样的

高手们给看看 我融合后的角度上面有噪声 怎么去掉 我试着增加Q值 可是增加Q值之后跟随加速度计有点儿缓慢
希望知道怎么回事的大神给予帮助 谢谢

happyxieqiang

各位高手 在吗?

happyxieqiang

凤姐 求帮助啊

好好搞

请问楼主你那个示波软件哪里有啊！