请问这个用卡尔曼滤波法解算MPU6050代码什么意思，哪位大佬可以帮我讲解下，非常感谢

hahahafgfghchch · 发表于 2022-3-13 16:43:06

char str[100]={0};
short aacx,aacy,aacz; //加速度传感器原始数据
short gyrox,gyroy,gyroz; //陀螺仪原始数据
float aacx1,aacy1,aacz1; //加速度传感器原始数据
float gyrox1,gyroy1,gyroz1,angle1; //陀螺仪原始数据
float Q=10000;
float dt=0.03;
float w=0;
float angle=0;
float angle2=0;
float p1=0;
float p2=0;
float p3=0;
float p4=0;
int q=1;
static float k1,k0,LTR1,LTR0,Ts;
static float TTR=100;
  int R=10;
static float mea;
static int i=0;
static float j=0;

char a[100]={0};
char b[100]={0};
char c[100]={0};
char d[100]={0};
//// *********************************卡尔曼滤波****************************************//
angle=angle+w*dt;
p1=p1+(p2+p3)*dt+p4*dt*dt+0.5*dt*dt*dt*Q;
p2=p2+p4*dt+0.25*dt*dt*dt*dt*Q;
p3=p3+p4*dt+dt*dt*Q;
p4=p4+0.5*dt*dt*dt*Q;
k0=p2/(p4+R);
k1=p4/(p4+R);
angle=angle+k0*(mea-w);
w=w+k1*(mea-w);
p1=p1-p3*k0;
p2=p2-p4*k0;
p3=p3-p3*k1;
p4=p4-p4*k1;
if(i/10!=0)
{
angle=180*(atan2(aacx1,aacy1))/3.1415;
i=0;

}
LTR0=0.17*(aacx1*cos(angle*3.14/180)-aacy1*sin(angle*3.14/180))*cos(angle*3.14/180)+1.67*sin(angle*3.14/180);
for(j=0;j<=3;)
{

Ts=Ts+0.1;
j=j+0.1;
angle2=angle+mea*Ts;

LTR1=0.16*(aacx1*cos(angle2*3.14/180)-aacy1*sin(angle2*3.14/180))*cos(angle2*3.14/180)+1.6*sin(angle2*3.14/180);
if(LTR1>=0.9||LTR1<=-0.9)
{

if(q==0)
{
TTR=Ts;
j=4;
}
   else
{
GPIO_WriteBit(GPIOC,GPIO_Pin_6,Bit_RESET);
q=0;
TTR=Ts;
j=4;
}
}
}
TTR=Ts;
if(TTR>3)
{
TTR=100;
q=1;
GPIO_WriteBit(GPIOC,GPIO_Pin_6,Bit_SET);
}
sprintf(str,"angle=%.2f\r\n",angle);
LCD_ShowString(10,120,tftlcd_data.width,tftlcd_data.height,24,str);
sprintf(a,"TTR=%.2f\r\n",TTR);
LCD_ShowString(10,160,tftlcd_data.width,tftlcd_data.height,24,a);
sprintf(b,"LTR0=%.2f\r\n",LTR0);
LCD_ShowString(10,200,tftlcd_data.width,tftlcd_data.height,24,b);
ESP8266_SendString ( ENABLE, str, 0, Single_ID_0 );