后向平滑與抗差估計(jì)融合的SRCKF濾波

孫 鵬，趙長(zhǎng)勝，譚興龍，張立凱

(江蘇師范大學(xué)地理測(cè)繪與城鄉(xiāng)規(guī)劃學(xué)院，江蘇 徐州 221116)

后向平滑與抗差估計(jì)融合的SRCKF濾波

孫 鵬，趙長(zhǎng)勝，譚興龍，張立凱

(江蘇師范大學(xué)地理測(cè)繪與城鄉(xiāng)規(guī)劃學(xué)院，江蘇 徐州 221116)

將后向平滑平方根容積卡爾曼濾波用于GPS動(dòng)態(tài)單點(diǎn)定位數(shù)據(jù)處理，并探討了粗差對(duì)后向平滑濾波的影響。借鑒經(jīng)典卡爾曼濾波抗差估計(jì)思想，給出平方根容積卡爾曼濾波的抗差算法以抵抗量測(cè)粗差，而當(dāng)判斷不含粗差時(shí)使用后向平滑算法，在有效提高濾波精度的同時(shí)避免了抗差濾波對(duì)每個(gè)歷元都需進(jìn)行迭代運(yùn)算。實(shí)測(cè)GPS動(dòng)態(tài)數(shù)據(jù)驗(yàn)證了算法的有效性。

非線性濾波；平方根容積卡爾曼濾波；抗差濾波；后向平滑

擴(kuò)展卡爾曼濾波(EKF)、無跡卡爾曼濾波(UKF)及粒子濾波(PF)是常用的非線性濾波算法[1]。EKF存在線性化誤差，且常難以獲得雅可比矩陣求導(dǎo)[2]，但在單點(diǎn)定位中，觀測(cè)函數(shù)非強(qiáng)非線性函數(shù)，故常采用EKF進(jìn)行處理[3-4]。UKF無需線性化，處理強(qiáng)非線性系統(tǒng)時(shí)精度明顯優(yōu)于EKF[5]，但不適當(dāng)?shù)牟蓸硬呗院蛥?shù)選取可能導(dǎo)致UKF濾波發(fā)散[6]。PF則存在計(jì)算量大、粒子退化等問題[7-8]。容積卡爾曼濾波(CKF)[9-10]采用基于三階球面-徑向變換的容積準(zhǔn)則計(jì)算高斯加權(quán)積分近似值，數(shù)學(xué)推導(dǎo)過程嚴(yán)格，在維數(shù)較高時(shí)也可取得良好的濾波效果[11]。本文將一種后向平滑平方根容積卡爾曼濾波(BS-SRCKF)用于單點(diǎn)定位，給出SRCKF的抗差算法，并探討了抗差算法與后向平滑的融合。

1 平方根容積卡爾曼濾波(SRCKF)

計(jì)算機(jī)舍入誤差可能導(dǎo)致協(xié)方差陣失去對(duì)稱正定性，導(dǎo)致濾波失敗，而平方根容積卡爾曼濾波只計(jì)算并傳遞協(xié)方差陣平方根因子，在提高數(shù)值穩(wěn)定性的同時(shí)減小了計(jì)算量[12]。當(dāng)系統(tǒng)狀態(tài)為n維時(shí)，三階CKF取2n個(gè)容積點(diǎn)用于積分近似值的計(jì)算。設(shè)有離散非線性系統(tǒng)

(1)

(2)

1.1 時(shí)間更新

求解、更新容積點(diǎn)

(3)

狀態(tài)預(yù)測(cè)值及其協(xié)方差陣平方根因子

(4)

tria()表示一種三角化運(yùn)算[9]，其中

1.2 量測(cè)更新

求并更新容積點(diǎn)

(5)

求預(yù)測(cè)觀測(cè)值、新息協(xié)方差陣的平方根因子與互協(xié)方差陣

(6)

式中

計(jì)算增益矩陣，更新狀態(tài)及其協(xié)方差陣平方根因子

(7)

2 后向平滑平方根容積卡爾曼濾波(BS-SRCKF)

后向平滑濾波可有效提升濾波精度，在平方根容積卡爾曼濾波下，其流程可表示為

(1) 用式(3)—式(7)進(jìn)行前向?yàn)V波得k時(shí)刻濾波值。

(2) 按下式獲取k-1時(shí)刻狀態(tài)平滑值[13]

(8)

(3) 將后向平滑值作為k時(shí)刻初值再次按式(3)—式(7)前向?yàn)V波。

3 抗差平方根容積卡爾曼濾波(R-SRCKF)

