四轴飞行器飞控研究（三）--姿态完整改进算法

继之前研究了一些飞行姿态理论方面的问题后，又找到了之前很流行的一段外国大神写的代码，来分析分析。       第二篇文章的最后，讲到了文章中的算法在实际使用中有重大缺陷。

       大家都知道，分析算法理论的时候很多情况下我们没有考虑太多外界干扰的情况，原因是很多情况下，传感器的精度以及受到的干扰并不会特别大，而显著的影响到算法。但是在IMU系统中，有点不同。由于地磁场十分微弱，而我们生活中有大量使用电子设备，使得磁场非常的混乱，以至于地磁传感器非常容易受到干扰。

       由于以上算法把地磁传感器一同加入到姿态的测定中，并基本给予了地磁传感器与加速度传感器同样的加权，导致地磁传感器一旦被干扰，会对姿态产生地球重力突然被干扰一样的结果。。。对姿态的测量是毁灭性的。

       综上，考虑到磁场的不稳定性，必须对地磁传感器进行降权处理，使得他对姿态的影响变小。

       于是设计了以下的算法。

       将磁场传感器的数据在姿态角度中剔除，更新姿态的俯仰角（PITCH）以及横滚角（ROLL）的时候只使用加速度传感器以及陀螺仪(角速度传感器)。

       只在计算偏航角的（YAW）的时候使用磁场传感器。也就是只使用磁场传感器作为一个电子指南针，定位整个姿态在水平面旋转的角度。这样设计，让磁场传感器只影响姿态中的一个数值，减少了磁场的权重，即使磁场收到干扰，也不会导致姿态骤变，使得四轴坠机。

       在对YAW进行计算的时候使用了如下函数。

       eulerAngleRaw.yaw = 0.9 * (eulerAngleRaw.yaw - gzF*2*halfT) + 0.1 * angleMagYaw;

       此处的0.9和0.1是可以变动的但他们相加应该为1，此处为一个最简单的1阶低通滤波器，增加0.1则是增大截止频率。

       算法的流程图是这样的：

程序流程图

       新的姿态更新算法是这样的

voidAHRSupdate(float gxf, float gyf, float gzf, float axf, float ayf, float azf, float mxf, float myf, float mzf)

{