抗差自適應(yīng)無(wú)味粒子濾波＊

秦 臻 高井祥 王 堅(jiān)

(中國(guó)礦業(yè)大學(xué)國(guó)土環(huán)境與災(zāi)害監(jiān)測(cè)國(guó)家測(cè)繪局重點(diǎn)實(shí)驗(yàn)室,徐州 221116)

抗差自適應(yīng)無(wú)味粒子濾波＊

秦 臻 高井祥 王 堅(jiān)

(中國(guó)礦業(yè)大學(xué)國(guó)土環(huán)境與災(zāi)害監(jiān)測(cè)國(guó)家測(cè)繪局重點(diǎn)實(shí)驗(yàn)室,徐州 221116)

針對(duì)系統(tǒng)觀測(cè)值含有粗差或者存在狀態(tài)異常的情況,基于無(wú)味粒子濾波算法 (UPF)和抗差自適應(yīng)濾波原理,提出了一種新的抗差自適應(yīng)無(wú)味粒子濾波算法(AR-UPF)。該方法采用方差膨脹模型抑制觀測(cè)粗差的影響;利用自適應(yīng)UKF算法來(lái)實(shí)時(shí)調(diào)整狀態(tài)參數(shù)預(yù)報(bào)值的協(xié)方差,控制狀態(tài)異常的影響。算例分析表明,新方法能夠有效地控制觀測(cè)粗差與狀態(tài)異常,提高了濾波精度。

無(wú)味粒子濾波;抗差自適應(yīng)濾波;抗差自適應(yīng)無(wú)味粒子濾波;方差膨脹;濾波精度

1 引言

無(wú)味粒子濾波算法(UPF)是一種非線性濾波算法,它是在粒子濾波 (PF)的基礎(chǔ)上[1],采用 Unscented Kalman濾波(UKF)生成 PF的建議分布,每一次采樣后的粒子都使用UKF算法進(jìn)行更新,所得均值和方差用于下次粒子采樣,解決了傳統(tǒng) PF算法中以轉(zhuǎn)換先驗(yàn)密度函數(shù)作為建議分布所引發(fā)的各種問題[2]。目前,UPF算法已逐漸應(yīng)用于目標(biāo)跟蹤[3,4]、導(dǎo)航[5]等領(lǐng)域。在實(shí)際應(yīng)用中,由于儀器操作不當(dāng)或者其他原因會(huì)造成觀測(cè)異常,而狀態(tài)異常也會(huì)影響狀態(tài)參數(shù)的估值。系統(tǒng)存在異常,會(huì)導(dǎo)致粒子權(quán)重選取錯(cuò)誤,致使 UPF算法濾波精度降低。利用抗差自適應(yīng)濾波原理[6],構(gòu)造合理的等價(jià)權(quán)函數(shù)以及自適應(yīng)因子,進(jìn)而控制狀態(tài)異常與觀測(cè)異常對(duì)粒子權(quán)重的影響。本文在UPF算法的基礎(chǔ)上,提出了AP-UPF算法,并設(shè)計(jì)了 3種計(jì)算方案,通過比較表明,新方法的濾波結(jié)果穩(wěn)定,不僅能有效地抑制觀測(cè)值粗差的影響,而且能夠抵制狀態(tài)異常對(duì)狀態(tài)濾波估值的影響。

2 UPF算法

UPF算法步驟如下[7]：

3)根據(jù)該建議分布函數(shù)抽取新的粒子。

4)計(jì)算粒子的權(quán)值。

5)權(quán)重進(jìn)行歸一化。

7)UPF濾波結(jié)果為:

3 抗差自適應(yīng)濾波

