关于卡尔曼滤波的一点理解

伤痕我心 · 发表于 2014-9-11 21:58:59

参加完第九届国赛，已经大半个月过去了，这段时间，闲来无事，研究一下卡尔曼滤波；
故此写下一点心得，不知道对于不对，望大神们指正指正。
代码：
//-------------------------------------------------------
//Kalman滤波，64MHz的处理时间约100um;
//-------------------------------------------------------
static float angle, AngleSpeed; //外部需要引用的变量
//-------------------------------------------------------
static const float Q_angle=0, Q_gyro=0, R_angle=0, dt=0.005;
//注意：dt的取值为kalman滤波器采样时间;
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=0, angle_err=0, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
void Kalman_Filter(float angle_m,float gyro_m) //gyro_m:gyro_measure
{
angle+=(gyro_m-q_bias)*dt;
Pdot[0]=Q_angle - P[0][1] - P[1][0];
Pdot[1]=- P[1][1];
Pdot[2]=- P[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 = 1.0*PCt_0 / E;
K_1 = 1.0*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/2;
q_bias+= K_1 * angle_err;
AngleSpeed=gyro_m-q_bias;

}
{其实这些代码大家已经都知道了}

在调试过程中，我发现了，加速度计素提供的是一个类似于PI速度控制中的I所积分出来的一个作用；
陀螺仪所提供的作用是，让融合曲线跟踪加速度计的曲线；
如此这般，加速度计所提供的类似于PI控制中的I,不能有太大的波动，否则，车子会震动得特别厉害；
看了华中科技大学的报告，有点难以理解，他们的报告中这样写道（如图）；
对于他们报告中的方法，我有点不解；
希望各位大神给予指点，谢谢，如果我所的错了，不要怕浪费了大哥的口水，尽量喷我，喷完后，我给你寄一瓶水过去。小弟真心想学东西，请指教，谢谢。