卡尔曼滤波请教

陶新成

下面是卡尔曼滤波程序的一种，在很多程序里都看到过，但是不知道如何使用，请大家讲解一下
static float gyro,acceler,Vref=2.048,fangdabeishu=24.6;
static float acc,gg,acce;

static float angle, angle_dot;                 
static const float Q_angle = 0.001, Q_gyro = 0.003, R_angle = 0.5, dt = 0.006;
static float PP[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;
static float gyro,acceler;

void Kalman_Filter(float angle_m,float gyro_m)               
{
          angle += (gyro_m-q_bias)*dt;

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

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

          angle_err = angle_m-angle;

          PCt_0 = C_0*PP[0][0];
          PCt_1 = C_0*PP[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*PP[0][1];

          PP[0][0] -= K_0*t_0;
          PP[0][1] -= K_0*t_1;
          PP[1][0] -= K_1*t_0;
          PP[1][1] -= K_1*t_1;

          angle += K_0*angle_err;
          q_bias += K_1*angle_err;
          angle_dot = (gyro_m-q_bias);
}