4、姿态解算与滤波：互补滤波与卡尔曼滤波（EKF）的原理对比，以及在实际飞控中的代码实现逻辑。
各位同学，欢迎来到第四章。这一章我们聊点硬核的——姿态解算与滤波。
说白了，飞控的大脑就是姿态解算。传感器给的数据，比如陀螺仪、加速度计、磁力计，它们各自都有毛病。陀螺仪飘，加速度计吵，磁力计容易受干扰。怎么把这些“半真半假”的数据揉成一个靠谱的姿态？这就是滤波干的事。
我入行那会儿，第一个项目就是调四旋翼的悬停。当时不懂滤波，直接拿原始数据算欧拉角，结果飞机像喝醉了酒一样晃。嗯，从那以后我就明白了，滤波不是锦上添花，是生死攸关。
4.1 为什么需要姿态滤波？
先看一个简单的问题：你拿手机转一下，屏幕上的姿态指示器是不是跟着动？但如果你快速晃动，它是不是会滞后或者抖动？
这就是传感器的特性决定的：
- 陀螺仪：短时间精度高，动态响应快。但积分会漂移，时间越长越不准。
- 加速度计：长时间稳定，能给出重力方向。但容易受振动干扰，高频噪声大。
- 磁力计：提供航向参考。但容易受周围铁磁物质干扰，噪声也大。
所以，我们需要一个“裁判”，把这三个传感器的优点结合起来，缺点互相抵消。这个裁判就是滤波器。
核心思想：用陀螺仪的高频响应做“骨架”，用加速度计和磁力计的低频稳定做“锚点”，通过滤波算法融合出平滑、无漂移的姿态。
4.2 互补滤波：简单、高效、够用
互补滤波，名字听着玄乎，其实原理很朴素。你想想看，陀螺仪的高频信号好，加速度计的低频信号好。那把陀螺仪的高频部分和加速度计的低频部分“互补”起来，不就行了？
公式也很简单：
姿态 = α × (陀螺仪积分姿态) + (1 - α) × (加速度计/磁力计姿态)这里的 α 是一个系数，通常在 0.98 左右。α 越大，越信任陀螺仪；α 越小，越信任加速度计。
4.2.1 互补滤波的代码实现（简化版）
我在 Pixhawk 的早期固件里见过这种实现，非常经典。下面是一个基于 Mahony 互补滤波的简化代码片段，用于四元数更新：
// 输入：陀螺仪角速度 (gx, gy, gz)，加速度计 (ax, ay, az)，磁力计 (mx, my, mz)
// 输出：更新后的四元数 (q0, q1, q2, q3)
void MahonyAHRSupdate(float gx, float gy, float gz,
float ax, float ay, float az,
float mx, float my, float mz) {
float recipNorm;
float q0q0, q0q1, q0q2, q0q3;
float q1q1, q1q2, q1q3;
float q2q2, q2q3, q3q3;
float hx, hy, bx, bz;
float halfvx, halfvy, halfvz;
float halfwx, halfwy, halfwz;
float ex, ey, ez;
float qa, qb, qc;
// 1. 归一化加速度计数据
recipNorm = invSqrt(ax*ax + ay*ay + az*az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// 2. 归一化磁力计数据
recipNorm = invSqrt(mx*mx + my*my + mz*mz);
mx *= recipNorm;
my *= recipNorm;
mz *= recipNorm;
// 3. 计算参考方向（重力方向在机体坐标系下的投影）
halfvx = q1*q3 - q0*q2;
halfvy = q0*q1 + q2*q3;
halfvz = q0*q0 - 0.5f + q3*q3;
// 4. 计算误差（叉积）
ex = (ay * halfvz - az * halfvy);
ey = (az * halfvx - ax * halfvz);
ez = (ax * halfvy - ay * halfvx);
// 5. 用PI控制器修正陀螺仪偏差
integralFBx += Ki * ex * dt;
integralFBy += Ki * ey * dt;
integralFBz += Ki * ez * dt;
gx += integralFBx + Kp * ex;
gy += integralFBy + Kp * ey;
gz += integralFBz + Kp * ez;
// 6. 四元数微分方程更新
qa = q0;
qb = q1;
qc = q2;
q0 += (-qb*gx - qc*gy - q3*gz) * halfT;
q1 += ( qa*gx + qc*gz - q3*gy) * halfT;
q2 += ( qa*gy - qb*gz + q3*gx) * halfT;
q3 += ( qa*gz + qb*gy - qc*gx) * halfT;
// 7. 归一化四元数
recipNorm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
}个人经验：互补滤波的 Kp 和 Ki 参数很关键。Kp 太大，姿态会跟着加速度计的噪声抖；Kp 太小，又跟不上陀螺仪的漂移。我一般先调 Kp，让姿态响应不滞后，再给一点点 Ki 消除稳态误差。Ki 千万别给大，否则飞机会“硬邦邦”的，像被弹簧拉着。
4.3 卡尔曼滤波（EKF）：精准、复杂、资源消耗大
互补滤波够用，但不够“聪明”。它不会根据传感器的噪声水平动态调整信任度。卡尔曼滤波就不一样了，它有一套完整的数学框架，能实时估计状态的最优值，还能告诉你这个估计有多准。
扩展卡尔曼滤波（EKF）是卡尔曼滤波在非线性系统上的推广。飞控系统本质上是非线性的，所以用 EKF 更合适。
4.3.1 EKF 的核心思想
EKF 分两步走：预测和更新。
- 预测：根据上一时刻的状态和当前的控制输入（比如陀螺仪角速度），预测当前时刻的状态和协方差。
- 更新：用当前的观测值（比如加速度计、磁力计、GPS）去修正预测值，得到最优估计。
说白了，就是“先猜，再看，再修正”。
4.3.2 EKF 的代码实现逻辑（伪代码）
下面是一个简化的 EKF 姿态估计伪代码，展示了核心的预测和更新步骤：
// 状态向量 x = [q0, q1, q2, q3, bgx, bgy, bgz] (四元数 + 陀螺仪零偏)
// 观测向量 z = [ax, ay, az, mx, my, mz] (加速度计 + 磁力计)
// 1. 预测步骤
x_pred = f(x_prev, u) // 用陀螺仪数据更新四元数，零偏保持不变
P_pred = F * P_prev * F^T + Q // 更新协方差矩阵，Q是过程噪声
// 2. 更新步骤
y = z - h(x_pred) // 计算残差（观测值与预测观测值的差）
S = H * P_pred * H^T + R // 计算残差协方差，R是观测噪声
K = P_pred * H^T * inv(S) // 计算卡尔曼增益
x_updated = x_pred + K * y // 更新状态
P_updated = (I - K * H) * P_pred // 更新协方差避坑指南：我曾经在一个项目中，EKF 的协方差矩阵 P 初始化得太小，导致滤波器“过于自信”，新观测值几乎不起作用。飞机起飞后姿态慢慢漂移，最后炸机了。教训就是：P 矩阵的初始值不能太小，要给滤波器留出学习空间。
4.4 互补滤波 vs EKF：对比表格
为了让你看得更清楚，我整理了一个对比表：
| 特性 | 互补滤波 | 扩展卡尔曼滤波（EKF） |
|---|---|---|
| 计算复杂度 | 低（几十行代码，无矩阵运算） | 高（需要矩阵乘法、求逆，计算量大） |
| 内存占用 | 低（几个全局变量） | 高（需要存储协方差矩阵等） |
| 精度 | 中等（依赖固定系数） | 高（能动态调整，融合多传感器） |
| 鲁棒性 | 较好（参数调好后很稳定） | 依赖模型和噪声参数（调参困难） |
| 适用场景 | 低成本飞控、玩具级无人机 | 高端飞控、工业级、需要高精度定位 |
| 调参难度 | 低（2-3个参数） | 高（Q、R矩阵，初始P，模型参数） |
4.5 如何选择？我的建议
很多新手会问：到底用哪个好？我的回答是：看你的硬件和需求。
- 如果你用的是 STM32F103 这种低端 MCU，或者做的是玩具级飞控，互补滤波完全够用。我早期做的小四轴，用的就是 Mahony 互补滤波，飞得也挺稳。
- 如果你用的是 STM32F4/F7 或者更高性能的芯片，而且需要融合 GPS、光流、气压计等多传感器做精准定位，那 EKF 是更好的选择。PX4 和 ArduPilot 的默认姿态估计都是 EKF。
我的经验：不要盲目追求“高级”。EKF 虽然精度高，但调参是个大坑。Q 矩阵和 R 矩阵的取值，直接影响滤波效果。我见过有人调了两个月 EKF 还没调好，最后换回互补滤波，飞机反而稳了。所以，先跑通互补滤波，再考虑升级到 EKF。
4.6 本章知识体系图
下面这张 SVG 图，帮你理清本章的核心逻辑：
这张图很直观：传感器数据进来，经过融合滤波，输出稳定姿态。两种滤波各有优劣，选哪个，看你的具体场景。
4.7 实际飞控中的代码实现逻辑
最后，我们聊聊在实际飞控代码里，滤波是怎么嵌入到主循环中的。
以 PX4 为例，它的姿态估计模块（EKF2）在主循环中是这样工作的：
- 传感器数据采集：从陀螺仪、加速度计、磁力计、气压计、GPS 等读取原始数据。
- 数据预处理：校准、去偏、坐标系转换。
- 时间更新（预测）：用陀螺仪数据更新状态（四元数）和协方差。
- 观测更新（修正）：依次用加速度计、磁力计、气压计、GPS 等观测数据修正状态。
- 输出：将更新后的四元数转换为欧拉角或旋转矩阵，供控制模块使用。
小技巧：在实际代码中，EKF 的观测更新顺序很重要。我一般先更新加速度计（修正俯仰和横滚），再更新磁力计（修正航向），最后更新 GPS（修正位置和速度）。这样能避免不同观测之间的相互干扰。
好了，这一章的内容就到这里。滤波是飞控的基石，理解透了，后面的控制算法才能站得稳。希望你能动手写一写互补滤波的代码，感受一下数据融合的魅力。