matlab粒子滤波程序,粒子滤波（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')

文章参考链接：