【直观理解】粒子滤波原理及实现

该博文集成了几个重要的参考博客，首先感谢这些博主的讲解和实现，因此是转载，不是原创。

一. 首先从通俗易懂的层面来理解一下粒子滤波，主要是博主(饮水思源)的博客。

粒子滤波可以先分为几个主要的阶段：初始化阶段 ---> 预测阶段--->矫正阶段--->重采样--->滤波

初始化阶段：主要就是选定粒子数量。也就是博主所说的放狗去搜索目标；放狗的方式有很多中，一种是让他们均匀分布，第二种是让他们按照高斯分布，即可能性大的地方就多放一点。一般来讲，比较简单的实现都是先让粒子们均匀分布在一个区域，但是在实际应用中，比如目标跟踪，SLAM中的特征跟踪都是先让这些粒子们呈高斯分布。

预测阶段：说的是预测，其实这个阶段就是对粒子们的返回数据进行分析，比如在目标跟踪中，我的特征是一个向量，那么每个粒子（狗狗）就会返回一个同样大小的向量回来，那么我可以计算这些向量之间的相似度；当然在实际应用中，为了方便比较，所有粒子的相似度之和最好等于1，因此我们需要对这些返回的相似度做归一化处理。

决策阶段：在这个阶段，首先要知道每个狗狗身上都带着什么，狗狗看到的东西，以及狗狗们的可信度；对应到粒子上也就是每个粒子的返回值(或返回向量)以及这个返回值的置信度；当所有的狗狗们都带着消息过来时，主要就要有所选择，这个部分类似于决策树，可以选择相信置信度最大的分支，也可以考虑所有分支的信息；粒子滤波在这里使用的就是第二种，综合考虑所有的信息，那么最终的结果，也就是预测值x_hat就是这些信息×置信度之和；如果你需要预测的变量是坐标，那么预测值x_hat = p1*x1 + p2*x2+...pn*xn; x1..xn是每个狗狗返回的坐标值，p1...pn是每个狗狗消息的可信度；

重采样阶段：在上一步中得到的数值其实还是有很大的误差，所以接下来要做的是进一步的估计，依然是让狗狗们去搜索信息，但是怎样去分布狗狗们呢？还是按照之前的那种均匀分布吗？这次肯定要换别的方法，也就是按照狗狗的可信度，可信度高的狗狗所报告的位置附近就多派一点狗狗过去，而其他可信度比较低的地方，就少派一点过去；也就是博主们所说的根据重要性重采样；Sampling Importance Resampling；

最后，可以再次重复地进行决策阶段和重采样阶段，直到最终的狗狗们的反馈结果符合条件(误差值较小等等)，这个时候，根据权重和置信度计算出来的值，也就是我们最终的预测值；

接下来看一个博主的matlab实现：

clc;
clear all;
close all;
x = 0; %初始值
R = 1;
Q = 1;
tf = 100; %跟踪时长
N = 100;  %粒子个数
P = 2;
xhatPart = x;
for i = 1 : N    xpart(i) = x + sqrt(P) * randn;%初始状态服从0均值，方差为sqrt(P)的高斯分布
end
xArr = [x];
yArr = [x^2 / 20 + sqrt(R) * randn];
xhatArr = [x];
PArr = [P];
xhatPartArr = [xhatPart];
for k = 1 : tf    x = 0.5 * x + 25 * x / (1 + x^2) + 8 * cos(1.2*(k-1)) + sqrt(Q) * randn; %k时刻真实值
y = x^2 / 20 + sqrt(R) * randn;  %k时刻观测值for i = 1 : Nxpartminus(i) = 0.5 * xpart(i) + 25 * xpart(i) / (1 + xpart(i)^2) ...+ 8 * cos(1.2*(k-1)) + sqrt(Q) * randn;%采样获得N个粒子ypart = xpartminus(i)^2 / 20;%每个粒子对应观测值vhat = y - ypart;%与真实观测之间的似然q(i) = (1 / sqrt(R) / sqrt(2*pi)) * exp(-vhat^2 / 2 / R); %每个粒子的似然即相似度endqsum = sum(q);
for i = 1 : Nq(i) = q(i) / qsum;%权值归一化
end
%----重采样阶段for i = 1 : N %根据权值重新采样u = rand;qtempsum = 0;for j = 1 : Nqtempsum = qtempsum + q(j);if qtempsum >= uxpart(i) = xpartminus(j);break;endendend
xhatPart = mean(xpart);
%最后的状态估计值即为N个粒子的平均值，这里经过重新采样后各个粒子的权值相同
xArr = [xArr x];
yArr = [yArr y];
% xhatArr = [xhatArr xhat];
PArr = [PArr P];
xhatPartArr = [xhatPartArr xhatPart];
end
t = 0 : tf;
figure;
plot(t, xArr, 'b-.', t, xhatPartArr, 'k-');
legend('Real Value','Estimated Value');
set(gca,'FontSize',10);
xlabel('time step');
ylabel('state');
title('Particle filter')
xhatRMS = sqrt((norm(xArr - xhatArr))^2 / tf);
xhatPartRMS = sqrt((norm(xArr - xhatPartArr))^2 / tf);
figure;
plot(t,abs(xArr-xhatPartArr),'b');
title('The error of PF')

