4. 姿态估计基础：互补滤波原理、Mahony滤波算法、扩展卡尔曼滤波（EKF）入门、状态观测器概念
各位同学，欢迎来到《抗干扰姿态稳定控制策略实战》的第四讲。
今天我们要聊的，是整个飞控系统里最核心、也最容易被忽视的一环——姿态估计。
说白了，就是回答一个问题：“我的飞机现在到底朝哪边？”
你可能会说：“这不简单吗？陀螺仪测角速度，积分不就得到角度了？”
嗯，我刚入行时也这么想。直到有一次，我在调试一个四旋翼，发现飞机悬停时莫名其妙地往一边漂。查了半天，原来是陀螺仪零偏随时间累积，积分出来的角度已经偏了十几度。从那以后，我再也不敢只用纯积分做姿态估计了。
核心观点：没有任何单一传感器能完美提供姿态信息。我们需要融合多种传感器的数据，取长补短。这就是姿态估计的本质。
4.1 互补滤波原理：简单粗暴但有效
互补滤波，是我个人最喜欢用来给学生讲“传感器融合”概念的入门算法。它不复杂，但思想非常漂亮。
想象一下，你面前有两个信息源：
- 陀螺仪：动态响应快，但低频有漂移（积分误差）。
- 加速度计/磁力计：长期稳定，但高频噪声大（振动干扰）。
互补滤波的思路就是：让陀螺仪负责高频部分，让加速度计负责低频部分，然后加起来。
数学上，它就是一个一阶低通滤波器和一个一阶高通滤波器的组合。公式很简单：
角度_估计 = α * (角度_估计 + 陀螺仪_角速度 * dt) + (1 - α) * 加速度计_角度这里的 α 是滤波系数，通常在 0.9 到 0.99 之间。α 越大，信任陀螺仪越多；α 越小，信任加速度计越多。
我的经验：α 的取值很关键。我一般会先设成 0.98，然后看实际飞行效果。如果飞机在悬停时角度抖动厉害，说明加速度计权重太大，我会把 α 调高到 0.99。如果飞机在快速机动时角度跟踪滞后，说明陀螺仪权重太大，我会把 α 调低到 0.95。没有万能参数，得试。
互补滤波的优点是计算量极小，适合在低算力的单片机上跑。缺点也很明显：它假设噪声是白噪声，且只能处理线性问题。对于大角度机动或强振动环境，效果会打折扣。
4.2 Mahony滤波算法：从互补滤波到SO(3)的升级
Mahony 滤波，你可以把它看作是互补滤波的“豪华升级版”。它不再用简单的标量加减，而是直接在 SO(3) 旋转群 上做运算。
为什么要这么做？因为欧拉角有万向锁问题，而且插值不直观。Mahony 用四元数或旋转矩阵来表示姿态，从根本上避免了这些问题。
Mahony 的核心思想是：用加速度计和磁力计的测量值，来修正陀螺仪积分产生的漂移。
具体做法是：
- 用陀螺仪积分预测当前姿态（四元数）。
- 用加速度计和磁力计测量值，计算出一个“期望”的姿态。
- 计算预测姿态与期望姿态之间的误差（在 SO(3) 上）。
- 用这个误差去修正陀螺仪的角速度测量值（PI 控制器）。
- 重复步骤 1-4。
你看，它本质上还是一个闭环反馈系统。陀螺仪是前馈，加速度计/磁力计是反馈。
避坑指南：我曾经在一个项目中，直接用 Mahony 的默认 PI 参数，结果飞机在起飞时剧烈震荡。后来发现，是因为加速度计在起飞瞬间受到电机振动干扰，导致误差计算异常。我的解决办法是：在电机启动后的前 0.5 秒，降低加速度计的权重，让陀螺仪主导。等振动稳定后，再恢复正常权重。
Mahony 的代码实现比互补滤波复杂一些，但网上有很多开源实现（比如 Pixhawk 的代码）。我建议你直接拿过来用，但一定要理解它的数学原理。
4.3 扩展卡尔曼滤波（EKF）入门：状态估计的“瑞士军刀”
如果说互补滤波是“小刀”，Mahony 是“菜刀”，那 EKF 就是“瑞士军刀”——功能强大，但用起来也最复杂。
EKF 的核心思想是：用概率论的方法，融合所有可用的传感器信息，给出一个最优的状态估计。
它分两步走：
- 预测步：根据系统模型（比如陀螺仪数据），预测下一时刻的状态和不确定性（协方差）。
- 更新步：当有新的测量值（比如加速度计数据）到来时，用这个测量值去修正预测的状态，同时更新不确定性。
为什么叫“扩展”卡尔曼？因为标准的卡尔曼滤波只能处理线性系统，而我们的飞控系统是非线性的（比如旋转运动）。EKF 通过泰勒展开，把非线性系统近似成线性系统，然后套用标准卡尔曼滤波的框架。
一个典型的 EKF 姿态估计状态向量通常包含：
状态向量 = [四元数(q0, q1, q2, q3), 陀螺仪零偏(bx, by, bz)]测量向量通常包含：
测量向量 = [加速度计(ax, ay, az), 磁力计(mx, my, mz)]注意：EKF 的调参非常痛苦。你需要调节过程噪声协方差矩阵 Q 和测量噪声协方差矩阵 R。Q 设大了，滤波器响应快但噪声大；Q 设小了，滤波器平滑但滞后。R 同理。我当年调 EKF 参数，整整花了两周时间，最后发现是加速度计的噪声模型没建对。
EKF 的优点是精度高，能同时估计姿态和传感器零偏。缺点是计算量大，对硬件有一定要求，而且对非线性强的系统（比如剧烈机动）容易发散。
4.4 状态观测器概念：从另一个角度看问题
讲到这里，我想引入一个更通用的概念——状态观测器。
你想想看，我们刚才讲的互补滤波、Mahony、EKF，本质上都是在做同一件事：根据系统的输入（陀螺仪）和输出（加速度计/磁力计），去估计系统的内部状态（姿态）。
这不就是状态观测器的定义吗？
在控制理论中，状态观测器是一个数学模型，它利用系统的输入和输出，来重构系统的状态。最经典的观测器是 Luenberger 观测器（适用于线性系统）和 滑模观测器（适用于非线性系统）。
互补滤波和 Mahony，其实可以看作是 Luenberger 观测器的一种特殊形式。而 EKF，则是针对非线性随机系统的最优观测器。
我的理解：不要把姿态估计算法看成孤立的“滤波技巧”。把它放在“状态观测器”这个框架下，你会发现所有算法都是相通的。它们都在解决同一个问题：如何用不完美的传感器，去估计一个不可直接测量的状态。
理解了这一点，你就能举一反三。比如，当你的系统有多个传感器时，你可以设计一个多速率观测器；当你的系统有延迟时，你可以设计一个预测观测器。这些，都是状态观测器理论的延伸。
4.5 本章知识体系图
为了让你更直观地理解这些算法之间的关系，我画了一张图：
从这张图你可以看到，所有算法都指向同一个目标，只是复杂度和适用场景不同。我个人建议：初学者从互补滤波入手，理解“融合”的思想；进阶者掌握 Mahony，用于实际飞控；高手研究 EKF，追求极致精度。
好了，这一讲的内容就到这里。记住，姿态估计没有银弹。选择哪种算法，取决于你的硬件平台、算力限制、以及你对精度的要求。多动手实践，多分析数据，你自然能找到最适合你的方案。
最后分享一个我的习惯：每次调试完姿态估计算法，我都会把陀螺仪原始数据、加速度计原始数据、以及估计出的姿态角，同时录下来。然后用 MATLAB 或 Python 画在一起对比。这样能直观地看到算法在动态和静态下的表现，比看任何理论分析都管用。