武 勇，王 俊，曹運(yùn)合，張培川

（西安電子科技大學(xué) 雷達(dá)信號(hào)處理國(guó)家重點(diǎn)實(shí)驗(yàn)室，西安710071）

0 引 言

非線性濾波問題廣泛存在于信號(hào)處理、雷達(dá)探測(cè)、目標(biāo)跟蹤、衛(wèi)星導(dǎo)航等諸多領(lǐng)域［1－2］，這類問題可歸納為存在觀測(cè)噪聲時(shí)非線性系統(tǒng)的狀態(tài)估計(jì)問題。貝葉斯概率統(tǒng)計(jì)理論為這種狀態(tài)估計(jì)問題提供了一套完整的數(shù)學(xué)解決方案，其基本思想是將狀態(tài)估計(jì)問題轉(zhuǎn)化為后驗(yàn)概率密度函數(shù)或后驗(yàn)分布的逼近問題，并通過遞歸的方式實(shí)現(xiàn)對(duì)狀態(tài)空間的迭代估計(jì)。對(duì)于線性高斯模型來說，傳統(tǒng)的卡爾曼濾波被公認(rèn)為是最優(yōu)的貝葉斯估計(jì)，其通過遞推式更新有限維統(tǒng)計(jì)量來精確計(jì)算狀態(tài)空間的后驗(yàn)分布［3－4］。然而對(duì)于大部分非線性、非高斯的情況，狀態(tài)空間后驗(yàn)分布的閉式解不存在或難以求解［5］，這就需要對(duì)后驗(yàn)分布進(jìn)行近似計(jì)算。文獻(xiàn)［6－7］提出的粒子濾波算法是一種通用的非線性濾波算法，其采用一系列帶有權(quán)值的樣本點(diǎn)對(duì)狀態(tài)的后驗(yàn)概率分布進(jìn)行近似，這種算法本質(zhì)上是基于狀態(tài)搜索進(jìn)行的。首先通過重要性密度函數(shù)隨機(jī)生成若干個(gè)粒子，每個(gè)粒子代表一個(gè)可能的狀態(tài)，然后在狀態(tài)空間對(duì)它們進(jìn)行傳播，使用似然函數(shù)選出最有可能生成當(dāng)前觀測(cè)值的若干個(gè)粒子點(diǎn)，并賦予一個(gè)較大的權(quán)值，最后通過保留下來的粒子對(duì)狀態(tài)的后驗(yàn)分布進(jìn)行近似。由于在狀態(tài)逼近的過程中使用了大量的粒子，因此該算法具有較高的計(jì)算復(fù)雜度。粒子濾波因優(yōu)良的性能，一經(jīng)被提出便獲得了廣泛的應(yīng)用［8－10］。但在粒子濾波中存在著兩個(gè)主要的問題：①當(dāng)粒子采樣不準(zhǔn)確時(shí)，如采樣得到的粒子位于實(shí)際后驗(yàn)分布的拖尾區(qū)域，經(jīng)過狀態(tài)搜索后，絕大部分的粒子權(quán)值都會(huì)趨于0。這會(huì)為后驗(yàn)分布的近似帶來很大的誤差，甚至有可能引起濾波發(fā)散。因此，采樣粒子的質(zhì)量直接決定著對(duì)后驗(yàn)分布近似的精度。從最優(yōu)重要性密度函數(shù)的表達(dá)式可以看出，在采樣粒子時(shí)，應(yīng)考慮最新的觀測(cè)信息［7］?；诖耍嗬^出現(xiàn)了輔助粒子濾波［11］、無跡粒子濾波［12－13］、擴(kuò)展卡爾曼粒子濾波［14］以及多種卡爾曼相結(jié)合的粒子濾波［15］等方法，這幾種方法都是將當(dāng)前時(shí)刻的觀測(cè)值引入到粒子的采樣過程中，以提高所選粒子的質(zhì)量。②在對(duì)狀態(tài)進(jìn)行迭代估計(jì)過程中，會(huì)出現(xiàn)粒子退化以及粒子貧化的現(xiàn)象［16］，減少了狀態(tài)估計(jì)中可用粒子的數(shù)量和種類，增大了狀態(tài)估計(jì)誤差，一般通過重采樣技術(shù)來緩解這種現(xiàn)象的發(fā)生，即在每次迭代后對(duì)粒子進(jìn)行重新采樣，根據(jù)重采樣的策略不同，狀態(tài)估計(jì)的精度也各不相同，如差分演化粒子濾波及其變種［17］。

