基于MPU9250九轴数据获取姿态

基于MPU9250九轴数据获取姿态

使用了正点原子阿波罗F429开发板（底板包含MPU9250），将计算的航向角（姿态）在串口打印出来。

主要分为以下几个部分：

1. 读取传感器原始数据；

2. 对原始数据做零偏矫正，平滑滤波处理；

3. 融合数据，计算姿态。

读取数据没什么好说的，I2C和SPI均可，电脑串口直接读就行，网上很多资料。

MPU9250有两部分：MPU6500和AK8963。其中，MPU6500是一个集成了三轴加速度计和三轴陀螺仪的传感器，AK8963则是一个三轴磁力计。它们的坐标标准不同，计算时需要变换一下。

零偏矫正：对加速度计和磁力计使用了基于最小二乘思想的椭球矫正（假设相互正交，无非正交干扰）；对陀螺仪，每次通电后判断传感器是否静止，若是，收集数据作为零偏，做减去均值处理。

平滑滤波：对加速度计和磁力计做窗口为8的滑动均值滤波，陀螺仪也可以这么做，不过因为陀螺仪是高动态器件，随机误差较小，未如此处理。

融合数据，计算姿态：有一些简单的方法，比如mahony互补滤波，madgwick梯度下降法。个人觉得mahony算法的PI参数不好调，遂使用基于四元数的madgwick算法。

利用加速度计和磁力计数据初始化四元数：

式中左侧三个参数分别表示通过磁力计数据和加速度数据解算得到的初始俯仰角、横滚角以及航向角。

参考的椭球矫正代码来自空间二次曲面数据拟合算法推导及仿真分析

参考的梯度下降法代码来自四元数姿态的梯度下降法推导和解读，来源于madgwick的两篇论文。

最后，附上matlab和C代码，留作纪念。

2020年6月10日，星期三下午18点。