姿态解算进阶：互补滤波（陀螺仪、加速度计、地磁计数据融合）

互补滤波原理：  

     在四轴入门理论知识那节我们说，加速度计和磁传感器都是极易受外部干扰的传感器，都只能得到2维的角度关系，但是测量值随时间的变化相对较小，结合加速度计和磁传感器可以得到3维的角度关系。陀螺仪可以积分得到三维的角度关系，动态性能好，受外部干扰小，但测量值随时间变化比较大。可以看出，它们优缺点互补，结合起来才能有好的效果。那么三者的数据如何融合呢，接下来介绍互补滤波算法。

     互补滤波就是在短时间内采用陀螺仪得到的角度做为最优，定时对加速度采样来的角度进行取平均值来校正陀螺仪的得到的角度。即短时间内用陀螺仪比较准确，以它为主；长时间用加速度计比较准确，这时候加大它的比重，这就是互补了，不过滤波在哪里？加速度计要滤掉高频信号，陀螺仪要滤掉低频信号，互补滤波器就是根据传感器特性不同，通过不同的滤波器（高通或低通，互补的），然后再相加得到整个频带的信号，例如，加速度计测倾角，其动态响应较慢，在高频时信号不可用，所以可通过低通抑制高频；陀螺响应快，积分后可测倾角，不过由于零漂等，在低频段信号不好。通过高通滤波可抑制低频噪声。将两者结合，就将陀螺和加表的优点融合起来，得到在高频和低频都较好的信号，互补滤波需要选择切换的频率点，即高通和低通的频率。

互补滤波原理框图：

加速度计补偿：

    假设n系为地理坐标系，b系为机体坐标系，在地理坐标系中，加速度的输出为：，经过矩阵转换后的值为：。在b系中，加速度的测量值为：，现在和