粒子滤波（Particle filter）算法简介及MATLAB实现

粒子滤波是以贝叶斯推理（点击打开链接）和重要性采样为基本框架的。因此，想要掌握粒子滤波，对于上述两个基本内容必须有一个初步的了解。重要性采样呢，其实就是根据对粒子的信任程度添加不同的权重，添加权重的规则就是：对于我们信任度高的粒子，给它们添加的权重就相对大一些；否则，就加的权重小一些。根据权重的分布形式，实际上就是它与目标的相似程度。

粒子滤波的结构实际上就是加一层重要性采样思想在里面的蒙特卡罗方法（Monte Carlo method,即以某时间出现的频率来指代该事件的概率）。该方法的基本思想是用一组样本（或称粒子）来近似表示系统的后验概率分布，然后使用这一近似的表示来估计非线性系统的状态。采用此思想，在滤波过程中粒子滤波可以处理任意形式的概率，而不像Kalman老伯伯只能处理线性高斯分布的概率问题。粒子滤波的一大优势也在于此。

一、粒子滤波步骤：

1、初始状态：用大量粒子模拟X（t），粒子在空间内均匀分布；

2、预测阶段：根据状态转移方程，每一个粒子得到一个预测粒子；

3、校正阶段：对预测粒子进行评价，越接近于真实状态的粒子，其权重越大；

4、重采样：根据粒子权重对粒子进行筛选，筛选过程中，既要大量保留权重大的粒子，又要有一小部分权重小的粒子；

5、滤波：将重采样后的粒子带入状态转移方程得到新的预测粒子，即步骤2。

二、上述过程，每一步的具体做法

首先，看看如下任意状态下的状态方程;

x(t)=f(x(t-1),u(t),w(t))

y(t)=h(x(t),e(t))

其中，x（t）为t时刻状态，u（t）为控制量，w(t)和e(t)分别为状态噪音和观测噪音。前一个方程描述是状态转移，后一个是观测方程。对于这么一个问题粒子滤波怎么来从观测y(t)和x(t-1),u(t)滤出真实状态x（t）呢？

初始状态：由于开始对x(0)一无所知，所有我们认为x（0）在全状态空间内平均分布。然后将所有采样输入状态转移方程，得到预测粒子。

预测阶段：粒子滤波首先根据x(t-1)的概率分布生成大量的采样，这些采样就称之为粒子。那么这些采样在状态空间中的分布实际是x(t-1)的概率分布了。接下来依据状态转移方程加上控制量可以对每一粒子得到一个预测粒子。

校正阶段：观测值y到达后，利用观测方程即条件概率P(y|xi)，对所有的粒子进行评价。直白的说，这个条件概率代表了假设真实状态x(t)取第i个粒子xi时获得观测y的概率。令这个条件概率为第i个粒子的权重。如此这样，继续对所有的粒子都进行这么个评价，那么越有可能获得观测y的粒子，当然获得的权重越高。

重采样：去除低权值的粒子，复制高权值的粒子，得到我们需要的真实状态x(t)。而这些重采样后的粒子，就代表了真实状态的概率分布。下一轮滤波，再将重采样后的粒子集输入到状态转移方程中，直接就能够获得预测粒子了。

三、实例

1、初始阶段：该阶段需要人工指定跟踪目标，程序计算跟踪目标的特征。比如可以采用目标的颜色特征。具体到Rob Hess的代码，开始时需要人工用鼠标拖出一个跟踪区域，然后程序自动计算该区域色调（Hue）的直方图，即为目标的特征。直方图可以用一个向量来表示，所以目标特征就是一个N*1的向量V。

