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

粒子濾波

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

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

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

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

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

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

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

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

具體過程：

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

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

2)搜索階段-放狗

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

3)決策階段

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

4)重采樣階段Resampling

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

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

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

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

clc;

clear all;

close all;

x = 0; %初始值

R = 1;

Q = 1;

tf = 100; %跟蹤時(shí)長

N = 100; ?%粒子個(gè)數(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時(shí)刻真實(shí)值

y = x^2 / 20 + sqrt(R) * randn; ?%k時(shí)刻觀測(cè)值

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個(gè)粒子

ypart = xpartminus(i)^2 / 20;%每個(gè)粒子對(duì)應(yīng)觀測(cè)值

vhat = y - ypart;%與真實(shí)觀測(cè)之間的似然

q(i) = (1 / sqrt(R) / sqrt(2*pi)) * exp(-vhat^2 / 2 / R);

%每個(gè)粒子的似然即相似度

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)估計(jì)值即為N個(gè)粒子的平均值，這里經(jīng)過重新采樣后各個(gè)粒子的權(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)作獎(jiǎng)勵(lì)來咯，堅(jiān)持創(chuàng)作打卡瓜分現(xiàn)金大獎(jiǎng)

總結(jié)

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

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