mpu6050姿态解算_MPU6050姿态解算系列三：互补滤波

背景介绍：
姿态解算入门基础系列推文已经发过两篇：《MPU6050 姿态解算系列一：加速度姿态解算》和《MPU6050 姿态解算系列二：陀螺仪姿态解算》。
跟下来的读者已经掌握了分别单独用“加速度计”和“陀螺仪”解算姿态的方法，同时也了解了各自的优缺点。本篇目标：同时使用加速度计和陀螺仪的输出数据解算出相对准确、抗噪、不飘移的姿态估计。本篇特点：少而精，用易懂的生活语言讲难懂的数学原理。

数据融合

在谈数据融合之前，我们先来总结一下已知的要点：

对比项	加速度计	陀螺仪
高频振动噪声	敏感	无感
低频姿态飘移	不飘	会飘

我们用 Yes 和 No 的方式重新表示上面的表格：

对比项	加速度计	陀螺仪
抗高频干扰能力	No	Yes
抗低频干扰能力	Yes	No

这时能够很明显地发现：加速度计不足的地方陀螺仪正好能补上，陀螺仪欠佳的地方加速度计正好能补上。“互补”就是本篇要表达的数据融合方法。

互补滤波

1、滤波

对于 ardupilot 的上一版 PID 控制器 Sugar 曾写过一篇《arducopter 低通滤波与 D 控制器解耦合》(现在 ardupilot 最新版的 AC_PID 库已经修复了这个 BUG)，在这篇推文中 Sugar 用通俗的方法解释了“低通滤波”(这里 Sugar 修正一个错误的表述：y(n-1) 应是“上一次滤波后的值”，而不是“上一次的采样值”)。

在“低通”滤波里我们滤除的是高频噪声，用通俗的方式表达滤波方法就是：在“旧的滤波值”和“新的采样值”之间权衡相信两者中的哪一个更多一些来达到合适的滤波效果。滤波的核心思想是：在两者中选择相信谁更多一些。Sugar 下面要说的“互补滤波”也同样遵循这个核心思想。

2、互补滤波

上面说出了滤波的核心思想，互补滤波与低通滤波类似：通过一个介于 0 和 1 之间的 α 值来选择相信加速度计多一些，还是相信陀螺仪多一些。用数学方式表述就是：

进一步拆开表述就是：

在 MATLAB 里表述就是:

其中：
(1) attitude 表示互补滤波姿态;
(2) trans 表示由陀螺仪输出到姿态角速度的转换矩阵;
(3) attitude_acc 表示加速度计算得的姿态;
(4) gyro 表示陀螺仪的输出;
(5) alpha 控制相信谁更多一些。

3、要点trans 要用经过互补滤波的姿态角来算，而不是像上期一样用陀螺仪积分出的姿态角来算，在 MATLAB 里这样表示：

按本篇定义，我们记：

这样就容易理解 phi_flt 和 theta_flt 是什么东西了。

互补滤波的效果

姿态解算的中点 or 终点

现在我们已经能够得到相对准确、抗噪、不飘移的姿态数据了，可以算是一个大里程碑、一个小终点。

什么叫“小终点”，难道还有什么不足补充上会更好么？当然有。其中一个不足是：互补滤波的 α 是个定值，需要手动调大小。而在实际当中我们希望算法能自动地根据采样得到最优滤波效果，这样能节省多次“手动调整 -> 实际测试”的时间。

卡尔曼滤波可以动态调整置信度，这个后面说。