|
我用printf("%0.2f %0.2f %0.2f\r\n",Angle,Angle_ax,Gyro_y);函數分別讀取的加速度，角速度和傾角，我發現當我快速的改變板子的傾角的時候，比如快速變化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-先驗估計誤差協方差的微分 Pdot[1]=-PP[1][1]; Pdot[2]=-PP[1][1]; Pdot[3]=Q_gyro; PP[0][0]+= Pdot[0] * dt; // Pk-先驗估計誤差協方差微分的積分 PP[0][1]+= Pdot[1] * dt; // =先驗估計誤差協方差 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; //后驗估計誤差協方差 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); //卡爾曼濾波計算傾角
| 
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
