標(biāo)題: mpu6050卡爾曼濾波STM32單片機程序 詳細(xì)注釋說明 [打印本頁]
作者: zpwgf    時間: 2024-11-29 12:10
標(biāo)題: mpu6050卡爾曼濾波STM32單片機程序 詳細(xì)注釋說明
基于STM32F103 MPU6050卡爾曼濾波算法，詳細(xì)注釋說明！
  1. #include "kalman.h"
  2. #include "mpu6050.h"
  3. #include "math.h"


  6. short aacx,aacy,aacz;                /*加速度傳感器原始數(shù)據(jù)*/
  7. short gyrox,gyroy,gyroz;        /*陀螺儀原始數(shù)據(jù)*/
  8. short temperature;                        /*陀螺儀溫度數(shù)據(jù)*/
  9. float Accel_x;                             /*X軸加速度值暫存*/
  10. float Accel_y;                            /*Y軸加速度值暫存*/
  11. float Accel_z;                             /*Z軸加速度值暫存*/
  12. float Gyro_x;                                /*X軸陀螺儀數(shù)據(jù)暫存*/
  13. float Gyro_y;                        /*Y軸陀螺儀數(shù)據(jù)暫存*/
  14. float Gyro_z;                                 /*Z軸陀螺儀數(shù)據(jù)暫存*/
  15. float Angle_x_temp;                  /*由加速度計算的x傾斜角度*/
  16. float Angle_y_temp;                  /*由加速度計算的y傾斜角度*/
  17. float Angle_X_Final;                 /*X最終傾斜角度*/
  18. float Angle_Y_Final;                 /*Y最終傾斜角度*/

  20. /**
  21.   * @brief  讀取數(shù)據(jù)預(yù)處理
  22.   *         
  23.   * @param  NULL
  24.   *
  25.   * @retval NULL
  26.   */
  27. void Angle_Calcu(void)         
  28. {
  29.         /*1.原始數(shù)據(jù)讀取*/
  30.         float accx,accy,accz;                                                /*三方向角加速度值*/
  31.         MPU_Get_Accelerometer(&aacx,&aacy,&aacz);        /*得到加速度傳感器數(shù)據(jù)*/
  32.         MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);        /*得到陀螺儀數(shù)據(jù)*/
  33.         temperature = MPU_Get_Temperature();                /*得到溫度值*/
  34.         Accel_x = aacx;
  35.         Accel_y = aacy;
  36.         Accel_z = aacz;
  37.         Gyro_x  = gyrox;
  38.         Gyro_y  = gyroy;
  39.         Gyro_z  = gyroz;
  40.         
  41.         /*2.角加速度原始值處理過程*/        
  42.         /*加速度傳感器配置寄存器0X1C內(nèi)寫入0x01,設(shè)置范圍為±2g。換算關(guān)系：2^16/4 = 16384LSB/g*/
  43.         if(Accel_x<32764) accx=Accel_x/16384;
  44.         else              accx=1-(Accel_x-49152)/16384;
  45.         if(Accel_y<32764) accy=Accel_y/16384;
  46.         else              accy=1-(Accel_y-49152)/16384;
  47.         if(Accel_z<32764) accz=Accel_z/16384;
  48.         else              accz=(Accel_z-49152)/16384;
  49.         /*加速度反正切公式計算三個軸和水平面坐標(biāo)系之間的夾角*/
  50.         Angle_x_temp=(atan(accy/accz))*180/3.14;
  51.         Angle_y_temp=(atan(accx/accz))*180/3.14;
  52.         /*判斷計算后角度的正負(fù)號*/                                                                                       
  53.         if(Accel_x<32764) Angle_y_temp = +Angle_y_temp;
  54.         if(Accel_x>32764) Angle_y_temp = -Angle_y_temp;
  55.         if(Accel_y<32764) Angle_x_temp = +Angle_x_temp;
  56.         if(Accel_y>32764) Angle_x_temp = -Angle_x_temp;
  57.         
  58.         /*3.角速度原始值處理過程*/
  59.         /*陀螺儀配置寄存器0X1B內(nèi)寫入0x18，設(shè)置范圍為±2000deg/s。換算關(guān)系：2^16/4000=16.4LSB/(°/S)*/
  60.         /*計算角速度*/
  61.         if(Gyro_x<32768) Gyro_x=-(Gyro_x/16.4);
  62.         if(Gyro_x>32768) Gyro_x=+(65535-Gyro_x)/16.4;
  63.         if(Gyro_y<32768) Gyro_y=-(Gyro_y/16.4);
  64.         if(Gyro_y>32768) Gyro_y=+(65535-Gyro_y)/16.4;
  65.         if(Gyro_z<32768) Gyro_z=-(Gyro_z/16.4);
  66.         if(Gyro_z>32768) Gyro_z=+(65535-Gyro_z)/16.4;
  67.         
  68.         /*4.調(diào)用卡爾曼函數(shù)*/
  69.         Kalman_Filter_X(Angle_x_temp,Gyro_x);  /*卡爾曼濾波計算X傾角*/
  70.         Kalman_Filter_Y(Angle_y_temp,Gyro_y);  /*卡爾曼濾波計算Y傾角*/                                                                                                                          
  71. }


  74. /************************ 卡爾曼參數(shù) ****************************/               
  75. float Q_angle = 0.001;                /*角度數(shù)據(jù)置信度，角度噪聲的協(xié)方差*/
  76. float Q_gyro  = 0.003;                /*角速度數(shù)據(jù)置信度，角速度噪聲的協(xié)方差*/
  77. float R_angle = 0.5;                /*加速度計測量噪聲的協(xié)方差*/
  78. float dt      = 0.02;                /*濾波算法計算周期，由定時器定時20ms*/
  79. char  C_0     = 1;                        /*H矩陣值*/
  80. float Q_bias, Angle_err;        /*Q_bias:陀螺儀的偏差  Angle_err:角度偏量*/
  81. float PCt_0, PCt_1, E;                /*計算的過程量*/
  82. float K_0, K_1, t_0, t_1;        /*卡爾曼增益  K_0:用于計算最優(yōu)估計值  K_1:用于計算最優(yōu)估計值的偏差 t_0/1:中間變量*/
  83. float P[4] ={0,0,0,0};                /*過程協(xié)方差矩陣的微分矩陣，中間變量*/
  84. float PP[2][2] = { { 1, 0 },{ 0, 1 } };/*過程協(xié)方差矩陣P*/

  86. /**
  87.   * @brief  卡爾曼函數(shù)
  88.   *         
  89.   * @param  Accel
  90.   * @param  Gyro
  91.   *
  92.   * @retval NULL
  93.   */
  94. void Kalman_Filter_X(float Accel,float Gyro)               
  95. {
  96.         /*
  97.                 步驟一，先驗估計
  98.                 公式：X(k|k-1) = AX(k-1|k-1) + BU(k)
  99.                 X = (Angle,Q_bias)
  100.                 A(1,1) = 1,A(1,2) = -dt
  101.                 A(2,1) = 0,A(2,2) = 1
  102.         */        
  103.         Angle_X_Final += (Gyro - Q_bias) * dt;         /*狀態(tài)方程,角度值等于上次最優(yōu)角度加角速度減零漂后積分*/
  104.         
  105.         /*
  106.                 步驟二，計算過程協(xié)方差矩陣的微分矩陣
  107.                 公式：P(k|k-1)=AP(k-1|k-1)A^T + Q
  108.                 Q(1,1) = cov(Angle,Angle)        Q(1,2) = cov(Q_bias,Angle)
  109.                 Q(2,1) = cov(Angle,Q_bias)        Q(2,2) = cov(Q_bias,Q_bias)
  110.         */
  111.         P[0]= Q_angle - PP[0][1] - PP[1][0];
  112.         P[1]= -PP[1][1];                                                /*先驗估計誤差協(xié)方差*/
  113.         P[2]= -PP[1][1];
  114.         P[3]= Q_gyro;
  115.         PP[0][0] += P[0] * dt;   
  116.         PP[0][1] += P[1] * dt;   
  117.         PP[1][0] += P[2] * dt;
  118.         PP[1][1] += P[3] * dt;        
  119.         
  120.         /*
  121.                 步驟三，計算卡爾曼增益
  122.                 公式：Kg(k)= P(k|k-1)H^T/(HP(k|k-1)H^T+R)
  123.                 Kg = (K_0,K_1) 對應(yīng)Angle,Q_bias增益
  124.                 H = (1,0)        可由z=HX+v求出z:Accel
  125.         */
  126.         PCt_0 = C_0 * PP[0][0];
  127.         PCt_1 = C_0 * PP[1][0];
  128.         E = R_angle + C_0 * PCt_0;
  129.         K_0 = PCt_0 / E;
  130.         K_1 = PCt_1 / E;
  131.         
  132.         /*
  133.                 步驟四，后驗估計誤差協(xié)方差
  134.                 公式：P(k|k)=(I-Kg(k)H)P(k|k-1)
  135.                 也可寫為：P(k|k)=P(k|k-1)-Kg(k)HP(k|k-1)
  136.         */
  137.         t_0 = PCt_0;
  138.         t_1 = C_0 * PP[0][1];
  139.         PP[0][0] -= K_0 * t_0;               
  140.         PP[0][1] -= K_0 * t_1;
  141.         PP[1][0] -= K_1 * t_0;
  142.         PP[1][1] -= K_1 * t_1;
  143.         
  144.         /*
  145.                 步驟五，計算最優(yōu)角速度值
  146.                 公式：X(k|k)= X(k|k-1)+Kg(k)(Z(k)-X(k|k-1))
  147.         */
  148.         Angle_err = Accel - Angle_X_Final;                /*Z(k)先驗估計 計算角度偏差*/
  149.         Angle_X_Final += K_0 * Angle_err;                 /*后驗估計，給出最優(yōu)估計值*/
  150.         Q_bias        += K_1 * Angle_err;                 /*后驗估計，跟新最優(yōu)估計值偏差*/
  151.         Gyro_x         = Gyro - Q_bias;         
  152. }

  154. void Kalman_Filter_Y(float Accel,float Gyro)                 
  155. {
  156.         Angle_Y_Final += (Gyro - Q_bias) * dt;
  157.         P[0]=Q_angle - PP[0][1] - PP[1][0];
  158.         P[1]=-PP[1][1];
  159.         P[2]=-PP[1][1];
  160.         P[3]=Q_gyro;        
  161.         PP[0][0] += P[0] * dt;
  162.         PP[0][1] += P[1] * dt;  
  163.         PP[1][0] += P[2] * dt;
  164.         PP[1][1] += P[3] * dt;        
  165.         Angle_err = Accel - Angle_Y_Final;               
  166.         PCt_0 = C_0 * PP[0][0];
  167.         PCt_1 = C_0 * PP[1][0];        
  168.         E = R_angle + C_0 * PCt_0;        
  169.         K_0 = PCt_0 / E;
  170.         K_1 = PCt_1 / E;        
  171.         t_0 = PCt_0;
  172.         t_1 = C_0 * PP[0][1];
  173.         PP[0][0] -= K_0 * t_0;               
  174.         PP[0][1] -= K_0 * t_1;
  175.         PP[1][0] -= K_1 * t_0;
  176.         PP[1][1] -= K_1 * t_1;               
  177.         Angle_Y_Final        += K_0 * Angle_err;
  178.         Q_bias        += K_1 * Angle_err;         
  179.         Gyro_y   = Gyro - Q_bias;         
  180. }

