概率与统计基础：高斯分布、协方差矩阵、条件概率、贝叶斯公式在传感器融合中的角色

传感器融合的本质，是在不确定信息之间做最优折中。无论是GPS与惯性测量单元（IMU）的松耦合，还是视觉与激光雷达的紧耦合，每个传感器都只能提供对物理世界某个侧面的带噪声观测。要回答“当前系统状态最可能是什么”这个问题，必须依赖一套描述不确定性的数学语言。本章聚焦四个核心工具：高斯分布、协方差矩阵、条件概率与贝叶斯公式，并阐明它们在卡尔曼滤波框架中的具体角色。理解这些概念，是后续推导状态预测与量测更新方程的前提。

4.1 高斯分布：传感器噪声的数学画像

核心原理：高斯分布（正态分布）是描述传感器测量噪声最常用的概率模型。其概率密度函数为：

p(x) = (1 / (σ√(2π))) · exp(-(x-μ)² / (2σ²))

其中 μ 为均值（真值），σ 为标准差（噪声水平）。在传感器融合中，我们假设每个传感器的测量误差服从零均值高斯分布，即 μ=0，σ 由传感器数据手册或标定实验确定。这一假设的合理性在于：大量独立微小误差源叠加后趋于正态分布（中心极限定理），且高斯分布具有优良的数学性质——线性变换后仍为高斯，边缘分布和条件分布也保持高斯。

多维扩展：当系统状态包含位置、速度、姿态等多个变量时，需要使用多元高斯分布：

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

这里 Σ 为协方差矩阵，描述各维度之间的统计相关性。例如，车辆匀速行驶时，位置误差与速度误差并非独立——位置误差会随时间累积，导致两者正相关。

4.2 协方差矩阵：不确定性传播的引擎

核心原理：协方差矩阵 Σ 是多元高斯分布的核心参数，其对角线元素 σᵢ² 表示各状态变量的方差，非对角线元素 σᵢⱼ 表示变量间的协方差。在卡尔曼滤波中，协方差矩阵承担三个关键角色：

• 状态不确定性度量：P 矩阵（状态协方差）表示当前估计的置信度，P 越小说明估计越确定。
• 噪声传播载体：预测步骤中，P = F·P·Fᵀ + Q，其中 F 为状态转移矩阵，Q 为过程噪声协方差。这一公式将上一时刻的不确定性映射到当前时刻，并叠加系统自身的随机扰动。
• 融合权重计算：更新步骤中，卡尔曼增益 K = P·Hᵀ·(H·P·Hᵀ + R)⁻¹，其中 R 为量测噪声协方差。K 的大小决定了传感器量测与模型预测之间的信任比例。

判断方法：如何判断协方差矩阵设置是否合理？

1. 可观测性检查：如果 P 矩阵对角线元素持续增大而不收敛，说明系统状态不可观测或噪声参数设置过大。
2. 一致性检验：计算归一化估计误差平方（NEES），若其统计均值显著大于理论自由度，说明滤波器过于乐观（P 过小）；反之则过于保守。
3. 残差分析：量测残差（z - H·x̂）的协方差应与 S = H·P·Hᵀ + R 一致，若实际残差方差远大于 S，说明 R 被低估。

4.3 条件概率与贝叶斯公式：融合的数学骨架

核心原理：条件概率 P(A|B) 描述在事件 B 发生的条件下事件 A 的概率。在传感器融合中，最核心的条件概率是量测似然 p(z|x)——给定系统状态 x 时，传感器输出 z 的概率分布。这个似然函数直接来源于传感器噪声模型：若噪声为高斯，则 p(z|x) = N(z; Hx, R)。

贝叶斯公式将先验知识、似然函数和后验概率联系起来：

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

在滤波语境下：

• 先验 p(x)：来自状态预测步骤，即上一时刻后验经过状态转移模型传播后的结果。
• 似然 p(z|x)：当前传感器的量测模型，描述“如果状态是 x，那么观测到 z 的可能性”。
• 后验 p(x|z)：融合量测后的修正状态估计，即滤波器的输出。
• 归一化常数 p(z)：通常不需要显式计算，因为高斯分布下后验仍为高斯，只需匹配均值和协方差。

操作流程：卡尔曼滤波的递归更新本质上就是贝叶斯公式的迭代应用：

1. 预测（先验）：x̂⁻ = F·x̂ + B·u，P⁻ = F·P·Fᵀ + Q
2. 更新（后验）：K = P⁻·Hᵀ·(H·P⁻·Hᵀ + R)⁻¹，x̂ = x̂⁻ + K·(z - H·x̂⁻)，P = (I - K·H)·P⁻

其中 K 就是贝叶斯公式中“先验与似然权重”的具体体现。当量测噪声 R 很小（传感器很准）时，K 趋近于 1，滤波器几乎完全相信量测；当 R 很大时，K 趋近于 0，滤波器更依赖模型预测。

4.4 实战场景：多传感器融合中的贝叶斯递归

场景描述：考虑一个移动机器人定位问题，使用轮式里程计（提供速度信息）和单线激光雷达（提供位置观测）。系统状态 x = [px, py, vx, vy]ᵀ，状态转移模型为匀速模型：

F = [[1,0,Δt,0],
     [0,1,0,Δt],
     [0,0,1,0],
     [0,0,0,1]]

过程噪声 Q 反映加速度扰动，量测矩阵 H 将状态映射到激光雷达观测（位置）：

H = [[1,0,0,0],
     [0,1,0,0]]

量测噪声 R 由激光雷达测距精度决定（例如 σ=0.03m）。

分析判断过程：

• 初始时刻：机器人静止，先验 x̂₀ = [0,0,0,0]ᵀ，P₀ = diag(0.1², 0.1², 0.5², 0.5²)（位置较确定，速度不确定）。
• 第 1 秒：里程计测得速度 [0.5, 0] m/s，预测位置 [0.5, 0] m，P 增大（不确定性扩散）。
• 激光雷达观测到位置 [0.48, 0.02] m，计算残差 y = [0.48-0.5, 0.02-0] = [-0.02, 0.02] m。
• 计算卡尔曼增益 K，由于 R 较小（0.03²），K 接近 [0.9, 0; 0, 0.9; ...]，后验位置修正为 [0.482, 0.018] m。
• P 矩阵对角线元素减小，表示不确定性降低。

结果对比：若仅使用里程计，10 秒后位置误差累积到 1.5m；若仅使用激光雷达，单次观测噪声 0.03m 但更新频率低（10Hz）；融合后位置误差稳定在 0.05m 以内，且输出频率与里程计一致（100Hz）。

风险与代价：融合系统的脆弱点在于模型失配——如果机器人发生打滑（过程噪声 Q 被低估），滤波器会过度信任预测，导致量测被“忽略”。解决方法是增大 Q 或引入自适应噪声估计（如 Sage-Husa 滤波器）。此外，激光雷达在玻璃墙或强反射表面会产生异常量测（离群点），需通过马氏距离检验剔除。

4.5 总结：四个工具如何协同工作

高斯分布为所有不确定性提供了统一的数学语言；协方差矩阵量化了多维状态之间的相关性和置信度；条件概率建立了传感器观测与系统状态之间的桥梁；贝叶斯公式则提供了递归融合的框架。四者共同构成了卡尔曼滤波的理论基石：