引言：阿波罗登月的幕后英雄

1969 年，阿波罗 11 号飞船在飞向月球的过程中，面临着一个棘手的问题：

飞船上的导航传感器（陀螺仪、加速度计）存在误差，且误差会随时间累积（漂移）；地面雷达的追踪虽然没有漂移，但存在巨大的随机噪声。 更糟糕的是，飞船的推力并不完全稳定，存在内部扰动。

如果你是当年的工程师，你该相信谁？ 相信飞船内部的传感器？它会把你带偏。 相信地面的雷达？它的读数跳动得像心电图。

鲁道夫·卡尔曼（Rudolf Kalman）给出的答案是：都不要全信，但也都要信一点。

这就是卡尔曼滤波（Kalman Filter）。它不是一种过滤杂质的物理筛子，而是一种**信息融合（Information Fusion）**的哲学。它在充满不确定性（Uncertainty）的世界中，寻找方差最小的“最优估计”。

第一章：直觉——两个高斯分布的相遇

要理解卡尔曼滤波，我们必须先忘掉矩阵，回到概率论的最基础：高斯分布（正态分布）。

假设我们在黑暗的走廊里行走。 1. 预测（Prediction）：根据我的步幅和走路时间，我觉得我向前走了 10 米。但我不敢确定，可能走了 9 米，也可能 11 米。这种不确定性，可以用一个高斯分布 \(N(\mu_1, \sigma_1^2)\) 来描述。 2. 观测（Measurement）：此时我伸手摸到了墙上的一个标记。根据手感，我测量出我现在在 10.2 米处。但我的手感也有误差，这又是一个高斯分布 \(N(\mu_2, \sigma_2^2)\)。

现在的核心问题是：我到底在哪里？

直觉告诉我们，真值应该介于 10 和 10.2 之间。如果我的测量（手感）非常准（\(\sigma_2\) 很小），我就应该更偏向 10.2；如果我对自己的步幅非常有信心（\(\sigma_1\) 很小），我就更偏向 10。

在数学上，最佳的估计位置，就是这两个高斯分布概率密度函数（PDF）乘积后的新高斯分布的峰值。

