1. 从贝叶斯视角重新认识卡尔曼滤波
很多做状态估计的朋友第一次接触卡尔曼滤波，都是从那一堆矩阵公式开始的：预测、更新、卡尔曼增益、协方差矩阵，推导过程翻来覆去，最后代码跑通了，但心里始终有个疙瘩——这些公式到底是怎么冒出来的？为什么卡尔曼增益长那个样子？为什么协方差矩阵要那样更新？
我当初也是这么过来的。后来把贝叶斯滤波的框架吃透之后，再回头看卡尔曼滤波，才发现它根本不是什么凭空冒出来的公式集合，而是贝叶斯滤波在线性高斯假设下的一次精确求解。换句话说，卡尔曼滤波就是贝叶斯滤波的一个特例，而且是唯一一个能写出解析解的特例。这个认知一旦建立，后面不管是调参、排查发散问题，还是往扩展卡尔曼、无迹卡尔曼上迁移，都会顺畅很多。
这篇内容适合两类人：一类是已经会用卡尔曼滤波但没搞明白推导逻辑的工程师，另一类是想从贝叶斯滤波体系往下打通状态估计这条线的学习者。我会从贝叶斯滤波的递推框架讲起，一步步把卡尔曼滤波的五个核心公式推出来，中间穿插参数含义、矩阵维度、实操中的坑，以及我自己在项目里踩过的经验。不追求数学上的极致严谨，但保证每一步都有直觉上的解释，让你看完能自己把公式默写出来。
2. 贝叶斯滤波的递推框架回顾
2.1 贝叶斯滤波到底在做什么
贝叶斯滤波的核心思想可以用一句话概括：用上一时刻的状态估计和当前时刻的观测，去推断当前时刻状态的可信程度。这里的“可信程度”不是单个数值，而是一个概率分布。也就是说，贝叶斯滤波维护的不是一个状态点，而是状态空间上的一个概率密度函数。
具体来说，它分两步走。第一步叫预测，也叫时间更新：根据系统的运动模型，把上一时刻的后验分布往前推，得到当前时刻的先验分布。第二步叫更新，也叫观测更新：拿到当前时刻的观测值后，用贝叶斯公式把先验分布修正成后验分布。这两步交替执行，就构成了完整的递推滤波。
用公式写出来是这样的。假设我们用 ( x_k ) 表示第 k 时刻的状态，( z_k ) 表示第 k 时刻的观测，( u_k ) 表示控制输入。那么预测步是：
[ p(x_k | z_{1:k-1}, u_{1:k}) = \int p(x_k | x_{k-1}, u_k) , p(x_{k-1} | z_{1:k-1}, u_{1:k-1}) , dx_{k-1} ]
更新步是：
[ p(x_k | z_{1:k}, u_{1:k}) = \frac{p(z_k | x_k) , p(x_k | z_{1:k-1}, u_{1:k})}{p(z_k | z_{1:k-1}, u_{1:k})} ]
这两个公式看起来简单，但实际用起来有个致命问题：积分那一步，除了极少数特殊情况，根本没有解析解。你没法用一个闭式表达式去表示那个积分的结果。这就是为什么贝叶斯滤波在理论上很完美，但直接落地几乎不可能。
2.2 为什么需要卡尔曼滤波
既然贝叶斯滤波的积分没法算，那工程上就得想办法。思路有两条：一条是用数值方法近似，比如粒子滤波，用一堆采样点去逼近那个分布；另一条是加假设，让积分变得可算。卡尔曼滤波走的就是第二条路。
它加了两个关键假设。第一个假设是系统是线性的，也就是说状态转移和观测过程都可以用线性方程表示。第二个假设是噪声是高斯的，过程噪声和观测噪声都服从零均值的高斯分布。这两个假设一加，事情就变得非常漂亮了：高斯分布经过线性变换还是高斯分布，两个高斯分布相乘还是高斯分布。这意味着，如果你上一时刻的后验是高斯的，那么预测出来的先验还是高斯的，更新之后的后验依然是高斯的。
高斯分布只需要均值和协方差两个参数就能完全描述。所以贝叶斯滤波里那个无穷维的函数递推，一下子退化成了两个有限维参数的递推：均值怎么变，协方差怎么变。卡尔曼滤波的五个公式，本质上就是在算这两个参数在预测和更新两步中分别怎么演化。
注意：线性高斯假设是卡尔曼滤波成立的前提。如果你的系统非线性很强，或者噪声明显不是高斯，标准卡尔曼滤波的效果会大打折扣，这时候需要考虑扩展卡尔曼或者无迹卡尔曼。
3. 卡尔曼滤波五大公式的完整推导
3.1 系统模型与符号约定
在推导之前，先把符号和模型定清楚。线性高斯系统的状态方程和观测方程写成：
[ x_k = A x_{k-1} + B u_k + w_{k-1} ]
[ z_k = H x_k + v_k ]
其中 ( A ) 是状态转移矩阵，( B ) 是控制输入矩阵，( H ) 是观测矩阵。( w_{k-1} ) 是过程噪声，服从 ( N(0, Q) )；( v_k ) 是观测噪声，服从 ( N(0, R) )。两者相互独立，且与初始状态无关。
我们记 ( \hat{x}{k|k-1} ) 为预测步得到的先验均值，( P{k|k-1} ) 为先验协方差；( \hat{x}{k|k} ) 为更新后的后验均值，( P{k|k} ) 为后验协方差。初始时刻的 ( \hat{x}{0|0} ) 和 ( P{0|0} ) 需要人为给定，通常 ( \hat{x}{0|0} ) 取第一个观测值或者零，( P{0|0} ) 取一个较大的对角阵表示初始不确定性大。
这里有个实操细节：( P_{0|0} ) 取太大，滤波器初期会过度信任观测，收敛快但可能抖动；取太小，滤波器过度信任模型，收敛慢甚至发散。我一般取观测噪声协方差的十倍到一百倍之间，具体看传感器精度。
3.2 预测步：均值和协方差怎么往前推
预测步的目标是求 ( p(x_k | z_{1:k-1}) )。根据全概率公式，它等于对 ( p(x_k | x_{k-1}) p(x_{k-1} | z_{1:k-1}) ) 关于 ( x_{k-1} ) 积分。由于两个分布都是高斯的，积分结果依然是高斯，我们只需要算它的均值和协方差。
先算均值。对状态方程两边取期望：
[ E[x_k] = E[A x_{k-1} + B u_k + w_{k-1}] ]
由于 ( w_{k-1} ) 零均值，且与 ( x_{k-1} ) 独立：
[ \hat{x}{k|k-1} = A \hat{x}{k-1|k-1} + B u_k ]
这就是预测步的均值公式，非常直观：把上一时刻的后验均值代入状态方程，去掉噪声项即可。
再算协方差。协方差的定义是 ( P = E[(x - \hat{x})(x - \hat{x})^T] )。把状态方程代入，令误差 ( e_{k|k-1} = x_k - \hat{x}_{k|k-1} )，则：
[ e_{k|k-1} = A (x_{k-1} - \hat{x}{k-1|k-1}) + w{k-1} ]
令 ( e_{k-1|k-1} = x_{k-1} - \hat{x}_{k-1|k-1} )，则：
[ P_{k|k-1} = E[(A e_{k-1|k-1} + w_{k-1})(A e_{k-1|k-1} + w_{k-1})^T] ]
展开后，由于 ( e_{k-1|k-1} ) 和 ( w_{k-1} ) 独立且 ( w_{k-1} ) 零均值，交叉项期望为零，得到：
[ P_{k|k-1} = A P_{k-1|k-1} A^T + Q ]
这就是预测步的协方差公式。它的含义是：上一时刻的不确定性经过状态转移矩阵的线性变换后，再加上过程噪声带来的新不确定性。
提示：( Q ) 的选取直接决定滤波器对模型的信任程度。( Q ) 大表示模型不准，滤波器会更依赖观测；( Q ) 小表示模型很准，滤波器会更平滑但可能滞后。
3.3 更新步：卡尔曼增益的来历
更新步要做的事情是：拿到观测 ( z_k ) 后，把先验分布 ( N(\hat{x}{k|k-1}, P{k|k-1}) ) 和观测似然 ( N(H x_k, R) ) 结合起来，得到后验分布。两个高斯分布相乘，结果还是高斯，我们只需要算后验的均值和协方差。
这里有两种推导路径。一种是直接套用高斯乘积公式，另一种是从最小均方误差估计的角度推导。我用后者，因为卡尔曼增益的物理意义会更清楚。
我们假设后验估计是先验估计和观测残差的线性组合：
[ \hat{x}{k|k} = \hat{x}{k|k-1} + K_k (z_k - H \hat{x}_{k|k-1}) ]
其中 ( K_k ) 是待定的增益矩阵。我们的目标是让后验估计的均方误差最小，也就是让 ( P_{k|k} = E[(x_k - \hat{x}{k|k})(x_k - \hat{x}{k|k})^T] ) 的迹最小。
令先验误差 ( e_{k|k-1} = x_k - \hat{x}{k|k-1} )，观测残差 ( \tilde{z}k = z_k - H \hat{x}{k|k-1} = H e{k|k-1} + v_k )。则后验误差：
[ e_{k|k} = x_k - \hat{x}{k|k} = e{k|k-1} - K_k (H e_{k|k-1} + v_k) = (I - K_k H) e_{k|k-1} - K_k v_k ]
后验协方差：
[ P_{k|k} = (I - K_k H) P_{k|k-1} (I - K_k H)^T + K_k R K_k^T ]
对 ( K_k ) 求导并令导数为零，经过矩阵求导运算，得到：
[ K_k = P_{k|k-1} H^T (H P_{k|k-1} H^T + R)^{-1} ]
这就是卡尔曼增益的公式。它的结构很有意思：分子 ( P_{k|k-1} H^T ) 反映的是先验不确定性和观测矩阵的耦合，分母 ( H P_{k|k-1} H^T + R ) 反映的是观测预测的不确定性加上观测噪声。所以卡尔曼增益本质上是一个信噪比：先验越不确定，增益越大，越信任观测；观测噪声越大，增益越小，越信任模型。
3.4 后验协方差的简化形式
把 ( K_k ) 代回 ( P_{k|k} ) 的表达式，可以化简。展开后利用 ( K_k ) 的定义式，最终得到两种等价形式：
[ P_{k|k} = (I - K_k H) P_{k|k-1} ]
或者
[ P_{k|k} = (I - K_k H) P_{k|k-1} (I - K_k H)^T + K_k R K_k^T ]
第一种形式计算量小，但数值上可能因为矩阵不对称导致协方差失去正定性。第二种形式计算量大，但数值稳定性好。工程上我一般用第一种，然后每隔一段时间做一次对称化处理：( P = (P + P^T) / 2 )。如果发现协方差出现负特征值，就切换到第二种形式。
注意：协方差矩阵必须保持对称正定。一旦出现非正定，滤波器可能发散。定期做对称化和特征值检查是个好习惯。
3.5 五个公式汇总与维度检查
把上面的结果整理一下，卡尔曼滤波的五个核心公式是：
预测步：
- 状态预测：( \hat{x}{k|k-1} = A \hat{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} )
- 状态更新：( \hat{x}{k|k} = \hat{x}{k|k-1} + K_k (z_k - H \hat{x}_{k|k-1}) )
- 协方差更新：( P_{k|k} = (I - K_k H) P_{k|k-1} )
维度检查很重要。假设状态维度是 n，观测维度是 m。那么 A 是 n×n，B 是 n×p（p 是控制维度），H 是 m×n，Q 是 n×n，R 是 m×m，K 是 n×m。每次写完代码，我都会把维度打印出来对一遍，这一步能省掉大量调试时间。
4. 从推导到代码的落地细节
4.1 一个完整的数值例子
光看公式容易飘，我们用一个简单的例子走一遍。假设一个一维匀速运动系统，状态是位置和速度，观测只有位置。状态转移矩阵：
[ A = \begin{bmatrix} 1 & \Delta t \ 0 & 1 \end{bmatrix} ]
观测矩阵 ( H = [1 \quad 0] )。取 ( \Delta t = 1 )，过程噪声 ( Q = \text{diag}(0.01, 0.01) )，观测噪声 ( R = 0.1 )。初始状态 ( \hat{x}{0|0} = [0, 0]^T )，初始协方差 ( P{0|0} = \text{diag}(1, 1) )。
第一时刻观测 ( z_1 = 1.2 )。预测步：
[ \hat{x}{1|0} = A \hat{x}{0|0} = [0, 0]^T ]
[ P_{1|0} = A P_{0|0} A^T + Q = \begin{bmatrix} 1 & 1 \ 1 & 2 \end{bmatrix} + \begin{bmatrix} 0.01 & 0 \ 0 & 0.01 \end{bmatrix} = \begin{bmatrix} 1.01 & 1 \ 1 & 2.01 \end{bmatrix} ]
更新步，先算增益：
[ S = H P_{1|0} H^T + R = 1.01 + 0.1 = 1.11 ]
[ K_1 = P_{1|0} H^T S^{-1} = \begin{bmatrix} 1.01 \ 1 \end{bmatrix} \times \frac{1}{1.11} = \begin{bmatrix} 0.910 \ 0.901 \end{bmatrix} ]
状态更新：
[ \hat{x}_{1|1} = [0, 0]^T + \begin{bmatrix} 0.910 \ 0.901 \end{bmatrix} \times (1.2 - 0) = [1.092, 1.081]^T ]
协方差更新：
[ P_{1|1} = (I - K_1 H) P_{1|0} = \begin{bmatrix} 0.090 & -0.910 \ 0 & 1 \end{bmatrix} \begin{bmatrix} 1.01 & 1 \ 1 & 2.01 \end{bmatrix} = \begin{bmatrix} 0.091 & -1.739 \ 1 & 2.01 \end{bmatrix} ]
这个结果看起来有点怪，因为 ( P_{1|1} ) 不对称。这就是前面提到的数值问题，实际代码里要做对称化。手工算的时候不用太纠结，重点是理解每一步在干什么。
4.2 Python 实现与逐行注释
下面是一个可以直接跑的 Python 实现，不依赖任何滤波库，纯 NumPy：
import numpy as np def kalman_filter(A, B, H, Q, R, x0, P0, zs, us=None): n = A.shape[0] m = H.shape[0] x = x0.copy() P = P0.copy() results = [] for k, z in enumerate(zs): # 预测步 u = us[k] if us is not None else np.zeros((B.shape[1], 1)) x_pred = A @ x + B @ u P_pred = A @ P @ A.T + Q # 更新步 S = H @ P_pred @ H.T + R K = P_pred @ H.T @ np.linalg.inv(S) innovation = z - H @ x_pred x = x_pred + K @ innovation P = (np.eye(n) - K @ H) @ P_pred P = (P + P.T) / 2 # 对称化 results.append((x.copy(), P.copy())) return results这段代码里有两个细节值得说。第一，np.linalg.inv(S)在 S 接近奇异时会出问题，工程上更推荐用np.linalg.solve(S.T, (P_pred @ H.T).T).T或者scipy.linalg.cho_solve，数值稳定性更好。第二，对称化那一步不能省，尤其是长时间运行后，浮点误差会让协方差慢慢失去对称性。
4.3 参数调优的实操经验
Q 和 R 的选取是卡尔曼滤波落地时最头疼的事。理论上它们分别是过程噪声和观测噪声的协方差，应该从系统辨识或者传感器手册里来。但实际项目中，往往拿不到准确的噪声统计，只能靠调。
我的经验是：先把 R 设成传感器标称精度的平方，然后调 Q。Q 从一个小值开始，比如 1e-4 乘以状态量纲的平方，然后看滤波效果。如果滤波结果滞后于真实值，说明 Q 太小，滤波器太信任模型，适当增大 Q。如果滤波结果抖动厉害，说明 Q 太大或者 R 太小，滤波器太信任观测，适当减小 Q 或增大 R。
还有一个技巧：如果状态量纲差异大，比如位置是米级、速度是米每秒，Q 的对角元素要分别按各自量纲的平方来设，不要统一用一个数。我见过有人把 Q 设成单位阵，结果位置通道和速度通道的噪声比例完全不对，滤波效果很差。
提示：可以用新息序列做一致性检验。如果滤波器调得好，新息 ( z_k - H \hat{x}{k|k-1} ) 应该是零均值白噪声，其协方差应该接近 ( H P{k|k-1} H^T + R )。如果新息有偏或者自相关，说明模型或噪声参数有问题。
5. 常见问题与排查技巧实录
5.1 滤波器发散的几种典型原因
卡尔曼滤波发散是实际项目里最常见的问题。发散的表现是估计误差越来越大，协方差矩阵失去正定性，最终输出完全不可用。根据我的经验，发散原因大概分几类。
第一类是模型和实际系统不匹配。比如系统明明是非线性的，你硬用线性模型去套，预测步的系统误差会不断累积。这种情况下，要么改模型，要么用扩展卡尔曼做局部线性化。
第二类是 Q 和 R 设置严重失衡。Q 设得极小，滤波器几乎不信任观测，预测误差无法被修正，慢慢就飘走了。反过来 R 设得极小，滤波器几乎完全信任观测，观测噪声被直接引入状态，也会导致估计抖动甚至发散。
第三类是数值问题。协方差矩阵在迭代中失去对称正定性，卡尔曼增益计算出现数值不稳定。解决办法是每次更新后做对称化，必要时用 Joseph 形式更新协方差。
第四类是观测异常值。某个时刻的观测严重偏离真实值，滤波器被带偏，之后需要很长时间才能恢复。工程上一般会加一个新息门限：如果新息超过某个阈值，就认为该观测是异常值，直接跳过更新步。
5.2 新息门限的具体实现
新息门限的逻辑很简单。正常新息应该服从 ( N(0, S) )，其中 ( S = H P_{k|k-1} H^T + R )。我们可以算新息的马氏距离：
[ d = \tilde{z}_k^T S^{-1} \tilde{z}_k ]
如果 ( d ) 超过某个卡方分布的分位数，就认为观测异常。对于 m 维观测，通常取 95% 或 99% 分位数。一维情况下，95% 对应约 3.84，99% 对应约 6.63。
代码里加一行判断就行：
d = innovation.T @ np.linalg.inv(S) @ innovation if d < threshold: x = x_pred + K @ innovation P = (np.eye(n) - K @ H) @ P_pred else: x = x_pred P = P_pred这个技巧在传感器偶尔跳变或者有遮挡的场景下特别有用。我用它处理过激光雷达的偶发野值，效果很稳。
5.3 常见问题速查表
| 问题现象 | 可能原因 | 排查方法 | 解决措施 |
|---|---|---|---|
| 估计滞后于真实值 | Q 太小或 R 太大 | 检查新息是否持续同号 | 增大 Q 或减小 R |
| 估计抖动厉害 | Q 太大或 R 太小 | 观察新息幅值是否过大 | 减小 Q 或增大 R |
| 协方差失去正定 | 数值误差累积 | 检查 P 的特征值 | 对称化，改用 Joseph 形式 |
| 滤波器突然发散 | 观测异常值 | 检查新息马氏距离 | 加新息门限，跳过异常观测 |
| 初始收敛慢 | P0 太小 | 观察前几步增益 | 增大 P0 |
| 稳态误差大 | 模型有偏 | 检查新息均值是否非零 | 修正模型或加偏差状态 |
这张表是我自己排查问题时用的，基本覆盖了八成以上的常见情况。遇到问题先对号入座，能省不少时间。
5.4 几个容易被忽略的实操细节
第一个细节是时间同步。预测步和更新步的时间戳必须对齐。如果观测有延迟，直接拿当前观测去更新当前状态，会引入系统性误差。解决办法是把状态预测到观测时刻再更新，或者把观测对齐到状态时刻。
第二个细节是矩阵求逆的替代。前面提过，inv(S)不如solve稳定。在嵌入式平台上，如果 S 维度小，可以直接手写解析逆；如果维度大，用 Cholesky 分解求解。
第三个细节是协方差的初始化。P0 不要设成零矩阵，否则滤波器会认为初始状态完全已知，增益恒为零，永远不更新。也不要设成单位阵了事，最好根据实际初始不确定性来设。
第四个细节是浮点精度。在单片机上跑卡尔曼滤波，如果状态维度高，单精度浮点可能不够，协方差会慢慢失去正定性。条件允许的话用双精度，或者定期做协方差重置。
6. 从卡尔曼到非线性滤波的过渡
标准卡尔曼滤波的线性高斯假设在实际项目里经常不成立。比如雷达测距测角，观测方程里有三角函数；比如机器人运动学，状态转移里有旋转矩阵。这时候就需要扩展卡尔曼滤波或者无迹卡尔曼滤波。
扩展卡尔曼的思路是在当前估计点对非线性函数做一阶泰勒展开，用雅可比矩阵代替原来的 A 和 H。推导框架和标准卡尔曼完全一样，只是 A 和 H 变成了时变的雅可比矩阵。它的优点是形式简单，计算量小；缺点是线性化误差大，强非线性下容易发散。
无迹卡尔曼走的是另一条路，用一组确定性采样点（Sigma 点）去传播均值和协方差，不做线性化。它对强非线性的适应性更好，但计算量比扩展卡尔曼大。选择哪种，取决于你的非线性程度和计算资源。
我个人的经验是：如果非线性温和，比如观测方程只有轻微弯曲，扩展卡尔曼够用；如果非线性强，比如角度观测跨越 ±π，或者状态转移有大角度旋转，无迹卡尔曼更稳。不管用哪种，标准卡尔曼滤波的推导逻辑都是基础，把这五个公式吃透，往上迁移会快很多。
最后分享一个我自己的习惯：每次实现一个新的滤波器，我都会先用仿真数据跑一遍，把真实状态、观测、估计值画在一张图上，再把新息序列和协方差迹画出来。图一看，滤波器工作是否正常一目了然。这个习惯帮我省掉了大量盲调参数的时间。
