粒子濾波是以貝葉斯推理和重要性采樣為基本框架的。因此，想要掌握粒子濾波，對于上述兩個基本內容必須有一個初步的了解。貝葉斯公式非常perfect，但是在實際問題中，由于變量維數很高，被積函數很難積分，常常會給粒子濾波帶來很大的麻煩。為了克服這個問題，它引入了重要性采樣。即先設計一個重要性密度，根據重要性密度與實際分布之間的關系，給采樣得到的粒子分配權重。再利用時變貝葉斯公式，給出粒子權重的更新公式及重要性密度的演變形式。在實際問題中，由于直接從重要性密度中采樣非常困難，因此做出了妥協，重要性密度選為狀態轉移分布，隨之可得權值更新遵循的規律與量測方程有關。

粒子濾波

下來看看對任意如下的狀態方程：

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

創作挑戰賽新人創作獎勵來咯，堅持創作打卡瓜分現金大獎

總結

以上是生活随笔為你收集整理的高斯粒子滤波matlab,粒子滤波(Particle filter)matlab实现 | 学步园的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。