粒子滤波介绍

粒子滤波的主要流程可以分为以下 5 个步骤：

粒子滤波（PF） vs. ESKF（误差状态卡尔曼滤波）

粒子滤波的主要流程可以分为以下 5 个步骤：

初始化（Initialization）
- 生成 N 个粒子，每个粒子代表 状态空间中的一个可能位置，并初始化权重。
- 初始状态可能来自先验分布，或者均匀分布。
预测（Prediction, 运动更新）
- 使用系统的运动模型（如卡尔曼运动模型、惯性导航）来 预测粒子的新状态。
- 由于运动不确定性，加入 随机噪声（如高斯噪声） 以模拟真实运动。
更新（Update, 观测更新）
- 计算每个粒子的观测概率（即权重），权重表示粒子与观测数据的匹配程度，通常用误差大小来映射权重大小。
- 权重越大，说明该粒子更可能代表真实状态。
重采样（Resampling）
- 高权重粒子有更高的概率被保留，而低权重粒子可能被淘汰。
- 采用 低方差重采样（Low Variance Resampling） 或 多项式重采样（Multinomial Resampling）。
估计（Estimate）
- 计算最终的状态估计（通常使用加权均值）。
- 进入下一轮循环，重复 预测 → 更新 → 重采样 过程

粒子滤波相比较于ESKF的优缺点：

在 机器人定位、自动驾驶、SLAM 等任务中，粒子滤波（Particle Filter, PF） 和 误差状态卡尔曼滤波（Error-State Kalman Filter, ESKF） 是两种常见的状态估计算法。它们各有优缺点，适用于不同的应用场景。

1️⃣ 粒子滤波（PF）的特点

✅ 优点

✔ 适用于非线性、非高斯系统：

粒子滤波不假设系统是线性、高斯的，适用于高度非线性和非高斯噪声的环境。

✔ 能够处理多模态分布：

适用于存在多种可能状态的情况，例如多路径问题（机器人可能在多个位置）。

✔ 适用于全局定位（Global Localization）：

可以在初始位置未知时进行全局搜索，而EKF 只能用于增量更新。

✔ 可以直接结合传感器数据：

可以直接使用激光雷达（LiDAR）、视觉（Camera）、IMU 等传感器数据进行估计，而不需要严格的线性化处理。

❌ 缺点

✖ 计算量大，实时性差：

需要大量粒子来表示状态分布，计算复杂度 O(N)O(N)O(N) 较高，不适用于高频状态更新。

✖ 容易退化（Degeneracy）：

若粒子数不足，可能会导致低权重粒子占比过高，降低滤波效果。

需要低方差重采样来缓解退化问题。

✖ 难以调整粒子数：

粒子数目要根据问题复杂度调整，过少会影响精度，过多会降低计算效率。

2️⃣ 误差状态卡尔曼滤波（ESKF）的特点

✅ 优点

✔ 计算效率高，适用于高频更新：

计算复杂度 O(n2)O(n^2)O(n2)，远低于粒子滤波的 O(N)O(N)O(N)，适用于实时性要求高的应用（如自动驾驶）。

✔ 状态维度较高时表现优异：

适用于高维状态估计（如 IMU + 轮速 + GNSS 融合），不会因状态维度增加导致计算量急剧上升。

✔ 误差建模更加精确：

ESKF 采用误差状态方程，避免了直接估计全局状态的不稳定性，使得滤波更加稳定。

✔ 适用于增量式定位（Incremental Localization）：

适用于车辆 里程计（Odometry）+ IMU 的方案，例如惯性导航系统（INS）。

❌ 缺点

✖ 要求系统近似线性 & 高斯噪声：

适用于 高斯噪声和小范围非线性系统，但对于强非线性、高噪声系统，效果较差。

✖ 无法处理全局定位（Global Localization）：

只能进行增量更新，无法全局搜索。

如果初始位置不准，可能会发散。

✖ 难以处理多模态分布：

不能有效处理多种可能状态（如双重解），只能跟踪单个高斯分布的状态。

3️⃣ 什么时候选择 PF vs. ESKF？

需求	选择 PF	选择 ESKF
非线性系统	✅ 适用于强非线性	❌ 仅适用于小范围非线性
非高斯噪声	✅ 适用	❌ 不适用
全局定位（Global Localization）	✅ 适合（机器人初始化时位置未知）	❌ 只能做局部跟踪
计算资源受限	❌ 计算量大	✅ 计算效率高
实时性（高频更新）	❌ 更新较慢	✅ 适用于高频 IMU+里程计
状态维度较高（如IMU+LiDAR+GNSS融合）	❌ 计算量过大	✅ 适用于高维状态
多模态分布（如多路径问题）	✅ 适用于多种可能状态	❌ 只能跟踪单个解

4️⃣ 具体应用案例分析

✅ 适合使用 PF 的场景

机器人全球定位（Global Localization）
机器人不知道自己在哪，通过激光雷达、地图匹配来确定位置。
需要遍历整个状态空间，因此使用粒子滤波。
自动驾驶中的地图匹配（Map Matching）
车辆可能有多条可能行驶路线，需要考虑多个假设，因此适合用 PF。
激光雷达（LiDAR）+ 视觉（Camera）融合
LiDAR & Camera 数据通常具有非高斯噪声，适合用 PF 处理。
✅ 适合使用 ESKF 的场景
惯性导航（INS）+ GNSS 组合导航
- IMU 数据是高频更新，EKF 计算效率更高，适用于高维状态估计。
- 误差状态建模能够提高精度，适用于 IMU + GNSS + 轮速编码器 组合方案。
5️⃣ 结论
粒子滤波（PF）：适用于全局定位、多模态分布、非高斯噪声系统，但计算量较大，适合低频更新。
误差状态卡尔曼滤波（ESKF）：适用于增量式定位、高维状态、高频 IMU 更新，计算效率高，但难以处理全局定位和多模态分布。
🔹 一句话总结：
- 如果已知初始位置，且需要高频更新，选 ESKF。
- 如果初始位置未知，或可能有多个可能状态，选 PF。
- 若计算资源有限，优先选 ESKF。
- 如果非线性和非高斯问题较强，选 PF。
车辆定位（基于里程计 + IMU）
- 适用于 增量式定位（Incremental Localization）。
- ESKF 可以处理 IMU + 里程计（Odometry）+ GNSS 组合，提供高精度定位。
自动驾驶的高精度定位（High-Precision Localization）
- 如高精地图匹配（HD-Map Matching），基于 IMU + GNSS + 轮速。
- 需要实时性高、精度高的滤波方法，ESKF 计算量低，适合用在自动驾驶场景。