【直观理解】粒子滤波原理及实现相关推荐

崔岩的笔记——粒子滤波原理及应用（3）粒子滤波原理及算法流程
崔岩的笔记--粒子滤波原理及应用(1)概率论与数理统计基础_今天也是睡觉的一天的博客-CSDN博客崔岩的笔记--粒子滤波原理及应用(2)蒙特卡洛法与贝叶斯滤波_今天也是睡觉的一天的博客-CSDN博客 ...
卡尔曼滤波、扩展卡尔曼滤波、无迹卡尔曼滤波以及粒子滤波原理
所有滤波问题其实都是求感兴趣的状态的后验概率分布,只是由于针对特定条件的不同,可通过求解递推贝叶斯公式获得后验概率的解析解(KF.EKF.UKF),也可通过大数统计平均求期望的方法来获得后验概率(PF ...
粒子滤波到底是怎么得到的？
一.前言粒子滤波(particle filter)是一种常见的滤波算法,广泛应用于目标跟踪.移动机器人等领域.网络上有不少关于粒子滤波的资料,但大多是直接给出了粒子滤波的相关公式和证明,或较为直观上 ...
目标跟踪-粒子滤波算法
http://blog.csdn.net/hujingshuang/article/details/45535423 前言: 粒子滤波广泛的应用于目标跟踪,粒子滤波器是一种序列蒙特卡罗滤波方法,其实质 ...
【多源融合】滤波融合之卡尔曼滤波与粒子滤波
多源融合之滤波对比最近在看CSDN滤波方面的知识,但是很多都是激光雷达.视觉.信号处理等方面的大佬们总结的,我也做过卡尔曼滤波和粒子滤波.博主大地测量学与测量工程专业,主要做定位方面的内容,现在从我 ...
用相似矩阵的几何意义直观理解PCA降维方法
PCA(主成分分析)是降维中最经典的方法,其推导求解的常用两种方法包括最大方差理论(样本点到超平面的投影都尽可能分开)以及最小平方误差理论(样本点到超平面的距离都足够近),以上两种方法都需要进行严格意 ...
蒙特卡洛粒子滤波定位算法_蒙特卡罗定位算法(基于粒子滤波的定位算法) ——原理、理解与仿真...
1 算法原理 1.1 机器人定位问题关于机器人定位,有三大问题,它们分别是: (1)"全局定位":指初始位置未知,机器人靠自身运动确定自己在地图中的位姿. (2)"位姿 ...
Dijkstra、RRT两类路径规划算法原理的直观理解
在路径规划的算法里,有两大类算法是很常用的,一类是基于搜索和图的Dijkstra算法,还有一类是基于采样的RRT算法.本文对其算法原理进行简单的理解,力图生动的展示枯燥的数学公式背后精彩的思想. Di ...
双边滤波原理与参数的理解+双边滤波matlab实现
基本思路高斯滤波是一种常用而且简单的降噪算法.但其缺点也很明显,就是会不加区分地将噪声与图像边缘等细节一起平滑处理. 而双边滤波是基于高斯滤波进行改进的方法.在高斯滤波的基础上,双边滤波引入了像素值 ...

【直观理解】粒子滤波原理及实现

【直观理解】粒子滤波原理及实现相关推荐

最新文章

热门文章

【直观理解】粒子滤波 原理及实现

【直观理解】粒子滤波 原理及实现相关推荐

最新文章

热门文章

【直观理解】粒子滤波原理及实现

【直观理解】粒子滤波原理及实现相关推荐