當(dāng)預(yù)測(cè)信息準(zhǔn)確時(shí)，粗差將直接反映在預(yù)測(cè)殘差中。通過預(yù)測(cè)殘差及其協(xié)方差陣可構(gòu)建抗差因子，若存在粗差，放大該觀測(cè)值的噪聲協(xié)方差以降低該觀測(cè)值在狀態(tài)估值計(jì)算中的權(quán)重，即可抵抗粗差影響。

k時(shí)刻預(yù)測(cè)殘差及其協(xié)方差陣為

(9)

(10)

則參考Huber法可求得抗差因子[14]

(11)

式中，c為閾值，可取為c=1.0～1.5，當(dāng)標(biāo)準(zhǔn)化殘差大于c時(shí)，認(rèn)為該觀測(cè)值含粗差。c的取值越小，對(duì)粗差的判斷越為嚴(yán)格。求得抗差因子后，膨脹量測(cè)噪聲協(xié)方差

(12)

在非對(duì)角線上，按雙因子等價(jià)協(xié)方差模型取

(13)

以新的量測(cè)噪聲協(xié)方差陣按式(6)、式(7)求得抗差濾波值及其協(xié)方差陣，并對(duì)量測(cè)更新過程進(jìn)行迭代直至收斂。

4 帶抗差估計(jì)的后向平滑平方根容積卡爾曼濾波(BS-R-SRCKF)

后向平滑濾波采用k時(shí)刻濾波結(jié)果對(duì)k-1時(shí)刻濾波結(jié)果進(jìn)行平滑。由式(8)可知，若k時(shí)刻存在量測(cè)粗差，則k時(shí)刻濾波值將不準(zhǔn)確，進(jìn)而后向平滑值，亦即第二次前向?yàn)V波的初值也將受到粗差影響，可見，當(dāng)粗差存在時(shí)，BS-SRCKF相比SRCKF受粗差影響更大。同時(shí)，單純使用抗差濾波處理粗差需要進(jìn)行迭代求解，這將增加計(jì)算負(fù)擔(dān)。本文在平方根容積濾波下將后向平滑算法與抗差濾波融合(如圖1所示)，在判定觀測(cè)值含有粗差時(shí)迭代求解抗差濾波值，否則使用后向平滑算法提高濾波精度。

圖1 后向平滑與抗差估計(jì)融合的SRCKF流程

5 算 例

取一航攝飛機(jī)的一段GPS觀測(cè)數(shù)據(jù)進(jìn)行濾波解算，采用雙頻消電離層組合偽距，并改正對(duì)流層延遲誤差。狀態(tài)參數(shù)取

偽距量測(cè)噪聲方差初值取1 m2。系統(tǒng)噪聲協(xié)方差陣Q取

Q=diag([0.09 0.09 0.09 9 0.09 0.09 0.09 9])

P0=diag([5005005001000100100100100])

分別使用SRCKF和BS-SRCKF進(jìn)行濾波，以CSRS精密單點(diǎn)定位在線處理輸出值為參考，在WGS-84坐標(biāo)系下兩種濾波誤差如圖2所示。

現(xiàn)每隔50歷元在其中一顆衛(wèi)星的組合偽距上加50 m粗差，分別使用BS- SRCKF、BS-R-SRCKF進(jìn)行濾波，所得結(jié)果如圖3所示。

表1為各方案誤差的均方根誤差。

表1 各方案均方根誤差 m

濾波結(jié)果表明，后向平滑算法可以有效提高濾波精度，但當(dāng)量測(cè)值含有粗差時(shí)，粗差對(duì)濾波的影響明顯，而融合抗差估計(jì)的后向平滑可有效抵抗粗差、提高濾波的精度。

---表示SRCKF，…表示 BS-SRCKF圖2 SRCKF與BS-SRCKF濾波結(jié)果

…為BS- SRCKF，—為BS-R-SRCKF圖3 BS-SRCKF與BS-R-SRCKF濾波結(jié)果

6 結(jié) 語

本文將后向平滑平方根容積卡爾曼濾波用于動(dòng)態(tài)單點(diǎn)定位數(shù)據(jù)處理，針對(duì)粗差對(duì)后向平滑濾波的影響，給出了平方根容積卡爾曼濾波下的抗差因子取值。將后向平滑與抗差估計(jì)融合，當(dāng)觀測(cè)值含有粗差時(shí)進(jìn)行抗差迭代，當(dāng)不含粗差時(shí)使用后向平滑濾波，提高濾波精度的同時(shí)避免抗差濾波每個(gè)歷元都迭代帶來的計(jì)算量增大問題。實(shí)測(cè)GPS動(dòng)態(tài)數(shù)據(jù)的計(jì)算結(jié)果表明，后向平滑可有效提高濾波精度，與抗差估計(jì)的融合也切實(shí)可行。

