加速度计、磁力计校正陀螺仪求姿态角（互补滤波算法）

加速度计测的是三轴加速度，磁力计测的是三轴磁感应强度。陀螺仪测得是角速度，用来更新旋转矩阵（这里注意一下，我们老说角速度积分得到姿态角，实际上是利用角速度更新旋转矩阵，然后通过旋转矩阵求姿态角的）。前面已经讲过，重力加速度在机体坐标系的投影对应旋转矩阵的第三行，我们认为飞机自身加速度很小，认为加计测得三轴加速度就是重力加速度，那么它就对应旋转矩阵的第三行元素；同时陀螺仪也会得到一个旋转矩阵的第三行元素。我们就是通过两者来校正的。磁力计同理。

首先讲一下互补滤波算法。http://wenku.baidu.com/link?url=cpNnAh-5UFtIVc5F9CJZBk6eG9FAcyUsh6DJ_f1tk7MuzrsHCI2eIMUzOoS4UsKLjVJTseSwkIYraWtiJ_iKqeZ3RE7CvijiVQxCLy_oYQ这个文档还讲得挺清楚的。由于对信号处理、滤波不熟，我是这么来理解的。加计、磁力计