粒子濾波,程序

1、粒子濾波(Particle Filter，PF)，又稱為序貫蒙特卡羅算法，是一種基于蒙特卡羅方法的貝葉斯濾波技術(shù)。粒子濾波的基本原理是尋找一組在狀態(tài)空間傳播的隨機(jī)粒子（樣本）描述系統(tǒng)的狀態(tài)，通過蒙特卡羅方法處理貝葉斯估計(jì)中的積分運(yùn)算，從而得到系統(tǒng)狀態(tài)的最小均方差估計(jì)。當(dāng)粒子數(shù)量區(qū)域無窮時(shí)可以逼近服從任意概率分布的系統(tǒng)狀態(tài)。與其他濾波技術(shù)相比，粒子濾波不需要對(duì)系統(tǒng)狀態(tài)做任何先驗(yàn)性假設(shè)，原則上可以應(yīng)用于任何能用狀態(tài)空間模型描述的隨機(jī)系統(tǒng)。一、 貝葉斯估計(jì)貝葉斯定理是貝葉斯估計(jì)方法的理論基礎(chǔ)。貝葉斯定理表達(dá)如下：其中，x為待估計(jì)參數(shù)，y為樣本觀測(cè)值信息，即樣本信息，f(x)是待估計(jì)參數(shù)x的先驗(yàn)分布密

2、度函數(shù)，f(x|y)是x的后驗(yàn)分布密度函數(shù)，f(y)和f(y|x)是y的密度函數(shù)。因此通過上式可以看出，后驗(yàn)信息正比于樣本信息與先驗(yàn)信息的乘積。可以通過樣本信息對(duì)先驗(yàn)信息進(jìn)行修正來得到更準(zhǔn)確的后驗(yàn)信息。得到后驗(yàn)分布的密度函數(shù)后，就可以此為基礎(chǔ)進(jìn)行參數(shù)的點(diǎn)估計(jì)、區(qū)間軌跡和假設(shè)檢驗(yàn)。二、 序貫重要性采樣方法序貫重要性采樣方法的核心思想是利用一系列隨機(jī)樣本的加權(quán)和所需的驗(yàn)后概率密度得到狀態(tài)的估計(jì)值。當(dāng)樣本點(diǎn)的數(shù)量無窮多時(shí)，蒙特卡羅特性與驗(yàn)后概率密度的函數(shù)表達(dá)等價(jià)，序貫重要性采樣濾波器近似于貝葉斯濾波器。對(duì)于如下的非線性系統(tǒng)：式中，f(·)和h(·)是非線性函數(shù)，w(k)和v(k)

3、是系統(tǒng)的狀態(tài)噪聲和觀測(cè)噪聲。設(shè)為從0k時(shí)刻所有狀態(tài)向量的集合，為1k時(shí)刻所有觀測(cè)向量的集合。濾波過程中利用獲得最優(yōu)的xk+1，即一般而言，是多變量且非高斯的很難直接采樣，可以用與其近似的分布代替它進(jìn)行采樣，則式中稱為重要性函數(shù)，而稱為重要性權(quán)值。考慮到狀態(tài)演變和觀測(cè)條件的相對(duì)獨(dú)立性，通常認(rèn)為狀態(tài)具有馬爾可夫性，z(k)僅和x(k)有關(guān)，那么將上式代入重要性權(quán)值公式，則wx(k)可以寫成在適當(dāng)選取重要性函數(shù)的基礎(chǔ)上，可通過貝葉斯體系中的時(shí)間更新和觀測(cè)更新步驟，以序貫更新重要性權(quán)值。對(duì)于重要性函數(shù)，如果能夠從目標(biāo)驗(yàn)后概率密度采樣出N個(gè)粒子xi(k),i=1,2,···

