14、传感器融合简介：卡尔曼滤波在姿态估计中的基本思想

各位同学，欢迎来到第14章。前面我们聊了陀螺仪、加速度计、磁力计各自怎么用。但你有没有发现一个问题？

陀螺仪短期很准，但时间一长就飘。加速度计和磁力计长期稳定，但短期噪声大。那能不能把它们结合起来，取长补短？

嗯，这就是传感器融合要干的事。而卡尔曼滤波，是其中最经典的方法之一。

为什么需要融合？

我刚开始做姿态估计时，犯过一个低级错误。我只用陀螺仪积分算角度，结果跑了10分钟，角度偏了快20度。为什么？因为陀螺仪有零偏，积分误差会累积。

反过来，只用加速度计呢？手机稍微震动一下，角度就跳来跳去，根本没法用。

说白了，单个传感器都有短板。融合的目的就是：

• 短期靠陀螺仪——响应快，动态性能好
• 长期靠加速度计/磁力计——修正漂移，提供绝对参考

卡尔曼滤波就是一套数学框架，帮我们优雅地完成这件事。

卡尔曼滤波的核心思想

卡尔曼滤波听起来高大上，其实核心就两句话：

1. 预测：根据上一时刻的状态，预测当前时刻的状态（比如用陀螺仪积分）。
2. 更新：用当前时刻的观测值（比如加速度计读数），修正预测结果。

它本质上是一个递归的、最优的估计算法。每次只保留上一时刻的状态，不需要存历史数据，非常适合嵌入式实时系统。

姿态估计中的卡尔曼滤波模型

在姿态估计场景下，我们通常把状态向量设为：

状态向量 x = [角度, 陀螺仪零偏]

为什么要把零偏也放进状态里？因为陀螺仪的零偏会随时间缓慢变化，卡尔曼滤波可以在线估计这个零偏，实时修正。

下面我画了一张流程图，帮你理解整个预测-更新循环：

五个核心公式（别怕，我帮你拆解）

卡尔曼滤波有5个公式。很多人一看公式就头大，其实你只要理解它们在干什么就行。

步骤	公式	通俗理解
预测状态	x̂ₖ⁻ = A·x̂ₖ₋₁ + B·uₖ	用陀螺仪积分，猜当前角度
预测协方差	Pₖ⁻ = A·Pₖ₋₁·Aᵀ + Q	估算这个猜测有多不确定
卡尔曼增益	Kₖ = Pₖ⁻·Hᵀ·(H·Pₖ⁻·Hᵀ + R)⁻¹	算一下该信预测还是该信观测
更新状态	x̂ₖ = x̂ₖ⁻ + Kₖ·(zₖ - H·x̂ₖ⁻)	用观测值修正预测结果
更新协方差	Pₖ = (I - Kₖ·H)·Pₖ⁻	更新不确定性，为下一轮准备

一个简化的姿态估计示例

下面是一段伪代码，展示一维角度（俯仰角）的卡尔曼滤波过程：

// 初始化
float angle = 0;        // 估计角度
float bias = 0;         // 陀螺仪零偏
float P[2][2] = {{1,0},{0,1}};  // 协方差矩阵

// 每次传感器数据到来时执行
void kalmanUpdate(float gyroRate, float accAngle, float dt) {
    // 1. 预测
    angle += (gyroRate - bias) * dt;
    P[0][0] += dt * (dt*P[1][1] - P[0][1] - P[1][0] + Q_angle);
    P[0][1] -= dt * P[1][1];
    P[1][0] -= dt * P[1][1];
    P[1][1] += Q_bias * dt;

    // 2. 更新（用加速度计观测）
    float y = accAngle - angle;  // 残差
    float S = P[0][0] + R_measure;
    float K[2];  // 卡尔曼增益
    K[0] = P[0][0] / S;
    K[1] = P[1][0] / S;

    // 3. 修正
    angle += K[0] * y;
    bias  += K[1] * y;

    // 4. 更新协方差
    float P00_temp = P[0][0];
    float P01_temp = P[0][1];
    P[0][0] -= K[0] * P00_temp;
    P[0][1] -= K[0] * P01_temp;
    P[1][0] -= K[1] * P00_temp;
    P[1][1] -= K[1] * P01_temp;
}

卡尔曼滤波 vs 互补滤波

你可能也听说过互补滤波。它比卡尔曼滤波简单，但效果在某些场景下也不差。

对比项	卡尔曼滤波	互补滤波
计算量	较大（涉及矩阵运算）	很小（只需一个系数）
调参难度	较高（Q、R矩阵）	低（一个截止频率）
自适应能力	强（协方差自动调整）	弱（固定权重）
适用场景	对精度要求高、计算资源充足	资源受限、快速原型

我个人习惯是：原型阶段先用互补滤波，快速验证算法逻辑。等系统稳定了，再换成卡尔曼滤波提升精度。你想想看，如果一开始就调卡尔曼的Q和R，可能调一周都找不到感觉。

避坑指南

• 我曾经在初始化协方差矩阵P时设得太大，导致前几秒的估计值剧烈震荡。后来我让P初始化为单位矩阵乘以一个较小的值（比如0.1），收敛就平滑了。
• 注意陀螺仪数据的时间戳。如果dt不准，预测步骤会引入误差。我建议用硬件定时器记录时间戳，而不是依赖系统延时。
• 还有一点：加速度计在剧烈运动时不可靠。这时候可以适当增大R值，让卡尔曼滤波更信任陀螺仪的预测。说白了，就是动态调整噪声参数。

小结

卡尔曼滤波在姿态估计中的基本思想，其实就是用陀螺仪做短期预测，用加速度计/磁力计做长期修正。它通过协方差矩阵自动权衡两者的权重，实现最优融合。

嗯，这一章的内容就到这里。记住，理论是死的，传感器是活的。多动手调参，你才能真正理解卡尔曼滤波的脾气。

---

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