智能车制作

标题: 求高人给看一下我的角度融合波形 [打印本页]

作者: happyxieqiang 时间: 2013-1-25 18:09
标题: 求高人给看一下我的角度融合波形
我参考网上资料写了一个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;
}
调了一天了波形是这样的
[attach]40403[/attach]
高手们给看看我融合后的角度上面有噪声怎么去掉我试着增加Q值可是增加Q值之后跟随加速度计有点儿缓慢
希望知道怎么回事的大神给予帮助谢谢

作者: happyxieqiang 时间: 2013-1-25 18:14
各位高手在吗?

作者: happyxieqiang 时间: 2013-1-25 18:18
凤姐求帮助啊

作者: 好好搞 时间: 2013-3-13 19:16

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

欢迎光临智能车制作 (http://dns.znczz.com/)