请问这段通过卡尔曼滤波法解算出mpu6050的代码什么意思，哪位大神帮忙讲解一下，谢谢
char str={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={0};
char b={0};
char c={0};
char d={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 );
帮顶。 这是飞行控制的吧？ 这有专门的研究的，不是这一行的资深博士级别，难以搞懂的。 没那么复杂。不用了解细节，按照函数来用就好了。做过一个压差传感器数据采集，初始数据波动较大，用了卡尔曼滤波，效果杠杠的！
页:
[1]