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

粒子濾波

下來看看對任意如下的狀態(tài)方程：

x(t)=f(x(t-1),u(t),w(t))

y(t)=h(x(t),e(t))

其中的x(t)為t時刻狀態(tài)，u(t)為控制量，w(t) 和e(t)分別為狀態(tài)噪聲和觀測噪聲。前一個方程描述是狀態(tài)轉(zhuǎn)移，后一個是觀測方程。對于這么一個問題粒子濾波怎么來從觀測y(t)，和x(t-1),u(t) 濾出真實狀態(tài)x(t)呢？

預(yù)測階段：粒子濾波首先根據(jù)x(t-1) 的概率分布生成大量的采樣，這些采樣就稱之為粒子。那么這些采樣在狀態(tài)空間中的分布實際上就是x(t-1) 的概率分布了。好，接下來依據(jù)狀態(tài)轉(zhuǎn)移方程加上控制量可以對每一粒子得到一個預(yù)測粒子。

校正階段：觀測值y到達后，利用觀測方程即條件概率P(y|xi),對所有的粒子進行評價，直白的說，這個條件概率代表了假設(shè)真實狀態(tài)x(t)取第i個粒子xi時獲得觀測y的概率。令這個條件概率為第i個粒子的權(quán)重。如此這般下來，對所有粒子都進行這么一個評價，那么越有可能獲得觀測y的粒子，當然獲得的權(quán)重越高。

重采樣算法：去除低權(quán)值的粒子，復制高權(quán)值的粒子。所得到的當然就是我們說需要的真實狀態(tài)x(t)了，而這些重采樣后的粒子，就代表了真實狀態(tài)的概率分布了。下一輪濾波，再將重采樣過后的粒子集輸入到狀態(tài)轉(zhuǎn)移方程中，直接就能夠獲得預(yù)測粒子了。

初始狀態(tài)的問題： 由于開始對x(0)一無所知，所有我們可以認為x(0)在全狀態(tài)空間內(nèi)平均分布。于是初始的采樣就平均分布在整個狀態(tài)空間中。然后將所有采樣輸入狀態(tài)轉(zhuǎn)移方程，得到預(yù)測粒子。然后再評價下所有預(yù)測粒子的權(quán)重，當然我們在整個狀態(tài)空間中只有部分粒子能夠獲的高權(quán)值。最后進行重采樣，去除低權(quán)值的，將下一輪濾波的考慮重點縮小到了高權(quán)值粒子附近。

具體過程：

1)初始化階段-提取跟蹤目標特征

該階段要人工指定跟蹤目標，程序計算跟蹤目標的特征，比如可以采用目標的顏色特征。具體到Rob Hess的代碼，開始時需要人工用鼠標拖動出一個跟蹤區(qū)域，然后程序自動計算該區(qū)域色調(diào)(Hue)空間的直方圖，即為目標的特征。直方圖可以用一個向量來表示，所以目標特征就是一個N*1的向量V。

2)搜索階段-放狗

好，我們已經(jīng)掌握了目標的特征，下面放出很多條狗，去搜索目標對象，這里的狗就是粒子particle。狗有很多種放法。比如，a)均勻的放：即在整個圖像平面均勻的撒粒子(uniform distribution)；b)在上一幀得到的目標附近按照高斯分布來放，可以理解成，靠近目標的地方多放，遠離目標的地方少放。Rob Hess的代碼用的是后一種方法。狗放出去后，每條狗怎么搜索目標呢？就是按照初始化階段得到的目標特征(色調(diào)直方圖，向量V)。每條狗計算它所處的位置處圖像的顏色特征，得到一個色調(diào)直方圖，向量Vi，計算該直方圖與目標直方圖的相似性。相似性有多種度量，最簡單的一種是計算sum(abs(Vi-V)).每條狗算出相似度后再做一次歸一化，使得所有的狗得到的相似度加起來等于1.

3)決策階段

我們放出去的一條條聰明的狗向我們發(fā)回報告，“一號狗處圖像與目標的相似度是0.3”,“二號狗處圖像與目標的相似度是0.02”,“三號狗處圖像與目標的相似度是0.0003”,“N號狗處圖像與目標的相似度是0.013”...那么目標究竟最可能在哪里呢？我們做次加權(quán)平均吧。設(shè)N號狗的圖像像素坐標是(Xn,Yn),它報告的相似度是Wn,于是目標最可能的像素坐標X = sum(Xn*Wn),Y = sum(Yn*Wn).

4)重采樣階段Resampling

既然我們是在做目標跟蹤，一般說來，目標是跑來跑去亂動的。在新的一幀圖像里，目標可能在哪里呢？還是讓我們放狗搜索吧。但現(xiàn)在應(yīng)該怎樣放狗呢？讓我們重溫下狗狗們的報告吧。“一號狗處圖像與目標的相似度是0.3”,“二號狗處圖像與目標的相似度是0.02”,“三號狗處圖像與目標的相似度是0.0003”,“N號狗處圖像與目標的相似度是0.013”...綜合所有狗的報告，一號狗處的相似度最高，三號狗處的相似度最低，于是我們要重新分布警力，正所謂好鋼用在刀刃上，我們在相似度最高的狗那里放更多條狗，在相似度最低的狗那里少放狗，甚至把原來那條狗也撤回來。這就是Sampling

Importance Resampling，根據(jù)重要性重采樣(更具重要性重新放狗)。

(2)->(3)->(4)->(2)如是反復循環(huán)，即完成了目標的動態(tài)跟蹤。

粒子濾波簡單實現(xiàn)

clc;

clear all;

close all;

x = 0; %初始值

R = 1;

Q = 1;

tf = 100; %跟蹤時長

N = 100; ?%粒子個數(shù)

P = 2;

xhatPart = x;

for i = 1 : N

xpart(i) = x + sqrt(P) * randn;%初始狀態(tài)服從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;%每個粒子對應(yīng)觀測值

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;%權(quán)值歸一化

end

for i = 1 : N %根據(jù)權(quá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);

%最后的狀態(tài)估計值即為N個粒子的平均值，這里經(jīng)過重新采樣后各個粒子的權(quá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')

創(chuàng)作挑戰(zhàn)賽新人創(chuàng)作獎勵來咯，堅持創(chuàng)作打卡瓜分現(xiàn)金大獎

總結(jié)

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

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。