3、数据融合基础数学：概率论、贝叶斯公式、高斯分布、协方差矩阵、卡尔曼滤波基础、最小二乘估计

多传感器数据融合的本质，是在不确定信息环境中进行最优推断。无论是激光雷达与相机的目标级融合，还是IMU与GPS的组合导航，其底层都依赖一套统一的数学语言：用概率分布描述不确定性，用贝叶斯公式实现信息更新，用高斯模型简化计算，用协方差矩阵刻画相关性，用卡尔曼滤波或最小二乘完成状态估计。本章将系统梳理这六大数学基石，并重点阐明它们之间的逻辑递进关系——从概率公理出发，逐步构建起可用于工程实现的融合算法框架。

3.1 概率论与贝叶斯公式：融合推理的基石

核心原理

多传感器融合面临的核心挑战是：每个传感器都提供关于同一目标的不完整、带噪声的观测。概率论为这种不确定性提供了严格的量化框架。设传感器A的观测为 \(z_A\)，传感器B的观测为 \(z_B\)，目标状态为 \(x\)，则融合问题可表述为：已知 \(z_A\) 和 \(z_B\)，求后验概率 \(P(x|z_A, z_B)\)。贝叶斯公式给出了精确的求解路径：

P(x|z_A, z_B) = P(z_A, z_B|x) · P(x) / P(z_A, z_B)

其中 \(P(x)\) 为先验分布（融合前对目标状态的已有认知），\(P(z_A, z_B|x)\) 为似然函数（在给定状态下，各传感器观测的联合概率），分母为归一化常数。当传感器观测条件独立时，似然函数可分解为 \(P(z_A|x)·P(z_B|x)\)，这极大简化了计算。

判断方法与操作流程

• 先验选择：若无历史信息，采用无信息先验（如均匀分布）；若有历史轨迹，采用上一时刻后验作为当前先验。
• 似然建模：根据传感器噪声特性建立观测模型。例如，雷达测距噪声通常建模为零均值高斯分布，相机像素误差可建模为泊松分布。
• 后验计算：通过贝叶斯公式融合多源信息，得到综合后验分布。
• 决策输出：取后验最大（MAP估计）或后验均值（MMSE估计）作为融合结果。

案例拆解：雷达与摄像头目标关联

某L2级辅助驾驶系统需融合前向毫米波雷达和单目摄像头数据。雷达输出目标距离 \(r=50m\)，方差 \(\sigma_r^2=1m^2\)；摄像头输出目标横向位置 \(y=1.8m\)，方差 \(\sigma_y^2=0.25m^2\)。两传感器均检测到同一目标，但雷达报告距离52m，摄像头报告横向位置2.0m。通过贝叶斯融合，后验均值为：

μ_fused = (σ_y²·μ_r + σ_r²·μ_y) / (σ_r² + σ_y²)
        = (0.25×52 + 1×2.0) / (1 + 0.25) = 42m（距离方向）

融合后距离为50.4m，方差缩小至0.8m²，比单一传感器精度提升约20%。该案例表明，贝叶斯融合能根据各传感器置信度自动加权，实现优于任何单一传感器的估计精度。

常见误区与复盘

3.2 高斯分布与协方差矩阵：多维不确定性的紧凑表达

核心原理

高斯分布是连续随机变量中使用最广泛的概率模型，其概率密度函数为：

p(x) = (1/√(2π)^d|Σ|) · exp(-½(x-μ)ᵀΣ⁻¹(x-μ))

其中 \(μ\) 为均值向量，\(Σ\) 为协方差矩阵。高斯分布具有两个关键性质：线性变换封闭性（高斯变量经线性变换仍为高斯）和边缘化/条件化封闭性（高斯的边缘分布和条件分布仍为高斯）。这两个性质使得高斯分布在多传感器融合中成为计算上最友好的模型。

协方差矩阵 \(Σ\) 的对角线元素表示各维度的方差，非对角线元素表示维度间的相关性。例如，在GPS/IMU融合中，位置和速度的协方差矩阵为：

Σ = [[σ_x², σ_xv],
     [σ_xv, σ_v²]]

其中 \(\sigma_{xv}\) 表示位置与速度的耦合程度——IMU积分导致位置误差与速度误差强相关。

操作流程与判断方法

步骤	操作内容	关键公式/工具
1. 状态定义	确定融合状态向量（位置、速度、姿态等）	x = [px, py, vx, vy]ᵀ
2. 协方差初始化	根据传感器精度设定初始方差	Σ₀ = diag(σ₁², σ₂², ...)
3. 相关性分析	计算状态量间的相关系数	ρ = σ_ij / (σ_i·σ_j)
4. 协方差传播	通过状态转移矩阵传播不确定性	Σₖ = FΣₖ₋₁Fᵀ + Q

案例拆解：GPS/IMU组合导航中的协方差演化

某无人机组合导航系统，GPS更新频率1Hz，IMU更新频率100Hz。初始位置方差 \(\sigma_p^2=1m^2\)，速度方差 \(\sigma_v^2=0.01(m/s)^2\)。IMU积分1秒后，位置方差传播为：