綜合以上的分析，對(duì)粒子濾波存在問題的改進(jìn)主要從如何有效利用觀測(cè)信息提高采樣粒子的質(zhì)量和重采樣策略兩個(gè)方面進(jìn)行。本文從第一個(gè)方面出發(fā)，提出了一種基于二次預(yù)測(cè)的粒子濾波算法，稱為二次預(yù)測(cè)粒子濾波（SP－PF）。算法首先使用狀態(tài)轉(zhuǎn)移密度函數(shù)對(duì)粒子進(jìn)行初采樣，獲得預(yù)測(cè)粒子，然后在預(yù)測(cè)粒子的基礎(chǔ)上，通過求解一個(gè)最小二乘問題，將當(dāng)前時(shí)刻的觀測(cè)信息引入到粒子的二次預(yù)測(cè)中，并使用似然函數(shù)計(jì)算新粒子的權(quán)值，最后通過粒子加權(quán)對(duì)當(dāng)前的狀態(tài)進(jìn)行估計(jì)，估計(jì)完后再對(duì)粒子進(jìn)行重采樣。SP－PF 算法的優(yōu)勢(shì)在于：將當(dāng)前時(shí)刻的觀測(cè)信息融合在粒子采樣階段，充分利用了可信的觀測(cè)信息，避免了傳統(tǒng)粒子濾波中觀測(cè)信息僅用來評(píng)判粒子好壞的問題，提高了位于真實(shí)后驗(yàn)分布附近采樣粒子的數(shù)量，緩解了迭代過程中出現(xiàn)的粒子退化問題，進(jìn)而改善了狀態(tài)估計(jì)的精度。

1 二次預(yù)測(cè)粒子濾波算法

假設(shè)非線性系統(tǒng)的狀態(tài)模型和觀測(cè)模型分別為：

式中：xk∈Rm為k 時(shí)刻的m 維狀態(tài)向量；yk∈Rn為k 時(shí)刻的n 維觀測(cè)向量；f（·）、h（·）為已知的函數(shù)；uk、vk分別為分布已知的系統(tǒng)噪聲和觀測(cè)噪聲，它們之間相互獨(dú)立。

濾波的目的就是利用當(dāng)前時(shí)刻的觀測(cè)值和上一時(shí)刻的狀態(tài)估計(jì)值，對(duì)當(dāng)前時(shí)刻的狀態(tài)（后驗(yàn)概率密度函數(shù)）進(jìn)行迭代更新。

SP－PF算法是在經(jīng)典粒子濾波的框架下發(fā)展的，通過蒙特卡洛積分的方法對(duì)狀態(tài)的后驗(yàn)概率密度函數(shù)進(jìn)行估計(jì)，其估計(jì)的數(shù)學(xué)表達(dá)式為：

式中：N 為采樣的粒子數(shù)；y1：k 為到k 時(shí)刻所有觀測(cè)量 構(gòu) 成 的 觀 測(cè) 序 列y1：k ＝｛y1，y2，…，yk｝；為從重要性密度函數(shù)q（xk｜xk－1，y1：k）采樣得到的粒子序列為對(duì)應(yīng)于每個(gè)粒子的歸一化重要性權(quán)重，滿足

歸一化前，粒子的重要性權(quán)重通過下式獲得：

通常選擇狀態(tài)轉(zhuǎn)移密度函數(shù)作為重要性密度函數(shù)，即：

通過式（5）得到的預(yù)測(cè)粒子沒有包含當(dāng)前時(shí)刻的觀測(cè)信息，僅是對(duì)當(dāng)前狀態(tài)的先驗(yàn)估計(jì)。為了使采樣粒子包含當(dāng)前時(shí)刻的觀測(cè)信息，構(gòu)建如下最小二乘估計(jì)問題：

上式是一個(gè)無約束極小化問題，其代價(jià)函數(shù)為：

式（6）所述問題可轉(zhuǎn)換為代價(jià)函數(shù)求導(dǎo)等于0的問題，即：