復(fù)制代碼


  1. /**
  2.   ******************************************************************************
  3.   * @file    main.c
  4.   * @author  
  5.   * @version V1.0
  6.   * @date    2023-4-7
  7.   * @brief   MPU6050陀螺儀數(shù)據(jù)解算（卡爾曼濾波）
  8.   ******************************************************************************
  9.   *
  10.   *
  11.   *
  12.   ******************************************************************************
  13.   */
  14. #include "main.h"
  15. /**
  16.   * @brief  主函數(shù)
  17.   * @param  無
  18.   * @retval 無
  19.   */
  20. int main(void)
  21. {        
  22.         SYS_Init();
  23.         while(1)
  24.         {               
  25.                 /*MPU6050數(shù)據(jù)上報*/
  26.                 DATA_Report();
  27.         }
  28. }
  29. /**
  30.   * @brief  系統(tǒng)初始化總函數(shù)
  31.   * @param  無
  32.   * @retval 無
  33.   */
  34. void SYS_Init(void)
  35. {
  36.         delay_init();  
  37.         NVIC_PriorityGroupConfig(NVIC_PriorityGroup_1);
  38.         uart_init(115200);
  39.         LED_Init();
  40.         MPU_Init();
  41.         /*10Khz的計數(shù)頻率，計數(shù)到200為20ms*/
  42.         TIM4_Int_Init(199,7199);
  43. }
  44. /**
  45.   * @brief  MPU6050數(shù)據(jù)上報
  46.   * @param  無
  47.   * @retval 無
  48.   */
  49. void DATA_Report(void)
  50. {
  51.         /*串口發(fā)送實時俯仰角，橫滾角，XYZ三軸角加速度原始值，XYZ三軸角速度原始值*/
  52.         printf("俯仰角:%.4f 橫滾角:%.4f \
  53.         X加速度:%5d y加速度:%5d z加速度:%5d \
  54.         x速度:%5d y速度:%5d z速度:%5d\r\n",\
  55.         Angle_X_Final,Angle_Y_Final,aacx,aacy,aacz,\
  56.         gyrox,gyroy,gyroz);
  57.         
  58.         
  59. }
復(fù)制代碼

原理圖: 無
仿真: 無
代碼: STM32F103C8T6驅(qū)動MPU6050程序(卡爾曼濾波_軟件IIC_OLED顯示_串口上傳).7z (236.32 KB, 下載次數(shù): 0)




歡迎光臨 (http://www.torrancerestoration.com/bbs/) Powered by Discuz! X3.1