基于無跡卡爾曼濾波和權(quán)值優(yōu)化的改進(jìn)粒子濾波算法

冉星浩，陶建鋒，楊春曉

(1.空軍工程大學(xué)防空反導(dǎo)學(xué)院，陜西 西安 710051;2.中國人民解放軍93567部隊(duì)，河北 保定 074100)

0 引言

相比于傳統(tǒng)的卡爾曼濾波算法，粒子濾波算法不受線性化誤差或高斯噪聲假定的限制，適用于非線性、非高斯系統(tǒng)。近年來，隨著計(jì)算機(jī)計(jì)算能力的不斷提高，以Monte-Carlo模擬為特點(diǎn)的粒子濾波(Particle filtering)算法逐漸成為解決非線性、非高斯系統(tǒng)估計(jì)問題的研究熱點(diǎn)，廣泛應(yīng)用于雷達(dá)、聲納、紅外、導(dǎo)航制導(dǎo)、計(jì)算機(jī)視覺、金融統(tǒng)計(jì)和目標(biāo)跟蹤等領(lǐng)域[1-6]。

1993年，Gordon等人提出了自舉粒子濾波算法[7](Bookskrap Particle Filter)，通過在遞推過程中引入重采樣[8]的思想以克服粒子退化現(xiàn)象[9]，奠定了粒子濾波算法的基礎(chǔ)。但是，在標(biāo)準(zhǔn)粒子濾波算法中基于先驗(yàn)分布的重要性函數(shù)與真實(shí)后驗(yàn)概率密度函數(shù)[10]存在較大偏差，因此，Merwe等人提出一種無跡粒子濾波[11](Unscented Particle Filtering，UPF)，該算法使用無跡卡爾曼濾波(Unscented KalmanFiltering，UKF)算法產(chǎn)生重要性密度函數(shù)，一定程度上提高了濾波精度，然而粒子集在重采樣過程中不可避免會(huì)產(chǎn)生樣本貧化[12]問題。本文針對(duì)重要性密度函數(shù)的的合理選取，粒子退化現(xiàn)象以及樣本貧化等問題，在UPF算法的基礎(chǔ)上，結(jié)合權(quán)值優(yōu)化[13]思想，提出了新的改進(jìn)粒子濾波算法，(Improved Unscented Particle Filtering，IMUPF)。

1 算法原理

1.1 標(biāo)準(zhǔn)粒子濾波算法

粒子濾波是一種基于蒙特卡洛方法和遞推貝葉斯估計(jì)的統(tǒng)計(jì)濾波方法，它依據(jù)大數(shù)定理采用蒙特卡羅方法來求解貝葉斯估計(jì)中的積分運(yùn)算[14]。其基本思想是：首先依據(jù)系統(tǒng)狀態(tài)向量的經(jīng)驗(yàn)條件分布在狀態(tài)空間產(chǎn)生一組隨機(jī)樣本的集合，稱為粒子，并計(jì)算每個(gè)粒子權(quán)值，然后利用樣本和其對(duì)應(yīng)的權(quán)系數(shù)來近似表示目標(biāo)狀態(tài)的后驗(yàn)概率密度函數(shù)，從而利用近似的后驗(yàn)概率密度函數(shù)實(shí)現(xiàn)對(duì)目標(biāo)狀態(tài)的估計(jì)。

假設(shè)系統(tǒng)方程與狀態(tài)方程如下所示：

xk=f(xk-1)+wk-1

(1)

zk=h(xk)+vk-1

(2)

式(1)，式(2)中，xk為系統(tǒng)的狀態(tài)變量，zk為系統(tǒng)的量測(cè)變量，wk為系統(tǒng)噪聲，vk為過程噪聲，且分別為獨(dú)立同分布的噪聲序列；映射f(·)，h(·)分別為系統(tǒng)狀態(tài)轉(zhuǎn)移函數(shù)和量測(cè)函數(shù)。

后驗(yàn)概率密度為：

(3)

在標(biāo)準(zhǔn)粒子濾波中，為了求解方便，一般取重要性密度函數(shù)為先驗(yàn)概率分布，即：

(4)

標(biāo)準(zhǔn)粒子濾波算法描述如下：

1) 初始化：k=0

2) 重要性采樣：k=1，2，…，N

a)從建議分布函數(shù)中隨機(jī)抽取N個(gè)粒子：

(5)

權(quán)值更新公式為：

(6)

b)更新粒子權(quán)值并對(duì)權(quán)值進(jìn)行歸一化：

(7)

歸一化：

(8)

3) 重采樣：

4) 狀態(tài)估計(jì)：對(duì)各個(gè)粒子加權(quán)估計(jì)系統(tǒng)狀態(tài)及誤差

(9)

(10)

5) 令k=k+1，返回步驟2)。

1.2 UPF算法

UPF算法具體步驟如下：

1) 初始化：k=0。

(11)

(12)

(13)

(14)

2)k=1，2，…

①重要性采樣：i=1，2，…，N，使用UKF算法更新粒子

