一、卡尔曼滤波九轴融合算法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 angle10 gyroXangle = roll;11 compAngleX = roll;12 13 setAngle(&kalmanY,pitch); // Then pitch14 gyroYangle = pitch;15 compAngleY = pitch;16 17 setAngle(&kalmanZ,yaw); // And finally yaw18 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<<2011 12 /* Roll and pitch estimation */13 updatePitchRoll(); //用采集的加速计的值计算roll和pitch的值14 gyroXrate = gyroX / 131.0; // Convert to deg/s 把陀螺仪的角加速度按照当初设定的量程转换为°/s15 gyroYrate = gyroY / 131.0; // Convert to deg/s16 17 #ifdef RESTRICT_PITCH //如果上面有#define RESTRICT_PITCH就采用这种方法计算，防止出现-180和180之间的跳跃18 // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees19 if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {20 setAngle(&kalmanX,roll);21 compAngleX = roll;22 kalAngleX = roll;23 gyroXangle = roll;24 } else25 kalAngleX = getAngle(&kalmanX, roll, gyroXrate, dt); // Calculate the angle using a Kalman filter26 27 if (fabs(kalAngleX) > 90)28 gyroYrate = -gyroYrate; // Invert rate, so it fits the restricted accelerometer reading29 kalAngleY = getAngle(&kalmanY,pitch, gyroYrate, dt);30 #else31 // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees32 if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) {33 kalmanY.setAngle(pitch);34 compAngleY = pitch;35 kalAngleY = pitch;36 gyroYangle = pitch;37 } else38 kalAngleY = getAngle(&kalmanY, pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter39 40 if (abs(kalAngleY) > 90)41 gyroXrate = -gyroXrate; // Invert rate, so it fits the restricted accelerometer reading42 kalAngleX = getAngle(&kalmanX, roll, gyroXrate, dt); // Calculate the angle using a Kalman filter43 #endif44 45 46 /* Yaw estimation */47 updateYaw();48 gyroZrate = gyroZ / 131.0; // Convert to deg/s49 // This fixes the transition problem when the yaw angle jumps between -180 and 180 degrees50 if ((yaw < -90 && kalAngleZ > 90) || (yaw > 90 && kalAngleZ < -90)) {51 setAngle(&kalmanZ,yaw);52 compAngleZ = yaw;53 kalAngleZ = yaw;54 gyroZangle = yaw;55 } else56 kalAngleZ = getAngle(&kalmanZ, yaw, gyroZrate, dt); // Calculate the angle using a Kalman filter57 58 59 /* Estimate angles using gyro only */60 gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter61 gyroYangle += gyroYrate * dt;62 gyroZangle += gyroZrate * dt;63 //gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate from the Kalman filter64 //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 filter69 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 much73 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的工程：
- 3轴加速计网页pdf版使用详细资料（公式，计算）：
- 加速计和磁力计倾斜补偿算法网页pdf资料：
- 上述工程代码（你得自己解决dt问题）：
- MPU6050寄存器中文版：
- MPU6050中文资料：
- MPU6050数据轻松分析（基于arduino的kalman滤波讲解含代码）：
- pitch yaw roll 相关知识(1)：
- pitch yaw roll 相关知识(2)：
- pitch yaw roll 相关知识(3)：
- 四元数与欧拉角知识：
本文转自beautifulzzzz博客园博客，原文链接：http://www.cnblogs.com/zjutlitao/p/3915786.html ，如需转载请自行联系原作者