从贝叶斯到粒子滤波：一张图看懂KF、EKF、UKF、PF、ESKF的本质区别与适用场景

从贝叶斯到粒子滤波：一张图看懂KF、EKF、UKF、PF、ESKF的本质区别与适用场景

在自动驾驶汽车感知周围环境时，传感器数据总是伴随着噪声和不确定性。工程师们需要一种方法，能够从这些嘈杂的数据中提取出真实的状态信息。这就是滤波算法大显身手的时刻——它们像一位经验丰富的侦探，从混乱的线索中还原案件真相。本文将带您深入探索五种主流滤波算法：卡尔曼滤波(KF)、扩展卡尔曼滤波(EKF)、无迹卡尔曼滤波(UKF)、粒子滤波(PF)和误差状态卡尔曼滤波(ESKF)，揭示它们如何在贝叶斯框架下各显神通。

1. 贝叶斯滤波：所有算法的共同语言

想象你是一位气象预报员，每天需要根据卫星云图、气压计读数等不确定信息预测明天的天气。贝叶斯定理就是你最强大的工具——它告诉我们如何用新证据更新已有信念。所有现代滤波算法本质上都是贝叶斯定理在不同场景下的具体实现。

贝叶斯滤波的核心公式可以表示为：

后验概率 ∝ 似然函数 × 先验概率

这个简洁的公式背后蕴含着深刻的哲学：我们对世界的认识应该随着新证据的出现而不断调整。在状态估计问题中，这转化为两个关键步骤：

1. 预测步骤：根据系统动力学模型预测下一时刻状态（先验）
2. 更新步骤：融合新观测数据修正预测（后验）

提示：贝叶斯滤波的美妙之处在于它提供了一个统一框架，不同算法只是在这个框架下对系统做了不同假设。

下表展示了五种算法在贝叶斯框架下的共性与特性：

2. 卡尔曼滤波家族：从KF到ESKF的进化之路

2.1 经典卡尔曼滤波(KF)：线性世界的完美解

1960年，Rudolf E. Kálmán发表了他的开创性工作，为控制工程带来了革命。KF假设系统具有两个关键特性：

• 线性动力学：状态转移和观测模型都必须是线性的
• 高斯噪声：过程噪声和观测噪声都服从正态分布

KF的五大方程构成了一个优雅的预测-更新循环：

# 预测步骤 x_pred = F * x_prev # 状态预测 P_pred = F * P_prev * F.T + Q # 协方差预测 # 更新步骤 K = P_pred * H.T * inv(H * P_pred * H.T + R) # 卡尔曼增益 x_updated = x_pred + K * (z - H * x_pred) # 状态更新 P_updated = (I - K * H) * P_pred # 协方差更新

注意：KF在满足其假设条件时能提供最优估计，但现实世界很少完全线性，这促使了各种非线性滤波算法的诞生。

2.2 扩展卡尔曼滤波(EKF)：非线性问题的局部线性解

当系统存在非线性时，EKF采用了一种直观的策略——在工作点附近进行泰勒展开线性化。这就像用许多小直线段来近似一条曲线。EKF的关键步骤包括：

1. 计算雅可比矩阵（一阶偏导数）
2. 在当前估计点线性化系统方程
3. 应用标准KF公式

EKF在以下场景表现良好：

• GPS/IMU融合定位
• 机器人里程计估计
• 化工过程控制

然而，EKF有两个主要局限：

• 线性化误差会随着非线性程度增加而变大
• 雅可比矩阵计算可能复杂且耗时

2.3 无迹卡尔曼滤波(UKF)：Sigma点的艺术

UKF采用了一种完全不同的思路——既然难以精确线性化，不如精心选择一组样本点（Sigma点）来捕捉概率分布的特征。这些Sigma点经过非线性变换后，能够更准确地保持分布的统计特性。

UKF的核心优势包括：

• 无需计算雅可比矩阵
• 对非线性系统有更好的近似
• 能精确捕获均值和协方差到三阶项

典型的Sigma点选择策略遵循以下规则：

χ[0] = x χ[i] = x + (√((n+λ)P))_i, i=1,...,n χ[i] = x - (√((n+λ)P))_{i-n}, i=n+1,...,2n

其中λ是缩放参数，n是状态维度。

2.4 误差状态卡尔曼滤波(ESKF)：间接估计的智慧

ESKF采用了一种独特的间接方法——不是直接估计系统状态，而是估计状态误差。这种方法特别适合具有以下特点的系统：

• 名义状态变化明显非线性
• 误差状态变化相对平缓
• 需要避免过参数化问题

ESKF的工作流程可分为三个阶段：

1. 名义状态预测：通过IMU积分等物理模型推进
2. 误差状态估计：使用标准KF估计小误差
3. 状态修正与重置：将误差补偿到名义状态后重置误差

这种方法在无人机和自动驾驶定位中特别流行，因为它：

