第2章多传感器融合基础

多传感器融合基础：从卡尔曼到粒子滤波

做导航这么多年，我越来越觉得——传感器融合不是选择题，而是必答题。你想想看，单个传感器总有掉链子的时候：GPS被高楼遮挡、IMU随时间漂移、激光雷达在雨雪天失效。怎么办？

答案就是：把多个传感器的数据揉在一起，取长补短。而这背后最核心的工具，就是今天我们要聊的这四种滤波方法。

核心观点：滤波的本质不是“消除噪声”，而是“在不确定性中做最优估计”。

1. 卡尔曼滤波：一切的基础

先说卡尔曼滤波。说实话，我第一次接触它是在研究生做无人机定位的时候。当时觉得这公式怎么这么绕？后来做多了才发现——它本质上就是个“预测-修正”的循环。

核心思想就两句话：

预测：根据上一时刻的状态，猜一下当前时刻的状态
更新：用当前时刻的观测值，修正这个猜测

我习惯用一个比喻来理解：你闭着眼睛走路，每走一步你大概知道自己在哪（预测），但偶尔睁开眼看到路标（观测），你就调整一下位置估计。卡尔曼滤波干的就是这个事。

关键公式（离散形式）：

预测步：
  x̂ₖ|ₖ₋₁ = A·x̂ₖ₋₁|ₖ₋₁ + B·uₖ
  Pₖ|ₖ₋₁ = A·Pₖ₋₁|ₖ₋₁·Aᵀ + Q

更新步：
  Kₖ = Pₖ|ₖ₋₁·Hᵀ·(H·Pₖ|ₖ₋₁·Hᵀ + R)⁻¹
  x̂ₖ|ₖ = x̂ₖ|ₖ₋₁ + Kₖ·(zₖ - H·x̂ₖ|ₖ₋₁)
  Pₖ|ₖ = (I - Kₖ·H)·Pₖ|ₖ₋₁

这里面的A、H、Q、R，说白了就是你对系统和传感器的建模精度。我在项目中踩过最大的坑就是——Q和R调不好，滤波效果还不如不用。

个人经验：调Q和R矩阵时，我一般先让系统跑一段开环，统计一下预测误差的方差作为Q的初值。R则直接看传感器数据手册的噪声指标，再乘以1.5~2倍作为安全余量。

2. 扩展卡尔曼滤波：对付非线性

卡尔曼滤波有个硬伤——它假设系统是线性的。但现实世界哪有那么多线性？你想想看，无人机做姿态估计，旋转矩阵是非线性的；车辆做转弯，运动模型也是非线性的。

EKF的思路很直接：既然非线性不好处理，那我就把它近似成线性的。怎么近似？用泰勒展开，取一阶项。

具体做法：

在估计点附近，对非线性函数f(x)做一阶泰勒展开
用展开后的雅可比矩阵代替原来的A和H
剩下的步骤和标准卡尔曼滤波一模一样

注意：EKF的线性化误差会随着非线性程度增加而急剧增大。我曾经在一个强非线性系统上硬套EKF，结果滤波直接发散。后来换成UKF才解决问题。

EKF的代码实现其实不复杂，核心就是求雅可比矩阵：

def ekf_predict(x, P, f, F, Q):
    # f: 状态转移函数（非线性）
    # F: f的雅可比矩阵
    x_pred = f(x)
    P_pred = F @ P @ F.T + Q
    return x_pred, P_pred

def ekf_update(x_pred, P_pred, z, h, H, R):
    # h: 观测函数（非线性）
    # H: h的雅可比矩阵
    y = z - h(x_pred)          # 残差
    S = H @ P_pred @ H.T + R   # 残差协方差
    K = P_pred @ H.T @ np.linalg.inv(S)  # 卡尔曼增益
    x_upd = x_pred + K @ y
    P_upd = (np.eye(len(x)) - K @ H) @ P_pred
    return x_upd, P_upd