经过推导，新的均值 \(\mu'\) 为：

请盯着这个公式。如果我们令 \(K = \frac{\sigma_1^2}{\sigma_1^2 + \sigma_2^2}\)，那么：

这个 \(K\)，就是传说中的卡尔曼增益（Kalman Gain）。 它的物理含义极其优美：它是“预测误差”占“总误差”的比例。 如果预测误差很大（\(\sigma_1\) 大），\(K\) 就接近 1，我们就完全听从观测值；如果观测误差很大（\(\sigma_2\) 大），\(K\) 就接近 0，我们就完全听从预测值。

这就是卡尔曼滤波的全部灵魂。剩下的，只是把它扩展到多维矩阵而已。

!

第二章：状态空间模型——用数学描述世界

现实系统不是一维的。我们需要处理位置、速度、加速度等多个变量，而且它们之间相互关联（速度改变位置）。因此，我们引入矩阵。

2.1 状态向量与状态转移

设系统的状态为向量 \(x_k\)（例如包含位置 \(p\) 和速度 \(v\)）。 根据物理定律（如牛顿力学），我们可以根据上一时刻的状态 \(x_{k-1}\) 预测当前时刻的状态：

• \(F\)：状态转移矩阵（State Transition Matrix）。例如 \(p_k = p_{k-1} + v_{k-1} \Delta t\)。
• \(u_k\)：控制输入（Control Input）。例如踩下油门产生的加速度。
• \(w_k\)：过程噪声（Process Noise）。来自风阻、路面不平整等不可控因素。假设服从 \(N(0, Q)\)，其中 \(Q\) 是过程噪声协方差矩阵。

2.2 观测模型

我们通过传感器观察世界，但传感器只能看到一部分信息（例如只能测速，不能测位置）：

• \(H\)：观测矩阵（Observation Matrix）。将状态空间映射到观测空间。
• \(v_k\)：测量噪声（Measurement Noise）。假设服从 \(N(0, R)\)，其中 \(R\) 是测量噪声协方差矩阵。

第三章：协方差矩阵 \(P\)——不确定性的形状

在卡尔曼滤波中，最关键的变量不是状态 \(x\)，而是协方差矩阵 \(P\)。

如果 \(x\) 告诉我们要去哪里，那么 \(P\) 就告诉我们有多大的把握。 \(P_{ij}\) 描述了第 \(i\) 个变量和第 \(j\) 个变量之间的相关性。在几何上，它定义了一个高维椭球体。

• 随着时间推移，因为有过程噪声 \(Q\)，预测会越来越不准，椭球体会变大。
• 每当我们获得一次观测，信息增加了，不确定性减少，椭球体会变小。

卡尔曼滤波，就是这个椭球体不断“呼吸”（变大又变小）的过程。

第四章：算法的五大公式——预测与更新的循环

卡尔曼滤波是一个递归（Recursive）过程，分为两个阶段：预测（Predict）和更新（Update）。

阶段一：预测（先验估计）

我们闭上眼睛，根据物理模型瞎猜下一步在哪。

1. 状态预测：
   \[ \hat{x}_{k}^- = F \hat{x}_{k-1} + B u_k \]
2. 协方差预测（不确定性扩散）：
   \[ P_{k}^- = F P_{k-1} F^T + Q \]
   注：这里加上 \(Q\) 表示因为外部干扰，我们对系统的信心下降了。

阶段二：更新（后验修正）

我们睁开眼睛，看了一眼传感器，用来修正刚才的瞎猜。

1. 计算卡尔曼增益 \(K\)（权重的分配）：

   \[ K_k = P_k^- H^T (H P_k^- H^T + R)^{-1} \]
   注：这正是第一章中 \(K = \frac{\sigma_1^2}{\sigma_1^2 + \sigma_2^2}\) 的矩阵形式。\(R\) 越大，\(K\) 越小。

2. 状态更新（融合预测与观测）：

   \[ \hat{x}_k = \hat{x}_k^- + K_k (z_k - H \hat{x}_k^-) \]
   注：\(z_k - H \hat{x}_k^-\) 称为残差（Innovation），即观测值与预测值的差异。

3. 协方差更新（信心增强）：

   \[ P_k = (I - K_k H) P_k^- \]
   注：更新后的协方差 \(P_k\) 必定小于预测的 \(P_k^-\)，因为观测带来了信息。

!

第五章：非线性的世界——EKF 与 UKF

标准的卡尔曼滤波假设系统是线性的（矩阵乘法）。但现实世界充满了非线性（例如正弦函数、机器人关节角度）。

如果我们直接对高斯分布进行非线性变换，它就不再是高斯分布了，算法会失效。为了解决这个问题：

1. 扩展卡尔曼滤波 (EKF)： 使用泰勒级数展开，在当前工作点将非线性函数线性化（计算雅可比矩阵 Jacobian）。

   • 缺点：计算繁琐，且如果非线性太强，截断误差会导致发散。

2. 无迹卡尔曼滤波 (UKF)： 不搞复杂的导数，而是采用确定性采样（Sigma Points）。选几个代表性的点，穿过非线性函数，再统计变换后的均值和方差。

   • 优点：更准、更鲁棒，不用算雅可比矩阵。

结论：贝叶斯眼中的世界

卡尔曼滤波之所以伟大，不在于它能算出导弹的轨迹，而在于它提供了一种世界观：

我们永远无法得知世界的真值（Ground Truth），真值是上帝的特权。作为凡人，我们拥有的只有先验知识（物理模型）和感官数据（传感器观测），且两者都充满噪声。

卡尔曼滤波告诉我们：不要傲慢地只相信模型，也不要盲目地只相信数据。 唯有根据它们各自的可信度（方差），动态地权衡与融合，我们才能在充满噪声的混沌中，无限逼近那个不可触碰的真理。

这就是算法的禅意。