抗差自適應(yīng)濾波的基本思想是,當(dāng)觀測(cè)值存在異常,則對(duì)觀測(cè)值采用抗差估計(jì)原則[8];當(dāng)動(dòng)力學(xué)模型存在異常,則將動(dòng)力學(xué)模型信息作為一個(gè)整體,采用自適應(yīng)因子調(diào)整動(dòng)力學(xué)模型信息對(duì)狀態(tài)參數(shù)的整體貢獻(xiàn)[9]?；诖?改進(jìn)的UPF算法采用方差膨脹原理來(lái)調(diào)節(jié)觀測(cè)噪聲的協(xié)方差;通過構(gòu)建自適應(yīng)UKF重點(diǎn)采樣模型實(shí)時(shí)調(diào)節(jié)狀態(tài)噪聲的協(xié)方差,從而控制觀測(cè)異常與動(dòng)態(tài)模型異常對(duì)狀態(tài)參數(shù)的影響。

假設(shè)系統(tǒng)動(dòng)力學(xué)方程與觀測(cè)方程為[8,9]：

其中,wk是系統(tǒng)噪聲矩陣,協(xié)方差矩陣為 Pwk,vk是觀測(cè)噪聲矩陣,協(xié)方差矩陣為 Pvk;wk與 vk不相關(guān),且都為高斯白噪聲。

3.1 觀測(cè)噪聲等價(jià)協(xié)方差的確定[10]

觀測(cè)噪聲等價(jià)協(xié)方差通過方差膨脹模型來(lái)獲得。觀測(cè)噪聲的協(xié)方差陣反映觀測(cè)值的離散程度,若觀測(cè)值精度高,可靠性好,則相應(yīng)觀測(cè)值的方差就小,從而該觀測(cè)值在狀態(tài)估計(jì)中所占的權(quán)重就大;反之,若觀測(cè)值含有粗差,可靠性差,則相應(yīng)觀測(cè)值的方差就大,從而該觀測(cè)值在狀態(tài)估計(jì)中的權(quán)重就小。所以,通過適當(dāng)擴(kuò)大異常觀測(cè)的方差來(lái)降低異常觀測(cè)對(duì)參數(shù)估值的影響。

假設(shè)觀測(cè)值不相關(guān),只考慮方差元素,則觀測(cè)值yk的方差為如果 yk中含有粗差,令其中,βk為膨脹因子,為等價(jià)方差。

膨脹因子可以構(gòu)造為:

通過構(gòu)造出的膨脹因子βk可以看出,當(dāng)觀測(cè)值含有粗差時(shí),即觀測(cè)誤差超出一定限值時(shí),其相應(yīng)的方差膨脹;反之,方差保持不變。

3.2 自適應(yīng)狀態(tài)參數(shù)協(xié)方差的確定

如果系統(tǒng)狀態(tài)發(fā)生異常時(shí),需要通過調(diào)節(jié)狀態(tài)噪聲協(xié)方差來(lái)控制動(dòng)態(tài)模型噪聲異常對(duì)狀態(tài)參數(shù)估值的影響?；谧赃m應(yīng)濾波思想和UKF算法,利用自適應(yīng)因子實(shí)時(shí)調(diào)整狀態(tài)參數(shù)協(xié)方差,從而控制動(dòng)態(tài)模型異常對(duì)參數(shù)的影響。具體計(jì)算步驟如下：

1)計(jì)算自適應(yīng)因子αk

采用狀態(tài)不符值統(tǒng)計(jì)量作為自適應(yīng)因子的判別統(tǒng)計(jì)量[6,11]。設(shè)為狀態(tài)預(yù)報(bào)值,為狀態(tài)抗差解,為狀態(tài)預(yù)報(bào)值與狀態(tài)抗差解的差值,當(dāng)相差較大時(shí),則認(rèn)為系統(tǒng)狀態(tài)發(fā)生異常。

其中,tr(·)為求跡運(yùn)算。自適應(yīng)因子分段函數(shù)形式為[12]:

其中,c為常數(shù),取值范圍為 1.0～2.5。

2)計(jì)算 Sigma點(diǎn)集