[1] 趙長(zhǎng)勝.測(cè)量數(shù)據(jù)處理研究[M].北京：測(cè)繪出版社，2013.

[2] 譚興龍,王堅(jiān),趙長(zhǎng)勝. 神經(jīng)網(wǎng)絡(luò)輔助的GPS/INS組合導(dǎo)航自適應(yīng)UKF算法[J]. 測(cè)繪學(xué)報(bào),2015,44(4):384-391.

[3] 祁芳. 卡爾曼濾波算法在GPS非差相位精密單點(diǎn)定位中的應(yīng)用研究[D].武漢：武漢大學(xué),2003.

[4] 王堅(jiān),王金嶺,高井祥. 基于抗差EKF的GNSS導(dǎo)航模型研究[J]. 中國(guó)礦業(yè)大學(xué)學(xué)報(bào),2008,37(4):473-477.

[5] 吳江飛,雷輝. 星載GPS衛(wèi)星定軌的UKF-EKF算法[J]. 測(cè)繪學(xué)報(bào)，2014,43(5):446-451.

[6] 王小旭,潘泉,黃鶴,等. 非線性系統(tǒng)確定采樣型濾波算法綜述[J]. 控制與決策，2012,27(6):801-812.

[7] 聶建亮,楊元喜,吳富梅. 一種基于改進(jìn)粒子濾波的動(dòng)態(tài)精密單點(diǎn)定位算法[J]. 測(cè)繪學(xué)報(bào),2010,39(4):338-343.

[8] 薛長(zhǎng)虎,聶桂根,汪晶. 擴(kuò)展卡爾曼濾波與粒子濾波性能對(duì)比[J]. 測(cè)繪通報(bào),2016(4):10-14.

[9] ARASARATNAM I, HAYKIN S. Cubature Kalman Filters[J].IEEE Transactions on Automatic Control,2009,54(6):1254-1269.

[10] ARASARATNAM I. Cubature Kalman Filtering: Theory & Applications[D].Hamilton：McMaster University, 2009.

[11] 唐李軍. Cubature卡爾曼濾波及其在導(dǎo)航中的應(yīng)用研究[D].哈爾濱：哈爾濱工程大學(xué),2012.

[12] 郝燕玲,楊峻巍,陳亮,等 平方根容積卡爾曼濾波器[J]. 彈箭與制導(dǎo)學(xué)報(bào),2012,32(2):169-172.

[13] 湯霞清,黃湘遠(yuǎn),武萌,等. 平方根形式的CKF/后向平滑非線性濾波研究[J]. 裝甲兵工程學(xué)院學(xué)報(bào),2015(2):65-69.

[14] 楊元喜.自適應(yīng)動(dòng)態(tài)導(dǎo)航定位[M].北京：測(cè)繪出版社，2006.

[15] 高為廣. 自適應(yīng)融合導(dǎo)航理論與方法及其在GPS和INS中的應(yīng)用[D].鄭州：信息工程大學(xué),2005.

Square-root Cubature Kalman Filter with Backward-smoothing and Robust Estimation

SUN Peng，ZHAO Changsheng，TAN Xinglong， ZHANG Likai

(School of Geodesy and Geomatics, Jiangsu Normal University, Xuzhou 221116， China)

Backward-smoothing square-root cubature Kalman filter(BS-SRCKF) is used to caculate observation data of GPS dynamic single point positioning,and the gross errors’ bad influence on backward-smoothing filter is analyzed in this article. In order to reduce gross errors’ impacts, robust SRCKF is put forward referring to robust Kalman filter. And when coarse errors don’t exist,backward-smoothing filter can be used to improve the precision of the results and avoid iterative computation if using robust filter each epoch.Measured data is adopted to prove that the algorithm is effective.

nonlinear filtering; SRCKF; robust filter; backward smoothing

孫鵬，趙長(zhǎng)勝，譚興龍，等.后向平滑與抗差估計(jì)融合的SRCKF濾波[J].測(cè)繪通報(bào)，2017(4)：17-20.

10.13474/j.cnki.11-2246.2017.0111.

2016-07-22；

2016-12-31

江蘇省自然科學(xué)青年基金(BK20150236)；江蘇師范大學(xué)研究生科研創(chuàng)新計(jì)劃重點(diǎn)項(xiàng)目(2016YZD021)

孫 鵬(1991—)，男，碩士生，研究方向?yàn)镚NSS數(shù)據(jù)處理。E-mail：spcxs@sohu.com

P207

A

0494-0911(2017)04-0017-04