第1章：卡尔曼滤波（KF）推导——从贝叶斯视角看预测与更新
各位同学，今天我们来啃一块硬骨头——卡尔曼滤波的推导。
说实话，我刚入行那会儿，看到卡尔曼滤波的公式就头疼。一堆矩阵、协方差、增益系数，感觉像天书。后来我师父跟我说了一句话，让我豁然开朗：“卡尔曼滤波，本质上就是个贝叶斯滤波器”。
嗯，今天我就带你从贝叶斯视角，把卡尔曼滤波的预测和更新步骤，一步步拆开来看。
1.1 贝叶斯滤波的核心思想
先问大家一个问题：状态估计到底在干什么？
说白了，就是根据已有的观测数据，去猜系统当前的状态。比如你开车时，GPS信号偶尔会丢，但你仍然知道车大概在哪儿——因为你记得上一秒的位置和速度，这就是“预测”；等GPS信号恢复，你再用它修正一下猜测，这就是“更新”。
贝叶斯滤波的核心公式就一个：
P(x_k | z_{1:k}) ∝ P(z_k | x_k) · P(x_k | z_{1:k-1})翻译成人话：后验概率 ∝ 似然 × 先验概率
其中：
- 先验概率 P(x_k | z_{1:k-1})：根据之前所有观测，预测当前状态的概率
- 似然 P(z_k | x_k)：如果当前状态是x_k，观测到z_k的可能性有多大
- 后验概率 P(x_k | z_{1:k})：结合观测后，对当前状态的最新估计
卡尔曼滤波，就是假设所有概率分布都是高斯分布时，这个公式的解析解。
关键点：卡尔曼滤波不是凭空造出来的，它只是贝叶斯滤波在高斯假设下的特例。理解这一点，后面所有公式你都能自己推出来。
1.2 卡尔曼滤波的两个假设
卡尔曼滤波之所以能写出漂亮的解析解，是因为它做了两个强假设：
- 线性系统：状态转移和观测都是线性的
- 高斯噪声：过程噪声和观测噪声都是零均值高斯分布
用数学语言描述：
状态方程：x_k = F_k · x_{k-1} + B_k · u_k + w_k, w_k ~ N(0, Q_k)
观测方程：z_k = H_k · x_k + v_k, v_k ~ N(0, R_k)其中：
- F_k：状态转移矩阵
- B_k：控制输入矩阵
- H_k：观测矩阵
- Q_k：过程噪声协方差
- R_k：观测噪声协方差
注意：这两个假设在实际工程中往往不成立。比如雷达跟踪目标时，目标运动可能是非线性的。这时候就要用扩展卡尔曼滤波（EKF）或无迹卡尔曼滤波（UKF）。我后面会专门讲。
1.3 预测步骤：从贝叶斯视角看
预测步骤，对应贝叶斯公式中的先验概率计算。
已知k-1时刻的后验分布：
x_{k-1} ~ N(μ_{k-1}, Σ_{k-1})我们要预测k时刻的状态分布。根据状态方程：
x_k = F_k · x_{k-1} + B_k · u_k + w_k因为线性变换下高斯分布仍然是高斯分布，所以：
预测均值：μ_k^- = F_k · μ_{k-1} + B_k · u_k
预测协方差：Σ_k^- = F_k · Σ_{k-1} · F_k^T + Q_k这里上标“-”表示“预测值”或“先验值”。
我个人习惯把这一步叫做“时间更新”——因为我们只用了运动模型，还没用到观测数据。
避坑指南：我曾经在项目中犯过一个低级错误——忘记加Q_k。结果滤波器收敛后，协方差矩阵越来越小，最后完全不相信观测了。嗯，这就是所谓的“过度自信”。记住，过程噪声Q_k是防止滤波器“飘”的关键。
1.4 更新步骤：从贝叶斯视角看
更新步骤，对应贝叶斯公式中的后验概率计算。
现在我们有了先验分布 x_k ~ N(μ_k^-, Σ_k^-)，又得到了新的观测 z_k。我们要计算后验分布。
观测的似然函数：
P(z_k | x_k) = N(z_k; H_k · x_k, R_k)根据贝叶斯公式，后验分布正比于先验乘以似然。两个高斯分布相乘，结果仍然是高斯分布。经过推导（这里省略中间矩阵运算），得到：
卡尔曼增益：K_k = Σ_k^- · H_k^T · (H_k · Σ_k^- · H_k^T + R_k)^{-1}
更新均值：μ_k = μ_k^- + K_k · (z_k - H_k · μ_k^-)
更新协方差：Σ_k = (I - K_k · H_k) · Σ_k^-这三行公式，就是卡尔曼滤波的核心。
你想想看，卡尔曼增益K_k的作用是什么？它决定了我们更相信预测还是更相信观测。
- 如果观测噪声R_k很大，K_k会变小，更新主要依赖预测
- 如果过程噪声Q_k很大（导致Σ_k^-很大），K_k会变大，更新更依赖观测
直观理解：卡尔曼增益就是“预测”和“观测”之间的一个权衡旋钮。调好了，滤波器稳如老狗；调不好，要么发散，要么震荡。
1.5 完整算法流程
把预测和更新串起来，就是卡尔曼滤波的完整流程：
初始化：x_0 ~ N(μ_0, Σ_0)
对于每个时间步 k = 1, 2, ...:
// 预测步骤
μ_k^- = F_k · μ_{k-1} + B_k · u_k
Σ_k^- = F_k · Σ_{k-1} · F_k^T + Q_k
// 更新步骤
K_k = Σ_k^- · H_k^T · (H_k · Σ_k^- · H_k^T + R_k)^{-1}
μ_k = μ_k^- + K_k · (z_k - H_k · μ_k^-)
Σ_k = (I - K_k · H_k) · Σ_k^-这个流程，我建议你手推一遍。我记得当年我推了不下十遍，才真正理解每个矩阵的物理含义。
1.6 一个简单的数值例子
光说不练假把式。我们来看一个一维的例子：
假设你用一个雷达测距，目标做匀速运动。状态向量 x = [位置, 速度]^T。
F = [[1, Δt],
[0, 1]]
H = [1, 0] // 只观测位置
Q = [[0.1, 0],
[0, 0.1]]
R = [1.0] // 观测噪声方差
初始状态：μ_0 = [0, 1]^T, Σ_0 = [[1, 0], [0, 1]]假设Δt=1秒，第一次观测z_1=1.2米。我们来手动算一步：
预测：
μ_1^- = F · μ_0 = [1, 1]^T
Σ_1^- = F · Σ_0 · F^T + Q = [[1.1, 1.0], [1.0, 1.1]]
更新：
K_1 = Σ_1^- · H^T · (H · Σ_1^- · H^T + R)^{-1}
= [1.1, 1.0]^T · (1.1 + 1.0)^{-1}
= [0.524, 0.476]^T
μ_1 = μ_1^- + K_1 · (z_1 - H · μ_1^-)
= [1, 1]^T + [0.524, 0.476]^T · (1.2 - 1.0)
= [1.105, 1.095]^T
Σ_1 = (I - K_1 · H) · Σ_1^-
= [[0.476, -0.524], [-0.476, 0.524]] · [[1.1, 1.0], [1.0, 1.1]]
= [[0.524, 0.476], [0.476, 0.524]]你看，更新后的位置估计是1.105米，介于预测值1.0米和观测值1.2米之间。这就是卡尔曼滤波的“折中”思想。
实战经验：我建议你写个Python脚本，把这个例子跑100步，看看协方差矩阵怎么收敛的。你会发现，即使初始协方差设得很大，滤波器也能很快收敛到稳定状态。
1.7 卡尔曼滤波的贝叶斯解释总结
最后，我用一张图总结一下贝叶斯视角下的卡尔曼滤波：
这张图把整个流程串起来了：
- 左边是预测步骤，计算先验概率
- 右边是更新步骤，结合观测计算后验概率
- 底部的反馈回路表示时间推进，后验变成下一时刻的先验
说白了，卡尔曼滤波就是“预测-观测-修正”的循环。每次循环，我们对状态的认知就更精确一点。
核心总结：卡尔曼滤波不是魔法，它就是贝叶斯滤波在高斯线性假设下的解析解。理解了这个本质，你就能灵活地把它应用到各种实际问题中——比如雷达目标跟踪、GPS/IMU融合、机器人定位等等。
好了，这一章就到这里。下一章我们会讲扩展卡尔曼滤波（EKF），看看当系统不是线性时，我们怎么用泰勒展开来“强行线性化”。