Sigma點(diǎn)相應(yīng)的權(quán)值:

其中:n為狀態(tài)向量維數(shù);λ為合成比例參數(shù),λ的值為α2(n+κ)-n;α為正的縮放比例參數(shù),它控制粒子分布的距離,所以應(yīng)取較小的數(shù)值,一般選α=1; κ通常為 0;β用于融入隨即變量的驗(yàn)前信息。

將式(14)得到的采樣點(diǎn)代入狀態(tài)方程,得到預(yù)測(cè)采樣點(diǎn)點(diǎn)集:

同理,將式(16)得到的預(yù)測(cè)采樣點(diǎn)代入觀測(cè)方程,得到預(yù)測(cè)測(cè)量點(diǎn)點(diǎn)集:

3)預(yù)測(cè)、更新階段

根據(jù)所求得的 Sigma點(diǎn)集及其權(quán)值可得:

其中,Pk|k-1是預(yù)測(cè)采樣點(diǎn)的方差,Pvv、Pxv分別為預(yù)測(cè)測(cè)量點(diǎn)的方差和協(xié)方差;和為狀態(tài)方程和觀測(cè)方程的 Sigma點(diǎn)集的均值。

4)自適應(yīng)濾波協(xié)方差的確定

4 技術(shù)路線

AP-UPF算法的基本思想是通過方差膨脹原理求得觀測(cè)噪聲的等價(jià)協(xié)方差,利用自適應(yīng)UKF算法得到重點(diǎn)密度函數(shù),然后,利用其重點(diǎn)密度函數(shù)代替經(jīng)典 PF粒子濾波的重要密度函數(shù)。技術(shù)流程見圖1,具體步驟如下：

5)重采樣。根據(jù)有效粒子數(shù)判斷退化情況,重新生成一個(gè)新的粒子集合{i=1,…,M},且滿足6)狀態(tài)更新。

圖1 AR-UPF技術(shù)流程Fig.1 Technical flow ofAR-UPF

5 算例分析

5.1 仿真驗(yàn)證

首先采用非線性較強(qiáng)的標(biāo)量模型算例對(duì)算法進(jìn)行仿真驗(yàn)證,狀態(tài)方程及觀測(cè)方程如下:

式中:wt和 vt均為獨(dú)立的高斯白噪聲序列,其中,wt～N(0,5),vt～N(0,0.1)。仿真粒子數(shù)為 200,仿真歷元數(shù)為 200,采樣間隔為 1 s。分別運(yùn)用標(biāo)準(zhǔn)UPF算法和抗差自適應(yīng) UPF算法進(jìn)行比較計(jì)算,3種方案如下：1)觀測(cè)值與動(dòng)態(tài)模型都無(wú)異常;2)50～100歷元間加入連續(xù)觀測(cè)粗差、動(dòng)態(tài)模型無(wú)異常; 3)動(dòng)態(tài)模型異常、觀測(cè)值無(wú)異常。3種方案的計(jì)算結(jié)果與真值的差值曲線見圖 2～6,對(duì)應(yīng)的 RMS見表1。

由圖 2可知,在模型沒有異常的情況下,UPF算法與AP-UPF算法濾波結(jié)果基本一致,只是由于粒子選取的隨機(jī)性,粒子權(quán)重的選取存在細(xì)微差別。當(dāng)觀測(cè)值加入連續(xù)性粗差,與圖 2(a)相比,UPF算法濾波結(jié)果 (圖 3(a))受粗差的影響較大,濾波精度明顯降低;而 AP-UPF算法對(duì)異常觀測(cè)值的方差進(jìn)行膨脹處理,從而減小了粗差對(duì)粒子權(quán)重的影響,從誤差曲線(圖 3(b))可以看出,AP-UPF算法起到一定的抵制粗差的效果;圖 2(b)與圖 3(b)相比,后者的誤差曲線也受到粗差的影響,這是由于模型的非線性程度較高,非線性模型誤差較大引起的。當(dāng)狀態(tài)發(fā)生異常時(shí),通過圖 4可知,AP-UPF算法通過構(gòu)造自適應(yīng)因子調(diào)整動(dòng)態(tài)模型協(xié)方差,從而降低了狀態(tài)異常對(duì) UKF重點(diǎn)密度采樣過程的影響。綜合以上分析,抗差自適應(yīng)UPF從理論上不僅能夠抵制觀測(cè)異常的影響,而且對(duì)狀態(tài)異常具有較強(qiáng)的控制能力。