利用狀態(tài)的先驗(yàn)估計(jì)xk｜k－1，通過一階泰勒展開式對(duì)觀測(cè)函數(shù)h（xk）進(jìn)行近似，即：

式中：Hk＝?xkh（xk）｜xk＝xk｜k－1，將 式（9）代 入J（xk）的表達(dá)式易得：

g（yk，xk｜k－1）＝y(tǒng)k－h(huán)（xk｜k－1）＋Hkxk｜k－1

對(duì)J（xk）求導(dǎo)后式（8）可以轉(zhuǎn)化為：

求解式（11），經(jīng)整理變形后可得當(dāng)前狀態(tài)xk的最新估計(jì)為：

將從式（5）采樣得到的預(yù)測(cè)粒子分別代入式（12），即可獲得新的預(yù)測(cè)粒子。在新粒子中，不僅包含了對(duì)狀態(tài)的預(yù)測(cè)值（先驗(yàn)信息），同時(shí)也包含了當(dāng)前時(shí)刻的觀測(cè)值（后驗(yàn)信息），即：

通過式（3）計(jì)算新粒子的重要性權(quán)值，易得：

對(duì)重要性權(quán)值進(jìn)行歸一化處理：

利用最新采樣的粒子和相應(yīng)的權(quán)值對(duì)當(dāng)前的狀態(tài)進(jìn)行更新：

最后，按文獻(xiàn)［6］中的方法進(jìn)行粒子的重采樣，重采樣后各粒子的權(quán)值為1／N。

本文提出的基于二次預(yù)測(cè)的粒子濾波算法可歸納為如下步驟：

初始化：從先驗(yàn)概率分布p（x0）采樣得到粒子

（1）根據(jù)式（4）選擇重要性密度函數(shù)q（xk｜xk－1，y1：k）。

（2）根據(jù)q（xk｜xk－1，y1：k）對(duì)粒子進(jìn)行初采樣，得到預(yù)測(cè)粒子

（3）計(jì)算觀測(cè)函數(shù)h（xk）的雅克比矩陣Hk。

（4）根據(jù)式（13）進(jìn)行二次采樣，得到包含觀測(cè)信息的新粒子。

（7）根據(jù)式（16）對(duì)狀態(tài)進(jìn)行后驗(yàn)估計(jì)，得到xk。

（8）對(duì)粒子進(jìn)行重采樣。

（9）k＝k＋1，轉(zhuǎn)到步驟（2）。

2 算法討論

通過傳統(tǒng)采樣獲得的粒子僅是對(duì)當(dāng)前狀態(tài)的先驗(yàn)估計(jì)，這種粒子的生存能力不強(qiáng)，容易出現(xiàn)粒子分布嚴(yán)重偏離真實(shí)分布的情況，而式（12）恰好可以將觀測(cè)信息和狀態(tài)的先驗(yàn)信息有機(jī)結(jié)合在一起，充分利用觀測(cè)信息來提高采樣粒子的穩(wěn)健性。另外，SP－PF算法需要計(jì)算非線性觀測(cè)方程的雅克比矩陣，為狀態(tài)估計(jì)過程增加了額外的運(yùn)算量。由于對(duì)每個(gè)粒子的處理是相互獨(dú)立的，因此可通過并行技術(shù)（如GPU 高性能并行計(jì)算）來提高算法的處理效率。

通過式（12）得到的新粒子不僅包含了當(dāng)前時(shí)刻的觀測(cè)信息，而且也是當(dāng)前狀態(tài)的無偏估計(jì)，證明如下：

將式（9）代入式（1）中的觀測(cè)方程，可得：

將式（17）代入式（12），化解易得：

3 仿真實(shí)驗(yàn)

對(duì)于可信觀測(cè)的非線性濾波問題，本文所提出的SP－PF 算法具有良好的狀態(tài)估計(jì)精度。為了驗(yàn)證SP－PF算法的有效性，本文將SP－PF與基于序貫重要性采樣的標(biāo)準(zhǔn)粒子濾波（SPF）、輔助粒子濾波（APF）以及無跡粒子濾波（UPF）進(jìn)行了對(duì)比。實(shí)驗(yàn)使用的狀態(tài)方程、觀測(cè)方程分別為：

