卡尔曼系数问题

没文化

void getangle(void) {
  JSDJ_cha=JSDJ_Z-JSDJ_ZERO;
  JSDJ_angle=100*asin(JSDJ_cha/(JSDJ_K*9.8));    //加速度计值转换角度
  TLY_cha=TLY-TLY_ZERO;
  TLY_FINAL=TLY_cha*TLY_K;            //陀螺仪减去静止电压乘以系数          }


void Kalman_Filter(float angle_m,float gyro_m)               //送入加速度计反余弦得到的角度和陀螺仪角速度
{
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 = P[0][0];
PCt_1 = P[1][0];
E = R_angle + PCt_0;
K_0 = PCt_0 / E;
K_1 = PCt_1 / E;
t_0 = PCt_0;
t_1 = 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;
}
按照要求我将加速度计反正弦求得的角度和陀螺仪的角速度值减去一个静止电压乘以一个系数送到卡尔曼滤波程序中去Kalman_Filter(JSDJ_angle,TLY_FINAL);，但是得到的角度和加速度计的角度根本没有一个确定的关系，一会偏差大，一会偏差小，请高手看看什么原因