4、卡尔曼滤波入门：状态空间模型、预测与更新步骤、卡尔曼增益的直观理解
各位同学，欢迎来到卡尔曼滤波的世界。
说实话，我第一次接触卡尔曼滤波时，也觉得这玩意儿挺玄乎的。一堆矩阵、协方差、增益系数，看着就头大。但后来我在做组合导航项目时，发现它其实没那么神秘。说白了，卡尔曼滤波就是一个“猜得准”的算法——它帮你把传感器数据里的噪声滤掉，把真实状态估计出来。
4.1 状态空间模型：你在描述什么？
先问个问题：你开车时，怎么知道自己的位置？
GPS告诉你一个位置，IMU告诉你加速度和角速度。但GPS有噪声，IMU有漂移。那真实位置到底在哪？
卡尔曼滤波的思路是：我们用一个状态向量来描述系统。在组合导航里，这个状态向量通常包括位置、速度、姿态，甚至还有IMU的零偏误差。
状态空间模型的核心公式：
x_k = A * x_{k-1} + B * u_k + w_k
z_k = H * x_k + v_k第一行叫“状态方程”，描述系统怎么演化。第二行叫“观测方程”，描述传感器怎么测量。
我个人习惯把状态方程想象成“物理规律”。比如你以速度v匀速运动，那下一时刻的位置就是 x + v*dt。这里的A矩阵就是那个“规律”。
而观测方程呢？就是传感器读数与真实状态之间的关系。比如GPS直接测位置，那H矩阵就是[1, 0]（假设状态是位置和速度）。
避坑指南：我曾经在项目中把状态向量的维度搞错了。IMU的零偏我忘了加进去，结果滤波结果发散得一塌糊涂。记住：状态向量要包含所有你需要估计的量，一个都不能少。
4.2 预测步骤：先猜一个
卡尔曼滤波分两步走：预测和更新。先讲预测。
预测步骤，说白了就是“根据上一时刻的状态，猜一下当前时刻的状态”。
// 预测步骤
x_pred = A * x_est + B * u // 状态预测
P_pred = A * P_est * A^T + Q // 协方差预测这里有两个关键点：
- x_pred：预测的状态。比如你上一秒在位置10米处，速度2m/s，那下一秒你猜位置是12米。
- P_pred：预测的协方差。它代表你对这个预测的“信心”。如果模型不准，P_pred就会变大。
Q矩阵是过程噪声协方差。它代表模型本身的不确定性。比如你假设匀速运动，但实际可能有加速或减速，那Q就要设大一点。
注意：Q矩阵设得太小，滤波器会“过于自信”，跟不上真实变化。设得太大，又会被噪声带偏。我一般会先根据物理模型估算一个值，然后通过仿真调参。
4.3 更新步骤：用测量值修正
预测完了，我们有了一个“猜测”。但传感器给了我们一个测量值。怎么办？
当然是拿测量值来修正猜测。
// 更新步骤
K = P_pred * H^T * (H * P_pred * H^T + R)^(-1) // 卡尔曼增益
x_est = x_pred + K * (z - H * x_pred) // 状态更新
P_est = (I - K * H) * P_pred // 协方差更新这里最核心的就是卡尔曼增益K。它决定了你更相信预测，还是更相信测量。
4.4 卡尔曼增益的直观理解
卡尔曼增益K，说白了就是一个“权重系数”。
它的取值范围在0到1之间（对于标量情况）。
- K接近0：我更相信预测。测量噪声太大，我不太信传感器。
- K接近1：我更相信测量。模型不准，我信传感器多一点。
为什么会这样？你看K的计算公式：
K = P_pred * H^T / (H * P_pred * H^T + R)
如果测量噪声R很大，分母变大，K就变小。如果预测协方差P_pred很大，分子变大，K就变大。
直观理解：
想象你在黑暗中猜一个物体的位置。你的“预测”是凭感觉猜的，你的“测量”是用一把尺子量的。
- 如果尺子很准（R小），你就更相信尺子（K大）。
- 如果你感觉很好（P_pred小），你就更相信感觉（K小）。
卡尔曼增益就是那个“尺子准还是感觉准”的权衡系数。
我在项目中遇到过一种情况：GPS信号突然变差，R矩阵变得很大。这时候卡尔曼增益自动变小，滤波器更多地依赖IMU的预测。等GPS恢复后，R变小，K又变大，滤波器重新信任GPS。整个过程是自动的，不需要人工干预。这就是卡尔曼滤波的优雅之处。
4.5 完整流程：一个简单的例子
假设我们要估计一维位置和速度。状态向量 x = [位置, 速度]^T。
// 初始化
x = [0, 0]^T // 初始位置0，速度0
P = [[1, 0], // 初始协方差
[0, 1]]
Q = [[0.1, 0], // 过程噪声
[0, 0.1]]
R = [1] // 测量噪声
// 时间步长 dt = 0.1秒
A = [[1, dt],
[0, 1]]
H = [1, 0] // 只测量位置
// 循环
for each time step:
// 预测
x_pred = A * x
P_pred = A * P * A^T + Q
// 如果有测量值
if measurement_available:
K = P_pred * H^T / (H * P_pred * H^T + R)
x = x_pred + K * (z - H * x_pred)
P = (I - K * H) * P_pred
else:
x = x_pred
P = P_pred经验之谈：实际项目中，测量值不一定每个时刻都有。比如GPS更新频率低，IMU更新频率高。这时候你可以在没有测量值时只做预测，有测量值时再做更新。卡尔曼滤波天然支持这种“异步”模式。
4.6 知识体系图
下面我用一张SVG图来总结卡尔曼滤波的核心逻辑：
这张图展示了卡尔曼滤波的完整闭环。从初始化开始，经过预测、判断是否有测量、更新或直接输出，最后反馈到下一时刻。嗯，这个循环会一直持续下去。
4.7 总结
卡尔曼滤波入门，其实就三件事：
- 状态空间模型：把物理系统用数学公式描述出来。
- 预测与更新：先猜，再用测量值修正。
- 卡尔曼增益：自动权衡预测和测量的可信度。
我个人觉得，卡尔曼滤波最妙的地方在于：它不需要你手动调权重。卡尔曼增益会根据协方差自动计算。你只需要把模型建好，把噪声参数设对，剩下的交给算法。
当然，参数调优是个技术活。我刚开始做的时候，Q和R矩阵调了一周才收敛。但一旦调好了，效果是真的稳。
一句话总结：卡尔曼滤波 = 预测 + 测量 + 加权平均。加权系数就是卡尔曼增益。
公众号:蓝海数据掘金营，微信deep3321