数据融合

现在有了三个传感器，都能在一定程度上测量角度关系，但是究竟用哪一个值才能准确表现出目前飞行器的姿态，又姿态传感器方案分析可知，应该在短时间内相信陀螺仪，隔一段时间后，读取加速度计和磁传感器的值，来校准陀螺仪的值。关于数据融合，也有很多方法，这里采用一种最为形象，贴切的，简单易懂的方法，即互补滤波算法。

单轴融合原理如下图所示：

通过上图可以看出，陀螺仪在融合后的角度基础上积分，然后与加速度计测量的角度融合，然后又提供给下一次积分，看起来更像一个反馈系统。

上面仅仅是单轴的融合，理解了单轴就可以结合四元数，得到3维的姿态融合。

全姿态融合原理如下图所示：

图中的符号解释：

Bg是加速度计测得的值，扩充为四元数，Bg=[0，Bgx，Bgy，Bgz]，上标B代表是在体坐标系(Body Frame)下的测量值

Bh是磁传感器计测得的值，扩充为四元数，Bh=[0，Bhx，Bhy，Bhz]

Eh是参考坐标系下的固有磁场，地磁场，上标E代表是在参考坐标系(earth Frame)下的值

Eg是参考坐标系下的固有加速度，重力加速度

p，q，r是陀螺仪测得的角速度

a，b，c，d是姿态四元数Q

关于用加速度计和磁传感器求姿态

假设现在已经得到正确的姿态四元数Q，那么可以利用四元数旋转将参考坐标系和体坐标系下的向量互相转换，将Eh和Eg转换到体坐标系下(BEh=Q×Eh×Q* ， BEg=Q×Eg×Q*，BEh，BEg是参考坐标系的Eh，Eg由Q旋转到体坐标系下)如果在理想的条件下，那么Bg=BEg，Bh=BEh，也就是 { Q×Eh×Q* - Bh = 0} ， { Q×Eg×Q* - Bg = 0}这两个方程成立，联立这两个方程就可以解得姿态四元数Q，不过由于各种误差的存在，这个方程组无解，因此我们只能找到最优解，找最优解的最小方差问题有许多方法可以采用，如梯度下降]法和高斯牛顿法，图中是用的高斯牛顿法，由于高斯牛顿法含有矩阵求逆的计算，计算量超大。

陀螺仪积分求姿态

下图是一个全姿态互补滤波的流程：

加速度计和磁传感器经过高斯牛顿迭代得到姿态误差速率，陀螺仪直接通过四元数微

分方程得到姿态四元数速率，两个加起来积分得到姿态四元数，然后再有四元数变为欧拉角，从而对飞行器做出相应的控制。