智能车制作

标题: 高手帮忙看看这个卡尔曼程序。。。 [打印本页]

作者: happyxieqiang 时间: 2013-1-23 16:12
标题: 高手帮忙看看这个卡尔曼程序。。。
  我看了一些关于卡尔曼的资料之后自己写了一个卡尔曼的程序，不知是参数设的不对还是怎么回事，滤波之后的输出完全跟随加速度计的波形没有一点儿滤波的效果各位大神给看看怎么回事小弟先行谢过
附程序及示波器图
[attach]40292[/attach]
程序：
#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 时间: 2013-1-23 16:49
每一个人愿意帮忙么？

欢迎光临智能车制作 (http://111.231.132.190/)