MPU6050 获取角度理论推导(三)---9轴融合算法

再接着上一篇：

https://mp.csdn.net/postedit/101777722

• 姿态角解算---互补滤波补充(融合磁力计)

 

前面介绍了互补滤波法的算法流程和程序实现，但是仅仅只是融合了三轴加速度计和三轴陀螺仪的数据解算出姿态。

由于机体水平时，加速度计无法测量绕 Z 轴的旋转量，即偏航角yaw,并且磁力计也同样无法测得z轴的旋转量。

所以，我们考虑使用加速度计和磁力计同时对陀螺仪进行校正。这次介绍如何融合磁力计的数据进行姿态解算。

 

在介绍算法之前，想再提一点自己的感想。很多新手入门姿态解算算法时，最先接触的就是这个Mahony的互补滤波算法，也相对简单一些。有的人觉得数学推导很麻烦，也不愿意花时间研究代码，懵懵懂懂地把程序复制粘贴过去就随便填了几个参数，然后就去看现象。其实大家都知道这种方法是不对的吧，没出问题还好，一旦出了错就不知所措了。很多时候，飞控出了问题，不一定是控制的问题，有时候问题出在姿态解算上（“血与泪”的教训，我当初就是在这个地方被坑了很久）。得到了姿态角后，先看看波形，看看数据怎么样，滤波器有没有用。（不想自己写程序和调试的，请无视这些。）

这里再推荐一个很实用的工具：匿名上位机。功能很强大，可以自己写程序上传数据，在上位机上看波形，一切OK之后再进行下一步的工作。

这是匿名上位机的界面的截图，可以到匿名科创的官网上下载：

 

好了，开始介绍算法部分吧！ 
还是按照这张框图得流程来介绍： 

首先，假设磁力计测量得到的三轴磁场数据为  。

为方便运算，像此前一样，将数据归一化之后再进行运算。得到载体坐标系b下的标准磁场数据 ，

其中 。

为表示简便，将归一化之后的数据还是表示为 。

将这组载体坐标系b下测得的数据 旋转到导航坐标系n下，得到 。

坐标系的旋转相当于乘以一个旋转矩阵  。 
使用以前推导得到的结论，