2、搜索阶段：此时，需要去搜索目标对象，也就是粒子滤波了。首先，需要撒粒子。撒粒子的方法有很多种，最常用的两种方法，即：1）均匀的放（在整个图像平面均匀的撒上粒子）；2）在上一帧得到的目标附近按照高斯分布来放，可以理解成，靠近目标的地方多放，远离目标的少放。Rob Hess的代码用的是后一种方法。然后，按照初始化阶段得到的目标特征（色调直方图，向量V）对目标进行搜索。利用每个粒子所处的位置处图像的颜色特征，得到一个色调直方图，向量Vi，计算该直方图与目标直方图的相似性。相似性有多种度量，最简单的一种就是计算sum(abs(Vi-V)。每个粒子算出相似度后再做一次归一化，使得到的相似度加起来等于1.

3、决策阶段:根据上面计算出来的每个粒子与目标的相似度，我们做次加权平均。设N号粒子的图像像素坐标是（Xn,Yn），它报告的相似度是Wn，于是目标最可能的像素坐标X=sum(Xn*Wn)，Y=sum(Yn*Wn)。

4、重采样阶段：根据每个粒子处，相似度的大小，重新分布粒子。相似度高的地方，多放粒子；相似度低的地方，少放粒子。

2->3->4->2，如此反复，即完成了目标的动态跟踪。

四、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 : N
xpartminus(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);
%每个粒子的似然即相似度
end
qsum = sum(q);
for i = 1 : N
q(i) = q(i) / qsum;%权值归一化
end
for i = 1 : N %根据权值重新采样
u = rand;
qtempsum = 0;
for j = 1 : N

qtempsum = qtempsum + q(j);
if qtempsum >= u
xpart(i) = xpartminus(j);
break;
end
end
end
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')

文章参考链接：

http://blog.csdn.net/u010545732/article/details/17462941

http://blog.sina.com.cn/s/blog_86186c970101tn0o.html

http://blog.csdn.net/artista/article/details/51570878

粒子滤波（Particle filter）算法简介及MATLAB实现相关推荐

粒子滤波 particle filter —从贝叶斯滤波到粒子滤波—Part-III（重要性采样序贯重要性采样SIS）
粒子滤波 particle filter -从贝叶斯滤波到粒子滤波-Part-III(重要性采样&序贯重要性采样SIS) 原创不易,路过的各位大佬请点个赞机动目标跟踪/非线性滤波/传感器融合 ...
高斯粒子滤波matlab,粒子滤波(Particle filter)matlab实现 | 学步园
粒子滤波是以贝叶斯推理和重要性采样为基本框架的.因此,想要掌握粒子滤波,对于上述两个基本内容必须有一个初步的了解.贝叶斯公式非常perfect,但是在实际问题中,由于变量维数很高,被积函数很难积分,常 ...
【目标定位】基于matlab粒子滤波的定位算法【含Matlab源码 2161期】
一.基于粒子滤波污染源定位简介粒子滤波定位算法是目前最精准定位可移动物体的位置,由于水域的流动,工业固体废物污染源很可能随着水流移动位置,基于粒子滤波算法将污染物定位分为预测.测量以及重新采样可大大 ...
粒子滤波(Particle filter)
https://www.cnblogs.com/yangyangcv/archive/2010/05/23/1742263.html https://blog.csdn.net/zxia1/artic ...
【优化算法】粒子群优化灰狼算法【含Matlab源码 006期】
一.获取代码方式获取代码方式1: 完整代码已上传我的资源:[优化算法]粒子群优化灰狼算法[含Matlab源码 006期] 点击上面蓝色字体,直接付费下载,即可. 获取代码方式2: 付费专栏优化求解( ...
蒙特卡洛粒子滤波定位算法_蒙特卡罗定位算法(基于粒子滤波的定位算法) ——原理、理解与仿真...
1 算法原理 1.1 机器人定位问题关于机器人定位,有三大问题,它们分别是: (1)"全局定位":指初始位置未知,机器人靠自身运动确定自己在地图中的位姿. (2)"位姿 ...
蒙特卡洛粒子滤波定位算法_基于粒子滤波的TBD算法仿真—MATLAB仿真
目标跟踪的最终目的是在最小的误差下确定目标的位置,而在无线传感器网络中要实现这个目的需要很多相关技术的支持,如定位技术.目标检测技术.估计技术.节能技术等.目标跟踪问题的求解有很多方法, 从算法的考虑 ...
[转]基于粒子滤波的TBD算法仿真----MATLAB仿真
原文链接: https://blog.xxcxw.cn/2019/08/10/%e5%9f%ba%e4%ba%8e%e7%b2%92%e5%ad%90%e6%bb%a4%e6%b3%a2%e7%9a% ...
【回归预测】基于粒子滤波实现锂离子电池寿命预测附matlab代码
1 内容介绍随着现代生产生活对系统设备可靠性.安全性要求的提高,从成本.可靠性的角度考虑,电子系统正逐步由原来的定期维修变成视情维修(CBM, Condition Based Maintenanc ...
崔岩的笔记——粒子滤波原理及应用（3）粒子滤波原理及算法流程
崔岩的笔记--粒子滤波原理及应用(1)概率论与数理统计基础_今天也是睡觉的一天的博客-CSDN博客崔岩的笔记--粒子滤波原理及应用(2)蒙特卡洛法与贝叶斯滤波_今天也是睡觉的一天的博客-CSDN博客 ...

粒子滤波（Particle filter）算法简介及MATLAB实现

粒子滤波（Particle filter）算法简介及MATLAB实现相关推荐

最新文章

热门文章