式中：系統(tǒng)噪聲uk服從Gamma（3，2）分布、觀測(cè)噪聲vk服從高斯分布N（0，0.0001），狀態(tài)初值為1，觀測(cè)周期為1s，觀測(cè)時(shí)間為50s，粒子數(shù)量為50。如無特別說明，以上參數(shù)均為默認(rèn)值。

實(shí)驗(yàn)采用均方根誤差（RMSE）作為性能評(píng)價(jià)指標(biāo)：

式中：M 為蒙特卡洛仿真實(shí)驗(yàn)次數(shù)，默認(rèn)取值為500；xm為估計(jì)值為真值。

圖1為一次仿真實(shí)驗(yàn)中4種濾波算法對(duì)狀態(tài)的估計(jì)結(jié)果。從圖中可以看出：UPF和SP－PF都可以很好地對(duì)狀態(tài)進(jìn)行逼近，其中，SP－PF的濾波過程更加穩(wěn)定，與真值的擬合度更高。相比于UPF和SP－PF，SPF 和APF 的狀態(tài)估計(jì)過程不夠穩(wěn)定，在部分采樣點(diǎn)上出現(xiàn)了輕微的濾波發(fā)散現(xiàn)象。在本次實(shí)驗(yàn)中，從與真值的擬合度和濾波的穩(wěn)定度兩方面考慮，SP－PF優(yōu)于其他3種算法。

圖1 不同濾波算法的狀態(tài)估計(jì)結(jié)果Fig.1 State estimation results of different filters

圖2 給出了經(jīng)過500次蒙特卡洛實(shí)驗(yàn)后SPPF與其他3 種算法的RMSE 對(duì)比圖。由圖可知：SP－PF 的RMSE 明 顯 低 于SPF、APF 和UPF，這主要是由于SP－PF的每個(gè)粒子融入了最新的觀測(cè)信息，與UPF 相比，SP－PF 沒有對(duì)狀態(tài)的后驗(yàn)分布作任何形式的假設(shè)。表1分別統(tǒng)計(jì)了圖2中4種算法的單次平均運(yùn)行時(shí)間、RMSE 均值和標(biāo)準(zhǔn)差。RMSE 的均值和標(biāo)準(zhǔn)差表征了算法的估計(jì)精度和穩(wěn)定性。從表中可以清楚地看出：與SPF、APF 和UPF 相比，SP－PF 的RMSE均值和標(biāo)準(zhǔn)差是最小的，說明了SP－PF 具有更高的估計(jì)精度和更強(qiáng)的穩(wěn)健性。在算法的平均運(yùn)行時(shí)間方 面，SPF 最 短，SP－PF 和APF 次 之，UPF最長(zhǎng)。由于SP－PF 需要計(jì)算觀測(cè)方程的雅克比矩陣，為狀態(tài)估計(jì)過程引入了額外的運(yùn)算，因而導(dǎo)致其運(yùn)行時(shí)間略高于SPF。APF 在每次迭代中需要對(duì)粒子進(jìn)行兩次采樣，兩次權(quán)值計(jì)算和兩次重采樣，且每次的處理過程類似，因此APF 的平均運(yùn)行時(shí)間約為SPF的兩倍。由于UPF 將無跡卡爾曼濾波器（UKF）作為粒子產(chǎn)生器，每個(gè)粒子均需要通過運(yùn)行UKF 來產(chǎn)生，因此UPF 的運(yùn)行效率最低。

圖2 不同濾波算法的RMSE對(duì)比圖Fig.2 Comparison of the RMSE of different filters

表1 不同濾波算法的RMSE均值、標(biāo)準(zhǔn)差和平均運(yùn)行時(shí)間Table 1 RMSE mean，RMSE standard deviation and average running time of different filters

圖3 給出了粒子數(shù)量取不同值時(shí)，SPF、APF、UPF和SP－PF 算法的RMSE 均值變化情況。顯然，增加粒子數(shù)對(duì)改善算法估計(jì)精度是有益的，隨著粒子數(shù)的增加，4種濾波算法的估計(jì)精度逐漸提高，并趨于平穩(wěn)。此外，在粒子數(shù)量取不同值的情況下，SP－PF 算法的誤差均值均低于其他3 種算法，且SP－PF 算法可以使用更少的粒子，獲得更高的性能，進(jìn)一步說明了該算法的性能優(yōu)于其他3種濾波算法。

