[stm32] MPU6050 HMC5883 Kalman 融合算法移植

简介:


 

一、卡尔曼滤波九轴融合算法stm32尝试

 

1、Kalman滤波文件[.h已经封装为结构体]

  Kalman.h

2、I2C总线代码[这里把MPU和HMC挂接到上面,通过改变SlaveAddress的值来和不同的设备通信]

  I2C.c

3、MPU6050的驱动代码[updataMPU6050中获取数据为什么一正一负不是很清楚,是按照GitHub上arduino版本改的]

  MPU6050.c

4、HMC5883的驱动代码[updataHMC5883直接获取源数据,并未做大的处理]

  HMC5883.c

5、USART简单的单字节发送的串口驱动文件

  USART.c

6、非精确延时函数集[其他文件所需的一些延时放在这里]

  DELAY.c

7、main函数文件

  main.c

 

 

程序说明:

复制代码
 1 int main(void)
 2 {
 3       RCC_Configuration();                   //系统时钟配置    
 4       USART1_Configuration();
 5       I2C_GPIO_Config();
 6       InitHMC5883();
 7     InitMPU6050();
 8     InitAll();    
 9 //    SYSTICK_Configuration();                
10      while(1)
11     {
12         func();
13       }
14 }
复制代码
  • 主函数首先初始化系统时钟、串口、I2C总线、HMC5883磁力计和MPU6050加速计&陀螺仪,这里重点讲InitAll()函数和func()函数
复制代码
 1 void InitAll()
 2 {
 3     /* Set Kalman and gyro starting angle */
 4     updateMPU6050();
 5     updateHMC5883();
 6     updatePitchRoll();
 7     updateYaw();
 8     
 9     setAngle(&kalmanX,roll); // First set roll starting angle
10     gyroXangle = roll;
11     compAngleX = roll;
12     
13     setAngle(&kalmanY,pitch); // Then pitch
14     gyroYangle = pitch;
15     compAngleY = pitch;
16     
17     setAngle(&kalmanZ,yaw); // And finally yaw
18     gyroZangle = yaw;
19     compAngleZ = yaw;
20     
21 //    timer = micros; // Initialize the timer    
22 }
复制代码
  • 第4、5两行从传感器中读取原数据,第6行函数根据加速计的值由空间几何的知识刷新Pitch和Roll数据,第7行函数根据复杂计算(这个实在看不懂,大概是磁力计有偏差,一方面进行误差校正,另一方面还用到了kalman滤波的数据,挺麻烦的)其实就是刷新yaw的值。
  • 后面把kalman滤波值、陀螺仪计量值、磁力计计算值都赋值为上面计算的roll、pitch、yaw的值。
