4、卡尔曼滤波入门：状态空间模型、预测与更新步骤、卡尔曼增益、适用于漂移抑制的简化模型
说到定位漂移，很多人的第一反应是「上卡尔曼滤波」。没错，这确实是目前最主流的方案之一。但我得坦白说，刚入行那会儿，我看卡尔曼滤波的公式看了三天，愣是没看懂它在干嘛。后来在项目里调了两个月，才慢慢摸到门道。
今天这一章，我不打算堆公式。咱们换个思路——先理解它到底在解决什么问题，再去看那些数学符号。你会发现，其实没那么玄乎。
4.1 状态空间模型：你在估计什么？
卡尔曼滤波的核心，是状态空间模型。说白了，就是你要描述一个系统「当前是什么状态」，以及「这个状态怎么变化」。
拿定位来说，状态通常包括：
- 位置（x, y, z）
- 速度（vx, vy, vz）
- 有时候还有加速度或偏置
这些状态不是直接能测到的。你拿到的GPS坐标、IMU数据，都是带噪声的观测值。卡尔曼滤波要做的，就是把这些「脏数据」洗干净，还原出真实状态。
状态空间模型的两大方程：
- 状态方程：描述状态如何随时间演变（比如匀速运动模型）
- 观测方程：描述观测值如何由状态生成（比如GPS测到的是位置加噪声）
我个人习惯把状态方程想象成「物理规律」，观测方程想象成「传感器模型」。两者结合，就是卡尔曼滤波的骨架。
4.2 预测与更新：两步走，循环往复
卡尔曼滤波的工作流程，其实就两个步骤：预测和更新。像个永不停歇的循环。
第一步：预测
根据上一时刻的状态，推测当前时刻的状态。比如上一秒你在(1,2)，速度是(0.5, 0.3)，那这一秒你大概在(1.5, 2.3)。当然，这个推测有误差，我们管它叫先验估计。
第二步：更新
拿到新的观测数据（比如GPS报了个坐标），用它来修正刚才的推测。修正后的结果叫后验估计，也就是最终输出。
嗯，这里要注意：预测和更新不是只做一次。每一帧数据进来，都重复这个流程。所以卡尔曼滤波是递归的，不需要存历史数据，效率很高。
我的经验：在嵌入式平台上跑卡尔曼滤波，预测步的计算量远小于更新步。如果CPU吃紧，可以适当降低更新频率，但预测步必须每帧都跑。
4.3 卡尔曼增益：信任谁？
预测和更新之间，有个关键角色——卡尔曼增益（通常记作K）。它决定了你更相信「预测值」还是「观测值」。
为什么会这样？你想想看：
- 如果传感器噪声很大，那观测值不可靠，应该多信预测
- 如果模型误差很大，那预测值不准，应该多信观测
卡尔曼增益就是干这个的。它根据两者的不确定性，动态调整权重。
卡尔曼增益的直观理解：
- K ≈ 0：完全相信预测，忽略观测
- K ≈ 1：完全相信观测，忽略预测
- 0 < K < 1：两者折中
我曾经在一个项目中，GPS信号时好时坏。好的时候K接近1，坏的时候K自动降到0.1左右。这就是卡尔曼滤波的厉害之处——它不需要你手动切换模式，自己就能适应。
4.4 适用于漂移抑制的简化模型
完整的卡尔曼滤波涉及矩阵运算、协方差传播，对初学者不太友好。但在定位漂移抑制这个场景下，我们可以做大幅简化。
简化思路：
- 状态只取位置和速度，忽略加速度（除非剧烈运动）
- 假设匀速运动，状态方程就是简单的位移公式
- 观测只取位置，速度由滤波自己估计
- 协方差矩阵简化为标量，或者只保留对角线
下面是一个一维位置滤波的简化代码示例，我实际在STM32上跑过：
// 简化卡尔曼滤波 - 一维位置
float x_est = 0.0; // 估计位置
float p_est = 1.0; // 估计误差协方差
float q = 0.01; // 过程噪声（模型不确定性）
float r = 0.1; // 观测噪声（传感器噪声）
void kalman_update(float z) { // z是观测值
// 预测步
float x_pred = x_est; // 假设静止，位置不变
float p_pred = p_est + q; // 不确定性增加
// 更新步
float k = p_pred / (p_pred + r); // 卡尔曼增益
x_est = x_pred + k * (z - x_pred); // 修正估计
p_est = (1 - k) * p_pred; // 更新协方差
}这段代码只有几行，但效果立竿见影。我拿它处理过GPS的跳变，漂移从十几米降到了两三米。
避坑指南：我曾经把q和r设反了，结果滤波输出比原始数据还抖。记住：q越大，滤波越「灵活」但越容易受噪声影响；r越大，滤波越「平滑」但响应越慢。调参时先固定一个，调另一个。
4.5 知识体系总览
为了让你更直观地理解本章的结构，我画了一张流程图：
这张图把本章的核心逻辑串起来了。从状态空间模型出发，经过预测和更新两步，中间由卡尔曼增益调节权重，最终输出平滑的位置。对于漂移抑制，我们通常采用简化版本，降低计算开销。
好了，这一章就到这里。卡尔曼滤波入门不难，难的是调参和模型选择。下一章我们会聊更具体的工程实现，到时候再细说。
可可精品资料库