圖2 無(wú)異常時(shí)UPF與AR-UPF的誤差曲線Fig.2 UPF and AR-UPF error curveswithout anomalies

圖3 觀測(cè)異常時(shí)UPF與AR-UPF的誤差曲線Fig.3 UPF and AR-UPF error curveswith measurement outliers

圖4 狀態(tài)異常時(shí)UPF與AR-UPF的誤差曲線Fig.4 UPF and AR-UPF error curveswith state anomalies

表 1給出了 3種方案的RMS誤差統(tǒng)計(jì)結(jié)果,可以看出第一種方案中AP-UPF算法的 RMS誤差與UPF算法基本相等,而后兩種方案中 AP-UPF算法的RMS誤差均小于UPF算法,這與圖 2～4的濾波結(jié)果分析相一致。

5.2 實(shí)例分析

為了實(shí)際分析抗差自適應(yīng)無(wú)味粒子濾波算法的性能,真實(shí)算例選取 2009年 12月 6日采集的 GPS動(dòng)態(tài)定位數(shù)據(jù),實(shí)驗(yàn)利用自行研制的 GPS動(dòng)態(tài)定位實(shí)驗(yàn)平臺(tái),實(shí)驗(yàn)地點(diǎn)選取為中國(guó)礦業(yè)大學(xué)主樓廣場(chǎng)前,數(shù)據(jù)采集頻率為 1 s。初始化時(shí)間為 30分鐘,目的是保證初始定位坐標(biāo)的精度;實(shí)驗(yàn)平臺(tái)做大范圍機(jī)動(dòng),其運(yùn)動(dòng)軌跡見圖 5。解算中利用高精度的差分定位結(jié)果作為參考值,單點(diǎn)偽距定位進(jìn)行各種濾波解算,其結(jié)果與參考值的差值進(jìn)行比較。濾波過程中,初值 X0、P0由靜態(tài)定位給定,狀態(tài)模型動(dòng)態(tài)噪聲方差陣Q取為對(duì)角陣,對(duì)角元素均為 5×10-4,觀測(cè)噪聲方差陣 R也取對(duì)角陣,對(duì)角元素均為 5 m2。取 100歷元,分別采用以下 3種方案進(jìn)行處理：

方案 1：擴(kuò)展卡爾曼濾波(EKF);

方案 2：UPF(粒子數(shù) 1 000);

方案 3：AP-UPF(粒子數(shù) 1 000)。

圖 6～8中,坐標(biāo)系橫軸表示觀測(cè)歷元,縱軸表示濾波輸出與參考值之差,圖中只給出X分量的坐標(biāo)差,Y、Z方向的坐標(biāo)差與 X分量相似。濾波輸出量統(tǒng)計(jì)結(jié)果見表2。

圖5 載體運(yùn)動(dòng)軌跡平面圖Fig.5 Vehicle tracking

圖6 EKF殘差Fig.6 Residual errors of EKF

圖7 UPF殘差Fig.7 Residual errors ofUPF

圖8 AP-UPF殘差Fig.8 Residual errors ofAP-UPF

表 2 兩種濾波輸出的誤差統(tǒng)計(jì)結(jié)果(單位:m)Tab.2 Error results of filtering output by different algorithm(un it:m)