σ_p²(t+1) = σ_p²(t) + σ_v²(t)·Δt² + 2·σ_pv·Δt + Q_p
           = 1 + 0.01×1 + 2×0.01×1 + 0.1 = 1.13m²

此时GPS观测到达，通过卡尔曼更新，位置方差降至0.5m²。该过程展示了协方差矩阵如何精确刻画误差随时间的累积与修正。

常见误区与总结

3.3 卡尔曼滤波基础：递推融合的黄金标准

核心原理

卡尔曼滤波是贝叶斯滤波在高斯线性系统中的闭式解。其核心思想是：在预测阶段，利用系统模型传播状态和协方差；在更新阶段，利用传感器观测修正预测。基本方程为：

预测：x̂ₖ⁻ = Fx̂ₖ₋₁ + Buₖ
      Pₖ⁻ = FPₖ₋₁Fᵀ + Q
更新：Kₖ = Pₖ⁻Hᵀ(HPₖ⁻Hᵀ + R)⁻¹
      x̂ₖ = x̂ₖ⁻ + Kₖ(zₖ - Hx̂ₖ⁻)
      Pₖ = (I - KₖH)Pₖ⁻

卡尔曼增益 \(K\) 决定了预测与观测的权重分配：当观测噪声R小（传感器精度高）时，K增大，更信任观测；当过程噪声Q小（模型准确）时，K减小，更信任预测。

操作流程与实现要点

1. 系统建模：定义状态转移矩阵F、控制矩阵B、观测矩阵H。
2. 噪声标定：通过实验确定过程噪声协方差Q和观测噪声协方差R。
3. 初始化：设定初始状态x̂₀和协方差P₀。
4. 递推循环：按预测-更新顺序迭代执行。

案例拆解：激光雷达与IMU融合定位

某AGV小车使用2D激光雷达（10Hz）和IMU（100Hz）进行定位。激光雷达测距精度±2cm，IMU航迹推算漂移约0.5%/s。融合系统状态为[x, y, θ]ᵀ。IMU提供高频预测，但θ漂移导致位置误差随时间增长；激光雷达每次扫描提供位置修正。实验数据显示：纯IMU 30秒后位置误差达1.5m，纯激光雷达因扫描匹配失败有5%的概率丢失定位；融合后系统在30秒内最大误差不超过0.1m，且无定位丢失。卡尔曼滤波在此场景中不仅提高了精度，更增强了鲁棒性。

常见误区与复盘

3.4 最小二乘估计：批处理融合的经典方法

核心原理

最小二乘估计（Least Squares, LS）是另一种重要的融合方法，特别适用于离线批处理或静态参数估计场景。其核心思想是：找到使预测残差平方和最小的状态估计值：

x̂_LS = argmin_x ||z - Hx||² = (HᵀH)⁻¹Hᵀz

当考虑噪声协方差R时，加权最小二乘（WLS）为：

x̂_WLS = (HᵀR⁻¹H)⁻¹HᵀR⁻¹z

WLS与卡尔曼滤波的稳态解在数学上等价，但卡尔曼滤波是递推实现，WLS是批处理实现。在多传感器标定、多站定位等场景中，WLS因其简单性和全局最优性而被广泛使用。

判断方法与操作流程

场景特征	推荐方法	原因
在线实时融合	卡尔曼滤波	递推计算，内存占用小
离线标定/校准	最小二乘	利用全部数据，全局最优
传感器数量少且静态	加权最小二乘	实现简单，可解释性强
动态系统且非线性	EKF/UKF	处理非线性传播

案例拆解：多基站TDOA定位

某室内定位系统采用4个UWB基站，通过到达时间差（TDOA）定位标签位置。设标签位置为(x, y)，基站i坐标为(xᵢ, yᵢ)，测得标签到基站i与基站1的距离差为dᵢ₁。构建线性方程组：

2(xᵢ-x₁)x + 2(yᵢ-y₁)y = dᵢ₁² + xᵢ²+yᵢ² - x₁²-y₁²

使用加权最小二乘求解，权重矩阵R由TDOA测量噪声决定。实测中，4基站WLS定位精度达±10cm，优于任意3基站组合的±25cm。该案例展示了最小二乘在多源冗余观测中的融合优势。

常见误区与总结

3.5 数学工具的综合运用与选型策略

在实际多传感器融合系统中，上述数学工具并非孤立使用，而是形成互补。卡尔曼滤波适用于在线递推场景，最小二乘适用于离线标定，贝叶斯公式提供了统一的理论框架，高斯分布和协方差矩阵则是贯穿始终的数学语言。选型时需考虑：

• 实时性要求：在线系统优先选择卡尔曼滤波及其变体。
• 线性程度：强非线性系统需使用粒子滤波或无迹变换。
• 数据量：批处理场景（如传感器外参标定）使用最小二乘。
• 鲁棒性需求：存在粗差时需引入鲁棒估计方法。