• 避免万向节锁问题
• 保持误差状态接近线性
• 减少计算负担

3. 粒子滤波：非高斯世界的蒙特卡洛方法

当系统不仅非线性，而且噪声也不服从高斯分布时，PF提供了一种强大的解决方案。PF的基本思想是用一群"粒子"来近似概率分布，每个粒子代表系统可能处于的一个状态。

PF的核心步骤包括：

1. 初始化：从先验分布中采样N个粒子
2. 预测：根据系统模型传播每个粒子
3. 加权：根据观测似然为每个粒子分配权重
4. 重采样：按权重重新选择粒子，避免退化

一个简单的PF实现可能如下：

particles = initialize_particles() weights = np.ones(N)/N for t in range(time_steps): # 预测步骤 particles = system_model(particles, noise) # 更新权重 weights *= measurement_likelihood(observations[t], particles) weights /= np.sum(weights) # 归一化 # 重采样 indices = resample(weights) particles = particles[indices] weights = np.ones(N)/N

PF在以下领域表现出色：

• 金融时间序列预测
• 目标跟踪（特别是多模态分布）
• 机器人定位（ kidnapped robot问题）

但PF也有明显缺点：

• 计算成本随粒子数线性增长
• 高维空间需要大量粒子
• 重采样可能导致样本贫化

4. 算法选型指南：从理论到实践

选择滤波算法就像选择工具——没有最好的，只有最适合的。下面我们通过几个典型应用场景来说明如何做出明智选择。

4.1 自动驾驶多传感器融合

在自动驾驶定位系统中，通常需要融合GPS、IMU、轮速计和激光雷达数据。这种场景的特点是：

• 系统高度非线性（特别是姿态动力学）
• 实时性要求极高
• 传感器频率各不相同

推荐方案：ESKF + UKF组合

• 使用ESKF处理高频IMU数据
• 用UKF融合低频GPS和激光雷达数据
• 误差状态保持在小范围内确保线性

4.2 机器人SLAM

同时定位与建图(SLAM)面临的主要挑战是：

• 环境特征提取的非线性
• 数据关联的不确定性
• 计算资源受限

推荐方案：UKF或稀疏PF

• 对于特征点较少的场景，UKF效率更高
• 对于高度非高斯环境（如动态障碍物多），使用PF
• 现代方案常结合图优化后端

4.3 金融时间序列预测

金融市场数据通常表现出：

• 非高斯噪声特性
• 厚尾分布
• 波动聚集现象

推荐方案：PF或PF与KF混合

• 使用PF捕捉非高斯特性
• 对高斯部分使用KF提高效率
• 粒子数需要足够反映分布特性

4.4 工业过程控制

化工、电力等工业过程的特点是：

• 模型相对明确
• 非线性程度中等
• 实时性要求高

推荐方案：EKF或UKF

• 模型明确时EKF表现良好
• 对强非线性环节使用UKF
• 可结合模型预测控制(MPC)

5. 前沿发展与实用技巧

5.1 混合滤波策略

在实际工程中，常常组合多种滤波算法以获得更好性能。一些常见策略包括：

• 层级滤波：不同时间尺度使用不同算法
• 并行滤波：运行多个滤波器后融合结果
• 切换机制：根据条件动态切换算法

例如，在无人机导航中可以采用：

1. 高频IMU数据：ESKF（1000Hz）
2. 视觉里程计：UKF（30Hz）
3. GPS定位：EKF（1Hz）

5.2 参数调优经验

滤波算法的性能很大程度上取决于参数选择。以下是一些实用建议：

• 过程噪声Q：通常需要比理论值稍大，以补偿模型误差
• 观测噪声R：可通过传感器标定获得初始值，再微调
• UKF参数：β=2，κ=3-n，α=0.1是良好起点
• 粒子数：从1000开始，根据效果增减

提示：噪声参数的对角元素比例比绝对值更重要，保持合理的信噪比是关键。

5.3 常见陷阱与解决方案

即使经验丰富的工程师也会遇到一些典型问题：

问题1：滤波器发散

• 可能原因：模型误差累积、噪声参数不当
• 解决方案：增加过程噪声、加入重置机制

问题2：估计结果过于平滑

• 可能原因：观测噪声设置过大
• 解决方案：重新标定传感器噪声特性

问题3：计算延迟

• 可能原因：算法复杂度高
• 解决方案：改用ESKF、降低UKF Sigma点数

问题4：粒子退化

• 可能原因：重采样过于频繁
• 解决方案：采用自适应重采样策略

在实际项目中，我经常发现初学者过度关注算法实现而忽视了模型本身的准确性。一个精确的物理模型配合简单的KF，往往胜过复杂的算法搭配粗糙的模型。滤波算法本质上是将模型知识与实际观测相结合的艺术，理解被估计系统的物理本质比选择算法更重要。