粒子滤波(Particle filter)matlab实现

粒子滤波是以贝叶斯推理和重要性采样为基本框架的。因此，想要掌握粒子滤波，对于上述两个基本内容必须有一个初步的了解。贝叶斯公式非常perfect，但是在实际问题中，由于变量维数很高，被积函数很难积分，常常会给粒子滤波带来很大的麻烦。为了克服这个问题，它引入了重要性采样。即先设计一个重要性密度，根据重要性密度与实际分布之间的关系，给采样得到的粒子分配权重。再利用时变贝叶斯公式，给出粒子权重的更新公式及重要性密度的演变形式。在实际问题中，由于直接从重要性密度中采样非常困难，因此做出了妥协，重要性密度选为状态转移分布，随之可得权值更新遵循的规律与量测方程有关。

       粒子滤波算法源于Monte carlo思想，即以某事件出现的频率来指代该事件的概率。因此在滤波过程中，需要用到概率如P(x)的地方，一概对变量x采样，以大量采样及其相应的权值来近似表示P(x)。因此，采用此思想，在滤波过程中粒子滤波可以处理任意形式的概率，而不像Kalman滤波只能处理线性高斯分布的概率问题。粒子滤波的一大优势也在于此。

         下来看看对任意如下的状态方程：

                                  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(t-1) 的概率分布生成大量的采样，这些采样就称之为粒子。那么这些采样在状态空间中的分布实际上就是x(t-1) 的概率分布了。好，接下来依据状态转移方程加上控制量可以对每一粒子得到一个预测粒子。

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

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

初始状态的问题： 由于开始对x(0)一无所知，所有我们可以认为x(0)在全状态空间内平均分布。于是初始的采样就平均分布在整个状态空间中。然后将所有采样输入状态转移方程，得到预测粒子。然后再评价下所有预测粒子的权重，当然我们在整个状态空间中只有部分粒子能够获的高权值。最后进行重采样，去除低权值的，将下一轮滤波的考虑重点缩小到了高权值粒子附近。

具体过程：

1）初始化阶段-提取跟踪目标特征

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

2）搜索阶段-放狗

好，我们已经掌握了目标的特征，下面放出很多条狗，去搜索目标对象，这里的狗就是粒子particle。狗有很多种放法。比如，a)均匀的放：即在整个图像平面均匀的撒粒子(uniform distribution)；b)在上一帧得到的目标附近按照高斯分布来放，可以理解成，靠近目标的地方多放，远离目标的地方少放。Rob Hess的代码用的是后一种方法。狗放出去后，每条狗怎么搜索目标呢？就是按照初始化阶段得到的目标特征(色调直方图，向量V)。每条狗计算它所处的位置处图像的颜色特征，得到一个色调直方图，向量Vi，计算该直方图与目标直方图的相似性。相似性有多种度量，最简单的一种是计算sum(abs(Vi-V)).每条狗算出相似度后再做一次归一化，使得所有的狗得到的相似度加起来等于1.

3）决策阶段

我们放出去的一条条聪明的狗向我们发回报告，“一号狗处图像与目标的相似度是0.3”,“二号狗处图像与目标的相似度是0.02”,“三号狗处图像与目标的相似度是0.0003”,“N号狗处图像与目标的相似度是0.013”...那么目标究竟最可能在哪里呢？我们做次加权平均吧。设N号狗的图像像素坐标是(Xn,Yn),它报告的相似度是Wn,于是目标最可能的像素坐标X = sum(Xn*Wn),Y = sum(Yn*Wn).

4）重采样阶段Resampling

既然我们是在做目标跟踪，一般说来，目标是跑来跑去乱动的。在新的一帧图像里，目标可能在哪里呢？还是让我们放狗搜索吧。但现在应该怎样放狗呢？让我们重温下狗狗们的报告吧。“一号狗处图像与目标的相似度是0.3”,“二号狗处图像与目标的相似度是0.02”,“三号狗处图像与目标的相似度是0.0003”,“N号狗处图像与目标的相似度是0.013”...综合所有狗的报告，一号狗处的相似度最高，三号狗处的相似度最低，于是我们要重新分布警力，正所谓好钢用在刀刃上，我们在相似度最高的狗那里放更多条狗，在相似度最低的狗那里少放狗，甚至把原来那条狗也撤回来。这就是Sampling Importance Resampling，根据重要性重采样(更具重要性重新放狗)。

(2)->(3)->(4)->(2)如是反复循环，即完成了目标的动态跟踪。

粒子滤波简单实现

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')