結(jié)合濾波圖,分析上述統(tǒng)計(jì)結(jié)果可知：1)從圖 5可以看到,擴(kuò)展 Kalman殘差曲線經(jīng)波動(dòng)后再趨于平穩(wěn)狀態(tài),這是由于濾波初值選取的可靠性直接影響擴(kuò)展 Kalman濾波收斂的效率;2)分析圖 6、7和表 2可知,與擴(kuò)展 Kalman濾波相比,UPF算法對(duì) GPS動(dòng)態(tài)定位精度有明顯的提高,經(jīng)濾波后 GPS位置定位精度在 X分量上接近 8 m,在擴(kuò)展 Kal man濾波基礎(chǔ)上,精度提高了 36%。精度提高的原因主要是無(wú)味粒子濾波通過UT變換結(jié)合蒙特卡羅模擬方法來(lái)實(shí)現(xiàn)遞推貝葉斯濾波,用于非線性狀態(tài)模型和觀測(cè)模型的動(dòng)態(tài)濾波系統(tǒng),從而避免了線性化而忽略高次項(xiàng)所帶來(lái)的舍入誤差;其次,無(wú)味粒子濾波不要求觀測(cè)噪聲和動(dòng)力學(xué)模型噪聲為高斯白噪聲,能夠較好地抵制有色噪聲的影響;第三,無(wú)味粒子濾波充分考慮到了觀測(cè)值的影響,克服了傳統(tǒng)粒子算法中粒子發(fā)散問題;3)分析圖 7、圖 8和表 2可知,與UPF算法相比,AP-UPF算法定位精度為 2 m,精度提高了75%。精度提高的原因?yàn)?GPS做大幅度機(jī)動(dòng)或GPS信號(hào)受到干擾時(shí),觀測(cè)值受到污染,此時(shí)新算法通過對(duì)異常觀測(cè)值的方差進(jìn)行膨脹處理,通過構(gòu)造自適應(yīng)因子調(diào)整動(dòng)態(tài)模型協(xié)方差,從而降低了狀態(tài)異常對(duì)UKF重點(diǎn)密度采樣過程的影響,從而明顯地提高了 GPS動(dòng)態(tài)定位精度。

6 結(jié)語(yǔ)

針對(duì)觀測(cè)值含有粗差或狀態(tài)發(fā)生異常的情況,本文提出抗差自適應(yīng)UPF算法。它是UPF濾波與抗差自適應(yīng)濾波相結(jié)合的算法。其獨(dú)特之處在于它能夠自適應(yīng)地控制動(dòng)態(tài)模型異常與觀測(cè)異常的影響,不僅合理地平衡了動(dòng)力學(xué)模型預(yù)報(bào)信息與觀測(cè)信息的權(quán)比,而且一定程度上抵制了粗差對(duì)粒子重要性權(quán)重的影響。通過仿真分析與實(shí)際算例結(jié)果,與UPF算法相比,抗差自適應(yīng)UPF算法具有一定的抗差性,說明該算法是一種有效算法。對(duì)于如何降低非線性誤差的影響與完善自適應(yīng)因子的結(jié)構(gòu)等方面,有待于進(jìn)一步改進(jìn)。

1 Chen Z.Bayesian filtering：from Kalman filters to particle filters,and beyond[R].Hamilton：McMaster University, 2003.

2 Mer we R V,et al.The unscented particle filter[R].Technical Report CUED/F-I NPENG/TR 380,Cambridge University EngineeringDepartment,2000.

3 Guo Ronghua and Qin Zheng.An unscented particle filter for ground maneuvering target tracking[J].Journal of Zhejiang University(Science A：An International Applied Physicsamp; Engineering Journal),2007(10)：1 588-1 595.

4 李景熹,王樹宗.UPF算法及其在目標(biāo)跟蹤問題中的應(yīng)用[J].系統(tǒng)仿真學(xué)報(bào),2007,19(3)：675-677.(Li Jingxi andWang Shuzong.UPF algorithm and its application on target tracking problem[J].Journal of System Simulation, 2007,19(3)：675-677)

