问一下各位，加速度计和陀螺仪融合后的波形怎样才算最好，我用了卡尔曼（贴代码贴数据

zelight · 发表于 2012-2-9 20:31:57

本帖最后由 zelight 于 2012-2-9 20:43 编辑

我用了卡尔曼融合，代码是zlstone的。static float P[2][2] = {
   { 1, 0 },
   { 0, 1 }
   };

static float Pdot[4] ={0,0,0,0};
static const char C_0 = 1;
static float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
  int test_angel=0,test_angel_dot=0;
//-------------------------------------------------------
void Kalman_Filter(float angle_m,float gyro_m) //gyro_m:gyro_measure
{                                                 //加速度器，陀螺仪
//float angle=0, angle_dot=0;
  int k;

angle+=(gyro_m-q_bias) * dt;                // 对角速度进行积分

Pdot[0]=Q_angle - P[0][1] - P[1][0]; //=
Pdot[1]=- P[1][1];                //=-1
Pdot[2]=- P[1][1];                //=-1
Pdot[3]=Q_gyro;

P[0][0] += Pdot[0] * dt;
P[0][1] += Pdot[1] * dt;
P[1][0] += Pdot[2] * dt;
P[1][1] += Pdot[3] * dt;

angle_err = angle_m - angle;       //计算陀螺仪积分后的值和加速度器计算的角度之差。

PCt_0 = C_0 * P[0][0];
PCt_1 = C_0 * P[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 * P[0][1];
P[0][0] -= K_0 * t_0;
P[0][1] -= K_0 * t_1;
P[1][0] -= K_1 * t_0;
P[1][1] -= K_1 * t_1;

angle += K_0 * angle_err;       //角度
q_bias += K_1 * angle_err;
angle_dot = gyro_m-q_bias;       //角速度

我发现响应有点慢，比如从0到19，它会先到21再到19。。。。虽然修改Q和R可以纠正，但感觉响应太快又不好，到底怎样的滤波，才算好的波形？？急求？？

从下面的图片可以发现，难以去掉的尖峰（可以考虑增加Q，过程噪声的值），还有响应时间的缓慢（可以通过增加Q和R），这些都是通过实践所得，但不能太大，我也是试验所得，其实据说只有R可以改，Q最好不要随意修改。

补充内容 (2012-2-10 16:40):
http://www.megasociety.org/noesis/138/kalman.html  主要是这张表格

补充内容 (2012-2-10 16:41):
这张表格很有用