13、互补滤波：融合陀螺仪与加速度计，实现稳定的姿态估计
各位同学，欢迎来到第十三章。前面我们分别讲了陀螺仪和加速度计，你可能会想：这两个传感器各有优缺点，能不能把它们结合起来，取长补短？
答案是肯定的。这就是我们今天要聊的——互补滤波。说白了，就是用加速度计去修正陀螺仪的漂移，用陀螺仪去弥补加速度计的动态响应慢。两者一结合，就能得到一个相对稳定、准确的姿态估计。
核心思想：陀螺仪短期准，长期漂；加速度计长期准，短期噪。互补滤波就是让它们各司其职。
13.1 为什么需要融合？
先说说我个人的经历。早期做无人机飞控时，我试过只用陀螺仪积分算角度。刚开始几秒还行，但过了十几秒，角度就开始慢慢偏了。为什么？因为陀螺仪有零偏，积分一次，误差就累积一次。时间一长，角度就飞到天上去了。
反过来，只用加速度计呢？静止时很准，但手机一动，加速度计测到的就不仅仅是重力了，还有运动加速度。这时候算出来的角度，抖得跟筛子似的。
所以，我们需要一个方法：短期相信陀螺仪，长期相信加速度计。互补滤波就是干这个的。
13.2 互补滤波的原理
互补滤波的数学表达式其实很简单：
角度 = α × (角度 + 陀螺仪角速度 × dt) + (1 - α) × 加速度计角度这里 α 是一个系数，范围在 0 到 1 之间。通常 α 取 0.98 左右。
你想想看：
- α 越大，越相信陀螺仪。系统响应快，但长期会漂。
- α 越小，越相信加速度计。系统稳定，但动态响应慢。
为什么会这样？因为陀螺仪的积分项是高频的，加速度计的角度是低频的。互补滤波本质上就是一个低通+高通的组合滤波器。
我的经验：α 取 0.98 是一个不错的起点。如果你发现角度抖动厉害，可以稍微减小 α；如果发现跟随慢，可以增大 α。具体调参时，我习惯先设 0.98，然后跑一个旋转测试，看响应是否跟手。
13.3 算法流程图
下面这张图，是我画的一个简化流程。它展示了互补滤波的核心逻辑：
嗯，这里要注意：反馈回路的意思是，每次融合后的角度，会作为下一次积分的初始值。这样陀螺仪的漂移就会被加速度计慢慢拉回来。
13.3 代码实现
下面是一个完整的互补滤波实现。我建议你直接复制到你的项目里试试。
public class ComplementaryFilter {
private static final float ALPHA = 0.98f;
private static final float DT = 0.01f; // 采样周期 10ms
private float angle = 0.0f; // 最终融合角度
/**
* 更新姿态角
* @param gyroRate 陀螺仪角速度 (rad/s)
* @param accAngle 加速度计计算出的角度 (rad)
* @return 融合后的角度 (rad)
*/
public float update(float gyroRate, float accAngle) {
// 陀螺仪积分
float gyroAngle = angle + gyroRate * DT;
// 互补融合
angle = ALPHA * gyroAngle + (1.0f - ALPHA) * accAngle;
return angle;
}
/**
* 重置角度
*/
public void reset() {
angle = 0.0f;
}
}避坑指南：我曾经在采样周期 DT 上吃过亏。如果你用 10ms 采样，但实际循环跑下来是 12ms，那积分就会偏大。建议用 System.nanoTime() 精确测量两次调用之间的时间差，而不是写死 DT。
13.4 加速度计角度怎么算？
你可能注意到了，上面的代码里有个 accAngle。这个角度怎么来的？
对于绕 X 轴旋转（横滚角 Roll），公式是：
roll = atan2(ay, az)对于绕 Y 轴旋转（俯仰角 Pitch），公式是：
pitch = atan2(-ax, sqrt(ay*ay + az*az))这里有个细节：atan2 返回的是弧度，记得转成角度方便观察。
注意：加速度计在剧烈运动时，测到的加速度不全是重力。这时候算出来的角度是错的。互补滤波虽然能缓解，但如果你做的是高动态运动（比如无人机特技飞行），建议考虑更高级的卡尔曼滤波。
13.5 参数调优建议
互补滤波只有一个参数 α，但调起来也有讲究。我整理了一个表格，供你参考：
| 场景 | 推荐 α | 说明 |
|---|---|---|
| 静止或慢速运动 | 0.95 ~ 0.98 | 更相信陀螺仪，响应快 |
| 中等运动（手持设备） | 0.98 ~ 0.99 | 平衡响应与稳定 |
| 高动态运动（无人机） | 0.99 ~ 0.995 | 减少加速度计噪声影响 |
我个人习惯从 0.98 开始，然后跑一个来回旋转的测试。如果角度回不到原点，说明 α 太大，漂移没被完全修正。如果角度抖动明显，说明 α 太小，加速度计噪声进来了。
13.6 实际项目中的坑
最后分享几个我踩过的坑：
- 初始角度问题：互补滤波启动时，如果 angle 初始化为 0，而手机是倾斜的，那前几帧会有跳变。我建议启动时先用加速度计算一个初始角度赋值给 angle。
- 坐标系对齐：陀螺仪和加速度计的坐标系可能不一致。比如有的传感器 X 轴方向相反。记得先校准，否则融合出来的角度是错的。
- 采样率不匹配：如果陀螺仪和加速度计的采样率不同，比如陀螺仪 100Hz，加速度计 50Hz，那融合时要注意对齐时间戳。我一般用最近一次的加速度计数据。
总结一下：互补滤波不是最完美的算法，但它简单、高效、够用。对于大多数 Android 传感器应用——比如手机屏幕旋转、VR 头显、平衡车——它都能给出不错的结果。如果你追求更高精度，可以学卡尔曼滤波或 Mahony 滤波，但那是后话了。
好了，这一章就到这里。代码已经给你了，参数也给了建议。剩下的就是动手试试。记住，纸上得来终觉浅，绝知此事要躬行。