第2章：数学基础回顾：概率论、高斯分布、协方差矩阵
各位同学，欢迎来到第二讲。
说实话，很多做机器人或者做自动驾驶的朋友，一听到「数学基础」四个字就想关页面。我特别理解。当年我刚入行时也觉得，搞工程嘛，调调参数就行了，数学差不多就得了。
结果呢？第一次做多传感器融合，卡尔曼滤波死活发散。我盯着波形图看了三天，最后发现是协方差矩阵初始化出了问题。嗯，从那以后，我再也不敢轻视这些基础了。
这一章，咱们就把概率论、高斯分布、协方差矩阵这几个老朋友请出来，好好聊聊。别怕，我会用最工程化的语言来讲。
2.1 概率论：不确定性才是常态
你想想看，传感器数据有没有绝对准确的？没有。GPS定位误差几米，IMU有零偏，激光雷达有噪点。说白了，我们做融合，就是在跟不确定性打交道。
概率论给了我们一套描述不确定性的语言。核心就两个概念：
- 概率密度函数（PDF）：描述一个随机变量取某个值的可能性有多大。
- 条件概率：已知一些信息后，对未知量的重新估计。
举个我项目中的例子。有一次做AGV小车定位，轮式里程计每走一米大概漂移2厘米。这个「2厘米」就是不确定性。我们用概率模型来描述它——不是「绝对位置在哪」，而是「位置大概在哪，有多大概率」。这就是概率思维。
2.2 高斯分布：一切美好的起点
为什么卡尔曼滤波这么喜欢高斯分布？
因为高斯分布有一个极其优雅的性质：经过线性变换后，仍然是高斯分布。而且，两个高斯分布的乘积，还是高斯分布。
一维高斯分布大家都很熟悉：
p(x) = (1 / sqrt(2πσ²)) * exp(-(x-μ)² / (2σ²))其中 μ 是均值，σ² 是方差。均值告诉你「最可能的位置」，方差告诉你「有多不确定」。
我刚开始做滤波时，总觉得方差越小越好。后来发现不是这样。方差太小，滤波器对新观测数据就不敏感了，这叫「过自信」。我曾经在一个无人机项目中，把过程噪声设得太小，结果飞机一转弯，滤波器直接跟不上真实状态，差点炸机。
2.3 协方差矩阵：多维世界的「关系网」
一维情况很简单，就一个方差。但到了多维——比如同时估计位置(x, y)和速度(vx, vy)——事情就复杂了。
协方差矩阵就是用来描述多维随机变量之间关系的。它的对角线元素是各个维度的方差，非对角线元素是协方差——描述两个维度之间的相关性。
举个例子：
P = | σ²_x σ_xy |
| σ_yx σ²_y |如果 σ_xy 是正数，说明 x 和 y 正相关——x 大了，y 也倾向于大。这在很多物理系统中很常见，比如位置和速度就是强相关的。
我记得有一次调试一个轮式机器人，协方差矩阵的非对角线元素被我设成了零。结果滤波器完全无法利用位置和速度之间的耦合关系，收敛速度慢得离谱。后来我把它们设成合理的相关值，效果立竿见影。
| 矩阵元素 | 含义 | 典型值（位置-速度） |
|---|---|---|
| σ²_x | 位置x的不确定性 | 0.1 m² |
| σ²_vx | 速度vx的不确定性 | 0.01 (m/s)² |
| σ_x_vx | 位置与速度的协方差 | 0.03 m·m/s |
看到没？位置和速度的协方差不为零，说明它们之间存在物理上的耦合关系。卡尔曼滤波正是利用这种关系，通过速度观测来修正位置估计，反之亦然。
2.4 从理论到代码：协方差矩阵的初始化
理论说完了，咱们来点实际的。在扩展卡尔曼滤波中，协方差矩阵的初始化是个关键步骤。
我个人习惯这样初始化：
// 状态向量: [x, y, vx, vy]
// 初始协方差矩阵
P = | 0.1, 0, 0, 0 |
| 0, 0.1, 0, 0 |
| 0, 0, 0.5, 0 |
| 0, 0, 0, 0.5 |这里位置的不确定性设得小一些（0.1），速度的不确定性设得大一些（0.5）。为什么？因为初始时刻我们通常对位置有较准确的先验（比如GPS给出的初始位置），但对速度几乎一无所知。
2.5 小结与下章预告
这一章我们回顾了三个核心概念：
- 概率论给了我们描述不确定性的框架
- 高斯分布是卡尔曼滤波的数学基石
- 协方差矩阵是多维状态估计的「关系网」
下一章，我们将正式进入卡尔曼滤波的核心——预测与更新方程。我会用最直观的方式，把那些矩阵公式拆解成你能理解的东西。
记住一句话：滤波的本质，就是在「相信模型」和「相信观测」之间找到最优平衡。而这个平衡，全靠协方差矩阵来调节。
咱们下章见。