第2章：概率论基础回顾：高斯分布、协方差矩阵、条件概率与贝叶斯法则
各位同学好，欢迎来到第二章。说实话，很多做SLAM的朋友，最后代码跑不通，回头一查，往往是概率论的基础没打牢。EKF说白了就是一套概率论的工程实现。你想想看，机器人怎么知道自己在哪？它就是在不断地猜，然后用概率去量化这个猜测的准确度。
这一章，我们就把EKF背后那几个核心的概率工具捋一遍。我保证，不扯虚的，全是干货。
2.1 高斯分布：为什么是它？
在机器人领域，我们几乎默认所有噪声都是高斯分布。为什么？两个原因。
第一，数学上方便。 高斯分布经过线性变换后还是高斯分布，这对EKF的推导至关重要。
第二，物理上合理。 中心极限定理告诉我们，很多独立随机因素叠加起来，结果就趋近于高斯分布。传感器噪声、里程计误差，基本都符合这个规律。
一维高斯分布的概率密度函数长这样：
p(x) = (1 / (σ * sqrt(2π))) * exp(-0.5 * ((x - μ) / σ)^2)其中 μ 是均值，σ² 是方差。均值代表我们估计的最优值，方差代表我们对这个估计的信任程度。方差越小，说明我们越确定。
核心理解： 在EKF里，状态估计就是一个高斯分布。均值是当前最优估计，协方差矩阵是不确定性。
到了多维情况，就变成了多元高斯分布：
p(x) = (1 / sqrt((2π)^n * det(Σ))) * exp(-0.5 * (x - μ)^T * Σ^(-1) * (x - μ))这里 Σ 就是协方差矩阵。嗯，这里要注意，协方差矩阵是对称且半正定的，这是它能够分解和求逆的前提。
2.2 协方差矩阵：不确定性之间的关系
协方差矩阵是EKF的灵魂。它不光告诉我们每个状态量的方差，还告诉我们不同状态量之间的相关性。
举个例子。假设机器人状态是 [x, y, θ]ᵀ。如果机器人往前走了一米，x和y的估计都会变化，而且它们的不确定性会相互影响。协方差矩阵就捕捉了这种关系。
Σ = [[σ_xx, σ_xy, σ_xθ],
[σ_yx, σ_yy, σ_yθ],
[σθ_x, σθ_y, σθ_θ]]对角线元素是方差，非对角线元素是协方差。如果 σ_xy 是正的，说明x和y的误差倾向于同向变化。
个人经验： 我在调试EKF时，经常打印协方差矩阵看看。如果某个非对角线元素突然变得很大，说明状态估计可能出问题了。有一次，我发现σ_xθ异常大，排查了半天，原来是IMU的安装角度标定错了。
协方差矩阵还有一个重要性质：它可以通过特征值分解来分析不确定性在各个方向上的分布。这在SLAM的稀疏性优化中经常用到。
2.3 条件概率与贝叶斯法则
EKF的核心思想，就是贝叶斯法则。说白了，就是根据观测数据来更新我们对状态的信念。
贝叶斯法则的公式很简单：
P(A|B) = P(B|A) * P(A) / P(B)在EKF的语境下：
- P(A|B) 是后验概率——看到观测数据后，我们对状态的估计。
- P(B|A) 是似然——给定状态，观测数据出现的可能性。
- P(A) 是先验概率——看到观测数据前，我们对状态的估计。
- P(B) 是证据——归一化常数，保证概率和为1。
EKF的预测步骤，就是计算先验概率。更新步骤，就是利用观测数据计算后验概率。
避坑指南： 我曾经在项目中忽略了P(B)的计算，直接用了非归一化的后验。结果滤波器发散得一塌糊涂。后来才意识到，虽然EKF里P(B)被隐含地处理了，但理解它的物理意义对调试很有帮助。
2.4 条件高斯分布：EKF的数学基础
如果两个随机变量x和y是联合高斯分布，那么给定y后，x的条件分布也是高斯分布。这个性质是EKF更新步骤的数学基础。
假设：
[x; y] ~ N([μ_x; μ_y], [[Σ_xx, Σ_xy], [Σ_yx, Σ_yy]])那么给定y后，x的条件分布为：
p(x|y) = N(μ_x + Σ_xy * Σ_yy^(-1) * (y - μ_y), Σ_xx - Σ_xy * Σ_yy^(-1) * Σ_yx)你看，这个公式和EKF的更新公式长得一模一样。μ_x + ... 就是状态更新，Σ_xx - ... 就是协方差更新。
我建议： 把这个公式和EKF的更新公式对照着看。你会发现，卡尔曼增益K其实就是 Σ_xy * Σ_yy^(-1)。理解了这一点，EKF就不再是黑盒了。
2.5 知识体系总览
下面这张图，把本章的核心逻辑串起来了。你想想看，从高斯分布到协方差矩阵，再到条件概率和贝叶斯法则，最后汇聚到EKF。每一步都有它的工程意义。
2.6 本章小结
这一章的内容，说白了就是EKF的数学说明书。高斯分布给了我们建模的工具，协方差矩阵给了我们量化不确定性的方法，条件概率和贝叶斯法则给了我们更新估计的框架。
我个人习惯，在开始写EKF代码前，一定会先把这几个公式手推一遍。不是为了炫技，而是为了在代码出问题时，能快速定位是数学错了还是代码错了。
注意事项： 协方差矩阵的数值稳定性是个大坑。如果矩阵接近奇异，求逆会出问题。我建议在实际代码中，定期检查协方差矩阵的条件数，或者使用平方根滤波等数值稳定的变体。
好了，这一章就到这里。记住，EKF不是魔法，它就是贝叶斯法则加上高斯假设的工程实现。把基础打牢，后面的路就好走了。
公众号:蓝海资料掘金营，微信deep3321