4. 卡尔曼滤波原理：标准卡尔曼滤波（KF）的数学推导，状态预测与更新方程，适用条件
各位同学，咱们今天聊点硬核的——卡尔曼滤波。
说实话，我在做传感器融合之前，一直觉得卡尔曼滤波是个黑盒子。公式摆在那，但到底怎么来的？为什么它能“滤波”？我刚开始做项目时，照着书抄了一遍代码，结果数据全飘了。后来才明白，不懂原理就敢上，那是给自己挖坑。
今天咱们就把这个坑填上。我会带着大家，一步步把标准卡尔曼滤波的数学推导捋清楚。你不需要是数学天才，只要跟着我的思路走，保证你能看懂。
4.1 卡尔曼滤波在说什么？
说白了，卡尔曼滤波就是干一件事：用带噪声的测量值，去估计系统的真实状态。
你想想看，传感器测出来的数据，永远有噪声。比如GPS定位，误差几米很正常。但如果你知道车在匀速运动，上一秒的位置加上速度×时间，就能猜出下一秒的位置。卡尔曼滤波就是把这个“猜”和“测”结合起来，得到一个更靠谱的结果。
我习惯把它理解成“预测+修正”的循环。就像你闭着眼睛走路，先迈一步（预测），然后睁开眼看看实际走到哪了（测量），再调整下一步的方向（修正）。
核心思想：卡尔曼滤波假设系统是线性的，噪声是高斯白噪声。在这个前提下，它能给出最优的估计。
4.2 标准卡尔曼滤波的数学模型
先搭个架子。我们用一个离散时间线性系统来描述被控对象：
状态方程：x_k = A * x_{k-1} + B * u_{k-1} + w_{k-1}
测量方程：z_k = H * x_k + v_k这里每个符号都有它的含义：
| 符号 | 含义 | 维度 |
|---|---|---|
| x_k | k时刻的系统状态（比如位置、速度） | n×1 |
| A | 状态转移矩阵（描述系统怎么演化） | n×n |
| B | 控制输入矩阵 | n×m |
| u_k | 控制输入（比如油门、刹车） | m×1 |
| z_k | 测量值（传感器读数） | p×1 |
| H | 测量矩阵（状态到测量的映射） | p×n |
| w_k | 过程噪声（模型误差） | n×1 |
| v_k | 测量噪声（传感器噪声） | p×1 |
噪声的统计特性很重要：
- w_k ~ N(0, Q)，协方差矩阵为Q
- v_k ~ N(0, R)，协方差矩阵为R
- w_k 和 v_k 互不相关
嗯，这里要注意：Q和R是你要调的参数。Q越大，说明你越不相信模型；R越大，说明你越不相信传感器。我在项目中吃过亏，一开始把Q设得太小，结果滤波器反应迟钝，跟不上实际变化。
4.3 状态预测方程（时间更新）
预测阶段，就是利用上一时刻的状态，来猜当前时刻的状态。分两步：
第一步：先验状态估计
x̂_k^- = A * x̂_{k-1} + B * u_{k-1}这个公式很好理解。上一时刻的最优估计是 x̂_{k-1}，乘以状态转移矩阵A，再加上控制输入的影响，就得到了当前时刻的“先验估计”。注意这个上标“-”，表示还没用测量值修正。
第二步：先验误差协方差
P_k^- = A * P_{k-1} * A^T + QP代表估计的不确定性。上一时刻的不确定性是P_{k-1}，经过状态转移后，不确定性会放大（因为A乘过去），再加上过程噪声Q，就得到了当前时刻的先验不确定性。
我个人的习惯是，把P理解成“对估计的信心”。P越小，说明我越相信这个估计值。刚开始时P可以设大一点，让滤波器自己去收敛。
4.4 状态更新方程（测量更新）
有了预测值，现在来了测量值z_k。怎么把两者融合？
第一步：计算卡尔曼增益
K_k = P_k^- * H^T * (H * P_k^- * H^T + R)^{-1}这个K_k就是卡尔曼增益，它决定了你更相信预测还是测量。你看这个公式：
- 如果R很大（测量噪声大），分母变大，K_k变小——更相信预测
- 如果P_k^-很大（预测不确定性大），分子变大，K_k变大——更相信测量
说白了，K_k就是个权重。谁更靠谱，就多信谁一点。
第二步：更新状态估计
x̂_k = x̂_k^- + K_k * (z_k - H * x̂_k^-)括号里的 (z_k - H * x̂_k^-) 叫“新息”或“残差”。它表示测量值和预测值之间的差异。如果这个差异很大，说明预测可能不准，就用K_k去修正。如果差异很小，说明预测挺准的，就基本不动。
第三步：更新误差协方差
P_k = (I - K_k * H) * P_k^-这一步是更新我们对状态估计的信心。用了测量值之后，不确定性应该降低。你看，P_k 肯定小于 P_k^-，因为 (I - K_k * H) 是一个小于1的矩阵。
避坑指南：我曾经在代码里把P_k的更新公式写错了，用了P_k = (I - K_k * H) * P_k^- * (I - K_k * H)^T + K_k * R * K_k^T。这个公式在数值上更稳定，但计算量大。如果你的系统是线性的，用简化版就够了。如果遇到数值发散，再换这个稳定版。
4.5 卡尔曼滤波的完整流程
把上面这些串起来，就是卡尔曼滤波的完整循环。我画了个图，帮你理清思路：
这个循环每来一个测量值就跑一次。初始化时，你需要给 x̂₀ 和 P₀ 赋初值。P₀ 设大一点没关系，滤波器会自己收敛。
4.6 适用条件
卡尔曼滤波不是万能的。它有几个硬性条件：
- 系统必须是线性的——状态方程和测量方程都必须是线性关系。如果系统是非线性的，你需要用扩展卡尔曼滤波（EKF）或无迹卡尔曼滤波（UKF）。
- 噪声必须是高斯白噪声——均值为0，协方差已知。如果噪声不是高斯的，卡尔曼滤波给出的估计可能不是最优的。
- 噪声必须互不相关——过程噪声和测量噪声之间，以及不同时刻的噪声之间，都不能相关。
- 模型参数必须已知——A、B、H、Q、R这些矩阵，你得事先知道或者能辨识出来。
警告：我在实际项目中遇到过，Q和R设得不合理，滤波器直接发散。特别是Q，如果你设得太小，滤波器会“过于自信”，跟不上系统的真实变化。建议先用仿真数据调参，再上真机。
4.7 一个简单的例子
假设我们要估计一个物体的位置和速度。传感器只测位置，噪声方差为R=1。系统是匀速运动，过程噪声方差Q很小。
状态向量：x = [位置, 速度]ᵀ
状态转移矩阵：A = [[1, Δt], [0, 1]]
测量矩阵：H = [1, 0]
初始化：x̂₀ = [0, 0]ᵀ, P₀ = [[10, 0], [0, 10]]
然后就是循环跑预测和更新。你会发现，刚开始估计值波动比较大，但跑几步之后，P矩阵会收敛，估计值也越来越准。
嗯，这就是卡尔曼滤波的魅力——用数学把不确定的东西变得确定。
好了，这一章的内容就到这里。记住，卡尔曼滤波的核心就是“预测+修正”的循环。下一章我们会聊扩展卡尔曼滤波，看看非线性系统怎么处理。