最近在研究小四轴的飞行，姿态检测主要用到的传感器是MPU6050。从MPU6050读出来的加速度和角速度数据最后要转成姿态，可以转换成欧拉角（偏航角、俯仰角和滚转角）或四元数表示，为了减少计算量（欧拉角涉及正弦运算，运算量相对较大），方便在STM32主控上实现，可以转换成四元数表示。那么问题来了，四元数姿态融合算法哪家强？这里介绍圆点博士小四轴飞控开源代码关于四元数姿态融合的算法以及代码实现，不能说最强（国外有很多优秀的飞控算法），只是实现效果可以满足小四轴的成功飞行，个人水平渣渣，请大牛批评指正。博士的代码中主要用到的是互补滤波法。

先介绍一下互补滤波的基本概念，这是阿莫论坛上一个会员的总结：对mpu6050来说，加速度计对四轴或小车的加速度比较敏感，取瞬时值计算倾角误差比较大；而陀螺仪积分得到的角度不受小车加速度的影响，但是随着时间的增加积分漂移和温度漂移带来的误差比较大。所以这两个传感器正好可以弥补相互的缺点。不过要怎么弥补呢？经过上面的介绍是否感觉到可以用滤波器做文章呢？这里讲的互补滤波就是在短时间内采用陀螺仪得到的角度做为最优，定时对加速度采样来的角度进行取平均值来校正陀螺仪的得到的角度。就是，短时间内用陀螺仪比较准确，以它为主；长时间用加速度计比较准确，这时候加大它的比重，这就是互补了，不过滤波在哪里？加速度计要滤掉高频信号，陀螺仪要滤掉低频信号，互补滤波器就是根据传感器特性不同，通过不同的滤波器（高通或低通，互补的），然后再相加得到整个频带的信号，例如，加速度计测倾角，其动态响应较慢，在高频时信号不可用，所以可通过低通抑制高频；陀螺响应快，积分后可测倾角，不过由于零漂等，在低频段信号不好。通过高通滤波可抑制低频噪声。将两者结合，就将陀螺和加表的优点融合起来，得到在高频和低频都较好的信号，互补滤波需要选择切换的频率点，即高通和低通的频率。

我个人觉得互补滤波在博士代码里的基本思想是以角速度积分得到角度为主进行姿态解算，但是由于周围环境或自身器件的原因，角速度计积分出来的角度可能会产生误差的累积，长时间可能引起严重的角度偏移，而加速度计虽然精度不高但没有积累误差，可以用来对角度进行纠正。实验证明，这种算法很好的消除了角度偏移。