请问这段通过卡尔曼滤波法解算出mpu6050的代码什么意思，哪位大神帮忙讲解一下，谢谢

zzzzzcccccxxxxx

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 );

eric2013

帮顶。

caicaptain2

这是飞行控制的吧？ 这有专门的研究的，不是这一行的资深博士级别，难以搞懂的。

killalljp

没那么复杂。不用了解细节，按照函数来用就好了。做过一个压差传感器数据采集，初始数据波动较大，用了卡尔曼滤波，效果杠杠的！