3. 无迹卡尔曼滤波：不线性化也能行

EKF的线性化近似让我一直不太放心。你想想，一阶泰勒展开相当于用切线代替曲线，如果曲率大，误差就大了。

UKF换了个思路——我不去近似函数，而是去近似概率分布。具体来说：

在状态估计点周围，选取一组确定的采样点（称为Sigma点）
把这些Sigma点通过非线性函数传播
用传播后的点重新计算均值和协方差

这就是所谓的无迹变换。它不需要求导，也不需要雅可比矩阵，而且对非线性函数的近似精度能达到三阶泰勒展开的水平。

Sigma点的选取（n维状态，2n+1个点）：

χ⁰ = x̄
χⁱ = x̄ + (√((n+λ)P))ᵢ,  i=1,...,n
χⁱ = x̄ - (√((n+λ)P))ᵢ₋ₙ,  i=n+1,...,2n

我在做车辆定位时对比过EKF和UKF。同样的非线性系统，UKF的定位精度比EKF高了约15%，而且没有出现发散的情况。代价就是计算量稍微大一点——但现在的嵌入式处理器完全扛得住。

我的建议：如果你的系统非线性程度不高，EKF够用。但如果系统强非线性（比如大角度姿态估计、带回环的SLAM），直接上UKF，省心。

4. 粒子滤波：不假设任何分布

前面三种方法都有一个共同假设——噪声是高斯分布的。但现实中有很多情况不满足这个假设。比如：

多模态分布（机器人可能在走廊的多个位置）
非高斯噪声（传感器受突发干扰）
高度非线性的状态空间

这时候，粒子滤波就派上用场了。

粒子滤波的核心思想是：用一堆带权重的随机样本（粒子）来近似后验概率分布。粒子越多，近似越精确。

基本流程：

初始化：在状态空间撒一批粒子，权重均匀
预测：每个粒子根据系统模型向前传播
更新：根据观测值更新每个粒子的权重
重采样：权重高的粒子多复制，权重低的粒子淘汰
回到步骤2，循环

注意：粒子滤波的计算量跟粒子数成正比。我做过一个项目，用1000个粒子做室内定位，在树莓派上只能跑到10Hz。后来优化到500个粒子，勉强到20Hz。所以粒子数不是越多越好，够用就行。

重采样是粒子滤波的关键。我见过很多新手直接复制高权重粒子，结果所有粒子很快坍缩到同一个点——这叫粒子贫化。解决办法是加入少量随机扰动，保持粒子的多样性。

四种方法的对比

方法	适用场景	优点	缺点	我推荐的使用场景
KF	线性系统，高斯噪声	计算快，实现简单	不能处理非线性	GPS+IMU松耦合
EKF	弱非线性系统	成熟，应用广泛	线性化误差，需要雅可比	视觉SLAM前端
UKF	强非线性系统	精度高，无需求导	计算量稍大	车辆状态估计
PF	非高斯，多模态	任意分布都能近似	计算量大，粒子贫化	全局定位，绑架恢复

选型口诀：线性用KF，弱非线性用EKF，强非线性用UKF，非高斯多模态用PF。别一上来就上粒子滤波，杀鸡焉用牛刀。

好了，这一章的内容就到这里。四种滤波方法各有各的脾气，关键是要理解它们的核心假设和适用边界。我在实际项目中，经常是先用KF搭个基线，然后根据系统表现逐步升级到EKF或UKF。至于粒子滤波——嗯，那是最后的大招，轻易不用，用了就要确保计算资源够用。

公众号:蓝海资料掘金营，微信deep3321

📚 课程目录

多传感器融合基础：从卡尔曼到粒子滤波

1. 卡尔曼滤波：一切的基础

2. 扩展卡尔曼滤波：对付非线性

3. 无迹卡尔曼滤波：不线性化也能行

4. 粒子滤波：不假设任何分布

四种方法的对比