4、姿态解算核心算法（一）：互补滤波原理、线性互补滤波器设计、非线性互补滤波器（Mahony算法）推导与实现
各位同学，欢迎来到姿态解算的核心章节。
说实话，搞过几年姿态估计的工程师都知道，卡尔曼滤波虽然精度高，但调参调到你怀疑人生。而互补滤波，恰恰是那个「够用、稳定、好调」的方案。我最早在四旋翼飞控上做姿态解算时，用的就是互补滤波——当时项目周期紧，卡尔曼还没调明白，互补滤波一上，飞起来稳得很。
这一讲，咱们就把互补滤波的来龙去脉讲透。从原理到代码，一步不落。
4.1 互补滤波的核心思想
先问一个问题：为什么需要互补滤波？
因为IMU里的两类传感器——陀螺仪和加速度计——各有各的毛病。
- 陀螺仪：动态响应快，但积分会漂移。你让它静止放10分钟，角度可能已经飘到天边去了。
- 加速度计：没有长期漂移，但噪声大，尤其是有震动的时候，数据跳得像心电图。
说白了，一个高频准、低频漂；一个低频稳、高频噪。互补滤波的思路就是：把陀螺仪的高频部分和加速度计的低频部分拼在一起，各取所长。
核心公式（一阶互补滤波）：
angle = α × (angle + gyro × dt) + (1 - α) × acc_angle
其中 α 通常取 0.98 左右，dt 是采样周期。
这个公式你一看就懂：陀螺仪积分占大头（0.98），加速度计只用来拉回漂移（0.02）。α 越大，对陀螺仪信任越多，响应快但漂移大；α 越小，对加速度计信任越多，抗漂移强但噪声大。
调参经验：
α 的取值跟采样频率有关。我一般这样估算：α = τ / (τ + dt)，τ 是时间常数，通常取 0.5~2 秒。比如 dt = 0.005s（200Hz），τ = 1s，则 α ≈ 0.995。嗯，这个值比网上常见的 0.98 要大，但实际效果更平滑。
4.2 线性互补滤波器设计
一阶互补滤波虽然简单，但有个问题：它本质上是一个一阶低通+一阶高通的组合，相位滞后比较明显。如果你做的是高速旋转的机器人，比如竞速无人机，这个滞后会让你感觉「角度跟不上」。这时候就需要上高阶互补滤波器了。
线性互补滤波器的设计思路，其实就是在频域上做文章。我们来看传递函数：
// 一阶线性互补滤波器（C语言实现）
float complementary_filter(float gyro_rate, float acc_angle, float dt, float alpha) {
static float angle = 0.0f;
angle = alpha * (angle + gyro_rate * dt) + (1.0f - alpha) * acc_angle;
return angle;
}这段代码看起来简单，但我在实际项目中发现一个问题：当加速度计数据突然跳变时（比如受到冲击），角度会跟着抖一下。为什么？因为 (1-α) 虽然小，但冲击瞬间的 acc_angle 变化很大，乘起来依然可观。
避坑指南：
我曾经在一个四足机器人项目里，直接用原始加速度计数据做互补滤波，结果机器人每踩一步，角度就震荡一次。后来加了加速度计的低通预滤波（截止频率 5Hz），问题才解决。记住：互补滤波前，加速度计数据一定要先低通。
4.3 非线性互补滤波：Mahony算法推导
线性互补滤波处理单轴角度还行，但到了三维姿态（四元数/旋转矩阵），线性方法就不够用了。这时候就该 Mahony 算法登场了。
Mahony 算法的本质是什么？说白了，就是用加速度计和磁力计的测量值，去修正陀螺仪积分的漂移。它通过一个 PI 控制器来估计陀螺仪的零偏，然后补偿积分结果。
推导过程我尽量简洁：
- 定义误差：加速度计测量方向 与 当前姿态预测方向 之间的叉积。
- 误差经过 PI 控制器，得到陀螺仪修正量。
- 修正量补偿陀螺仪测量值，再更新四元数。
核心公式如下：
// Mahony算法核心步骤（伪代码）
void mahony_update(float gx, float gy, float gz, float ax, float ay, float az, float dt) {
// 1. 归一化加速度计测量值
norm = sqrt(ax*ax + ay*ay + az*az);
ax /= norm; ay /= norm; az /= norm;
// 2. 从当前四元数估计重力方向
vx = 2*(q1*q3 - q0*q2);
vy = 2*(q0*q1 + q2*q3);
vz = q0*q0 - q1*q1 - q2*q2 + q3*q3;
// 3. 计算误差（叉积）
ex = ay*vz - az*vy;
ey = az*vx - ax*vz;
ez = ax*vy - ay*vx;
// 4. PI控制器修正陀螺仪
ex_int += ex * Ki * dt;
ey_int += ey * Ki * dt;
ez_int += ez * Ki * dt;
gx += Kp*ex + ex_int;
gy += Kp*ey + ey_int;
gz += Kp*ez + ez_int;
// 5. 更新四元数（一阶龙格库塔）
q0 += 0.5f * (-q1*gx - q2*gy - q3*gz) * dt;
q1 += 0.5f * ( q0*gx + q2*gz - q3*gy) * dt;
q2 += 0.5f * ( q0*gy - q1*gz + q3*gx) * dt;
q3 += 0.5f * ( q0*gz + q1*gy - q2*gx) * dt;
// 6. 归一化四元数
norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 /= norm; q1 /= norm; q2 /= norm; q3 /= norm;
}参数调优建议：
Kp 和 Ki 是 Mahony 的灵魂。我个人的经验值：
- Kp = 0.5 ~ 2.0（越大收敛越快，但噪声也越大）
- Ki = 0.01 ~ 0.1（用于消除稳态漂移）
- 如果只用加速度计（无磁力计），Ki 可以设小一点，否则容易过冲
4.4 知识体系总览
为了让你对本章内容有个整体把握，我画了一张结构图。你看完应该能明白互补滤波家族的全貌：
4.5 实现中的几个关键细节
代码写完了，但离「能用」还有几步。我踩过的坑，你最好别踩：
- 四元数归一化不能省：哪怕你用的是 double 精度，跑几万步之后四元数模长也会偏离1。不归一化，姿态会慢慢扭曲。
- 积分步长 dt 要准确：我见过有人直接用固定值 0.004s（250Hz），但实际循环可能因为中断、延时而波动。建议用定时器捕获真实时间戳。
- 加速度计归一化前检查模长：如果模长偏离 1g 太多（比如 >1.3g 或 <0.7g），说明有剧烈运动，此时加速度计不可信，应该减小甚至暂停修正。
一个小技巧：
在 Mahony 的误差计算中，如果你发现姿态收敛很慢，可以先把 Kp 调大（比如 2.0），等姿态稳定后再逐渐减小到 0.5 左右。这叫「热启动」，我在无人机飞控里经常用。
4.6 小结
这一讲我们聊了互补滤波的三种形态：从最简单的一阶线性，到高阶线性，再到 Mahony 非线性。你想想看，它们的本质其实都一样——用加速度计的低频信息去修正陀螺仪的高频积分。只是实现方式从标量变成了四元数，从固定系数变成了 PI 自适应。
下一讲我们会深入 Mahony 的变种——Madgwick 算法，看看它和 Mahony 到底谁更优。嗯，到时候会有更刺激的推导等着你。
公众号:蓝海资料掘金营，微信deep3321