4、，N，則可以用經(jīng)驗(yàn)概率分布以近似驗(yàn)后概率密度式中， 為狄拉克函數(shù)。三、 基本粒子濾波器步驟基本粒子濾波包括樣本采樣和再采樣，結(jié)合貝葉斯濾波體系的時(shí)間更新和觀測(cè)更新兩步驟進(jìn)行，具體算法步驟如下：(1)、初始化：t=0。從i=1,2,···,N,按照初始重要性函數(shù)選取初始粒子群。從i=1,2,···,N，估計(jì)初始粒子額重要性權(quán)值從i=1,2,···,N，歸一化初始重要性權(quán)值(2)、時(shí)間更新：t=k-1,k>1。從i=1,2,···,N，按照重要性函數(shù)選取更新狀態(tài)后的粒子群，

5、且。(3)、觀測(cè)更新：t=k。從i=1,2,···,N，在已獲得z(k)的情況下，估計(jì)重要性權(quán)值系數(shù)從i=1,2,···,N，歸一化重要性權(quán)值(4)、重采樣從i=1,2,···,N，根據(jù)權(quán)值，分別復(fù)制高權(quán)值粒子，舍棄低權(quán)值粒子，從而中心產(chǎn)生N個(gè)粒子集。從i=1,2,···,N，歸一化權(quán)值(5)、輸出結(jié)果步驟(2)至(5)循環(huán)進(jìn)行。四、 粒子濾波的仿真在上圖中，藍(lán)色的點(diǎn)代表狀態(tài)的真值，綠色折線為粒子濾波的估計(jì)。從圖中可以看出粒子濾波具有較好的濾波效果。程序：x = 0.1; %

6、 初始狀態(tài)Q = 1; % 模型噪聲W的方差R = 1; % 測(cè)量噪聲V的方差tf = 50; % 仿真長(zhǎng)度N = 100; % 粒子濾波的粒子數(shù)xhat = x;P = 2; %狀態(tài)的方差xhatPart = x; % 初始化粒子濾波for i = 1 : N xpart(i) = x + sqrt(P) * randn; %狀態(tài)值采樣endxArr = x;xhatPartArr = xhatPart; for k = 1 : tf %tf為時(shí)間長(zhǎng)度，k可以理解為時(shí)間軸上的k時(shí)刻 % x的數(shù)據(jù)為時(shí)刻k的真實(shí)狀態(tài)值 x = 0.5 * x + 25 * x / (1 + x2) + 8 *

7、cos(1.2*(k-1) + sqrt(Q) * randn; %狀態(tài)方程(1) y = x2 / 20 + sqrt(R) * randn;%觀測(cè)方程(2)(真值) % Particle filter生成100個(gè)粒子并根據(jù)預(yù)測(cè)和觀測(cè)差值計(jì)算各個(gè)粒子的權(quán)重 for i = 1 : Nxpartminus(i) = 0.5 * xpart(i) + 25 * xpart(i) / (1 + xpart(i)2) + 8 * cos(1.2*(k-1) + sqrt(Q) * randn; ypart = xpartminus(i)2 / 20;%(預(yù)測(cè)值) vhat = y - ypart;

8、%觀測(cè)和預(yù)測(cè)的差 q(i) = (1 / sqrt(R) / sqrt(2*pi) * exp(-vhat2 / 2 / R); %根據(jù)差值給出權(quán)重 end % Normalize the likelihood of each a priori estimate. qsum = sum(q); for i = 1 : N q(i) = q(i) / qsum;%歸一化權(quán)重 end %類似與遺傳算法進(jìn)行選擇 for i = 1 : N u = rand; % uniform random number between 0 and 1 qtempsum = 0; for j = 1 : N qte

9、mpsum = qtempsum + q(j); if qtempsum >= u %重采樣低權(quán)重進(jìn)行剔除，同時(shí)保留高權(quán)重，防止退化方法 xpart(i) = xpartminus(j); break; end end end % The particle filter estimate is the mean of the particles. xhatPart = mean(xpart); %經(jīng)過粒子濾波處理后的初值 % Save data in arrays for later plotting xArr = xArr x; xhatPartArr = xhatPartArr xhatPart;endt = 0 : tf;figure;plot(t,