设为首页收藏本站
开启辅助访问 切换到宽版

 找回密码
 立即注册

QQ登录

只需一步，快速开始

热门： 51单片机 | 24小时必答区 | 单片机教程 | 单片机DIY制作 | STM32 | Cortex M3 | 模数电子 | 电子DIY制作 | 音响/功放 | 拆机乐园 | Arduino | 嵌入式OS | 程序设计

搜索
»论坛 › 嵌入式/单片机论坛 › STM32/8 › mpu6050卡尔曼滤波STM32单片机程序 详细注释说明
返回列表 发新帖
查看: 1109|回复: 0
打印 上一主题 下一主题
收起左侧

mpu6050卡尔曼滤波STM32单片机程序 详细注释说明

[复制链接]
跳转到指定楼层
楼主
ID:583948 发表于 2024-11-29 12:10 | 只看该作者 回帖奖励 |倒序浏览 |阅读模式
基于STM32F103 MPU6050卡尔曼滤波算法，详细注释说明！
  1. #include "kalman.h"
  2. #include "mpu6050.h"
  3. #include "math.h"


  6. short aacx,aacy,aacz;                /*加速度传感器原始数据*/
  7. short gyrox,gyroy,gyroz;        /*陀螺仪原始数据*/
  8. short temperature;                        /*陀螺仪温度数据*/
  9. float Accel_x;                             /*X轴加速度值暂存*/
  10. float Accel_y;                            /*Y轴加速度值暂存*/
  11. float Accel_z;                             /*Z轴加速度值暂存*/
  12. float Gyro_x;                                /*X轴陀螺仪数据暂存*/
  13. float Gyro_y;                        /*Y轴陀螺仪数据暂存*/
  14. float Gyro_z;                                 /*Z轴陀螺仪数据暂存*/
  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  读取数据预处理
  22.   *         
  23.   * @param  NULL
  24.   *
  25.   * @retval NULL
  26.   */
  27. void Angle_Calcu(void)         
  28. {
  29.         /*1.原始数据读取*/
  30.         float accx,accy,accz;                                                /*三方向角加速度值*/
  31.         MPU_Get_Accelerometer(&aacx,&aacy,&aacz);        /*得到加速度传感器数据*/
  32.         MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);        /*得到陀螺仪数据*/
  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内写入0x01,设置范围为±2g。换算关系：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.         /*加速度反正切公式计算三个轴和水平面坐标系之间的夹角*/
  50.         Angle_x_temp=(atan(accy/accz))*180/3.14;
  51.         Angle_y_temp=(atan(accx/accz))*180/3.14;
  52.         /*判断计算后角度的正负号*/                                                                                       
  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内写入0x18，设置范围为±2000deg/s。换算关系：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.调用卡尔曼函数*/
  69.         Kalman_Filter_X(Angle_x_temp,Gyro_x);  /*卡尔曼滤波计算X倾角*/
  70.         Kalman_Filter_Y(Angle_y_temp,Gyro_y);  /*卡尔曼滤波计算Y倾角*/                                                                                                                          
  71. }


  74. /************************ 卡尔曼参数 ****************************/               
  75. float Q_angle = 0.001;                /*角度数据置信度，角度噪声的协方差*/
  76. float Q_gyro  = 0.003;                /*角速度数据置信度，角速度噪声的协方差*/
  77. float R_angle = 0.5;                /*加速度计测量噪声的协方差*/
  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:用于计算最优估计值  K_1:用于计算最优估计值的偏差 t_0/1:中间变量*/
  83. float P[4] ={0,0,0,0};                /*过程协方差矩阵的微分矩阵，中间变量*/
  84. float PP[2][2] = { { 1, 0 },{ 0, 1 } };/*过程协方差矩阵P*/

  86. /**
  87.   * @brief  卡尔曼函数
  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;         /*状态方程,角度值等于上次最优角度加角速度减零漂后积分*/
  104.         
  105.         /*
  106.                 步骤二，计算过程协方差矩阵的微分矩阵
  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];                                                /*先验估计误差协方差*/
  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) 对应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.                 步骤四，后验估计误差协方差
  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.                 步骤五，计算最优角速度值
  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;                 /*后验估计，给出最优估计值*/
  150.         Q_bias        += K_1 * Angle_err;                 /*后验估计，跟新最优估计值偏差*/
  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. }

复制代码


  1. /**
  2.   ******************************************************************************
  3.   * @file    main.c
  4.   * @author  
  5.   * @version V1.0
  6.   * @date    2023-4-7
  7.   * @brief   MPU6050陀螺仪数据解算（卡尔曼滤波）
  8.   ******************************************************************************
  9.   *
  10.   *
  11.   *
  12.   ******************************************************************************
  13.   */
  14. #include "main.h"
  15. /**
  16.   * @brief  主函数
  17.   * @param  无
  18.   * @retval 无
  19.   */
  20. int main(void)
  21. {        
  22.         SYS_Init();
  23.         while(1)
  24.         {               
  25.                 /*MPU6050数据上报*/
  26.                 DATA_Report();
  27.         }
  28. }
  29. /**
  30.   * @brief  系统初始化总函数
  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的计数频率，计数到200为20ms*/
  42.         TIM4_Int_Init(199,7199);
  43. }
  44. /**
  45.   * @brief  MPU6050数据上报
  46.   * @param  无
  47.   * @retval 无
  48.   */
  49. void DATA_Report(void)
  50. {
  51.         /*串口发送实时俯仰角，横滚角，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. }
复制代码

原理图: 无
仿真: 无
代码: STM32F103C8T6驱动MPU6050程序(卡尔曼滤波_软件IIC_OLED显示_串口上传).7z (236.32 KB, 下载次数: 0)
本主题由 admin 执行 移动

评分

参与人数 1黑币 +50 收起 理由
admin + 50 共享资料的黑币奖励！

查看全部评分

分享到:  QQ好友和群QQ好友和群 QQ空间QQ空间 腾讯微博腾讯微博 腾讯朋友腾讯朋友
收藏收藏2 分享淘帖 顶顶 踩踩
回复

使用道具 举报

返回列表 发新帖
高级模式
B Color Image Link Quote Code Smilies
您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

小黑屋|51黑电子论坛 |51黑电子论坛6群 QQ 管理员QQ:125739409;技术交流QQ群281945664

Powered by 单片机教程网

快速回复 返回顶部 返回列表