请问用卡尔曼滤波算mpu6050角度为什么静止时角度一直往上升

hahahafgfghchch · 发表于 2022-4-5 15:24:59

请问用卡尔曼滤波算mpu6050角度  为什么静止时角度一直往上升，请大神帮我看一下是哪里出现了问题，谢谢！

void Kalman_Filter_X(float Angle_acc,float Gyro) //对x进行卡尔曼滤波
{
//step1：先验估计
Angle_X+=(Gyro-bias)*dt;

//stpe2:先验误差协方差
P[0][0]=0.0f;
P[0][1]=0.0f;
P[1][0]=0.0f;
P[1][1]=0.0f;

P[0][0]+=dt*(dt*P[1][1]-P[0][1]-P[1][0]+Q_angle);
P[0][1]-=dt*P[1][1];
P[1][0]-=dt*P[1][1];
P[1][1]+=dt*Q_bias;

//step3:计算卡尔曼增益
S=P[0][0]+R_measure;

K[0]=P[0][0]/S;
K[1]=P[1][0]/S;

//step5：更新误差协方差
P00_temp=P[0][0];
  P01_temp=P[0][1];

P[0][0]-=K[0]*P00_temp;
P[0][1]-=K[0]*P01_temp;
P[1][0]-=K[1]*P00_temp;
P[1][1]-=K[1]*P01_temp;

//step4:后验估计协方差
Angle_err=Angle_acc-Angle_X;

Angle_X+=K[0]*Angle_err;
bias+=K[1]*Angle_err;
Gyro-=bias;
}

void Kalman_Filter_Y(float Angle_acc,float Gyro) //对y进行卡尔曼滤波
{
Angle_Y+=(Gyro-bias)*dt;

P[0][0]=0.0;
P[0][1]=0.0;
P[1][0]=0.0;
P[1][1]=0.0;

P[0][0]+=dt*(dt*P[1][1]-P[0][1]-P[1][0]+Q_angle);
P[0][1]-=dt*P[1][1];
P[1][0]-=dt*P[1][1];
P[1][1]+=dt*Q_bias;

S=P[0][0]+R_measure;
K[0]=P[0][0]/S;
K[1]=P[0][1]/S;

  P00_temp=P[0][0];
  P01_temp=P[0][1];

P[0][0]-=K[0]*P00_temp;
P[0][1]-=K[0]*P01_temp;
P[1][0]-=K[1]*P00_temp;
P[1][1]-=K[1]*P01_temp;

   Angle_err=Angle_acc-Angle_Y;
Angle_Y+=K[0]*Angle_err;
bias+=K[1]*Angle_err;
Gyro-=bias;
}

while(1)
{

   //1.读取原始数据
MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); //读取陀螺仪传感器原始数据&
MPU_Get_Accelerometer(&aacx,&aacy,&aacz);  //读取加速度传感器原始数据

//2.处理加速度原始值
Accel_x=aacx/16384; //计算x轴加速度
Accel_y=aacy/16384; //计算y轴加速度
Accel_z=aacz/16384; //计算z轴加速度

//3.处理角速度原始值
Gyro_x=gyrox/16.4;       //计算x轴角速度
Gyro_y=gyroy/16.4;       //计算y轴角速度
Gyro_z=gyroz/16.4;       //计算z轴角速度

//4.计算两个轴和水平面之间的夹角（利用反正切公式）
Angle_x=atan2(Accel_x,Accel_z)*180/3.14; //x轴的夹角
Angle_y=atan2(Accel_y,Accel_z)*180/3.14; //y轴的夹角

//5.调用卡尔曼函数
delay_ms(50);//延时
Kalman_Filter_X(Angle_x,Gyro_y);
Kalman_Filter_Y(Angle_y,Gyro_x);

}