我用printf("%0.2f   %0.2f   %0.2f\r\n",Angle,Angle_ax,Gyro_y);函數(shù)分別讀取的加速度，角速度和傾角，我發(fā)現(xiàn)當(dāng)我快速的改變板子的傾角的時候，比如快速變化10度，Angle（卡爾曼濾波后的傾角）瞬時變化非�？�，可能會瞬間變化幾十度然后回到正常角度，而當(dāng)我緩慢變化10度的時候，Angle變化是正常線性變化到10度，在這兩種變化中，Angle_ax（從MPU6050讀取的值經(jīng)過處理后的陀螺儀的Y軸數(shù)據(jù)）的變化一直都是線性正常的，并且Angle的值特別接近Angle_ax的值

問題：1，我快速改變板子傾角時Angle的變化正常嗎？

     2，Angle正常變化的時候是應(yīng)該與Angle_ax的值相近嗎？

*************讀取數(shù)據(jù)********************

//定義MPU6050內(nèi)部地址

#define     SMPLRT_DIV             0x19          //陀螺儀采樣率典型值 0X07 125Hz

#define     CONFIG                        0x1A     //低通濾波頻率 典型值 0x00

#define     GYRO_CONFIG                  0x1B         //陀螺儀自檢及測量范圍                 典型值 0x18 不自檢 2000deg/s

#define     ACCEL_CONFIG        0x1C         //加速度計(jì)自檢及測量范圍及高通濾波頻率 典型值 0x01 不自檢 2G 5Hz

#define INT_PIN_CFG     0x37

#define INT_ENABLE      0x38

#define INT_STATUS      0x3A   //只讀

#define     ACCEL_XOUT_H       0x3B

#define     ACCEL_XOUT_L        0x3C

#define     ACCEL_YOUT_H       0x3D

#define     ACCEL_YOUT_L        0x3E

#define     ACCEL_ZOUT_H       0x3F

#define     ACCEL_ZOUT_L        0x40

#define     TEMP_OUT_H          0x41

#define     TEMP_OUT_L           0x42

#define     GYRO_XOUT_H    0x43

#define     GYRO_XOUT_L                  0x44         

#define     GYRO_YOUT_H        0x45

#define     GYRO_YOUT_L                  0x46

#define     GYRO_ZOUT_H        0x47

#define     GYRO_ZOUT_L                  0x48

//讀取寄存器原生數(shù)據(jù)

         

       MPU6050_Raw_Data.Accel_X = (buf[0]<<8 | buf[1]);

       MPU6050_Raw_Data.Accel_Y = (buf[2]<<8 | buf[3]);

       MPU6050_Raw_Data.Accel_Z = (buf[4]<<8 | buf[5]);

       MPU6050_Raw_Data.Temp =   (buf[6]<<8 | buf[7]);  

       MPU6050_Raw_Data.Gyro_X = (buf[8]<<8 | buf[9]);

       MPU6050_Raw_Data.Gyro_Y = (buf[10]<<8 | buf[11]);

       MPU6050_Raw_Data.Gyro_Z = (buf[12]<<8 | buf[13]);

      

      

       //將原生數(shù)據(jù)轉(zhuǎn)換為實(shí)際值，計(jì)算公式跟寄存器的配置有關(guān)

       MPU6050_Real_Data.Accel_X = -(float)(MPU6050_Raw_Data.Accel_X)/8192.0;

       MPU6050_Real_Data.Accel_Y = -(float)(MPU6050_Raw_Data.Accel_Y)/8192.0;

       MPU6050_Real_Data.Accel_Z = (float)(MPU6050_Raw_Data.Accel_Z)/8192.0;

             MPU6050_Real_Data.Gyro_X=-(float)(MPU6050_Raw_Data.Gyro_X- gyroADC_X_offset)/65.5;   

        MPU6050_Real_Data.Gyro_Y=-(float)(MPU6050_Raw_Data.Gyro_Y- gyroADC_Y_offset)/65.5;   

        MPU6050_Real_Data.Gyro_Z=(float)(MPU6050_Raw_Data.Gyro_Z- gyroADC_Z_offset)/65.5;   

    }

   

//******卡爾曼參數(shù)************

                  

const float Q_angle=0.001;  

const float Q_gyro=0.003;

const float R_angle=0.5;

const float dt=0.01;                           //dt為kalman濾波器采樣時間;

const 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 } };

/*****************卡爾曼濾波**************************************************/

void Kalman_Filter(float Accel,float Gyro)               

{

         Angle+=(Gyro- Q_bias) * dt; //先驗(yàn)估計(jì)

         

         Pdot[0]=Q_angle- PP[0][1] - PP[1][0]; // Pk-先驗(yàn)估計(jì)誤差協(xié)方差的微分

         Pdot[1]=-PP[1][1];

         Pdot[2]=-PP[1][1];

         Pdot[3]=Q_gyro;

         

         PP[0][0]+= Pdot[0] * dt;   // Pk-先驗(yàn)估計(jì)誤差協(xié)方差微分的積分

         PP[0][1]+= Pdot[1] * dt;   // =先驗(yàn)估計(jì)誤差協(xié)方差

         PP[1][0]+= Pdot[2] * dt;

         PP[1][1]+= Pdot[3] * dt;

                  

         Angle_err= Accel - Angle;        //zk-先驗(yàn)估計(jì)

         

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

         PP[0][0]-= K_0 * t_0;                  //后驗(yàn)估計(jì)誤差協(xié)方差

         PP[0][1]-= K_0 * t_1;

         PP[1][0]-= K_1 * t_0;

         PP[1][1]-= K_1 * t_1;

                  

         Angle        += K_0 * Angle_err;           //后驗(yàn)估計(jì)

         Q_bias      += K_1 * Angle_err;           //后驗(yàn)估計(jì)

         Gyro_y   = Gyro - Q_bias;       //輸出值(后驗(yàn)估計(jì))的微分=角速度

}

******************傾角計(jì)算*****************

void Angle_Calculate(void)

{

/****************************加速度****************************************/

         

         Accel_x  = MPU6050_Real_Data.Accel_X;           //讀取X軸加速度

         Angle_ax= Accel_x*1.2*180/3.14;     //弧度轉(zhuǎn)換為度

/****************************角速度****************************************/

         

          Gyro_y = MPU6050_Real_Data.Gyro_Y;           

時間dt，所以此處不用積分

/***************************卡爾曼融合*************************************/

         Kalman_Filter(Angle_ax,Gyro_y);       //卡爾曼濾波計(jì)算傾角