4、姿态解算算法：互补滤波原理、Mahony滤波算法、Madgwick滤波算法、EKF姿态估计

姿态解算，说白了就是回答一个问题：“我的飞行器现在到底朝哪边？”

IMU（惯性测量单元）给我们提供了加速度计和陀螺仪的数据。但这两兄弟各有各的毛病。陀螺仪短期准，但长期会飘；加速度计长期稳，但短期噪声大。你想想看，要是只信陀螺仪，飞着飞着角度就偏到姥姥家了；只信加速度计，稍微一震动，姿态就抖得像筛糠。

所以我们需要一个“裁判”，把这两路信号融合起来。这就是姿态解算算法的核心使命。

我个人习惯把姿态解算算法分为三大流派：互补滤波（及其进阶版Mahony和Madgwick）、卡尔曼滤波（EKF）。今天咱们就把它们挨个捋一遍。

4.1 互补滤波原理

互补滤波，名字听着高大上，其实核心思想特别朴素：“谁准信谁”。

陀螺仪在高频段表现好，加速度计在低频段表现好。那我们就设计一个滤波器，让陀螺仪通过高通，加速度计通过低通，然后加起来。这就是互补滤波的雏形。

数学上很简单：

姿态 = α × (陀螺仪积分) + (1-α) × (加速度计解算)

这里的α是一个权重系数，通常在0.98~0.995之间。α越大，越信任陀螺仪；α越小，越信任加速度计。

我在项目中遇到过一个问题：用互补滤波做四轴飞行器，悬停时姿态很稳，但一做剧烈机动，姿态就开始滞后。后来发现是α没调好——机动时应该临时降低α，让加速度计多拉一把。嗯，这里要注意，固定参数的互补滤波在动态场景下会露怯。

4.2 Mahony滤波算法

Mahony滤波是互补滤波的“正规军”版本。它不再简单加权，而是引入了PI控制器来修正陀螺仪的漂移。

核心思路是这样的：

1. 用陀螺仪积分得到当前姿态
2. 用加速度计（和磁力计）算出“期望的”姿态
3. 计算两者之间的误差
4. 用PI控制器把这个误差反馈到陀螺仪上，修正漂移

说白了，Mahony滤波就是给陀螺仪加了一个“自动纠偏”的PID环。

// Mahony滤波核心伪代码
void MahonyAHRSupdate(float gx, float gy, float gz, 
                      float ax, float ay, float az) {
    // 1. 计算加速度计与当前姿态的误差
    // 2. PI控制器计算修正量
    // 3. 修正陀螺仪数据
    // 4. 四元数积分更新姿态
}

Mahony滤波的好处是：计算量小，收敛快。在STM32F103这种老平台上，跑Mahony滤波只需要几十微秒。所以很多飞控开源项目（比如ArduPilot早期版本）都在用。

4.3 Madgwick滤波算法

Madgwick滤波是2010年由Sebastian Madgwick提出的。它和Mahony很像，但有一个关键区别：梯度下降法。

Mahony用的是PI控制器来修正，Madgwick用的是梯度下降法来寻找最优姿态。你可以理解为：Mahony是“慢慢拉回来”，Madgwick是“一步跨过去”。

具体来说，Madgwick定义了一个误差函数，表示当前姿态下加速度计的理论值与实际值的差异。然后沿着梯度方向下降，找到误差最小的姿态。

// Madgwick滤波核心步骤
1. 计算目标函数 f(q, a) = q* ⊗ a_measured ⊗ q - a_expected
2. 计算雅可比矩阵 J(q, a)
3. 梯度下降步长：∇f = J^T * f
4. 更新四元数：q_new = q - β * ∇f / ||∇f||

这里的β是步长，相当于Mahony里的Kp。β越大，收敛越快，但噪声也越大。

Madgwick和Mahony的对比：

特性	Mahony	Madgwick
核心方法	PI反馈控制	梯度下降优化
计算量	小	中等
收敛速度	中等	快
参数调优	Kp, Ki两个参数	β一个参数
适用场景	慢速运动、悬停	快速运动、剧烈机动

我个人习惯：低速场景用Mahony，高速场景用Madgwick。如果你懒得调参，Madgwick的β比Mahony的Kp/Ki好调一些——毕竟只有一个参数。

4.4 EKF姿态估计

EKF（扩展卡尔曼滤波）是姿态解算里的“重武器”。它不再靠直觉加权，而是基于概率论，用状态空间模型来估计姿态。

EKF的核心思想：

1. 预测：用陀螺仪积分预测下一时刻的姿态
2. 更新：用加速度计/磁力计的测量值来修正预测
3. 每一步都计算协方差矩阵，表示“我有多不确定”

你想想看，互补滤波是“我信你70%”，EKF是“我根据历史数据和噪声特性，算出我应该信你73.5%”。这就是本质区别。

// EKF姿态估计核心步骤（简化）
// 状态向量: x = [q0, q1, q2, q3, bgx, bgy, bgz]^T
// 其中q是四元数，bg是陀螺仪零偏

// 预测步骤
x_pred = f(x_prev, gyro_meas)  // 状态转移
P_pred = F * P_prev * F^T + Q   // 协方差预测

// 更新步骤
z = accel_meas                  // 测量值
y = z - h(x_pred)               // 残差
S = H * P_pred * H^T + R        // 创新协方差
K = P_pred * H^T * inv(S)       // 卡尔曼增益
x_upd = x_pred + K * y          // 状态更新
P_upd = (I - K * H) * P_pred    // 协方差更新

我在项目中遇到过：用Madgwick滤波的无人机，飞了5分钟后偏航角漂了3度。换成EKF后，同样条件下漂移小于0.5度。这就是EKF的威力。

但EKF也有代价：

• 计算量大：矩阵运算，尤其是求逆，在低端MCU上跑不动
• 调参复杂：Q矩阵（过程噪声）、R矩阵（测量噪声）需要仔细调
• 初始化敏感：初始协方差设不好，可能发散

最后总结一下四种算法的选择策略：

场景	推荐算法	理由
玩具级、低成本MCU	互补滤波	代码简单，几行搞定
消费级无人机、机器人	Mahony / Madgwick	计算量适中，效果够用
工业级、高精度导航	EKF	精度高，能估计零偏
剧烈运动、高动态场景	Madgwick	梯度下降收敛快

嗯，姿态解算这块内容就这些。每种算法都有它的脾气，选对了就是利器，选错了就是坑。我个人建议：先从Mahony入手，跑通了再换Madgwick对比效果。如果精度要求极高，再上EKF。别一上来就搞EKF，容易把自己绕晕。

---

公众号:蓝海资料掘金营，微信deep3321