复制代码
 1 void func()
 2 {
 3     double gyroXrate,gyroYrate,gyroZrate,dt=0.01;
 4     /* Update all the IMU values */
 5     updateMPU6050();
 6     updateHMC5883();
 7     
 8 //    dt = (double)(micros - timer) / 1000; // Calculate delta time
 9 //    timer = micros;
10 //    if(dt<0)dt+=(1<<20);    //时间是周期性的,有可能当前时间小于上次时间,因为这个周期远大于两次积分时间,所以最多相差1<<20
11 
12     /* Roll and pitch estimation */
13     updatePitchRoll();             //用采集的加速计的值计算roll和pitch的值
14     gyroXrate = gyroX / 131.0;     // Convert to deg/s    把陀螺仪的角加速度按照当初设定的量程转换为°/s
15     gyroYrate = gyroY / 131.0;     // Convert to deg/s
16     
17     #ifdef RESTRICT_PITCH        //如果上面有#define RESTRICT_PITCH就采用这种方法计算,防止出现-180和180之间的跳跃
18     // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
19     if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
20         setAngle(&kalmanX,roll);
21         compAngleX = roll;
22         kalAngleX = roll;
23         gyroXangle = roll;
24     } else
25     kalAngleX = getAngle(&kalmanX, roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
26     
27     if (fabs(kalAngleX) > 90)
28         gyroYrate = -gyroYrate; // Invert rate, so it fits the restricted accelerometer reading
29     kalAngleY = getAngle(&kalmanY,pitch, gyroYrate, dt);
30     #else
31     // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
32     if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) {
33         kalmanY.setAngle(pitch);
34         compAngleY = pitch;
35         kalAngleY = pitch;
36         gyroYangle = pitch;
37     } else
38     kalAngleY = getAngle(&kalmanY, pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter
39     
40     if (abs(kalAngleY) > 90)
41         gyroXrate = -gyroXrate; // Invert rate, so it fits the restricted accelerometer reading
42     kalAngleX = getAngle(&kalmanX, roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
43     #endif
44     
45     
46     /* Yaw estimation */
47     updateYaw();
48     gyroZrate = gyroZ / 131.0; // Convert to deg/s
49     // This fixes the transition problem when the yaw angle jumps between -180 and 180 degrees
50     if ((yaw < -90 && kalAngleZ > 90) || (yaw > 90 && kalAngleZ < -90)) {
51         setAngle(&kalmanZ,yaw);
52         compAngleZ = yaw;
53         kalAngleZ = yaw;
54         gyroZangle = yaw;
55     } else
56     kalAngleZ = getAngle(&kalmanZ, yaw, gyroZrate, dt); // Calculate the angle using a Kalman filter
57     
58     
59     /* Estimate angles using gyro only */
60     gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter
61     gyroYangle += gyroYrate * dt;
62     gyroZangle += gyroZrate * dt;
63     //gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate from the Kalman filter
64     //gyroYangle += kalmanY.getRate() * dt;
65     //gyroZangle += kalmanZ.getRate() * dt;
66     
67     /* Estimate angles using complimentary filter */互补滤波算法
68     compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter
69     compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch;
70     compAngleZ = 0.93 * (compAngleZ + gyroZrate * dt) + 0.07 * yaw;
71     
72     // Reset the gyro angles when they has drifted too much
73     if (gyroXangle < -180 || gyroXangle > 180)
74         gyroXangle = kalAngleX;
75     if (gyroYangle < -180 || gyroYangle > 180)
76         gyroYangle = kalAngleY;
77     if (gyroZangle < -180 || gyroZangle > 180)
78         gyroZangle = kalAngleZ;
79     
80     
81     send(roll,pitch,yaw);
82 //    send(gyroXangle,gyroYangle,gyroZangle);
83 //    send(compAngleX,compAngleY,compAngleZ);
84 //    send(kalAngleX,kalAngleY,kalAngleZ);
85 //    send(kalAngleY,compAngleY,gyroYangle);
86 } 
复制代码
  • 5、6两行获取传感器原数据
  • 8~10行计算两次测量的时间差dt[因为我采用很多方法试验来计算时间差都不奏效,所以最终还是放弃了这种算法,还是用我原来的DMP算法,DMP对水平方向的很好,z方向的不好,要用磁力计来纠正!可以参考这里面的算法!]
  • 13~56行是用kalman滤波来求当前的3个角并稳值
  • 60~62行是用陀螺仪的角速度积分获得当前陀螺仪测量的3个角度值
  • 67~70行使用互补滤波算法对磁力计当前测量3个角的值进行计算
  • 72~78行是稳值
  • 81行是串口发送

PS:总的来说按照arduino的代码进行照抄移植成c语言版的,当前卡在了如何较为准确的计算dt,即:两次测量的时间差(这里为了测试我给了dt一个定值0.01s,这是很不准确的做法!!!)[我采用定时器的方法总是会莫名的跑偏,我想可能是中断的影响,好吧,还是用原来实验的DMP吧,这个算法看似高大上,其实比较占MCU的资源啦,自带的DMP也存在一些缺陷,对水平方向的偏角测量较为精准,误差在1°左右,而在z轴方向上的误差较大,容易跑偏,所以还要用磁力计进行纠正Z轴的测量值~]

 

PS:相关链接





本文转自beautifulzzzz博客园博客,原文链接:http://www.cnblogs.com/zjutlitao/p/3915786.html ,如需转载请自行联系原作者
相关文章
|
8月前
|
缓存 Java C语言
嵌入式 LVGL移植到STM32F4
嵌入式 LVGL移植到STM32F4
|
1月前
|
消息中间件 Web App开发 API
FreeRTOS介绍 和 将FreeRTOS移植到STM32F103C8T6
FreeRTOS介绍 和 将FreeRTOS移植到STM32F103C8T6
FreeRTOS介绍 和 将FreeRTOS移植到STM32F103C8T6
|
1月前
|
机器学习/深度学习 算法 计算机视觉
[YOLOv8/YOLOv7/YOLOv5系列算法改进NO.5]改进特征融合网络PANET为BIFPN(更新添加小目标检测层yaml)
本文介绍了改进YOLOv5以解决处理复杂背景时可能出现的错漏检问题。
145 5
|
1月前
|
算法
MATLAB|【免费】融合正余弦和柯西变异的麻雀优化算法SCSSA-CNN-BiLSTM双向长短期记忆网络预测模型
这段内容介绍了一个使用改进的麻雀搜索算法优化CNN-BiLSTM模型进行多输入单输出预测的程序。程序通过融合正余弦和柯西变异提升算法性能,主要优化学习率、正则化参数及BiLSTM的隐层神经元数量。它利用一段简单的风速数据进行演示,对比了改进算法与粒子群、灰狼算法的优化效果。代码包括数据导入、预处理和模型构建部分,并展示了优化前后的效果。建议使用高版本MATLAB运行。
|
1月前
|
人工智能 算法 测试技术
论文介绍:进化算法优化模型融合策略
【5月更文挑战第3天】《进化算法优化模型融合策略》论文提出使用进化算法自动化创建和优化大型语言模型,通过模型融合提升性能并减少资源消耗。实验显示,这种方法在多种基准测试中取得先进性能,尤其在无特定任务训练情况下仍能超越参数更多模型。同时,该技术成功应用于创建具有文化意识的日语视觉-语言模型。然而,模型融合可能产生逻辑不连贯响应和准确性问题,未来工作将聚焦于图像扩散模型、自动源模型选择及生成自我改进的模型群体。[论文链接: https://arxiv.org/pdf/2403.13187.pdf]
123 1
|
1月前
|
C语言
【STM32 CubeMX】移植u8g2(一次成功)
【STM32 CubeMX】移植u8g2(一次成功)
272 0
|
1月前
|
机器学习/深度学习 算法 数据挖掘
SciPy与机器学习:融合科学计算与智能算法
【4月更文挑战第17天】本文探讨了如何结合SciPy与机器学习,SciPy作为Python科学计算库,为机器学习提供数学基础和工具。在机器学习中,SciPy用于特征选择(如ANOVA和SVD)、聚类(K-Means和层次聚类)、优化(梯度下降和牛顿法)以及信号处理。通过与scikit-learn等机器学习框架结合,实现高效数据处理和模式识别。
|
1月前
|
编解码 算法
基于双树复小波变换和稀疏表示的多光谱和彩色图像融合算法matlab仿真
基于双树复小波变换和稀疏表示的多光谱和彩色图像融合算法matlab仿真
|
1月前
|
算法
基于稀疏表示的小波变换多光谱图像融合算法matlab仿真
基于稀疏表示的小波变换多光谱图像融合算法matlab仿真
|
1月前
|
机器学习/深度学习 人工智能 供应链
人工智能与供应链行业融合:预测算法的通用化与实战化
人工智能与供应链行业融合:预测算法的通用化与实战化