目录
粒子滤波的主要流程可以分为以下 5 个步骤：
-
初始化（Initialization）
-
生成 N 个粒子，每个粒子代表 状态空间中的一个可能位置，并初始化权重。
-
初始状态可能来自先验分布，或者均匀分布。
-
-
预测（Prediction, 运动更新）
-
使用系统的运动模型（如卡尔曼运动模型、惯性导航）来 预测粒子的新状态。
-
由于运动不确定性，加入 随机噪声（如高斯噪声） 以模拟真实运动。
-
-
更新（Update, 观测更新）
-
计算每个粒子的观测概率（即权重），权重表示粒子与观测数据的匹配程度，通常用误差大小来映射权重大小。
-
权重越大，说明该粒子更可能代表真实状态。
-
-
重采样（Resampling）
-
高权重粒子有更高的概率被保留，而低权重粒子可能被淘汰。
-
采用 低方差重采样（Low Variance Resampling） 或 多项式重采样（Multinomial Resampling）。
-
-
估计（Estimate）
-
计算最终的状态估计（通常使用加权均值）。
-
进入下一轮循环，重复 预测 → 更新 → 重采样 过程
-
粒子滤波代码demo
#include <iostream>
#include <vector>
#include <random>
#include <cmath>
#include <algorithm>
using namespace std;
// 机器人状态 (x, y, 方向theta)
struct Particle {
double x, y, theta, weight;
};
// 生成高斯噪声
double gaussian(double mean, double std_dev) {
random_device rd;
mt19937 gen(rd());
normal_distribution<double> dist(mean, std_dev);
return dist(gen);
}
// 运动模型: 更新粒子位置 (假设噪声影响)
void motionUpdate(vector<Particle>& particles, double delta_t, double velocity, double yaw_rate) {
for (auto& p : particles) {
if (fabs(yaw_rate) > 1e-5) {
p.x += (velocity / yaw_rate) * (sin(p.theta + yaw_rate * delta_t) - sin(p.theta));
p.y += (velocity / yaw_rate) * (cos(p.theta) - cos(p.theta + yaw_rate * delta_t));
} else {
p.x += velocity * delta_t * cos(p.theta);
p.y += velocity * delta_t * sin(p.theta);
}
p.theta += yaw_rate * delta_t;
// 加噪声
p.x += gaussian(0.0, 0.1);
p.y += gaussian(0.0, 0.1);
p.theta += gaussian(0.0, 0.01);
}
}
// 计算粒子权重 (模拟传感器测量，如 GPS)
void updateWeights(vector<Particle>& particles, double sensor_x, double sensor_y) {
for (auto& p : particles) {
double error_x = sensor_x - p.x;
double error_y = sensor_y - p.y;
p.weight = exp(-0.5 * (error_x * error_x + error_y * error_y)); // 简单高斯权重
}
}
// 归一化权重
void normalizeWeights(vector<Particle>& particles) {
double sum_weights = 0.0;
for (const auto& p : particles) sum_weights += p.weight;
for (auto& p : particles) p.weight /= sum_weights;
}
// 低方差重采样 (Resampling Wheel)
vector<Particle> resampleParticles(const vector<Particle>& particles) {
vector<Particle> new_particles;
int N = particles.size();
random_device rd;
mt19937 gen(rd());
uniform_real_distribution<double> dist(0.0, 1.0);
int index = dist(gen) * N;
double beta = 0.0;
double max_weight = 0.0;
for (const auto& p : particles)
if (p.weight > max_weight) max_weight = p.weight;
for (int i = 0; i < N; i++) {
beta += dist(gen) * 2.0 * max_weight;
while (beta > particles[index].weight) {
beta -= particles[index].weight;
index = (index + 1) % N;
}
new_particles.push_back(particles[index]);
}
return new_particles;
}
// 估计最终位置
Particle estimatePosition(const vector<Particle>& particles) {
double sum_x = 0.0, sum_y = 0.0, sum_theta = 0.0;
for (const auto& p : particles) {
sum_x += p.x * p.weight;
sum_y += p.y * p.weight;
sum_theta += p.theta * p.weight;
}
return {sum_x, sum_y, sum_theta, 1.0};
}
int main() {
int num_particles = 100; // 粒子数
vector<Particle> particles(num_particles);
// 初始化粒子 (假设初始位置在(0,0)，角度随机)
for (auto& p : particles) {
p.x = gaussian(0.0, 1.0);
p.y = gaussian(0.0, 1.0);
p.theta = gaussian(0.0, 0.1);
p.weight = 1.0 / num_particles;
}
// 模拟运动 & 传感器更新
double velocity = 1.0; // 移动速度
double yaw_rate = 0.1; // 角速度
for (int t = 0; t < 10; t++) {
motionUpdate(particles, 1.0, velocity, yaw_rate);
updateWeights(particles, 10.0, 10.0); // 传感器测得目标位置(10,10)
normalizeWeights(particles);
particles = resampleParticles(particles);
Particle estimated = estimatePosition(particles);
cout << "Time: " << t << " Estimated Position: (" << estimated.x << ", " << estimated.y << ")\n";
}
return 0;
}
粒子滤波相比较于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 计算量低，适合用在自动驾驶场景。
-
扫一扫
8099
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
