抗差自適應無味粒子濾波＊

秦 臻 高井祥 王 堅

(中國礦業(yè)大學國土環(huán)境與災害監(jiān)測國家測繪局重點實驗室,徐州 221116)

抗差自適應無味粒子濾波＊

秦 臻 高井祥 王 堅

(中國礦業(yè)大學國土環(huán)境與災害監(jiān)測國家測繪局重點實驗室,徐州 221116)

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

無味粒子濾波;抗差自適應濾波;抗差自適應無味粒子濾波;方差膨脹;濾波精度

1 引言

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

2 UPF算法

UPF算法步驟如下[7]：

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

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

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

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

3 抗差自適應濾波

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

假設系統(tǒng)動力學方程與觀測方程為[8,9]：

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

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

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

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

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

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

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

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

1)計算自適應因子αk

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

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

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

2)計算 Sigma點集

Sigma點相應的權(quán)值:

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

將式(14)得到的采樣點代入狀態(tài)方程,得到預測采樣點點集:

同理,將式(16)得到的預測采樣點代入觀測方程,得到預測測量點點集:

3)預測、更新階段

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

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

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

4 技術(shù)路線

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

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

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

5 算例分析

5.1 仿真驗證

首先采用非線性較強的標量模型算例對算法進行仿真驗證,狀態(tài)方程及觀測方程如下:

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

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

圖2 無異常時UPF與AR-UPF的誤差曲線Fig.2 UPF and AR-UPF error curveswithout anomalies

圖3 觀測異常時UPF與AR-UPF的誤差曲線Fig.3 UPF and AR-UPF error curveswith measurement outliers

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

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

5.2 實例分析

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

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

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

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

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

圖5 載體運動軌跡平面圖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)計結(jié)果(單位:m)Tab.2 Error results of filtering output by different algorithm(un it:m)

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

6 結(jié)語

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

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算法及其在目標跟蹤問題中的應用[J].系統(tǒng)仿真學報,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在探測器自主導航中的應用[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 楊元喜.自適應動態(tài)導航定位[M].北京：測繪出版社, 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 楊元喜.自適應抗差最小二乘估計[J].測繪學報,1996, (3)：207-211.(Yang Yuanxi.Adaptively robust least squares estimation[J].Acta Geodaetica Et Cartographic Sinica,1996,(3)：207-211)

9 楊元喜,等.論動態(tài)自適應濾波[J].測繪學報,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 聶建亮.采用自適應Unscented Kal man的粒子濾波[J].大地測量與地球動力學,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 李濤.非線性濾波在導航系統(tǒng)中的應用研究[D].國防科技大學,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

國家自然科學基金(40904004);教育部博士點基金(200802900501amp;200802901516);江蘇省自然科學基金(BK2009099)

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

P207

A