lcr39101 發(fā)表于 2016-6-20 19:55
學習，學習，謝謝

學習

我看到第四橫，#include，后面沒有帶頭文件，能編譯通過嗎？？

學習，謝謝分享

rout那一行：為什么不是pi（直立環(huán)）+PD(速度環(huán))控制

學習學習

都是高手學習了，謝謝分享

我用printf("%0.2f    %0.2f    %0.2f\r\n",Angle,Angle_ax,Gyro_y);函數分別讀取的加速度，角速度和傾角，我發(fā)現當我快速的改變板子的傾角的時候，比如快速變化10度，Angle（卡爾曼濾波后的傾角）瞬時變化非常快，可能會瞬間變化幾十度然后回到正常角度，而當我緩慢變化10度的時候，Angle變化是正常線性變化到10度，在這兩種變化中，Angle_ax（從MPU6050讀取的值經過處理后的陀螺儀的Y軸數據）的變化一直都是線性正常的，并且Angle的值特別接近Angle_ax的值 問題：1，我快速改變板子傾角時Angle的變化正常嗎？       2，Angle正常變化的時候是應該與Angle_ax的值相近嗎？  現在情況就是，就算我是在減小傾角，只要我快速地改變，它顯示的傾角都會先增大再減小，而如果我慢速改變的話，傾角就會緩慢減小而不會出現中間的角度增大   *************讀取數據******************** //定義MPU6050內部地址 #define        SMPLRT_DIV                0x19        //陀螺儀采樣率 典型值 0X07 125Hz #define        CONFIG                          0x1A        //低通濾波頻率 典型值 0x00  #define        GYRO_CONFIG                0x1B        //陀螺儀自檢及測量范圍                 典型值 0x18 不自檢 2000deg/s #define        ACCEL_CONFIG        0x1C        //加速度計自檢及測量范圍及高通濾波頻率 典型值 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  //讀取寄存器原生數據                     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]);                         //將原生數據轉換為實際值，計算公式跟寄存器的配置有關         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;        }         //******卡爾曼參數************                  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; //先驗估計                  Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先驗估計誤差協(xié)方差的微分          Pdot[1]= -PP[1][1];         Pdot[2]= -PP[1][1];         Pdot[3]=Q_gyro;                  PP[0][0] += Pdot[0] * dt;   // Pk-先驗估計誤差協(xié)方差微分的積分         PP[0][1] += Pdot[1] * dt;   // =先驗估計誤差協(xié)方差         PP[1][0] += Pdot[2] * dt;         PP[1][1] += Pdot[3] * dt;                          Angle_err = Accel - Angle;        //zk-先驗估計                  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;                 //后驗估計誤差協(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;         //后驗估計         Q_bias        += K_1 * Angle_err;         //后驗估計         Gyro_y   = Gyro - Q_bias;         //輸出值(后驗估計)的微分=角速度  }  ******************傾角計算***************** void Angle_Calculate(void) {  /****************************加速度****************************************/                  Accel_x  =  MPU6050_Real_Data.Accel_X;          //讀取X軸加速度         Angle_ax = Accel_x*1.2*180/3.14;     //弧度轉換為度  /****************************角速度****************************************/                   Gyro_y = MPU6050_Real_Data.Gyro_Y;              時間dt，所以此處不用積分 /***************************卡爾曼融合*************************************/         Kalman_Filter(Angle_ax,Gyro_y);       //卡爾曼濾波計算傾角        

學習，學習，謝謝

好玩的東西都要MARK一下