📚 课程目录第2章概率论与状态估计基础

2. 概率论与状态估计基础

在机器人状态估计中，我们面对的从来不是“确定值”，而是带有噪声、误差和不确定性的观测量与系统状态。EKF 之所以成立，本质上是因为它用概率分布来描述“机器人现在可能在哪里、速度是多少、姿态偏了多少”。因此，理解随机变量、高斯分布、条件概率、贝叶斯滤波以及期望与协方差，是读懂 EKF 公式和工程调参逻辑的前提。

一、随机变量：把不确定性变成可计算的量

随机变量是对不确定事件的数值化描述。例如机器人轮速编码器读数、IMU 角速度、激光雷达测距值，都可以建模为随机变量。连续随机变量通常用概率密度函数 p(x) 描述，它不直接给出“某个值等于多少”的概率，而是给出“落在某个区间内的可能性密度”。

在状态估计中，我们最关心两类随机变量：

状态变量：如位置 x、速度 v、航向角 θ，它们构成状态向量 X = [x, y, θ, v]ᵀ。
观测变量：如 GPS 位置、激光测距、视觉特征点像素坐标，它们通过观测方程与状态关联。

随机变量之间并非孤立，状态估计的核心就是利用“观测变量”去推断“状态变量”的分布。

二、高斯分布：EKF 的数学基石

高斯分布（正态分布）由均值 μ 和方差 σ² 完全确定，多维情况下由均值向量 μ 和协方差矩阵 Σ 确定。其密度函数为：

p(x) = 1 / ( (2π)^(n/2) |Σ|^(1/2) ) * exp( -1/2 (x-μ)ᵀ Σ⁻¹ (x-μ) )

高斯分布有三个对 EKF 极其关键的性质：

线性变换后仍是高斯：若 X ~ N(μ, Σ)，则 AX + b ~ N(Aμ + b, AΣAᵀ)。
边缘化和条件化后仍是高斯：这使卡尔曼滤波的预测与更新可以闭式求解。
只需维护均值和协方差：EKF 不必存储完整概率密度，只需递推 μ 和 Σ。

工程上常说的“状态不确定度”，在 EKF 中就是协方差矩阵 Σ 的对角线元素；对角线越大，表示该状态越不可信。

三、条件概率与贝叶斯滤波：从观测反推状态

条件概率 p(x|z) 表示在观测 z 已发生的条件下，状态 x 的概率。贝叶斯公式把它与观测模型和先验联系起来：

p(x|z) = p(z|x) * p(x) / p(z)

其中 p(z|x) 是似然，表示“如果状态是 x，观测到 z 的可能性”；p(x) 是先验，表示“在观测前我们认为状态是什么”；p(x|z) 是后验，表示“看完观测后我们修正后的状态估计”。

贝叶斯滤波把这一过程递推化，分为两步：

预测：利用运动模型把上一时刻后验传播到当前时刻，得到先验 p(xₖ|z₁:ₖ₋₁)。
更新：利用当前观测 zₖ，通过贝叶斯公式修正先验，得到后验 p(xₖ|z₁:ₖ)。

EKF 正是贝叶斯滤波在高斯假设和非线性系统下的一阶线性化实现。

四、期望与协方差：EKF 真正递推的两个量

期望 E[X] 描述随机变量的“中心位置”，在 EKF 中对应状态估计值 μ。协方差 Cov(X) = E[(X-μ)(X-μ)ᵀ] 描述变量自身的离散程度以及变量之间的相关性，在 EKF 中对应不确定度矩阵 Σ。

协方差矩阵的对角线是各状态分量的方差，非对角线是两两之间的协方差。例如位置 x 与速度 v 若正相关，则 Σ_xv > 0，意味着“位置估计偏大时，速度估计也倾向偏大”。EKF 的预测和更新，本质上就是不断传播和收缩这个协方差矩阵。

概念	数学符号	EKF 中的对应量	工程含义
期望	E[X] = μ	状态估计 μ	机器人当前最可能的状态
协方差	Cov(X) = Σ	状态协方差 Σ	估计的不确定度与相关性
过程噪声	w ~ N(0, Q)	Q 矩阵	运动模型不准确带来的不确定度
观测噪声	v ~ N(0, R)	R 矩阵	传感器测量误差带来的不确定度
卡尔曼增益	K	K = Σ⁻Hᵀ(HΣ⁻Hᵀ+R)⁻¹	决定更信预测还是更信观测

五、案例：移动机器人一维位置估计

假设机器人沿直线运动，状态为位置 x，初始估计 μ₀ = 0 m，Σ₀ = 1.0 m²。控制量 u = 1.0 m，过程噪声 Q = 0.1 m²；传感器观测 z = 1.8 m，观测噪声 R = 0.4 m²。

预测：
μ⁻ = 0 + 1.0 = 1.0 m
Σ⁻ = 1.0 + 0.1 = 1.1 m²

更新：
K = 1.1 / (1.1 + 0.4) ≈ 0.733
μ = 1.0 + 0.733 × (1.8 - 1.0) ≈ 1.586 m
Σ = (1 - 0.733) × 1.1 ≈ 0.294 m²

可以看到，更新后协方差从 1.1 降到 0.294，说明观测有效降低了不确定度；同时估计值从预测的 1.0 被拉向观测的 1.8，但并未完全等于观测，因为观测本身也有噪声。

卡尔曼增益 K 是“预测”与“观测”之间的权重：R 越大，K 越小，系统越相信预测；Σ⁻ 越大，K 越大，系统越相信观测。

六、常见误区与复盘

误区一：把高斯假设当成绝对真理。真实传感器可能存在多峰、偏置或重尾噪声，此时 EKF 可能发散，需要引入鲁棒损失或切换滤波方法。
误区二：Q 和 R 随便填。Q 过小会导致滤波器过度自信、响应迟钝；R 过小会让观测噪声被放大，估计抖动。二者需要根据实际传感器和运动模型标定。
误区三：忽略协方差非对角线项。在强耦合系统中，忽略相关性会导致增益计算错误，估计精度下降。
误区四：认为贝叶斯滤波只能用于高斯系统。粒子滤波、直方图滤波同样基于贝叶斯递推，只是用不同方式表示后验分布。

工程调试建议：先固定 Q、R 做离线回放，观察新息 z - h(μ⁻) 是否零均值、协方差是否与 HΣ⁻Hᵀ + R 一致；若新息持续偏置，优先检查观测模型和坐标系，而不是盲目调大 R。

掌握随机变量、高斯分布、条件概率、贝叶斯滤波、期望与协方差这五个概念后，EKF 的预测与更新公式就不再是“背下来的矩阵运算”，而是一条清晰的概率推理链：用先验描述已知，用似然引入观测，用后验修正状态，并用协方差量化我们到底有多确定。