5 隋樹林,等.UPF在探測(cè)器自主導(dǎo)航中的應(yīng)用[J].微計(jì)算機(jī)信息,2008(4)：287-288.(Sui Shulin,et al.The use of unscented particle filter in the probes autonomous navigation [J].Microcomputer Information,2008,(4)：287-288)

6 楊元喜.自適應(yīng)動(dòng)態(tài)導(dǎo)航定位[M].北京：測(cè)繪出版社, 2006.(Yang Yuanxi.Adaptive navigation and kinematic positioning[M].Beijing：Publishing House of Surveying and Mapping,2006)

7 Julier S J andUhlmann J K.A new method for the nonlineartransformation of means and covariances in filters and estimators[J].IEEE Trans.A.C.(S0018-9286),2000,45 (3)：477-482.

8 楊元喜.自適應(yīng)抗差最小二乘估計(jì)[J].測(cè)繪學(xué)報(bào),1996, (3)：207-211.(Yang Yuanxi.Adaptively robust least squares estimation[J].Acta Geodaetica Et Cartographic Sinica,1996,(3)：207-211)

9 楊元喜,等.論動(dòng)態(tài)自適應(yīng)濾波[J].測(cè)繪學(xué)報(bào),2001,30 (4)：293-298.(Yang Yuanxi,et al.Adaptive robust filtering for kinematic GPS Positioning[J].Acta Geodaetica Et Cartographic Sinica,2001,30(4)：293-298)

10 Yang Yuanxi,et al.Robust estimator for correlated observations based on bifactor equivalent weights[J].Journal of Geodesy,2002,76(6/7)：353-358.

11 Yang Yuanxi,et al.Adaptively Robust Filtering for Kinematic Geodetic Position[J].Journal of Geodesy,2001,75 (2)：109-116.

12 聶建亮.采用自適應(yīng)Unscented Kal man的粒子濾波[J].大地測(cè)量與地球動(dòng)力學(xué),2008,(3)：87-91.(Nie Jianliang.Particle filter based on daptive unscented Kalman filter[J].Journal of geodesy and geodynamics,2008,(3)：87-91)

13 李濤.非線性濾波在導(dǎo)航系統(tǒng)中的應(yīng)用研究[D].國(guó)防科技大學(xué),2003.(Li Tao.Research on application of nonlinear filtering in navigation system[D].NationalUniversity ofDefense Technology,2003)

ROBUST ADAPTIVE SCENTLESS PARTICLE FILTER

Qin Zhen,Gao Jingxiang andWang Jian
(China University of M ining and Technology,Key Laboratory forLand Environm ent and D isasterM onitoring of SBSM,Xuzhou 221116)

In order to resist the bad influence ofmeasurement outliers and state model errors,a new robust adaptive unscented particle filter isproposed,which is based on unscented particle filter algorithm and adaptively robust filter theory.First,the new method uses the variance expandingmodel to resistmeasurementoutliers.Second, the adaptive unscented Kalman filter(UKF)is used to adjust the covariance of state predicted value,and to reduce the influence of state model errors.The examples have proved that the new procedure of the robust adaptive unscented particle filter is feasible and efficient.

unscented particle filter(UPF);robust adaptive filter;RobustAdaptive Unscented Particle Filter(ARUPF);variance inflation;precision of the filter

1671-5942(2010)05-0144-06

2010-05-23

國(guó)家自然科學(xué)基金(40904004);教育部博士點(diǎn)基金(200802900501amp;200802901516);江蘇省自然科學(xué)基金(BK2009099)

秦臻,男,1986年生,碩士研究生,主要從事 GNSS測(cè)量數(shù)據(jù)處理研究.E-mail：qinzhen-614@163.com

P207

A