基于STM32F103 MPU6050卡爾曼濾波算法，詳細注釋說明！
- #include "kalman.h"
- #include "mpu6050.h"
- #include "math.h"
- short aacx,aacy,aacz; /*加速度傳感器原始數據*/
- short gyrox,gyroy,gyroz; /*陀螺儀原始數據*/
- short temperature; /*陀螺儀溫度數據*/
- float Accel_x; /*X軸加速度值暫存*/
- float Accel_y; /*Y軸加速度值暫存*/
- float Accel_z; /*Z軸加速度值暫存*/
- float Gyro_x; /*X軸陀螺儀數據暫存*/
- float Gyro_y; /*Y軸陀螺儀數據暫存*/
- float Gyro_z; /*Z軸陀螺儀數據暫存*/
- float Angle_x_temp; /*由加速度計算的x傾斜角度*/
- float Angle_y_temp; /*由加速度計算的y傾斜角度*/
- float Angle_X_Final; /*X最終傾斜角度*/
- float Angle_Y_Final; /*Y最終傾斜角度*/
- /**
- * @brief 讀取數據預處理
- *
- * @param NULL
- *
- * @retval NULL
- */
- void Angle_Calcu(void)
- {
- /*1.原始數據讀取*/
- float accx,accy,accz; /*三方向角加速度值*/
- MPU_Get_Accelerometer(&aacx,&aacy,&aacz); /*得到加速度傳感器數據*/
- MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); /*得到陀螺儀數據*/
- temperature = MPU_Get_Temperature(); /*得到溫度值*/
- Accel_x = aacx;
- Accel_y = aacy;
- Accel_z = aacz;
- Gyro_x = gyrox;
- Gyro_y = gyroy;
- Gyro_z = gyroz;
-
- /*2.角加速度原始值處理過程*/
- /*加速度傳感器配置寄存器0X1C內寫入0x01,設置范圍為±2g。換算關系：2^16/4 = 16384LSB/g*/
- if(Accel_x<32764) accx=Accel_x/16384;
- else accx=1-(Accel_x-49152)/16384;
- if(Accel_y<32764) accy=Accel_y/16384;
- else accy=1-(Accel_y-49152)/16384;
- if(Accel_z<32764) accz=Accel_z/16384;
- else accz=(Accel_z-49152)/16384;
- /*加速度反正切公式計算三個軸和水平面坐標系之間的夾角*/
- Angle_x_temp=(atan(accy/accz))*180/3.14;
- Angle_y_temp=(atan(accx/accz))*180/3.14;
- /*判斷計算后角度的正負號*/
- if(Accel_x<32764) Angle_y_temp = +Angle_y_temp;
- if(Accel_x>32764) Angle_y_temp = -Angle_y_temp;
- if(Accel_y<32764) Angle_x_temp = +Angle_x_temp;
- if(Accel_y>32764) Angle_x_temp = -Angle_x_temp;
-
- /*3.角速度原始值處理過程*/
- /*陀螺儀配置寄存器0X1B內寫入0x18，設置范圍為±2000deg/s。換算關系：2^16/4000=16.4LSB/(°/S)*/
- /*計算角速度*/
- if(Gyro_x<32768) Gyro_x=-(Gyro_x/16.4);
- if(Gyro_x>32768) Gyro_x=+(65535-Gyro_x)/16.4;
- if(Gyro_y<32768) Gyro_y=-(Gyro_y/16.4);
- if(Gyro_y>32768) Gyro_y=+(65535-Gyro_y)/16.4;
- if(Gyro_z<32768) Gyro_z=-(Gyro_z/16.4);
- if(Gyro_z>32768) Gyro_z=+(65535-Gyro_z)/16.4;
-
- /*4.調用卡爾曼函數*/
- Kalman_Filter_X(Angle_x_temp,Gyro_x); /*卡爾曼濾波計算X傾角*/
- Kalman_Filter_Y(Angle_y_temp,Gyro_y); /*卡爾曼濾波計算Y傾角*/
- }
-
- /************************ 卡爾曼參數 ****************************/
- float Q_angle = 0.001; /*角度數據置信度，角度噪聲的協方差*/
- float Q_gyro = 0.003; /*角速度數據置信度，角速度噪聲的協方差*/
- float R_angle = 0.5; /*加速度計測量噪聲的協方差*/
- float dt = 0.02; /*濾波算法計算周期，由定時器定時20ms*/
- char C_0 = 1; /*H矩陣值*/
- float Q_bias, Angle_err; /*Q_bias:陀螺儀的偏差 Angle_err:角度偏量*/
- float PCt_0, PCt_1, E; /*計算的過程量*/
- float K_0, K_1, t_0, t_1; /*卡爾曼增益 K_0:用于計算最優估計值 K_1:用于計算最優估計值的偏差 t_0/1:中間變量*/
- float P[4] ={0,0,0,0}; /*過程協方差矩陣的微分矩陣，中間變量*/
- float PP[2][2] = { { 1, 0 },{ 0, 1 } };/*過程協方差矩陣P*/
- /**
- * @brief 卡爾曼函數
- *
- * @param Accel
- * @param Gyro
- *
- * @retval NULL
- */
- void Kalman_Filter_X(float Accel,float Gyro)
- {
- /*
- 步驟一，先驗估計
- 公式：X(k|k-1) = AX(k-1|k-1) + BU(k)
- X = (Angle,Q_bias)
- A(1,1) = 1,A(1,2) = -dt
- A(2,1) = 0,A(2,2) = 1
- */
- Angle_X_Final += (Gyro - Q_bias) * dt; /*狀態方程,角度值等于上次最優角度加角速度減零漂后積分*/
-
- /*
- 步驟二，計算過程協方差矩陣的微分矩陣
- 公式：P(k|k-1)=AP(k-1|k-1)A^T + Q
- Q(1,1) = cov(Angle,Angle) Q(1,2) = cov(Q_bias,Angle)
- Q(2,1) = cov(Angle,Q_bias) Q(2,2) = cov(Q_bias,Q_bias)
- */
- P[0]= Q_angle - PP[0][1] - PP[1][0];
- P[1]= -PP[1][1]; /*先驗估計誤差協方差*/
- P[2]= -PP[1][1];
- P[3]= Q_gyro;
- PP[0][0] += P[0] * dt;
- PP[0][1] += P[1] * dt;
- PP[1][0] += P[2] * dt;
- PP[1][1] += P[3] * dt;
-
- /*
- 步驟三，計算卡爾曼增益
- 公式：Kg(k)= P(k|k-1)H^T/(HP(k|k-1)H^T+R)
- Kg = (K_0,K_1) 對應Angle,Q_bias增益
- H = (1,0) 可由z=HX+v求出z:Accel
- */
- 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;
-
- /*
- 步驟四，后驗估計誤差協方差
- 公式：P(k|k)=(I-Kg(k)H)P(k|k-1)
- 也可寫為：P(k|k)=P(k|k-1)-Kg(k)HP(k|k-1)
- */
- 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;
-
- /*
- 步驟五，計算最優角速度值
- 公式：X(k|k)= X(k|k-1)+Kg(k)(Z(k)-X(k|k-1))
- */
- Angle_err = Accel - Angle_X_Final; /*Z(k)先驗估計 計算角度偏差*/
- Angle_X_Final += K_0 * Angle_err; /*后驗估計，給出最優估計值*/
- Q_bias += K_1 * Angle_err; /*后驗估計，跟新最優估計值偏差*/
- Gyro_x = Gyro - Q_bias;
- }
- void Kalman_Filter_Y(float Accel,float Gyro)
- {
- Angle_Y_Final += (Gyro - Q_bias) * dt;
- P[0]=Q_angle - PP[0][1] - PP[1][0];
- P[1]=-PP[1][1];
- P[2]=-PP[1][1];
- P[3]=Q_gyro;
- PP[0][0] += P[0] * dt;
- PP[0][1] += P[1] * dt;
- PP[1][0] += P[2] * dt;
- PP[1][1] += P[3] * dt;
- Angle_err = Accel - Angle_Y_Final;
- 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_Y_Final += K_0 * Angle_err;
- Q_bias += K_1 * Angle_err;
- Gyro_y = Gyro - Q_bias;
- }
- /**
- ******************************************************************************
- * @file main.c
- * @author
- * @version V1.0
- * @date 2023-4-7
- * @brief MPU6050陀螺儀數據解算（卡爾曼濾波）
- ******************************************************************************
- *
- *
- *
- ******************************************************************************
- */
- #include "main.h"
- /**
- * @brief 主函數
- * @param 無
- * @retval 無
- */
- int main(void)
- {
- SYS_Init();
- while(1)
- {
- /*MPU6050數據上報*/
- DATA_Report();
- }
- }
- /**
- * @brief 系統初始化總函數
- * @param 無
- * @retval 無
- */
- void SYS_Init(void)
- {
- delay_init();
- NVIC_PriorityGroupConfig(NVIC_PriorityGroup_1);
- uart_init(115200);
- LED_Init();
- MPU_Init();
- /*10Khz的計數頻率，計數到200為20ms*/
- TIM4_Int_Init(199,7199);
- }
- /**
- * @brief MPU6050數據上報
- * @param 無
- * @retval 無
- */
- void DATA_Report(void)
- {
- /*串口發送實時俯仰角，橫滾角，XYZ三軸角加速度原始值，XYZ三軸角速度原始值*/
- printf("俯仰角:%.4f 橫滾角:%.4f \
- X加速度:%5d y加速度:%5d z加速度:%5d \
- x速度:%5d y速度:%5d z速度:%5d\r\n",\
- Angle_X_Final,Angle_Y_Final,aacx,aacy,aacz,\
- gyrox,gyroy,gyroz);
-
-
- }
原理圖: 無
仿真: 無
代碼:
STM32F103C8T6驅動MPU6050程序(卡爾曼濾波_軟件IIC_OLED顯示_串口上傳).7z
(236.32 KB, 下載次數: 0)
2024-12-4 00:58 上傳
點擊文件名下載附件
|
