在噪声中看见真相：卡尔曼滤波的第一性原理推导
引言：阿波罗登月的幕后英雄
1969 年，阿波罗 11 号飞船在飞向月球的过程中，面临着一个棘手的问题：
飞船上的导航传感器（陀螺仪、加速度计）存在误差，且误差会随时间累积（漂移）；地面雷达的追踪虽然没有漂移，但存在巨大的随机噪声。
更糟糕的是，飞船的推力并不完全稳定，存在内部扰动。
如果你是当年的工程师，你该相信谁？
相信飞船内部的传感器？它会把你带偏。
相信地面的雷达？它的读数跳动得像心电图。
鲁道夫·卡尔曼（Rudolf Kalman）给出的答案是：都不要全信，但也都要信一点。
这就是卡尔曼滤波（Kalman Filter）。它不是一种过滤杂质的物理筛子，而是一种**信息融合（Information Fusion）**的哲学。它在充满不确定性（Uncertainty）的世界中，寻找方差最小的“最优估计”。
第一章：直觉——两个高斯分布的相遇
要理解卡尔曼滤波，我们必须先忘掉矩阵，回到概率论的最基础：高斯分布（正态分布）。
假设我们在黑暗的走廊里行走。
- 预测（Prediction）：根据我的步幅和走路时间，我觉得我向前走了 10 米。但我不敢确定，可能走了 9 米，也可能 11 米。这种不确定性，可以用一个高斯分布 $N(\mu_1, \sigma_1^2)$ 来描述。
- 观测（Measurement）：此时我伸手摸到了墙上的一个标记。根据手感，我测量出我现在在 10.2 米处。但我的手感也有误差，这又是一个高斯分布 $N(\mu_2, \sigma_2^2)$。
现在的核心问题是：我到底在哪里？
直觉告诉我们，真值应该介于 10 和 10.2 之间。如果我的测量（手感）非常准（$\sigma_2$ 很小），我就应该更偏向 10.2；如果我对自己的步幅非常有信心（$\sigma_1$ 很小），我就更偏向 10。
在数学上，最佳的估计位置，就是这两个高斯分布概率密度函数（PDF）乘积后的新高斯分布的峰值。
$$
N_{new}(\mu’, \sigma’^2) = N(\mu_1, \sigma_1^2) \times N(\mu_2, \sigma_2^2)
$$
经过推导，新的均值 $\mu’$ 为：
$$
\mu’ = \mu_1 + \frac{\sigma_1^2}{\sigma_1^2 + \sigma_2^2} (\mu_2 - \mu_1)
$$
请盯着这个公式。如果我们令 $K = \frac{\sigma_1^2}{\sigma_1^2 + \sigma_2^2}$，那么：
$$
\mu’ = \mu_1 + K (\text{观测值} - \text{预测值})
$$
这个 $K$，就是传说中的卡尔曼增益（Kalman Gain）。
它的物理含义极其优美：它是“预测误差”占“总误差”的比例。 如果预测误差很大（$\sigma_1$ 大），$K$ 就接近 1，我们就完全听从观测值；如果观测误差很大（$\sigma_2$ 大），$K$ 就接近 0，我们就完全听从预测值。
这就是卡尔曼滤波的全部灵魂。剩下的，只是把它扩展到多维矩阵而已。
!
第二章：状态空间模型——用数学描述世界
现实系统不是一维的。我们需要处理位置、速度、加速度等多个变量，而且它们之间相互关联（速度改变位置）。因此，我们引入矩阵。
2.1 状态向量与状态转移
设系统的状态为向量 $x_k$（例如包含位置 $p$ 和速度 $v$）。
根据物理定律（如牛顿力学），我们可以根据上一时刻的状态 $x_{k-1}$ 预测当前时刻的状态：
$$
x_k = F x_{k-1} + B u_k + w_k
$$
- $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 观测模型
我们通过传感器观察世界，但传感器只能看到一部分信息（例如只能测速，不能测位置）：
$$
z_k = H x_k + v_k
$$
- $H$：观测矩阵（Observation Matrix）。将状态空间映射到观测空间。
- $v_k$：测量噪声（Measurement Noise）。假设服从 $N(0, R)$，其中 $R$ 是测量噪声协方差矩阵。
第三章：协方差矩阵 $P$——不确定性的形状
在卡尔曼滤波中，最关键的变量不是状态 $x$，而是协方差矩阵 $P$。
如果 $x$ 告诉我们要去哪里，那么 $P$ 就告诉我们有多大的把握。
$P_{ij}$ 描述了第 $i$ 个变量和第 $j$ 个变量之间的相关性。在几何上，它定义了一个高维椭球体。
- 随着时间推移，因为有过程噪声 $Q$，预测会越来越不准，椭球体会变大。
- 每当我们获得一次观测，信息增加了，不确定性减少，椭球体会变小。
卡尔曼滤波，就是这个椭球体不断“呼吸”（变大又变小）的过程。
第四章：算法的五大公式——预测与更新的循环
卡尔曼滤波是一个递归（Recursive）过程，分为两个阶段：预测（Predict）和更新（Update）。
阶段一：预测（先验估计）
我们闭上眼睛，根据物理模型瞎猜下一步在哪。
- 状态预测：
$$ \hat{x}{k}^- = F \hat{x}{k-1} + B u_k $$ - 协方差预测（不确定性扩散）：
$$ P_{k}^- = F P_{k-1} F^T + Q $$
注：这里加上 $Q$ 表示因为外部干扰，我们对系统的信心下降了。
阶段二：更新（后验修正）
我们睁开眼睛，看了一眼传感器，用来修正刚才的瞎猜。
计算卡尔曼增益 $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$ 越小。状态更新（融合预测与观测）：
$$ \hat{x}_k = \hat{x}_k^- + K_k (z_k - H \hat{x}_k^-) $$
注：$z_k - H \hat{x}_k^-$ 称为残差（Innovation），即观测值与预测值的差异。协方差更新（信心增强）：
$$ P_k = (I - K_k H) P_k^- $$
注：更新后的协方差 $P_k$ 必定小于预测的 $P_k^-$，因为观测带来了信息。
!
第五章：非线性的世界——EKF 与 UKF
标准的卡尔曼滤波假设系统是线性的（矩阵乘法）。但现实世界充满了非线性（例如正弦函数、机器人关节角度）。
如果我们直接对高斯分布进行非线性变换，它就不再是高斯分布了，算法会失效。为了解决这个问题：
扩展卡尔曼滤波 (EKF)：
使用泰勒级数展开，在当前工作点将非线性函数线性化（计算雅可比矩阵 Jacobian）。- 缺点：计算繁琐，且如果非线性太强，截断误差会导致发散。
无迹卡尔曼滤波 (UKF)：
不搞复杂的导数，而是采用确定性采样（Sigma Points）。选几个代表性的点，穿过非线性函数，再统计变换后的均值和方差。- 优点：更准、更鲁棒，不用算雅可比矩阵。
结论：贝叶斯眼中的世界
卡尔曼滤波之所以伟大，不在于它能算出导弹的轨迹，而在于它提供了一种世界观：
我们永远无法得知世界的真值（Ground Truth），真值是上帝的特权。作为凡人，我们拥有的只有先验知识（物理模型）和感官数据（传感器观测），且两者都充满噪声。
卡尔曼滤波告诉我们：不要傲慢地只相信模型，也不要盲目地只相信数据。 唯有根据它们各自的可信度（方差），动态地权衡与融合，我们才能在充满噪声的混沌中，无限逼近那个不可触碰的真理。
这就是算法的禅意。