粒子濾波是以貝葉斯推理和重要性取樣為基本框架的。因此，想要掌握粒子濾波，對於上述兩個基本內容必須有一個初步的瞭解。貝葉斯公式非常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')