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

秦 臻 高井祥 王 堅

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

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

秦 臻 高井祥 王 堅

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

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

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

1 引言

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

2 UPF算法

UPF算法步驟如下[7]：

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

4)計算粒子的權值。

5)權重進行歸一化。

7)UPF濾波結果為:

3 抗差自適應濾波

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

假設系統動力學方程與觀測方程為[8,9]：

其中,wk是系統噪聲矩陣,協方差矩陣為 Pwk,vk是觀測噪聲矩陣,協方差矩陣為 Pvk;wk與 vk不相關,且都為高斯白噪聲。

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

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

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

膨脹因子可以構造為:

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

3.2 自適應狀態參數協方差的確定

如果系統狀態發生異常時,需要通過調節狀態噪聲協方差來控制動態模型噪聲異常對狀態參數估值的影響。基于自適應濾波思想和UKF算法,利用自適應因子實時調整狀態參數協方差,從而控制動態模型異常對參數的影響。具體計算步驟如下：

1)計算自適應因子αk

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

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

其中,c為常數,取值范圍為 1.0～2.5。

2)計算 Sigma點集

Sigma點相應的權值:

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

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

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

3)預測、更新階段

根據所求得的 Sigma點集及其權值可得:

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

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

4 技術路線

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

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

圖1 AR-UPF技術流程Fig.1 Technical flow ofAR-UPF

5 算例分析

5.1 仿真驗證

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

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

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

圖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 狀態異常時UPF與AR-UPF的誤差曲線Fig.4 UPF and AR-UPF error curveswith state anomalies

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

5.2 實例分析

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

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

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

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

圖 6～8中,坐標系橫軸表示觀測歷元,縱軸表示濾波輸出與參考值之差,圖中只給出X分量的坐標差,Y、Z方向的坐標差與 X分量相似。濾波輸出量統計結果見表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 兩種濾波輸出的誤差統計結果(單位:m)Tab.2 Error results of filtering output by different algorithm(un it:m)

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

6 結語

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

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].系統仿真學報,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 楊元喜.自適應動態導航定位[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 楊元喜,等.論動態自適應濾波[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 李濤.非線性濾波在導航系統中的應用研究[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測量數據處理研究.E-mail：qinzhen-614@163.com

P207

A