[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:相关链接

GitHub上面的基于arduino的工程：https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter.git
3轴加速计网页pdf版使用详细资料（公式，计算）：http://www.freescale.com/files/sensors/doc/app_note/AN3461.pdf
加速计和磁力计倾斜补偿算法网页pdf资料：http://www.freescale.com/files/sensors/doc/app_note/AN4248.pdf
上述工程代码（你得自己解决dt问题）：http://pan.baidu.com/s/1gdlATFH
MPU6050寄存器中文版：http://pan.baidu.com/s/1gdIKUK7
MPU6050中文资料：http://pan.baidu.com/s/1bnkxjhP
MPU6050数据轻松分析（基于arduino的kalman滤波讲解含代码）：http://pan.baidu.com/s/1eQvMtX4

pitch yaw roll 相关知识(1)：http://blog.163.com/vipwdp@126/blog/static/150224366201281935518196/
pitch yaw roll 相关知识(2)：http://www.cnblogs.com/wqj1212/archive/2010/11/21/1883033.html
pitch yaw roll 相关知识(3)：http://www.cnblogs.com/tclikang/archive/2012/11/09/2761988.html
四元数与欧拉角知识：http://www.cnblogs.com/wqj1212/archive/2010/11/21/1883033.html

本文转自beautifulzzzz博客园博客，原文链接：http://www.cnblogs.com/zjutlitao/p/3915786.html ，如需转载请自行联系原作者

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

热门文章

最新文章

相关课程

相关电子书

相关实验场景