a) 選取粒子：

(15)

b) 時(shí)間更新

(16)

(17)

(18)

(19)

(20)

c) 量測(cè)更新：

(21)

(22)

(23)

(24)

(25)

采樣粒子：

(26)

式(26)中，N(·)表示高斯函數(shù)。

令：

(27)

計(jì)算權(quán)重：

(28)

并歸一化權(quán)值。

③輸出融合結(jié)果：

(29)

2 改進(jìn)UPF算法

在UPF算法中，引入重采樣過程，抑制粒子退化現(xiàn)象。重采樣技術(shù)是為了解決重要性權(quán)值可能偏差很大的問題，一旦偏差過大，無效樣本數(shù)會(huì)增多，過多的無效樣本會(huì)降低蒙特卡洛方法的效率，重采樣是一個(gè)減少無效樣本、增加有效樣本的有效方法，它允許舍棄“壞”樣本，復(fù)制“好”樣本從而適應(yīng)目標(biāo)的動(dòng)態(tài)變化。常用的重采樣有隨機(jī)重采樣、多項(xiàng)式重采樣、殘差重采樣、分層重采樣、系統(tǒng)重采樣。上述重采樣方法由于簡(jiǎn)單重復(fù)復(fù)制大權(quán)值粒子同時(shí)輕易丟棄了小權(quán)值粒子，損失了粒子多樣性，引發(fā)粒子貧化甚至枯竭的問題[15]。

針對(duì)UPF算法重采樣導(dǎo)致的粒子貧化問題，本文采用了一種重要性權(quán)值優(yōu)化重采樣思想，通過引入一個(gè)影響因子來優(yōu)化粒子的重要性權(quán)重，加強(qiáng)了重采樣后粒子的有效性和多樣性，有效地提高了UPF算法的性能。

(30)

這樣每個(gè)粒子的權(quán)值得到了重新調(diào)整，權(quán)值小的粒子在優(yōu)化后權(quán)值得到提高，相反權(quán)值大的粒子權(quán)值會(huì)降低，改善了粒子集的多樣性。下面驗(yàn)證該方法的有效性：

(31)

(32)

(33)

同理：

結(jié)合UPF算法和上述權(quán)值優(yōu)化算法思想，改進(jìn)算法的具體步驟總結(jié)如下：

3) 權(quán)值計(jì)算：通過式(28)計(jì)算權(quán)值并進(jìn)行歸一化。

4) 權(quán)值優(yōu)化：引入一個(gè)影響因子α來優(yōu)化粒子的重要性權(quán)重，并進(jìn)行歸一化。

(34)

6) 狀態(tài)估計(jì)：

(35)

7) 返回步驟2)。

3 仿真結(jié)果與分析

為了驗(yàn)證改進(jìn)算法優(yōu)越性，本文分別用標(biāo)準(zhǔn)PF、UPF和IMUPF算法分別對(duì)非線性、非高斯的系統(tǒng)進(jìn)行狀態(tài)估計(jì)，其狀態(tài)方程和觀測(cè)方程分別為：

x(k)=1+sin(0.04πk)+0.5xk-1+uk-1

(36)

(37)

(38)

式中，量測(cè)噪聲vk服從Gamma分布γ(3，2)，系統(tǒng)噪聲uk是方差為1×10-5的高斯白噪聲。

仿真參數(shù)設(shè)置：粒子數(shù)N=100，仿真時(shí)間步長(zhǎng)T=60，時(shí)間間隔Δt=1，初始狀態(tài)方差intVar=0.75，初始位置x0=1，得到狀態(tài)估計(jì)仿真結(jié)果如圖1-圖3所示。

圖1和圖2分別是PF算法，UPF算法和IMUPF算法的跟蹤效果對(duì)比圖和估計(jì)誤差對(duì)比圖。從這兩幅圖可以看出IMUPF算法相比較PF算法和UPF算法而言，更加接近真實(shí)軌跡，同時(shí)誤差也更小。這是因?yàn)橛肬KF算法產(chǎn)生的重要性函數(shù)比用先驗(yàn)分布產(chǎn)生的重要性函數(shù)更加接近真實(shí)的后驗(yàn)分布，同時(shí)采用權(quán)值優(yōu)化的重采樣方法增加了粒子多樣性，改善了樣本貧化問題。

為了能夠進(jìn)一步驗(yàn)證本文改進(jìn)算法的優(yōu)越性，分別對(duì)以上三種算法進(jìn)行10次蒙特卡羅實(shí)驗(yàn)，得到各個(gè)時(shí)刻的均方根誤差(Root Mean Square Error，RMSE)如圖3所示。這里的RMSE引用文獻(xiàn)[12]中的RMSE公式，即

(39)

分別對(duì)三種算法求其運(yùn)行時(shí)間以及所有時(shí)刻的均方根誤差求平均，可以得到平均RMSE，進(jìn)行多次實(shí)驗(yàn)，得到各算法的平均RMSE如表1所示。

表1 三種算法的平均RMSE

Tab.1 The average RMSE of three algorithms

