卡尔曼滤波这个名字凡是做过状态估计、传感器融合、导航定位或者控制系统的朋友一定不陌生。但很多人第一次翻开推导时看到那一堆矩阵和概率密度函数脑子里冒出来的第一个念头往往是这玩意儿到底怎么从贝叶斯滤波里冒出来的我当初也是这么过来的。这篇内容就是想把这条推导链路彻底捋清楚——从贝叶斯滤波的递推框架出发一步步推到卡尔曼滤波的五大公式把每一步的来龙去脉、假设条件和矩阵维度的变化都摊开来讲。适合已经了解贝叶斯滤波基本思想、想真正搞懂卡尔曼滤波数学本质的读者也适合那些用惯了现成库函数、但心里总觉得不踏实、想回头补课的朋友。读完你至少能做到两件事第一自己动手把五大公式完整推一遍第二看到任意一个卡尔曼滤波的代码实现时能准确说出每一行对应的是哪个概率假设。1. 从贝叶斯滤波到卡尔曼滤波的推导主线1.1 贝叶斯滤波的递推框架回顾贝叶斯滤波的核心思想其实很朴素用概率分布来描述状态的不确定性然后不断地“预测—更新”循环。具体来说它维护一个后验概率密度 (p(x_k | z_{1:k}))其中 (x_k) 是 (k) 时刻的系统状态(z_{1:k}) 是到 (k) 时刻为止所有的观测值。整个递推分两步走。第一步是预测步也叫时间更新。利用系统的状态转移模型把上一时刻的后验分布传播到当前时刻得到先验分布[ p(x_k | z_{1:k-1}) \int p(x_k | x_{k-1}) , p(x_{k-1} | z_{1:k-1}) , dx_{k-1} ]这个积分就是著名的Chapman-Kolmogorov方程。它的物理含义很清晰当前状态的不确定性来自两个方面——上一时刻本身就不确定加上状态转移过程又引入了新的不确定性。两者卷积在一起得到先验。第二步是更新步也叫观测更新。拿到当前时刻的观测 (z_k) 后用贝叶斯公式把先验修正为后验[ p(x_k | z_{1:k}) \frac{p(z_k | x_k) , p(x_k | z_{1:k-1})}{p(z_k | z_{1:k-1})} ]其中分母是归一化常数[ p(z_k | z_{1:k-1}) \int p(z_k | x_k) , p(x_k | z_{1:k-1}) , dx_k ]这套框架在理论上完美无缺但问题在于那两个积分。对于一般的非线性、非高斯系统这两个积分没有解析解只能靠数值方法近似比如粒子滤波就是用蒙特卡洛采样来逼近这些积分。而卡尔曼滤波之所以能给出解析解恰恰是因为它引入了一组非常强的假设使得这些积分可以精确计算。1.2 卡尔曼滤波的三条核心假设卡尔曼滤波能成立靠的是三条假设缺一不可。第一条状态转移是线性的。系统状态从 (k-1) 到 (k) 的演化可以用一个线性方程描述[ x_k F_k x_{k-1} B_k u_k w_{k-1} ]这里 (F_k) 是状态转移矩阵(B_k) 是控制输入矩阵(u_k) 是已知的控制量(w_{k-1}) 是过程噪声。线性假设的意义在于线性变换作用于高斯分布结果仍然是高斯分布。这就保证了如果上一时刻的后验是高斯的那么预测出来的先验也是高斯的。第二条观测是线性的。观测值与状态之间的关系也是线性的[ z_k H_k x_k v_k ](H_k) 是观测矩阵(v_k) 是观测噪声。同样地线性观测模型保证了似然函数 (p(z_k | x_k)) 在高斯先验下仍然是高斯的。第三条噪声是高斯的。过程噪声和观测噪声都服从零均值高斯分布[ w_{k-1} \sim \mathcal{N}(0, Q_{k-1}), \quad v_k \sim \mathcal{N}(0, R_k) ]而且通常还假设它们之间互相独立也与初始状态独立。高斯假设的意义在于高斯分布经过线性变换和贝叶斯更新后仍然保持高斯形式。这就意味着我们不需要维护整个概率密度函数只需要跟踪均值和协方差两个参数就够了。这三条假设合在一起产生了一个极其漂亮的结论如果初始状态是高斯的那么在整个递推过程中所有的先验和后验分布都保持高斯形式。这就是卡尔曼滤波能够用有限个参数均值和协方差精确表示概率分布的根本原因。注意很多人误以为卡尔曼滤波要求噪声必须是高斯的才能用。实际上即使噪声不是高斯的卡尔曼滤波仍然是最优线性无偏估计BLUE只是它不再等价于贝叶斯最优估计。这个区别在实际工程中很重要后面会展开讲。1.3 高斯分布在线性变换下的封闭性要理解推导必须先搞清楚高斯分布的两个关键性质。性质一线性变换后的高斯分布仍是高斯分布。如果 (x \sim \mathcal{N}(\mu, \Sigma))且 (y Ax b)那么[ y \sim \mathcal{N}(A\mu b, A\Sigma A^T) ]这个性质的证明可以用特征函数或者直接做变量替换。直观理解是高斯分布的等概率面是椭球线性变换把椭球映射成另一个椭球而椭球对应的分布仍然是高斯分布。性质二高斯条件分布的解析形式。如果联合分布是高斯的[ \begin{bmatrix} x \ z \end{bmatrix} \sim \mathcal{N}\left(\begin{bmatrix} \mu_x \ \mu_z \end{bmatrix}, \begin{bmatrix} \Sigma_{xx} \Sigma_{xz} \ \Sigma_{zx} \Sigma_{zz} \end{bmatrix}\right) ]那么条件分布 (p(x|z)) 也是高斯的且[ p(x|z) \mathcal{N}(x | \mu_{x|z}, \Sigma_{x|z}) ]其中[ \mu_{x|z} \mu_x \Sigma_{xz} \Sigma_{zz}^{-1} (z - \mu_z) ][ \Sigma_{x|z} \Sigma_{xx} - \Sigma_{xz} \Sigma_{zz}^{-1} \Sigma_{zx} ]这两个公式是卡尔曼滤波更新步的直接来源。卡尔曼增益 (K \Sigma_{xz} \Sigma_{zz}^{-1}) 本质上就是条件高斯分布中的回归系数。理解了这一点后面推导更新步时就不会觉得那些矩阵公式是凭空冒出来的。2. 预测步的完整推导与矩阵维度解析2.1 状态预测从后验均值到先验均值假设 (k-1) 时刻的后验分布已知[ p(x_{k-1} | z_{1:k-1}) \mathcal{N}(x_{k-1} | \hat{x}{k-1|k-1}, P{k-1|k-1}) ]其中 (\hat{x}{k-1|k-1}) 是后验均值(P{k-1|k-1}) 是后验协方差。现在要预测 (k) 时刻的状态。状态转移方程是[ x_k F_k x_{k-1} B_k u_k w_{k-1} ]对两边取期望。由于 (w_{k-1}) 是零均值噪声且与 (x_{k-1}) 独立[ E[x_k] F_k E[x_{k-1}] B_k u_k E[w_{k-1}] ][ \hat{x}{k|k-1} F_k \hat{x}{k-1|k-1} B_k u_k ]这就是预测均值公式。它的含义很直白把上一时刻的估计值通过状态转移矩阵传播到当前时刻再加上已知的控制输入。这里有一个容易忽略的细节(B_k u_k) 这一项是确定性的它不引入任何额外的不确定性。所以在协方差传播中这一项不会出现。很多初学者在推导协方差时会把 (B_k u_k) 也带进去算结果发现多了一堆零白白浪费时间。2.2 协方差预测不确定性的传播协方差的推导稍微复杂一点。根据定义[ P_{k|k-1} E\left[(x_k - \hat{x}{k|k-1})(x_k - \hat{x}{k|k-1})^T\right] ]把 (x_k) 和 (\hat{x}_{k|k-1}) 的表达式代入[ x_k - \hat{x}{k|k-1} F_k x{k-1} B_k u_k w_{k-1} - F_k \hat{x}_{k-1|k-1} - B_k u_k ][ F_k (x_{k-1} - \hat{x}{k-1|k-1}) w{k-1} ]令 (\tilde{x}{k-1} x{k-1} - \hat{x}_{k-1|k-1})则[ P_{k|k-1} E\left[(F_k \tilde{x}{k-1} w{k-1})(F_k \tilde{x}{k-1} w{k-1})^T\right] ]展开[ F_k E[\tilde{x}{k-1} \tilde{x}{k-1}^T] F_k^T F_k E[\tilde{x}{k-1} w{k-1}^T] E[w_{k-1} \tilde{x}{k-1}^T] F_k^T E[w{k-1} w_{k-1}^T] ]由于 (\tilde{x}{k-1}) 和 (w{k-1}) 独立且 (w_{k-1}) 零均值中间两个交叉项为零。于是[ P_{k|k-1} F_k P_{k-1|k-1} F_k^T Q_{k-1} ]这就是协方差预测公式。它的物理意义非常清晰先验协方差等于后验协方差经过状态转移矩阵的线性传播再加上过程噪声协方差。第一项表示“旧的不确定性被放大或缩小”第二项表示“新的不确定性被引入”。实操心得(Q) 矩阵的设定是卡尔曼滤波调参中最关键也最玄学的部分。设得太小滤波器会过度信任模型对突变响应迟钝设得太大滤波器会过度信任观测输出抖动严重。我的经验是先把 (Q) 设为一个对角矩阵对角线元素对应每个状态量的“过程噪声强度”然后根据实际系统的动态特性逐步调整。如果某个状态量变化很快对应的 (Q) 元素就应该大一些。2.3 矩阵维度检查避免推导中的常见错误推导过程中最容易出错的地方就是矩阵维度不匹配。我们把维度明确标出来。假设状态维度为 (n)观测维度为 (m)控制输入维度为 (l)。符号含义维度(x_k)状态向量(n \times 1)(z_k)观测向量(m \times 1)(u_k)控制输入(l \times 1)(F_k)状态转移矩阵(n \times n)(B_k)控制输入矩阵(n \times l)(H_k)观测矩阵(m \times n)(P_{kk-1})先验协方差(Q_{k-1})过程噪声协方差(n \times n)(R_k)观测噪声协方差(m \times m)(K_k)卡尔曼增益(n \times m)检查预测步的两个公式(\hat{x}{k|k-1} F_k \hat{x}{k-1|k-1} B_k u_k)((n \times n)(n \times 1) (n \times l)(l \times 1) n \times 1)正确。(P_{k|k-1} F_k P_{k-1|k-1} F_k^T Q_{k-1})((n \times n)(n \times n)(n \times n) (n \times n) n \times n)正确。养成检查维度的习惯能在推导阶段就发现大部分错误。我见过太多人因为矩阵转置搞错方向调了一整天代码才发现问题出在推导上。3. 更新步的推导与卡尔曼增益的本质3.1 联合高斯分布的构造更新步的目标是计算后验 (p(x_k | z_{1:k}))。根据贝叶斯公式它正比于似然乘以先验[ p(x_k | z_{1:k}) \propto p(z_k | x_k) , p(x_k | z_{1:k-1}) ]我们已经知道先验是高斯的[ p(x_k | z_{1:k-1}) \mathcal{N}(x_k | \hat{x}{k|k-1}, P{k|k-1}) ]观测模型是线性的观测噪声是高斯的所以似然也是高斯的[ p(z_k | x_k) \mathcal{N}(z_k | H_k x_k, R_k) ]两个高斯分布相乘结果仍然是高斯分布。这是高斯分布的又一个重要性质——共轭性。我们可以直接写出后验的均值和协方差但更优雅的做法是构造联合高斯分布然后利用条件高斯公式。把状态和观测拼成一个向量[ \begin{bmatrix} x_k \ z_k \end{bmatrix} ]先计算这个联合分布的均值和协方差。均值[ E[x_k] \hat{x}_{k|k-1} ][ E[z_k] E[H_k x_k v_k] H_k \hat{x}_{k|k-1} ]协方差矩阵分块计算[ \text{Cov}(x_k, x_k) P_{k|k-1} ][ \text{Cov}(x_k, z_k) E\left[(x_k - \hat{x}{k|k-1})(H_k x_k v_k - H_k \hat{x}{k|k-1})^T\right] ][ E\left[(x_k - \hat{x}{k|k-1})(H_k (x_k - \hat{x}{k|k-1}) v_k)^T\right] ][ P_{k|k-1} H_k^T ]因为 (x_k - \hat{x}_{k|k-1}) 与 (v_k) 独立且 (v_k) 零均值。[ \text{Cov}(z_k, z_k) E\left[(H_k (x_k - \hat{x}{k|k-1}) v_k)(H_k (x_k - \hat{x}{k|k-1}) v_k)^T\right] ][ H_k P_{k|k-1} H_k^T R_k ]这个量通常记为 (S_k)叫做新息协方差。[ \text{Cov}(z_k, x_k) \text{Cov}(x_k, z_k)^T H_k P_{k|k-1} ]所以联合分布是[ \begin{bmatrix} x_k \ z_k \end{bmatrix} \sim \mathcal{N}\left( \begin{bmatrix} \hat{x}{k|k-1} \ H_k \hat{x}{k|k-1} \end{bmatrix}, \begin{bmatrix} P_{k|k-1} P_{k|k-1} H_k^T \ H_k P_{k|k-1} S_k \end{bmatrix} \right) ]3.2 条件高斯公式的直接应用有了联合高斯分布后验 (p(x_k | z_k)) 就是条件分布。直接套用前面提到的条件高斯公式[ \hat{x}{k|k} \hat{x}{k|k-1} P_{k|k-1} H_k^T S_k^{-1} (z_k - H_k \hat{x}_{k|k-1}) ][ P_{k|k} P_{k|k-1} - P_{k|k-1} H_k^T S_k^{-1} H_k P_{k|k-1} ]定义卡尔曼增益[ K_k P_{k|k-1} H_k^T S_k^{-1} ]则后验均值和协方差可以写成更紧凑的形式[ \hat{x}{k|k} \hat{x}{k|k-1} K_k (z_k - H_k \hat{x}_{k|k-1}) ][ P_{k|k} (I - K_k H_k) P_{k|k-1} ]这就是卡尔曼滤波更新步的两个核心公式。3.3 卡尔曼增益的物理意义解读卡尔曼增益 (K_k) 是整个算法的灵魂。它决定了我们应该在多大程度上信任观测、多大程度上信任预测。从公式 (K_k P_{k|k-1} H_k^T S_k^{-1}) 可以看出当先验协方差 (P_{k|k-1}) 很大时预测不确定性大(K_k) 变大滤波器更倾向于信任观测。当观测噪声协方差 (R_k) 很大时(S_k H_k P_{k|k-1} H_k^T R_k) 变大(K_k) 变小滤波器更倾向于信任预测。这完全符合直觉预测越不准就越应该听观测的观测越不准就越应该听预测的。卡尔曼增益本质上是在做一种“加权平均”权重由两者的不确定性决定。用一个标量例子来直观理解。假设状态是一维的(H_k 1)则[ K_k \frac{P_{k|k-1}}{P_{k|k-1} R_k} ]如果 (P_{k|k-1} \gg R_k)则 (K_k \approx 1)后验均值约等于观测值。如果 (P_{k|k-1} \ll R_k)则 (K_k \approx 0)后验均值约等于预测值。如果两者相等则 (K_k 0.5)后验均值是预测和观测的算术平均。注意新息 (z_k - H_k \hat{x}_{k|k-1}) 是实际观测与预测观测之间的差它携带了观测中“新”的信息。如果系统模型完全准确且没有噪声新息应该为零。实际中新息不为零正是因为它包含了模型误差和噪声的信息。卡尔曼滤波利用新息来修正状态估计修正的幅度由卡尔曼增益控制。3.4 协方差更新公式的两种等价形式协方差更新有两种常见的写法形式一标准形式[ P_{k|k} P_{k|k-1} - K_k H_k P_{k|k-1} ]形式二Joseph形式[ P_{k|k} (I - K_k H_k) P_{k|k-1} (I - K_k H_k)^T K_k R_k K_k^T ]这两种形式在数学上完全等价但在数值计算中表现不同。标准形式计算量小但在 (K_k) 接近最优值时由于浮点舍入误差可能导致 (P_{k|k}) 失去正定性。Joseph形式计算量稍大但数值稳定性更好因为它保证了 (P_{k|k}) 始终是正定的两个正定矩阵之和。在实际工程中如果状态维度不高我通常推荐用Joseph形式尤其是在嵌入式系统或者长时间运行的滤波器中。数值稳定性带来的收益远大于那一点额外的计算量。4. 完整算法流程与实操中的关键细节4.1 五大公式的完整梳理把预测步和更新步合在一起卡尔曼滤波的完整算法如下。预测步[ \hat{x}{k|k-1} F_k \hat{x}{k-1|k-1} B_k u_k ][ P_{k|k-1} F_k P_{k-1|k-1} F_k^T Q_{k-1} ]更新步[ S_k H_k P_{k|k-1} H_k^T R_k ][ K_k P_{k|k-1} H_k^T S_k^{-1} ][ \hat{x}{k|k} \hat{x}{k|k-1} K_k (z_k - H_k \hat{x}_{k|k-1}) ][ P_{k|k} (I - K_k H_k) P_{k|k-1} ]初始条件(\hat{x}{0|0}) 和 (P{0|0}) 需要人为设定。通常 (\hat{x}{0|0}) 设为第一次观测值或者根据先验知识给出(P{0|0}) 设为一个较大的对角矩阵表示初始时刻对状态估计不太确定。4.2 一个完整的数值算例用一个最简单的例子来走一遍流程。假设一个一维匀速运动系统状态是位置和速度[ x_k \begin{bmatrix} p_k \ v_k \end{bmatrix} ]状态转移矩阵采样时间 (T 1)[ F \begin{bmatrix} 1 1 \ 0 1 \end{bmatrix} ]没有控制输入(B 0)。观测只测位置[ H \begin{bmatrix} 1 0 \end{bmatrix} ]过程噪声协方差[ Q \begin{bmatrix} 0.01 0 \ 0 0.01 \end{bmatrix} ]观测噪声协方差[ R \begin{bmatrix} 0.1 \end{bmatrix} ]初始状态[ \hat{x}{0|0} \begin{bmatrix} 0 \ 0 \end{bmatrix}, \quad P{0|0} \begin{bmatrix} 10 0 \ 0 10 \end{bmatrix} ]假设第一个观测值 (z_1 1.2)。预测步[ \hat{x}{1|0} F \hat{x}{0|0} \begin{bmatrix} 0 \ 0 \end{bmatrix} ][ P_{1|0} F P_{0|0} F^T Q \begin{bmatrix} 1 1 \ 0 1 \end{bmatrix} \begin{bmatrix} 10 0 \ 0 10 \end{bmatrix} \begin{bmatrix} 1 0 \ 1 1 \end{bmatrix} \begin{bmatrix} 0.01 0 \ 0 0.01 \end{bmatrix} ]计算[ F P_{0|0} \begin{bmatrix} 10 10 \ 0 10 \end{bmatrix} ][ F P_{0|0} F^T \begin{bmatrix} 10 10 \ 0 10 \end{bmatrix} \begin{bmatrix} 1 0 \ 1 1 \end{bmatrix} \begin{bmatrix} 20 10 \ 10 10 \end{bmatrix} ][ P_{1|0} \begin{bmatrix} 20.01 10 \ 10 10.01 \end{bmatrix} ]更新步[ S_1 H P_{1|0} H^T R 20.01 0.1 20.11 ][ K_1 P_{1|0} H^T S_1^{-1} \begin{bmatrix} 20.01 \ 10 \end{bmatrix} \frac{1}{20.11} \begin{bmatrix} 0.995 \ 0.497 \end{bmatrix} ][ \hat{x}_{1|1} \begin{bmatrix} 0 \ 0 \end{bmatrix} \begin{bmatrix} 0.995 \ 0.497 \end{bmatrix} (1.2 - 0) \begin{bmatrix} 1.194 \ 0.597 \end{bmatrix} ][ P_{1|1} (I - K_1 H) P_{1|0} ][ I - K_1 H \begin{bmatrix} 1 - 0.995 0 \ -0.497 1 \end{bmatrix} \begin{bmatrix} 0.005 0 \ -0.497 1 \end{bmatrix} ][ P_{1|1} \begin{bmatrix} 0.005 0 \ -0.497 1 \end{bmatrix} \begin{bmatrix} 20.01 10 \ 10 10.01 \end{bmatrix} \begin{bmatrix} 0.100 0.050 \ -0.005 5.040 \end{bmatrix} ]可以看到经过一次更新位置的方差从 20.01 降到了 0.100说明观测提供了大量信息。速度的方差从 10.01 降到了 5.040也有所下降但不如位置明显因为观测不直接测速度。这个算例虽然简单但完整展示了卡尔曼滤波的每一步计算。建议自己动手算一遍比看十遍公式都管用。4.3 参数调优的实操经验卡尔曼滤波的调参主要围绕 (Q) 和 (R) 两个矩阵。这两个矩阵的比值决定了滤波器的“性格”。参数设置滤波器行为适用场景(Q) 大(R) 小信任观测响应快但输出抖动大观测质量高系统动态变化快(Q) 小(R) 大信任预测输出平滑但响应慢观测噪声大系统动态变化慢(Q) 和 (R) 都大增益适中但协方差收敛慢初始不确定性大(Q) 和 (R) 都小增益小滤波器“固执”模型和观测都很准确我的调参流程通常是这样的先把 (Q) 设为一个很小的值比如 (10^{-6}) 倍的单位矩阵(R) 设为实际观测噪声的方差估计。然后跑一段数据观察新息序列。如果新息均值不为零说明模型有偏差如果新息方差远大于 (S_k)说明 (Q) 设小了如果新息序列出现明显的自相关说明模型阶数不够或者 (Q) 的结构不合理。实操心得新息序列是调试卡尔曼滤波最好的工具。理论上最优滤波器的新息应该是零均值白噪声。如果新息有颜色自相关不为零说明滤波器没有提取完观测中的所有信息还有改进空间。我习惯把新息和新息协方差画出来一眼就能看出滤波器是否工作正常。5. 常见问题与排查技巧实录5.1 滤波器发散的原因与对策滤波器发散是实际中最常见的问题表现为估计误差越来越大甚至完全偏离真实值。原因通常有以下几种。原因一模型不准确。状态转移矩阵 (F) 或者观测矩阵 (H) 与实际系统不匹配。比如系统实际上有加速度但模型只建了匀速运动。对策是增加状态维度或者调整 (Q) 来补偿模型误差。原因二(Q) 设得太小。滤波器过度信任模型当模型有误差时协方差 (P) 会越来越小卡尔曼增益也越來越小最终滤波器“闭眼不看”观测完全靠模型外推。对策是适当增大 (Q)给模型误差留出余量。原因三数值误差累积。长时间运行后由于浮点舍入误差(P) 矩阵可能失去正定性或对称性。对策是使用Joseph形式更新协方差或者定期对 (P) 做对称化处理(P \leftarrow (P P^T)/2)。原因四观测异常值。如果观测中出现离群值滤波器会被“带偏”。对策是加入新息卡方检验当新息超过一定阈值时认为观测异常跳过更新步或者降低该观测的权重。5.2 新息卡方检验的实现新息卡方检验是判断观测是否异常的标准方法。新息 (r_k z_k - H_k \hat{x}_{k|k-1}) 在正常情况下服从 (\mathcal{N}(0, S_k))。构造统计量[ d_k r_k^T S_k^{-1} r_k ](d_k) 服从自由度为 (m) 的卡方分布。给定显著性水平 (\alpha)通常取 0.05 或 0.01查卡方分布表得到阈值 (\chi^2_{m, \alpha})。如果 (d_k \chi^2_{m, \alpha})则认为观测异常。import numpy as np from scipy.stats import chi2 def kalman_update(x_pred, P_pred, z, H, R, alpha0.05): m z.shape[0] S H P_pred H.T R r z - H x_pred d r.T np.linalg.inv(S) r threshold chi2.ppf(1 - alpha, m) if d threshold: # 观测异常跳过更新 return x_pred, P_pred, True K P_pred H.T np.linalg.inv(S) x_upd x_pred K r P_upd (np.eye(len(x_pred)) - K H) P_pred return x_upd, P_upd, False这个检验在实际系统中非常有用尤其是传感器偶尔会给出错误读数的情况。加了这层保护之后滤波器的鲁棒性会明显提升。5.3 常见问题速查表问题现象可能原因排查方法解决方案估计值滞后于真实值(Q) 太小或 (R) 太大观察新息是否持续同号增大 (Q) 或减小 (R)估计值抖动严重(Q) 太大或 (R) 太小观察后验协方差是否过大减小 (Q) 或增大 (R)滤波器发散模型不准或数值问题检查 (P) 是否正定增大 (Q)使用Joseph形式新息有自相关模型阶数不够画新息自相关图增加状态维度或调整 (Q) 结构初始收敛慢(P_0) 太小观察初始阶段增益增大 (P_0)观测异常导致跳变无异常检测计算新息卡方统计量加入卡方检验5.4 扩展卡尔曼滤波的过渡实际系统往往是非线性的这时候标准卡尔曼滤波就不够用了。扩展卡尔曼滤波EKF的思路是在工作点附近对非线性函数做一阶泰勒展开用雅可比矩阵代替线性矩阵。状态转移和观测模型变为[ x_k f(x_{k-1}, u_k) w_{k-1} ][ z_k h(x_k) v_k ]预测步用 (f) 传播均值用雅可比矩阵 (F_k \frac{\partial f}{\partial x}\big|{\hat{x}{k-1|k-1}}) 传播协方差。更新步用 (h) 计算预测观测用雅可比矩阵 (H_k \frac{\partial h}{\partial x}\big|{\hat{x}{k|k-1}}) 计算增益。EKF的推导本质上和标准卡尔曼滤波一样只是把线性矩阵换成了雅可比矩阵。但要注意EKF只在一阶意义上最优如果非线性很强雅可比矩阵的近似误差会导致滤波器性能下降甚至发散。这时候可以考虑无迹卡尔曼滤波UKF它用sigma点来传播分布不需要计算雅可比矩阵精度也更高。我在实际项目中的选择逻辑是如果非线性程度不高EKF足够用且计算量小如果非线性强或者雅可比矩阵难以计算直接上UKF。粒子滤波虽然理论上能处理任意非线性非高斯但计算量大通常只在EKF和UKF都搞不定的时候才考虑。5.5 从推导到代码的映射检查清单最后分享一个我常用的检查清单帮你确认代码实现和推导是否一致。[ ] 状态转移矩阵 (F) 的维度是 (n \times n)与状态维度一致[ ] 观测矩阵 (H) 的维度是 (m \times n)与观测维度一致[ ] (Q) 矩阵是对称正定的 (n \times n) 矩阵[ ] (R) 矩阵是对称正定的 (m \times m) 矩阵[ ] 预测步中 (B_k u_k) 只加到均值上不加到协方差上[ ] 更新步中新息是 (z_k - H_k \hat{x}{k|k-1})不是 (\hat{x}{k|k-1} - z_k)[ ] 卡尔曼增益的维度是 (n \times m)[ ] 协方差更新后检查对称性(P (P P^T)/2)[ ] 初始协方差 (P_0) 不要设为零矩阵这个清单看起来简单但每一条我都见过有人踩坑。尤其是新息的方向和协方差对称性出问题的时候现象很隐蔽不容易发现。卡尔曼滤波的推导到这里就完整了。从贝叶斯滤波的递推框架出发加上线性高斯假设一步步推出预测和更新的五大公式每一步都有明确的概率论依据。理解了这个推导过程再看任何卡尔曼滤波的变体——EKF、UKF、信息滤波、平方根滤波——都会觉得有迹可循因为它们本质上都是在处理同一个贝叶斯递推问题只是用了不同的近似方法。 
长河君 · 2026/10/13 13:35:32 · 阅读 1.2k 从贝叶斯滤波到卡尔曼滤波：五大公式推导与矩阵维度解析
卡尔曼滤波这个名字凡是做过状态估计、传感器融合、导航定位或者控制系统的朋友一定不陌生。但很多人第一次翻开推导时看到那一堆矩阵和概率密度函数脑子里冒出来的第一个念头往往是这玩意儿到底怎么从贝叶斯滤波里冒出来的我当初也是这么过来的。这篇内容就是想把这条推导链路彻底捋清楚——从贝叶斯滤波的递推框架出发一步步推到卡尔曼滤波的五大公式把每一步的来龙去脉、假设条件和矩阵维度的变化都摊开来讲。适合已经了解贝叶斯滤波基本思想、想真正搞懂卡尔曼滤波数学本质的读者也适合那些用惯了现成库函数、但心里总觉得不踏实、想回头补课的朋友。读完你至少能做到两件事第一自己动手把五大公式完整推一遍第二看到任意一个卡尔曼滤波的代码实现时能准确说出每一行对应的是哪个概率假设。1. 从贝叶斯滤波到卡尔曼滤波的推导主线1.1 贝叶斯滤波的递推框架回顾贝叶斯滤波的核心思想其实很朴素用概率分布来描述状态的不确定性然后不断地“预测—更新”循环。具体来说它维护一个后验概率密度 (p(x_k | z_{1:k}))其中 (x_k) 是 (k) 时刻的系统状态(z_{1:k}) 是到 (k) 时刻为止所有的观测值。整个递推分两步走。第一步是预测步也叫时间更新。利用系统的状态转移模型把上一时刻的后验分布传播到当前时刻得到先验分布[ p(x_k | z_{1:k-1}) \int p(x_k | x_{k-1}) , p(x_{k-1} | z_{1:k-1}) , dx_{k-1} ]这个积分就是著名的Chapman-Kolmogorov方程。它的物理含义很清晰当前状态的不确定性来自两个方面——上一时刻本身就不确定加上状态转移过程又引入了新的不确定性。两者卷积在一起得到先验。第二步是更新步也叫观测更新。拿到当前时刻的观测 (z_k) 后用贝叶斯公式把先验修正为后验[ p(x_k | z_{1:k}) \frac{p(z_k | x_k) , p(x_k | z_{1:k-1})}{p(z_k | z_{1:k-1})} ]其中分母是归一化常数[ p(z_k | z_{1:k-1}) \int p(z_k | x_k) , p(x_k | z_{1:k-1}) , dx_k ]这套框架在理论上完美无缺但问题在于那两个积分。对于一般的非线性、非高斯系统这两个积分没有解析解只能靠数值方法近似比如粒子滤波就是用蒙特卡洛采样来逼近这些积分。而卡尔曼滤波之所以能给出解析解恰恰是因为它引入了一组非常强的假设使得这些积分可以精确计算。1.2 卡尔曼滤波的三条核心假设卡尔曼滤波能成立靠的是三条假设缺一不可。第一条状态转移是线性的。系统状态从 (k-1) 到 (k) 的演化可以用一个线性方程描述[ x_k F_k x_{k-1} B_k u_k w_{k-1} ]这里 (F_k) 是状态转移矩阵(B_k) 是控制输入矩阵(u_k) 是已知的控制量(w_{k-1}) 是过程噪声。线性假设的意义在于线性变换作用于高斯分布结果仍然是高斯分布。这就保证了如果上一时刻的后验是高斯的那么预测出来的先验也是高斯的。第二条观测是线性的。观测值与状态之间的关系也是线性的[ z_k H_k x_k v_k ](H_k) 是观测矩阵(v_k) 是观测噪声。同样地线性观测模型保证了似然函数 (p(z_k | x_k)) 在高斯先验下仍然是高斯的。第三条噪声是高斯的。过程噪声和观测噪声都服从零均值高斯分布[ w_{k-1} \sim \mathcal{N}(0, Q_{k-1}), \quad v_k \sim \mathcal{N}(0, R_k) ]而且通常还假设它们之间互相独立也与初始状态独立。高斯假设的意义在于高斯分布经过线性变换和贝叶斯更新后仍然保持高斯形式。这就意味着我们不需要维护整个概率密度函数只需要跟踪均值和协方差两个参数就够了。这三条假设合在一起产生了一个极其漂亮的结论如果初始状态是高斯的那么在整个递推过程中所有的先验和后验分布都保持高斯形式。这就是卡尔曼滤波能够用有限个参数均值和协方差精确表示概率分布的根本原因。注意很多人误以为卡尔曼滤波要求噪声必须是高斯的才能用。实际上即使噪声不是高斯的卡尔曼滤波仍然是最优线性无偏估计BLUE只是它不再等价于贝叶斯最优估计。这个区别在实际工程中很重要后面会展开讲。1.3 高斯分布在线性变换下的封闭性要理解推导必须先搞清楚高斯分布的两个关键性质。性质一线性变换后的高斯分布仍是高斯分布。如果 (x \sim \mathcal{N}(\mu, \Sigma))且 (y Ax b)那么[ y \sim \mathcal{N}(A\mu b, A\Sigma A^T) ]这个性质的证明可以用特征函数或者直接做变量替换。直观理解是高斯分布的等概率面是椭球线性变换把椭球映射成另一个椭球而椭球对应的分布仍然是高斯分布。性质二高斯条件分布的解析形式。如果联合分布是高斯的[ \begin{bmatrix} x \ z \end{bmatrix} \sim \mathcal{N}\left(\begin{bmatrix} \mu_x \ \mu_z \end{bmatrix}, \begin{bmatrix} \Sigma_{xx} \Sigma_{xz} \ \Sigma_{zx} \Sigma_{zz} \end{bmatrix}\right) ]那么条件分布 (p(x|z)) 也是高斯的且[ p(x|z) \mathcal{N}(x | \mu_{x|z}, \Sigma_{x|z}) ]其中[ \mu_{x|z} \mu_x \Sigma_{xz} \Sigma_{zz}^{-1} (z - \mu_z) ][ \Sigma_{x|z} \Sigma_{xx} - \Sigma_{xz} \Sigma_{zz}^{-1} \Sigma_{zx} ]这两个公式是卡尔曼滤波更新步的直接来源。卡尔曼增益 (K \Sigma_{xz} \Sigma_{zz}^{-1}) 本质上就是条件高斯分布中的回归系数。理解了这一点后面推导更新步时就不会觉得那些矩阵公式是凭空冒出来的。2. 预测步的完整推导与矩阵维度解析2.1 状态预测从后验均值到先验均值假设 (k-1) 时刻的后验分布已知[ p(x_{k-1} | z_{1:k-1}) \mathcal{N}(x_{k-1} | \hat{x}{k-1|k-1}, P{k-1|k-1}) ]其中 (\hat{x}{k-1|k-1}) 是后验均值(P{k-1|k-1}) 是后验协方差。现在要预测 (k) 时刻的状态。状态转移方程是[ x_k F_k x_{k-1} B_k u_k w_{k-1} ]对两边取期望。由于 (w_{k-1}) 是零均值噪声且与 (x_{k-1}) 独立[ E[x_k] F_k E[x_{k-1}] B_k u_k E[w_{k-1}] ][ \hat{x}{k|k-1} F_k \hat{x}{k-1|k-1} B_k u_k ]这就是预测均值公式。它的含义很直白把上一时刻的估计值通过状态转移矩阵传播到当前时刻再加上已知的控制输入。这里有一个容易忽略的细节(B_k u_k) 这一项是确定性的它不引入任何额外的不确定性。所以在协方差传播中这一项不会出现。很多初学者在推导协方差时会把 (B_k u_k) 也带进去算结果发现多了一堆零白白浪费时间。2.2 协方差预测不确定性的传播协方差的推导稍微复杂一点。根据定义[ P_{k|k-1} E\left[(x_k - \hat{x}{k|k-1})(x_k - \hat{x}{k|k-1})^T\right] ]把 (x_k) 和 (\hat{x}_{k|k-1}) 的表达式代入[ x_k - \hat{x}{k|k-1} F_k x{k-1} B_k u_k w_{k-1} - F_k \hat{x}_{k-1|k-1} - B_k u_k ][ F_k (x_{k-1} - \hat{x}{k-1|k-1}) w{k-1} ]令 (\tilde{x}{k-1} x{k-1} - \hat{x}_{k-1|k-1})则[ P_{k|k-1} E\left[(F_k \tilde{x}{k-1} w{k-1})(F_k \tilde{x}{k-1} w{k-1})^T\right] ]展开[ F_k E[\tilde{x}{k-1} \tilde{x}{k-1}^T] F_k^T F_k E[\tilde{x}{k-1} w{k-1}^T] E[w_{k-1} \tilde{x}{k-1}^T] F_k^T E[w{k-1} w_{k-1}^T] ]由于 (\tilde{x}{k-1}) 和 (w{k-1}) 独立且 (w_{k-1}) 零均值中间两个交叉项为零。于是[ P_{k|k-1} F_k P_{k-1|k-1} F_k^T Q_{k-1} ]这就是协方差预测公式。它的物理意义非常清晰先验协方差等于后验协方差经过状态转移矩阵的线性传播再加上过程噪声协方差。第一项表示“旧的不确定性被放大或缩小”第二项表示“新的不确定性被引入”。实操心得(Q) 矩阵的设定是卡尔曼滤波调参中最关键也最玄学的部分。设得太小滤波器会过度信任模型对突变响应迟钝设得太大滤波器会过度信任观测输出抖动严重。我的经验是先把 (Q) 设为一个对角矩阵对角线元素对应每个状态量的“过程噪声强度”然后根据实际系统的动态特性逐步调整。如果某个状态量变化很快对应的 (Q) 元素就应该大一些。2.3 矩阵维度检查避免推导中的常见错误推导过程中最容易出错的地方就是矩阵维度不匹配。我们把维度明确标出来。假设状态维度为 (n)观测维度为 (m)控制输入维度为 (l)。符号含义维度(x_k)状态向量(n \times 1)(z_k)观测向量(m \times 1)(u_k)控制输入(l \times 1)(F_k)状态转移矩阵(n \times n)(B_k)控制输入矩阵(n \times l)(H_k)观测矩阵(m \times n)(P_{kk-1})先验协方差(Q_{k-1})过程噪声协方差(n \times n)(R_k)观测噪声协方差(m \times m)(K_k)卡尔曼增益(n \times m)检查预测步的两个公式(\hat{x}{k|k-1} F_k \hat{x}{k-1|k-1} B_k u_k)((n \times n)(n \times 1) (n \times l)(l \times 1) n \times 1)正确。(P_{k|k-1} F_k P_{k-1|k-1} F_k^T Q_{k-1})((n \times n)(n \times n)(n \times n) (n \times n) n \times n)正确。养成检查维度的习惯能在推导阶段就发现大部分错误。我见过太多人因为矩阵转置搞错方向调了一整天代码才发现问题出在推导上。3. 更新步的推导与卡尔曼增益的本质3.1 联合高斯分布的构造更新步的目标是计算后验 (p(x_k | z_{1:k}))。根据贝叶斯公式它正比于似然乘以先验[ p(x_k | z_{1:k}) \propto p(z_k | x_k) , p(x_k | z_{1:k-1}) ]我们已经知道先验是高斯的[ p(x_k | z_{1:k-1}) \mathcal{N}(x_k | \hat{x}{k|k-1}, P{k|k-1}) ]观测模型是线性的观测噪声是高斯的所以似然也是高斯的[ p(z_k | x_k) \mathcal{N}(z_k | H_k x_k, R_k) ]两个高斯分布相乘结果仍然是高斯分布。这是高斯分布的又一个重要性质——共轭性。我们可以直接写出后验的均值和协方差但更优雅的做法是构造联合高斯分布然后利用条件高斯公式。把状态和观测拼成一个向量[ \begin{bmatrix} x_k \ z_k \end{bmatrix} ]先计算这个联合分布的均值和协方差。均值[ E[x_k] \hat{x}_{k|k-1} ][ E[z_k] E[H_k x_k v_k] H_k \hat{x}_{k|k-1} ]协方差矩阵分块计算[ \text{Cov}(x_k, x_k) P_{k|k-1} ][ \text{Cov}(x_k, z_k) E\left[(x_k - \hat{x}{k|k-1})(H_k x_k v_k - H_k \hat{x}{k|k-1})^T\right] ][ E\left[(x_k - \hat{x}{k|k-1})(H_k (x_k - \hat{x}{k|k-1}) v_k)^T\right] ][ P_{k|k-1} H_k^T ]因为 (x_k - \hat{x}_{k|k-1}) 与 (v_k) 独立且 (v_k) 零均值。[ \text{Cov}(z_k, z_k) E\left[(H_k (x_k - \hat{x}{k|k-1}) v_k)(H_k (x_k - \hat{x}{k|k-1}) v_k)^T\right] ][ H_k P_{k|k-1} H_k^T R_k ]这个量通常记为 (S_k)叫做新息协方差。[ \text{Cov}(z_k, x_k) \text{Cov}(x_k, z_k)^T H_k P_{k|k-1} ]所以联合分布是[ \begin{bmatrix} x_k \ z_k \end{bmatrix} \sim \mathcal{N}\left( \begin{bmatrix} \hat{x}{k|k-1} \ H_k \hat{x}{k|k-1} \end{bmatrix}, \begin{bmatrix} P_{k|k-1} P_{k|k-1} H_k^T \ H_k P_{k|k-1} S_k \end{bmatrix} \right) ]3.2 条件高斯公式的直接应用有了联合高斯分布后验 (p(x_k | z_k)) 就是条件分布。直接套用前面提到的条件高斯公式[ \hat{x}{k|k} \hat{x}{k|k-1} P_{k|k-1} H_k^T S_k^{-1} (z_k - H_k \hat{x}_{k|k-1}) ][ P_{k|k} P_{k|k-1} - P_{k|k-1} H_k^T S_k^{-1} H_k P_{k|k-1} ]定义卡尔曼增益[ K_k P_{k|k-1} H_k^T S_k^{-1} ]则后验均值和协方差可以写成更紧凑的形式[ \hat{x}{k|k} \hat{x}{k|k-1} K_k (z_k - H_k \hat{x}_{k|k-1}) ][ P_{k|k} (I - K_k H_k) P_{k|k-1} ]这就是卡尔曼滤波更新步的两个核心公式。3.3 卡尔曼增益的物理意义解读卡尔曼增益 (K_k) 是整个算法的灵魂。它决定了我们应该在多大程度上信任观测、多大程度上信任预测。从公式 (K_k P_{k|k-1} H_k^T S_k^{-1}) 可以看出当先验协方差 (P_{k|k-1}) 很大时预测不确定性大(K_k) 变大滤波器更倾向于信任观测。当观测噪声协方差 (R_k) 很大时(S_k H_k P_{k|k-1} H_k^T R_k) 变大(K_k) 变小滤波器更倾向于信任预测。这完全符合直觉预测越不准就越应该听观测的观测越不准就越应该听预测的。卡尔曼增益本质上是在做一种“加权平均”权重由两者的不确定性决定。用一个标量例子来直观理解。假设状态是一维的(H_k 1)则[ K_k \frac{P_{k|k-1}}{P_{k|k-1} R_k} ]如果 (P_{k|k-1} \gg R_k)则 (K_k \approx 1)后验均值约等于观测值。如果 (P_{k|k-1} \ll R_k)则 (K_k \approx 0)后验均值约等于预测值。如果两者相等则 (K_k 0.5)后验均值是预测和观测的算术平均。注意新息 (z_k - H_k \hat{x}_{k|k-1}) 是实际观测与预测观测之间的差它携带了观测中“新”的信息。如果系统模型完全准确且没有噪声新息应该为零。实际中新息不为零正是因为它包含了模型误差和噪声的信息。卡尔曼滤波利用新息来修正状态估计修正的幅度由卡尔曼增益控制。3.4 协方差更新公式的两种等价形式协方差更新有两种常见的写法形式一标准形式[ P_{k|k} P_{k|k-1} - K_k H_k P_{k|k-1} ]形式二Joseph形式[ P_{k|k} (I - K_k H_k) P_{k|k-1} (I - K_k H_k)^T K_k R_k K_k^T ]这两种形式在数学上完全等价但在数值计算中表现不同。标准形式计算量小但在 (K_k) 接近最优值时由于浮点舍入误差可能导致 (P_{k|k}) 失去正定性。Joseph形式计算量稍大但数值稳定性更好因为它保证了 (P_{k|k}) 始终是正定的两个正定矩阵之和。在实际工程中如果状态维度不高我通常推荐用Joseph形式尤其是在嵌入式系统或者长时间运行的滤波器中。数值稳定性带来的收益远大于那一点额外的计算量。4. 完整算法流程与实操中的关键细节4.1 五大公式的完整梳理把预测步和更新步合在一起卡尔曼滤波的完整算法如下。预测步[ \hat{x}{k|k-1} F_k \hat{x}{k-1|k-1} B_k u_k ][ P_{k|k-1} F_k P_{k-1|k-1} F_k^T Q_{k-1} ]更新步[ S_k H_k P_{k|k-1} H_k^T R_k ][ K_k P_{k|k-1} H_k^T S_k^{-1} ][ \hat{x}{k|k} \hat{x}{k|k-1} K_k (z_k - H_k \hat{x}_{k|k-1}) ][ P_{k|k} (I - K_k H_k) P_{k|k-1} ]初始条件(\hat{x}{0|0}) 和 (P{0|0}) 需要人为设定。通常 (\hat{x}{0|0}) 设为第一次观测值或者根据先验知识给出(P{0|0}) 设为一个较大的对角矩阵表示初始时刻对状态估计不太确定。4.2 一个完整的数值算例用一个最简单的例子来走一遍流程。假设一个一维匀速运动系统状态是位置和速度[ x_k \begin{bmatrix} p_k \ v_k \end{bmatrix} ]状态转移矩阵采样时间 (T 1)[ F \begin{bmatrix} 1 1 \ 0 1 \end{bmatrix} ]没有控制输入(B 0)。观测只测位置[ H \begin{bmatrix} 1 0 \end{bmatrix} ]过程噪声协方差[ Q \begin{bmatrix} 0.01 0 \ 0 0.01 \end{bmatrix} ]观测噪声协方差[ R \begin{bmatrix} 0.1 \end{bmatrix} ]初始状态[ \hat{x}{0|0} \begin{bmatrix} 0 \ 0 \end{bmatrix}, \quad P{0|0} \begin{bmatrix} 10 0 \ 0 10 \end{bmatrix} ]假设第一个观测值 (z_1 1.2)。预测步[ \hat{x}{1|0} F \hat{x}{0|0} \begin{bmatrix} 0 \ 0 \end{bmatrix} ][ P_{1|0} F P_{0|0} F^T Q \begin{bmatrix} 1 1 \ 0 1 \end{bmatrix} \begin{bmatrix} 10 0 \ 0 10 \end{bmatrix} \begin{bmatrix} 1 0 \ 1 1 \end{bmatrix} \begin{bmatrix} 0.01 0 \ 0 0.01 \end{bmatrix} ]计算[ F P_{0|0} \begin{bmatrix} 10 10 \ 0 10 \end{bmatrix} ][ F P_{0|0} F^T \begin{bmatrix} 10 10 \ 0 10 \end{bmatrix} \begin{bmatrix} 1 0 \ 1 1 \end{bmatrix} \begin{bmatrix} 20 10 \ 10 10 \end{bmatrix} ][ P_{1|0} \begin{bmatrix} 20.01 10 \ 10 10.01 \end{bmatrix} ]更新步[ S_1 H P_{1|0} H^T R 20.01 0.1 20.11 ][ K_1 P_{1|0} H^T S_1^{-1} \begin{bmatrix} 20.01 \ 10 \end{bmatrix} \frac{1}{20.11} \begin{bmatrix} 0.995 \ 0.497 \end{bmatrix} ][ \hat{x}_{1|1} \begin{bmatrix} 0 \ 0 \end{bmatrix} \begin{bmatrix} 0.995 \ 0.497 \end{bmatrix} (1.2 - 0) \begin{bmatrix} 1.194 \ 0.597 \end{bmatrix} ][ P_{1|1} (I - K_1 H) P_{1|0} ][ I - K_1 H \begin{bmatrix} 1 - 0.995 0 \ -0.497 1 \end{bmatrix} \begin{bmatrix} 0.005 0 \ -0.497 1 \end{bmatrix} ][ P_{1|1} \begin{bmatrix} 0.005 0 \ -0.497 1 \end{bmatrix} \begin{bmatrix} 20.01 10 \ 10 10.01 \end{bmatrix} \begin{bmatrix} 0.100 0.050 \ -0.005 5.040 \end{bmatrix} ]可以看到经过一次更新位置的方差从 20.01 降到了 0.100说明观测提供了大量信息。速度的方差从 10.01 降到了 5.040也有所下降但不如位置明显因为观测不直接测速度。这个算例虽然简单但完整展示了卡尔曼滤波的每一步计算。建议自己动手算一遍比看十遍公式都管用。4.3 参数调优的实操经验卡尔曼滤波的调参主要围绕 (Q) 和 (R) 两个矩阵。这两个矩阵的比值决定了滤波器的“性格”。参数设置滤波器行为适用场景(Q) 大(R) 小信任观测响应快但输出抖动大观测质量高系统动态变化快(Q) 小(R) 大信任预测输出平滑但响应慢观测噪声大系统动态变化慢(Q) 和 (R) 都大增益适中但协方差收敛慢初始不确定性大(Q) 和 (R) 都小增益小滤波器“固执”模型和观测都很准确我的调参流程通常是这样的先把 (Q) 设为一个很小的值比如 (10^{-6}) 倍的单位矩阵(R) 设为实际观测噪声的方差估计。然后跑一段数据观察新息序列。如果新息均值不为零说明模型有偏差如果新息方差远大于 (S_k)说明 (Q) 设小了如果新息序列出现明显的自相关说明模型阶数不够或者 (Q) 的结构不合理。实操心得新息序列是调试卡尔曼滤波最好的工具。理论上最优滤波器的新息应该是零均值白噪声。如果新息有颜色自相关不为零说明滤波器没有提取完观测中的所有信息还有改进空间。我习惯把新息和新息协方差画出来一眼就能看出滤波器是否工作正常。5. 常见问题与排查技巧实录5.1 滤波器发散的原因与对策滤波器发散是实际中最常见的问题表现为估计误差越来越大甚至完全偏离真实值。原因通常有以下几种。原因一模型不准确。状态转移矩阵 (F) 或者观测矩阵 (H) 与实际系统不匹配。比如系统实际上有加速度但模型只建了匀速运动。对策是增加状态维度或者调整 (Q) 来补偿模型误差。原因二(Q) 设得太小。滤波器过度信任模型当模型有误差时协方差 (P) 会越来越小卡尔曼增益也越來越小最终滤波器“闭眼不看”观测完全靠模型外推。对策是适当增大 (Q)给模型误差留出余量。原因三数值误差累积。长时间运行后由于浮点舍入误差(P) 矩阵可能失去正定性或对称性。对策是使用Joseph形式更新协方差或者定期对 (P) 做对称化处理(P \leftarrow (P P^T)/2)。原因四观测异常值。如果观测中出现离群值滤波器会被“带偏”。对策是加入新息卡方检验当新息超过一定阈值时认为观测异常跳过更新步或者降低该观测的权重。5.2 新息卡方检验的实现新息卡方检验是判断观测是否异常的标准方法。新息 (r_k z_k - H_k \hat{x}_{k|k-1}) 在正常情况下服从 (\mathcal{N}(0, S_k))。构造统计量[ d_k r_k^T S_k^{-1} r_k ](d_k) 服从自由度为 (m) 的卡方分布。给定显著性水平 (\alpha)通常取 0.05 或 0.01查卡方分布表得到阈值 (\chi^2_{m, \alpha})。如果 (d_k \chi^2_{m, \alpha})则认为观测异常。import numpy as np from scipy.stats import chi2 def kalman_update(x_pred, P_pred, z, H, R, alpha0.05): m z.shape[0] S H P_pred H.T R r z - H x_pred d r.T np.linalg.inv(S) r threshold chi2.ppf(1 - alpha, m) if d threshold: # 观测异常跳过更新 return x_pred, P_pred, True K P_pred H.T np.linalg.inv(S) x_upd x_pred K r P_upd (np.eye(len(x_pred)) - K H) P_pred return x_upd, P_upd, False这个检验在实际系统中非常有用尤其是传感器偶尔会给出错误读数的情况。加了这层保护之后滤波器的鲁棒性会明显提升。5.3 常见问题速查表问题现象可能原因排查方法解决方案估计值滞后于真实值(Q) 太小或 (R) 太大观察新息是否持续同号增大 (Q) 或减小 (R)估计值抖动严重(Q) 太大或 (R) 太小观察后验协方差是否过大减小 (Q) 或增大 (R)滤波器发散模型不准或数值问题检查 (P) 是否正定增大 (Q)使用Joseph形式新息有自相关模型阶数不够画新息自相关图增加状态维度或调整 (Q) 结构初始收敛慢(P_0) 太小观察初始阶段增益增大 (P_0)观测异常导致跳变无异常检测计算新息卡方统计量加入卡方检验5.4 扩展卡尔曼滤波的过渡实际系统往往是非线性的这时候标准卡尔曼滤波就不够用了。扩展卡尔曼滤波EKF的思路是在工作点附近对非线性函数做一阶泰勒展开用雅可比矩阵代替线性矩阵。状态转移和观测模型变为[ x_k f(x_{k-1}, u_k) w_{k-1} ][ z_k h(x_k) v_k ]预测步用 (f) 传播均值用雅可比矩阵 (F_k \frac{\partial f}{\partial x}\big|{\hat{x}{k-1|k-1}}) 传播协方差。更新步用 (h) 计算预测观测用雅可比矩阵 (H_k \frac{\partial h}{\partial x}\big|{\hat{x}{k|k-1}}) 计算增益。EKF的推导本质上和标准卡尔曼滤波一样只是把线性矩阵换成了雅可比矩阵。但要注意EKF只在一阶意义上最优如果非线性很强雅可比矩阵的近似误差会导致滤波器性能下降甚至发散。这时候可以考虑无迹卡尔曼滤波UKF它用sigma点来传播分布不需要计算雅可比矩阵精度也更高。我在实际项目中的选择逻辑是如果非线性程度不高EKF足够用且计算量小如果非线性强或者雅可比矩阵难以计算直接上UKF。粒子滤波虽然理论上能处理任意非线性非高斯但计算量大通常只在EKF和UKF都搞不定的时候才考虑。5.5 从推导到代码的映射检查清单最后分享一个我常用的检查清单帮你确认代码实现和推导是否一致。[ ] 状态转移矩阵 (F) 的维度是 (n \times n)与状态维度一致[ ] 观测矩阵 (H) 的维度是 (m \times n)与观测维度一致[ ] (Q) 矩阵是对称正定的 (n \times n) 矩阵[ ] (R) 矩阵是对称正定的 (m \times m) 矩阵[ ] 预测步中 (B_k u_k) 只加到均值上不加到协方差上[ ] 更新步中新息是 (z_k - H_k \hat{x}{k|k-1})不是 (\hat{x}{k|k-1} - z_k)[ ] 卡尔曼增益的维度是 (n \times m)[ ] 协方差更新后检查对称性(P (P P^T)/2)[ ] 初始协方差 (P_0) 不要设为零矩阵这个清单看起来简单但每一条我都见过有人踩坑。尤其是新息的方向和协方差对称性出问题的时候现象很隐蔽不容易发现。卡尔曼滤波的推导到这里就完整了。从贝叶斯滤波的递推框架出发加上线性高斯假设一步步推出预测和更新的五大公式每一步都有明确的概率论依据。理解了这个推导过程再看任何卡尔曼滤波的变体——EKF、UKF、信息滤波、平方根滤波——都会觉得有迹可循因为它们本质上都是在处理同一个贝叶斯递推问题只是用了不同的近似方法。