圖3 粒子數(shù)量對(duì)算法性能的影響Fig.3 Effects of particle number on the performance of different filters

針對(duì)粒子濾波算法在實(shí)際應(yīng)用中存在的運(yùn)算量大、處理效率低的缺點(diǎn)，以SP－PF 算法為例，在英偉達(dá)型號(hào)為Tesla K20的圖形處理器（GPU）上對(duì)其進(jìn)行了并行實(shí)現(xiàn)，并與CPU 的處理時(shí)間（以Matlab處理時(shí)間為代表）進(jìn)行了對(duì)比，結(jié)果如表2所示。由表2可知：GPU 實(shí)現(xiàn)的處理時(shí)間均達(dá)到了毫秒級(jí)，且隨粒子數(shù)的增加變化緩慢，表現(xiàn)出了較強(qiáng)的數(shù)據(jù)擴(kuò)展性，與CPU 的處理時(shí)間相比，獲得了最高144倍的加速性能，極大地提高了SPPF算法的處理效率，這主要得益于GPU 中的上千個(gè)并行運(yùn)算核。因此，GPU 為實(shí)現(xiàn)粒子濾波算法的在線處理提供了一條有效途徑。

表2 SP－PF在CPU 和GPU 上實(shí)現(xiàn)的處理時(shí)間對(duì)比Table 2 Comparison of the processing time of the SP－PFimplemented on CPU and GPU

4 結(jié)束語

傳統(tǒng)粒子濾波在處理非線性濾波問題時(shí)所用的粒子是通過狀態(tài)轉(zhuǎn)移函數(shù)從上一時(shí)刻的粒子中采樣得到的，由于在采樣粒子中沒有任何關(guān)于當(dāng)前觀測(cè)值的信息，容易出現(xiàn)粒子分布嚴(yán)重偏離真實(shí)后驗(yàn)分布或粒子退化的情況，進(jìn)而影響了狀態(tài)估計(jì)的精度。針對(duì)這種情況，本文提出了一種基于二次預(yù)測(cè)的粒子濾波算法，其基本思想是將粒子采樣分為兩個(gè)階段進(jìn)行。首先采樣得到預(yù)測(cè)粒子，然后通過最小二乘估計(jì)，將觀測(cè)信息用于對(duì)預(yù)測(cè)粒子的采樣中，得到包含觀測(cè)信息的新的預(yù)測(cè)粒子，最后利用這些新粒子對(duì)狀態(tài)進(jìn)行估計(jì)。由于在粒子采樣中融入了觀測(cè)量信息，且所得到的新粒子都是對(duì)當(dāng)前狀態(tài)的無偏估計(jì)，因此促進(jìn)了采樣粒子向高似然區(qū)域的移動(dòng)，提高了各個(gè)粒子與真實(shí)狀態(tài)之間的關(guān)聯(lián)性，降低了粒子分布嚴(yán)重偏離真實(shí)后驗(yàn)分布的風(fēng)險(xiǎn)，改善了狀態(tài)估計(jì)的精度。通過計(jì)算機(jī)仿真，將該算法與標(biāo)準(zhǔn)粒子濾波、輔助粒子濾波和無跡卡爾曼粒子濾波進(jìn)行了對(duì)比，結(jié)果顯示該算法的估計(jì)精度更高、穩(wěn)定性更好。由于該算法在二次預(yù)測(cè)中需要計(jì)算觀測(cè)函數(shù)的雅克比矩陣，接下來的工作可從降低算法運(yùn)算量方面進(jìn)行深入研究。

［1］Sedai S，Bennamoum M，Huynh D Q.A Gausian process guided particle filter for tracking 3Dhuman pose in video［J］.IEEE Transactions on Image Processing，2013，22（11）：4286－4300.

［2］Yardim C，Gerstoft P，Hodgkiss W S.Tracking refractivity from clutter using Kalman and particle filters［J］.IEEE Transactions on Antennas and Propagation，2008，56（4）：1060－1069.

