亿加合和智能车制作

标题: K车模卡尔曼滤波问题 [打印本页]

作者: a_naiping 时间: 2021-6-28 21:22
标题: K车模卡尔曼滤波问题
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];

}

作者: a_naiping 时间: 2021-6-28 21:30
不用了，我知道问题在哪了

作者: LionelJoe 时间: 2021-6-29 21:49

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

问题出在那里呀？

欢迎光临亿加合和智能车制作 (http://www.znczz.com/)