算法平均RMSE運(yùn)行時(shí)間/sPF0.791 10.312 4UPF0.079 90.502 8IMUPF0.031 00.515 1

從圖3以及表1我們可以看出標(biāo)準(zhǔn)PF算法的平均RMSE為0.791 1，運(yùn)行時(shí)間為0.312 4；UPF算法的平均RMSE為0.079 9，運(yùn)行時(shí)間為0.502 8；IMUPF算法的平均RMSE為0.031 0，運(yùn)行時(shí)間為0.515 1，可以算出與標(biāo)準(zhǔn)PF算法相比IMUPF算法在運(yùn)行時(shí)間增加64.88%的同時(shí)，均方根誤差降低了96.08%;與UPF算法相比IMUPF算法在運(yùn)行時(shí)間增加2.44%的同時(shí)，均方根誤差降低了61.20%，所以改進(jìn)算法大大提高了濾波精度。

4 結(jié)論

本文結(jié)合無跡卡爾曼濾波和權(quán)值優(yōu)化的方法，提出了基于無跡卡爾曼濾波和權(quán)值優(yōu)化的改進(jìn)粒子濾波算法。該算法通過無跡卡爾曼濾波產(chǎn)生重要性密度函數(shù)，解決了重要性函數(shù)的選取問題，然后通過引入一個(gè)影響因子對(duì)粒子權(quán)值進(jìn)行優(yōu)化，增加了粒子的多樣性，解決了樣本貧化問題。仿真結(jié)果表明，該算法的跟蹤軌跡與真實(shí)軌跡最為接近，其平均RMSE最小，與標(biāo)準(zhǔn)粒子濾波，無跡粒子濾波等算法相比，具有更強(qiáng)的抗噪能力和更好的跟蹤性能，下一步需要在提高算法實(shí)時(shí)性上深入研究。

參考文獻(xiàn):

[1]吳兆平，朱凱然，蘇濤，等.采用改進(jìn)粒子濾波的雷達(dá)擴(kuò)展目標(biāo)檢測(cè)前跟蹤[J].西安電子科技大學(xué)學(xué)報(bào)，2011，38(2)：99-104.

[2]李浩，王潤亮，彭華.粒子濾波盲均衡譯碼聯(lián)合算法[J].西安電子科技大學(xué)學(xué)報(bào)，2013，40(1)：123-128.

[3]胡洪濤，敬忠良，李安平，等.非高斯條件下基于粒子濾波的目標(biāo)跟蹤[J].上海交通大學(xué)學(xué)報(bào)，2004，38(12)：1996-1999.

[4]周明，涂宏斌.一種基于改進(jìn)粒子濾波的多目標(biāo)檢測(cè)與跟蹤方法[J].華東交通大學(xué)學(xué)報(bào)，2016，33(2)：121-126.

[5]孫景樂，唐林波，趙保軍，等.基于瑞麗分布的粒子濾波跟蹤算法[J].電子信息學(xué)報(bào)，2013，35(4)：763-769.

[6]Zou J Y，Wu C H，Ma T G.Heterogeneous Integrated Beam-switching/Retrodirective Array Using Synthesized Transmission Lines[J].IEEE Transactions on Microwave Theory and Techniques，2013，61(8)：3128-3139.

[7]Gordon N J，Salmond D J，Smith A. A novel approach to nonlinear and non-Gaussian Bayesian state estimation[C]//IEEE Proceeding，1993，140：107-113.

[8]LIU J S，CHEN R.Sequential monte carlo methods for dynamic systems[J].J.of the American Statistical Association，1995，90(2)：567-576.

[9]DOUCET A，GODSILL S.On sequential monte carlo sampling methods for bayesian filtering[R].Cambridge：University of Cambridge，1998.

[10]TAI S.Cloud service engineering[C]//Proc of the 18th IEEE International Workshops on Enabling Technologies：Infrastructures for Collaborative Enterprises.Washington DC：IEEE Computer Society，2009：3-4.

[11]Wan E A，Van Der Merwe R.The unscented kalam filter for nonlinear estimation[C]//Proceeding of the IEEE 2000 Adaptive Systems for Signal Processing，Communications，and Control Symposium.Piscataway：IEEE，2000：153-158.

[12]ARULAMPPALAM M S，MASKELLS，GORDON N，et al.A tutorial on particle filter for online nonlinear/non gaussssian bayesian tracking[J].IEEE Trans.on Signal Processing，2002，50(2)：174-188.

[13]杜正聰，辛強(qiáng)，鄧尋，等.基于權(quán)值優(yōu)化的粒子濾波算法研究[J].重慶師范大學(xué)學(xué)報(bào)，2015，32(3)：124-129.

[14]朱志宇.粒子濾波算法及其應(yīng)用[M].北京：科學(xué)出版社，2010.

[15]朱軍.基于權(quán)值優(yōu)化的粒子濾波算法研究及其在目標(biāo)跟蹤中的應(yīng)用[D].安徽：安徽建筑大學(xué)，2014：1-61.