［3］夏楠，邱天爽，李景春，等.一種卡爾曼濾波與粒子濾波相結(jié)合的非線性濾波算法［J］.電子學(xué)報(bào)，2013，41（1）：148－152.Xia Nan，Qiu Tian－shuang，Li Jing－chun，et al.A nonlinear filter algorithm combining the Kalman filter and the particle filter［J］.Acta Electronica Sinica，2013，41（1）：148－152.

［4］Kalman R E.A new approach to linear filtering and prediction problems［J］.Transactions of the AMSE Journal of Basic Engineering，1960，82（1）：35－45.

［5］Kotecha J H，Djuric P M.Gaussian particle filter［J］.IEEE Transactions on Signal Processing，2003，51（10）：2591－2601.

［6］Li Hong－wei，Wang Jun.Particle filter for manoeuvring target tracking via passive radar measurements with glint noise［J］.IET Radar Sonar and Navigation，2012，6（3）：180－189.

［7］Arulampalam M S，Maskell S，Gordon N，et al.A tutorial on particle filters for online nonlinear／non－Gaussian Bayesian tracking［J］.IEEE Transactions on Signal Processing，2002，50（2）：174－188.

［8］Georgy Jacques，Korenberg Michael J，Bayoumi Mohamed M.Low－cost three－dimensional navigation solution for RISS／GPS integration using mixture particle filter［J］.IEEE Transactions on Vehicular Technology，2010，59（2）：599－614.

［9］戴連君，唐濤，蔡伯根.基于自適應(yīng)粒子濾波的北斗衛(wèi)星信號(hào)周跳探測(cè)［J］.吉林大學(xué)學(xué)報(bào)：工學(xué)版，2013，43（4）：1146－1152.Dai Lian－jun，Tang Tao，Cai Bo－gen.Cycle slip detection for BeiDou satellite based on adaptive particle filter［J］.Journal of Jilin University（Engineering and Technology Edition），2013，43（4）：1146－1152.

［10］Wang Ya－feng，Sun Fu－chun，Zhang You－an，et al.Central difference particle filter applied to transfer alignment for SINS on missiles［J］.IEEE Transactions on Aerospace and Electronic Systems，2012，48（1）：375－387.

［11］Pitt M K，Shephard N.Filtering via simulation：auxiliary particle filters［J］.Journal of the American Statistical Association，1999，94（446）：590－599.

［12］Zhan Rong－h(huán)ui，Xin Qin，Wan Jian－wei.Modified unscented particle filter for nonlinear Bayesian tracking［J］.Journal of Systems Engineering and Electronics，2008，19（1）：7－14.

［13］Li Hong－wei，Wang Jun，Su Hong－tao.Improved particle filter based on differential evolution［J］.Electronics Letters，2011，47（19）：1078－1079.

［14］侯靜，景占榮，羊彥.遠(yuǎn)距離干擾環(huán)境下目標(biāo)跟蹤的擴(kuò)展卡爾曼粒子濾波算法［J］.電子與信息學(xué)報(bào)，2013，35（7）：1587－1592.Hou Jing，Jing Zhan－rong，Yang Yan.Extended Kalman particle filter algorithm for target tracking in stand－off jammer［J］.Journal of Electronics ＆Information Technology，2013，35（7）：1587－1592.

［15］于洪波，王國(guó)宏，孫 蕓，等.一種融合UKF和EKF的粒子濾波狀態(tài)估計(jì)算法［J］.系統(tǒng)工程與電子技術(shù)，2013，35（7）：1375－1379.Yu Hong－bo，Wang Guo－h(huán)ong，Sun Yun，et al.Particle filtering algorithm of state estimation on fusion of UKF and EKF［J］.Systems Engineering and Electronics，2013，35（7）：1375－1379.

［16］何友，修建娟，關(guān)欣，等.雷達(dá)數(shù)據(jù)處理及應(yīng)用［M］.北京：電子工業(yè)出版社，2013：76－78.

［17］李紅偉，王俊，王海濤.一種基于差分演化的粒子濾波算法［J］.電子與信息學(xué)報(bào)，2011，33（7）：1639－1643.Li Hong－wei，Wang Jun，Wang Hai－tao.A new particle filter based on differential evolution method［J］.Journal of Electronics ＆Information on Technology，2011，33（7）：1639－1643.