粒子濾波(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|xi ),對所有的粒子進行評價，直白的說，這個條件機率代表了假設真實狀態x(t)取第i個粒子xi時獲得觀測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')