K车模卡尔曼滤波问题

a_naiping

K车模我们用的是DMP来读取四元数，当我们加了卡尔曼的函数后，OLED上显示角度值直接变成了十几位数，这是什么原因？

读取DMP四元数：       
    res = LQ_DMP_Read();                                // 读取DMP四元数
    sprintf(txt, "%2d ", res);
    OLED_P6x8Str(0, 0, (u8*)txt);
    sprintf(txt, "%2.2f %2.2f ", Pitch, Roll);
    OLED_P6x8Str(0, 0, (u8*)txt);                
    sprintf(txt, "gyrox:%d   ", gyro[0]);
    OLED_P6x8Str(0, 2, (u8*)txt);
    sprintf(txt, "gyroy:%d   ", gyro[1]);
    OLED_P6x8Str(0, 3, (u8*)txt);
    sprintf(txt, "gyroz:%d   ", gyro[2]);
    OLED_P6x8Str(0, 4, (u8*)txt);
    sprintf(txt, "accelx:%d   ", accel[0]);
    OLED_P6x8Str(0, 5, (u8*)txt);
    sprintf(txt, "accely:%d   ", accel[1]);
    OLED_P6x8Str(0, 6, (u8*)txt);
    sprintf(txt, "accelz:%d   ", accel[2]);
    OLED_P6x8Str(0, 7, (u8*)txt);

使用卡尔曼函数：
  Kalman_Filter(accel[2],gyro[1]); //卡尔曼
        Angle_Balance = AngleKEM;

卡尔曼函数：
float  AngleKEM;
float angle_dot;        
float Q_angle=0.001;// 过程噪声的协方差
float Q_gyro=0.003; //0.003 过程噪声的协方差 过程噪声的协方差为一个一行两列矩阵
float R_angle=0.003;  // 测量噪声的协方差 既测量偏差
float dt=0.01;     //                 
char   C_0 = 1;
float Q_bias, Angle_err;
float PCt_0, PCt_1, E;
float K_0, K_1, t_0, t_1;
float Pdot[4] ={0,0,0,0};
float PP[2][2] = { { 1, 0 },{ 0, 1 } };

float Kalman_Filter(float Accel,float Gyro)               
{
  AngleKEM+=(Gyro - Q_bias) * dt; //先验估计   (1.预测)
  Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分
  
  Pdot[1]=-PP[1][1];
  Pdot[2]=-PP[1][1];
  Pdot[3]=Q_gyro;
  PP[0][0] += Pdot[0] * dt;   // Pk-先验估计误差协方差微分的积分
  PP[0][1] += Pdot[1] * dt;   // =先验估计误差协方差
  PP[1][0] += Pdot[2] * dt;
  PP[1][1] += Pdot[3] * dt;
  
  Angle_err = Accel - AngleKEM;        //zk-先验估计
  
  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];
  

  
  AngleKEM += K_0 * Angle_err;           //后验估计
  Q_bias         += K_1 * Angle_err;           //后验估计
       
        PP[0][0] -= K_0*PP[0][0];  //更新协方差矩阵
  PP[0][1] -= K_0*PP[0][1];
        PP[1][0] -= K_1*PP[0][0];
        PP[1][0] -= K_1*PP[0][1];
       

}

LionelJoe

a_naiping 发表于 2021-6-28 21:30
不用了，我知道问题在哪了

问题出在那里呀？

a_naiping

不用了，我知道问题在哪了