4. 卡尔曼滤波（KF）原理：系统模型、预测与更新步骤、卡尔曼增益推导
卡尔曼滤波，说白了就是一套数学公式，帮你从一堆带噪声的传感器数据里，把真实状态给“猜”出来。我当年第一次接触它时，觉得这东西太神奇了——明明每个传感器都不准，组合起来却能给出比任何单个传感器都准的结果。
嗯，咱们今天就把它的核心原理拆开揉碎。你只要搞懂三件事：系统模型怎么建、预测和更新两步怎么走、卡尔曼增益到底是个啥。
4.1 系统模型：你得先知道系统怎么动
卡尔曼滤波假设系统是线性的，而且噪声是高斯分布。这个假设很关键，你想想看，如果系统是非线性的，那得用扩展卡尔曼滤波（EKF），那是后话了。
系统模型由两个方程组成：
- 状态方程：描述状态怎么随时间变化
- 观测方程：描述传感器怎么测量状态
写成数学形式：
x_k = A * x_{k-1} + B * u_k + w_k
z_k = H * x_k + v_k这里：
x_k是 k 时刻的状态向量（比如位置、速度）A是状态转移矩阵，描述上一时刻状态如何影响当前时刻B是控制输入矩阵，u_k是控制量（比如油门、转向）w_k是过程噪声，服从 N(0, Q)z_k是观测值，H是观测矩阵v_k是观测噪声，服从 N(0, R)
核心要点：Q 和 R 是两个协方差矩阵。Q 代表你对模型的信任程度，R 代表你对传感器的信任程度。我调参时最常折腾的就是这两个矩阵。
4.2 预测步骤：先猜一下状态
预测步骤，说白了就是“根据上一时刻的状态，猜当前时刻的状态”。分两步：
- 状态预测：
x̂_k|k-1 = A * x̂_{k-1|k-1} + B * u_k - 协方差预测：
P_k|k-1 = A * P_{k-1|k-1} * A^T + Q
这里 P 是状态协方差矩阵，代表你对当前估计的不确定度。预测完，不确定度会变大（因为加了 Q）。
我的经验：预测步骤其实就是在“开环”跑模型。如果模型不准，预测就会偏。我曾经在一个无人机项目里，因为没考虑风阻，预测的位置和实际差了十几米。后来把 Q 调大，让滤波器更相信传感器，才救回来。
4.3 更新步骤：用观测值修正预测
预测完了，传感器数据来了。这时候要做的是“用观测值修正预测”。更新步骤也分两步：
- 计算卡尔曼增益：
K_k = P_k|k-1 * H^T * (H * P_k|k-1 * H^T + R)^{-1} - 状态更新：
x̂_k|k = x̂_k|k-1 + K_k * (z_k - H * x̂_k|k-1) - 协方差更新：
P_k|k = (I - K_k * H) * P_k|k-1
你注意看，z_k - H * x̂_k|k-1 叫“残差”或“新息”。它代表“实际观测”和“预测观测”之间的差距。卡尔曼增益 K 决定了你多大程度上相信这个差距。
避坑指南：我曾经犯过一个低级错误——忘记把观测矩阵 H 转置。结果卡尔曼增益算出来全是错的，滤波器直接发散。嗯，这种细节你写代码时一定要检查。
4.4 卡尔曼增益推导：它到底在算什么？
卡尔曼增益的推导，核心思想是“最小化后验估计的协方差”。说白了，就是让更新后的状态估计尽可能准。
推导过程：
- 后验估计误差：
e_k = x_k - x̂_k|k - 后验协方差：
P_k|k = E[e_k * e_k^T] - 把
x̂_k|k = x̂_k|k-1 + K_k * (z_k - H * x̂_k|k-1)代入 - 对
K_k求导，令导数为零 - 解出
K_k = P_k|k-1 * H^T * (H * P_k|k-1 * H^T + R)^{-1}
你不需要每次都手推一遍，但理解它的物理意义很重要：
- 当 R 很大（传感器噪声大）时，K 趋近于 0，你更相信预测
- 当 Q 很大（模型不准）时，K 趋近于 1，你更相信观测
一句话总结：卡尔曼增益就是“预测和观测之间的信任分配器”。
4.5 完整算法流程
把预测和更新串起来，就是卡尔曼滤波的完整流程：
初始化：
x̂_0|0, P_0|0
对每个时刻 k = 1, 2, ...：
// 预测
x̂_k|k-1 = A * x̂_{k-1|k-1} + B * u_k
P_k|k-1 = A * P_{k-1|k-1} * A^T + Q
// 更新
K_k = P_k|k-1 * H^T * (H * P_k|k-1 * H^T + R)^{-1}
x̂_k|k = x̂_k|k-1 + K_k * (z_k - H * x̂_k|k-1)
P_k|k = (I - K_k * H) * P_k|k-14.6 核心逻辑流程图
下面这张图展示了卡尔曼滤波的完整逻辑。我建议你把它存下来，写代码时对照着看：
4.7 一个简单的例子
假设你要估计一个物体的位置和速度。传感器只测位置，有噪声。
| 参数 | 值 | 说明 |
|---|---|---|
| A | [[1, dt], [0, 1]] | 位置 = 位置 + 速度 * dt |
| H | [[1, 0]] | 只观测位置 |
| Q | [[0.01, 0], [0, 0.01]] | 过程噪声较小 |
| R | [[0.1]] | 观测噪声中等 |
初始化时，我一般把 P 设得大一些（比如 [[100, 0], [0, 100]]），表示初始估计很不确定。这样滤波器会快速收敛。
我的习惯：调试卡尔曼滤波时，先跑一遍开环（只用预测，不用更新），看看模型本身准不准。如果开环都跑飞了，那闭环肯定救不回来。
卡尔曼滤波就这么点东西。你把它理解成“预测 + 修正”的循环，再配上卡尔曼增益这个信任分配器，基本就掌握了。剩下的就是调参和写代码了。