导航及无人机姿态检测融合算法很难？用sensor fusion来搞定！

在后的后计算相乘的公式如下图：

至此初始化四元数完成。

第三步就是使用AHRSUpdate算法了，用完以后再根据公式计算出欧拉角，此公式跟旋转顺序和旋转使用的轴向有关，我们的旋转顺序是Z-X-Y，且绕Z是Yaw，绕X是Pitch，绕Y是Roll，推到过程如下图：

首先得出3个方向余旋矩阵：

下图绕Z轴Yaw：

下图绕X轴pitch：

下图绕Y轴Roll：

然后按照我们的Z-X-Y顺序求得C=Croll * Cpitch * Cyaw，如下图：

将上图的方向余旋矩阵C与下图的四元数姿态矩阵做对比，即可求出欧拉角，注意上图的方向余旋矩阵C是随着我们对坐标轴的定义变化而变化的，而下图的四元数姿态矩阵是固定的：

最后一步就是求出欧拉角，公式如下图：

以下是静止时，刚上电时的数据：

以下是上电半个小时以后的数据：

可以看出正常的静止状态下，数据的波动范围是不超过1度的，也不会有漂移。具体效果还需要上机架飞起来以后再做调整。

最后总结一下：

我的代码中，0°

注意跑108Mhz时，需要修改延迟函数和串口函数，适当增加一下延迟函数的数值避免I2C通信失败，修改串口函数是为了避免108Mhz下串口乱码问题