1、姿态估计概述：什么是姿态估计、为什么需要姿态估计、姿态估计的常见方法
各位同学好，我是你们这门课的老朋友。今天咱们正式开始聊《卡尔曼滤波在姿态估计中的应用》这门课。第一讲，我先带大家把「姿态估计」这个事儿彻底捋清楚。
说实话，我入行那会儿，姿态估计还是个挺「玄学」的东西。那时候做四轴飞行器，陀螺仪漂移得厉害，飞着飞着就翻跟头了。后来才明白，姿态估计说白了就是：我知道传感器在怎么转、怎么偏，但我得算出它到底「朝哪」。
1.1 什么是姿态估计？
姿态估计，英文叫 Attitude Estimation。简单讲，就是确定一个物体在三维空间中的朝向。比如你的手机横过来竖过去，系统得知道屏幕现在朝哪；无人机悬停时，飞控得知道机头指向哪边。
我们通常用三个角度来描述姿态：横滚角（Roll）、俯仰角（Pitch）、偏航角（Yaw）。这三个角合起来，就能唯一确定一个刚体的朝向。
核心概念：姿态估计不是测出来的，是「算」出来的。传感器给的是原始数据，我们要用算法把这些数据融合成可靠的姿态信息。
你想想看，一个加速度计只能测重力方向，一个陀螺仪只能测角速度，一个磁力计只能测地磁场方向。单独看哪个都不靠谱，但把它们凑一块儿，就能互相补短。这就是姿态估计的本质——多传感器数据融合。
1.2 为什么需要姿态估计？
这个问题我经常被学生问。我的回答是：没有姿态估计，很多设备就是个「瞎子」。
举个例子。我在做机器人平衡车项目时，如果不知道车体当前的俯仰角，PID控制器根本没法工作。你给电机发指令，不知道车身是前倾还是后仰，那控制就是瞎蒙。
具体来说，姿态估计在以下几个场景中不可或缺：
- 无人机/飞行器：飞控需要实时知道姿态，才能稳定悬停、转弯、爬升。我见过一个新手，陀螺仪数据直接用，结果飞机在空中疯狂震荡——因为没有滤波。
- 手机/可穿戴设备：屏幕旋转、步数计数、运动轨迹追踪，全依赖姿态估计。你手机里的「指南针」App，背后就是一套姿态估计算法。
- AR/VR 头显：头戴设备必须精确跟踪头部转动，否则画面会「飘」。延迟超过20ms，人就晕了。
- 汽车/船舶导航：惯性导航系统（INS）在GPS信号丢失时，全靠姿态估计推算位置。
我的经验：姿态估计的精度直接决定了系统的稳定性。我曾经在一个项目中，把滤波器的更新频率从100Hz降到50Hz，结果无人机在强风下直接失控。所以，别小看这个环节。
1.3 姿态估计的常见方法
好了，前面铺垫了这么多，咱们来看看具体怎么做。目前主流的姿态估计方法有三种：互补滤波、卡尔曼滤波、Mahony滤波。我一个个说。
1.3.1 互补滤波
互补滤波是最简单、最直观的方法。它的思路是：陀螺仪高频响应好，但低频漂移严重；加速度计和磁力计低频稳定，但高频噪声大。那好，我把它们「互补」起来。
具体公式很简单：
姿态 = α × (陀螺仪积分结果) + (1 - α) × (加速度计/磁力计解算结果)这里的 α 是一个权重系数，通常在 0.9~0.99 之间。α 越大，越信任陀螺仪；α 越小，越信任加速度计。
我在做低成本IMU模块时，就用的互补滤波。代码量不到50行，跑在STM32F103上，占用资源极少。但缺点也很明显：它假设噪声是白噪声，且权重固定，适应性差。遇到剧烈运动或强磁场干扰，容易跑偏。
避坑指南：我曾经在无人机上只用互补滤波，结果在快速旋转时，姿态估计滞后了将近100ms，导致炸机。后来我加了动态权重调整，才勉强能用。所以，如果你的应用场景动态范围大，互补滤波可能不够用。
1.3.2 卡尔曼滤波
卡尔曼滤波，这才是咱们这门课的主角。它本质上是一个最优状态估计器，能根据系统模型和观测数据，递归地估计出最可能的姿态。
卡尔曼滤波的核心思想是：预测 + 更新。先根据陀螺仪数据预测下一时刻的姿态，然后用加速度计和磁力计的观测值去修正这个预测。每一步都计算一个「卡尔曼增益」，决定预测和观测谁更可信。
它的数学形式稍微复杂一些，但用起来非常灵活。你可以把陀螺仪漂移、加速度计噪声、磁力计干扰都建模成协方差矩阵，算法会自动调整权重。
我个人习惯用扩展卡尔曼滤波（EKF）来处理姿态估计中的非线性问题。比如四元数更新方程就是非线性的，EKF 能很好地处理。
关键点：卡尔曼滤波不是「万能药」。它需要准确的系统模型和噪声统计特性。模型错了，滤波结果比互补滤波还差。我见过有人直接把卡尔曼滤波当黑盒用，结果越滤越飘。
1.3.3 Mahony 滤波
Mahony 滤波，全称 Mahony 互补滤波，是澳大利亚学者 Robert Mahony 提出的。它本质上是一种基于 PI 控制器的互补滤波。
它的思路很巧妙：把姿态误差看作一个「控制偏差」，用 PI 控制器去修正陀螺仪的偏差。这样既能抑制漂移，又能快速响应。
Mahony 滤波的优点是：计算量小、收敛快、不需要精确的噪声模型。很多开源飞控（比如 ArduPilot、PX4）都内置了 Mahony 滤波作为备选方案。
我记得有一次做水下机器人，IMU 被水压干扰得厉害，卡尔曼滤波怎么调都不收敛。换成 Mahony 滤波，参数随便调了调，居然稳住了。嗯，有时候简单的方法反而更鲁棒。
1.4 三种方法的对比
我把这三种方法的特点整理成了一张表，方便你对比：
| 方法 | 计算量 | 精度 | 鲁棒性 | 适用场景 |
|---|---|---|---|---|
| 互补滤波 | 极低 | 中等 | 一般 | 低成本MCU、静态或慢速运动 |
| 卡尔曼滤波 | 较高 | 高 | 依赖模型 | 高精度需求、动态范围大 |
| Mahony 滤波 | 低 | 较高 | 好 | 嵌入式实时系统、噪声不确定 |
你可能会问：「那我到底该选哪个？」我的建议是：先看你的硬件资源。如果 MCU 是 Cortex-M0 级别的，老老实实用互补滤波或 Mahony。如果跑着 Cortex-M4 甚至 A 系列芯片，那卡尔曼滤波值得一试。
我的习惯：做原型验证时先用互补滤波，快速跑通流程。等系统稳定了，再换成卡尔曼滤波做精度优化。这样既节省时间，又能对比效果。
1.5 本章小结
这一章咱们把姿态估计的「地基」打好了。你知道了：
- 姿态估计就是算物体的朝向，核心是数据融合。
- 没有姿态估计，很多设备没法正常工作。
- 三种主流方法各有优劣，互补滤波简单、卡尔曼滤波精确、Mahony 滤波鲁棒。
下一章，我会带你深入卡尔曼滤波的数学原理。别怕，我会用最直观的方式讲清楚。咱们下节课见。