彭 月

(四川省測(cè)繪地理信息局 西南測(cè)繪職工培訓(xùn)中心，四川 成都 610100)

?

一種高效抗差卡爾曼濾波的導(dǎo)航應(yīng)用

彭 月

(四川省測(cè)繪地理信息局 西南測(cè)繪職工培訓(xùn)中心，四川 成都 610100)

針對(duì)目標(biāo)導(dǎo)航定位中傳統(tǒng)卡爾曼濾波結(jié)果易受運(yùn)動(dòng)模型誤差影響的問(wèn)題，提出一種高效抗差卡爾曼濾波方法：以卡爾曼濾波中目標(biāo)位置參數(shù)的預(yù)測(cè)狀態(tài)不符值的標(biāo)準(zhǔn)化殘差作為抗差判斷量，對(duì)當(dāng)前運(yùn)動(dòng)模型的準(zhǔn)確程度進(jìn)行自適應(yīng)判斷；進(jìn)而確定是否利用運(yùn)動(dòng)模型的預(yù)測(cè)信息進(jìn)行濾波。實(shí)驗(yàn)結(jié)果表明：在復(fù)雜的運(yùn)動(dòng)環(huán)境中，該方法能夠有效抵抗運(yùn)動(dòng)模型誤差。

抗差卡爾曼濾波；運(yùn)動(dòng)模型誤差；預(yù)測(cè)狀態(tài)不符值；標(biāo)準(zhǔn)化殘差

0 引言

卡爾曼濾波方法應(yīng)用在動(dòng)態(tài)導(dǎo)航中時(shí)，需要有準(zhǔn)確的運(yùn)動(dòng)模型才能保證濾波精度[1-2]。關(guān)于目標(biāo)運(yùn)動(dòng)模型，國(guó)內(nèi)外學(xué)者分別提出了勻速模型、勻加速模型、時(shí)間相關(guān)模型、“當(dāng)前”統(tǒng)計(jì)模型等[3-5]。目前，單一的運(yùn)動(dòng)模型無(wú)法完全準(zhǔn)確地描述實(shí)際中所有機(jī)動(dòng)情況；當(dāng)運(yùn)動(dòng)模型與目標(biāo)機(jī)動(dòng)情況差異較大時(shí)，使用傳統(tǒng)卡爾曼濾波方法將導(dǎo)致定位精度不理想[6]。

卡爾曼濾波方法中，運(yùn)動(dòng)模型的準(zhǔn)確程度體現(xiàn)于預(yù)測(cè)狀態(tài)不符值[7]。為了抑制運(yùn)動(dòng)模型誤差，本文根據(jù)卡爾曼濾波中目標(biāo)位置參數(shù)的預(yù)測(cè)狀態(tài)不符值的標(biāo)準(zhǔn)化殘差實(shí)現(xiàn)自適應(yīng)抗差濾波。

1 卡爾曼濾波

卡爾曼濾波由狀態(tài)方程和觀測(cè)方程組成[1]，即：

Xk=φk,k-1Xk-1+Wk;

(1)

Lk=AkXk+ek。

(2)

式中：Xk為tk時(shí)刻的狀態(tài)向量；Xk-1為tk-1時(shí)刻狀態(tài)估計(jì)向量；φk,k-1為狀態(tài)轉(zhuǎn)移矩陣；Wk為狀態(tài)模型噪聲；Lk為觀測(cè)向量；Ak為觀測(cè)設(shè)計(jì)矩陣；ek為觀測(cè)噪聲。

2 抗差卡爾曼濾波

2.1 抗差卡爾曼濾波原理

卡爾曼濾波中，按照不同的求解準(zhǔn)則可以得到不同的濾波解[8]。若運(yùn)動(dòng)模型較準(zhǔn)確，則預(yù)測(cè)的狀態(tài)信息可信度較高，應(yīng)選擇傳統(tǒng)濾波求解準(zhǔn)則為

(3)

(4)

(5)

當(dāng)運(yùn)動(dòng)模型不適合目標(biāo)的機(jī)動(dòng)情況時(shí)，預(yù)測(cè)狀態(tài)信息誤差較大，如不作抗差處理，其定位精度會(huì)急劇降低[9]。針對(duì)運(yùn)動(dòng)模型誤差，業(yè)界主要有2種處理方式：一種是調(diào)整運(yùn)動(dòng)模型[10]；另一種是增加狀態(tài)模型噪聲[7]。論文提出一種簡(jiǎn)單高效的抗差處理方法：當(dāng)判斷量判斷出運(yùn)動(dòng)模型誤差較大時(shí)，直接忽略運(yùn)動(dòng)模型預(yù)測(cè)信息。這其實(shí)是增加狀態(tài)模型噪聲方法的一種特例，相當(dāng)于將狀態(tài)模型噪聲增加到無(wú)窮大。忽略運(yùn)動(dòng)模型預(yù)測(cè)信息，建立抗差濾波求解的準(zhǔn)則為

(6)

2.2 抗差判斷量

(7)

(8)

以各歷元的標(biāo)準(zhǔn)化殘差向量的絕對(duì)值均值aver作為為抗差判斷量，即

(9)

當(dāng)aver小于閾值時(shí)，說(shuō)明濾波所使用的運(yùn)動(dòng)模型與實(shí)際機(jī)動(dòng)情況相符，則選擇傳統(tǒng)濾波求解準(zhǔn)則；否則，選擇抗差濾波求解準(zhǔn)則。另外，當(dāng)k=1時(shí)選擇抗差濾波求解準(zhǔn)則。本文實(shí)驗(yàn)中，閾值取1。

3 實(shí)測(cè)與結(jié)果分析

車輛在路段復(fù)雜的市區(qū)內(nèi)行駛，車載GPS接收機(jī)的采樣間隔Δt為1 s。采用擴(kuò)展卡爾曼濾波方法[11-12]，利用測(cè)距碼偽距定位、多普勒頻率定速[12-13]；以高精度的加權(quán)最小二乘偽距定位軌跡作為接收機(jī)的參考軌跡。其中加權(quán)最小二乘偽距定位是依據(jù)衛(wèi)星高度角調(diào)整衛(wèi)星數(shù)據(jù)的權(quán)值，從而得到高精度定位軌跡[14]。

3.1 觀測(cè)方程

(10)

式中：ρju為經(jīng)過(guò)誤差改正后衛(wèi)星j到用戶u的偽距；Rj為衛(wèi)星j到用戶u近似位置的距離；vj為修正后的偽距誤差；(Δxu,Δyu,Δzu)為用戶位置改正量；Δbu為鐘差改正量。

利用多普勒頻率定速，將式(10)對(duì)時(shí)間求導(dǎo)，得

(11)

綜合式(10)和式(11)，求出觀測(cè)方程，形如

V=AX-L。

(12)

3.2 狀態(tài)方程

卡爾曼濾波所使用的模型為勻加速模型，則其狀態(tài)方程為

(13)

其中：

(14)

(15)

3.3 實(shí)驗(yàn)結(jié)果

分別對(duì)接收機(jī)觀測(cè)數(shù)據(jù)進(jìn)行傳統(tǒng)卡爾曼濾波和本文提出的抗差卡爾曼濾波，并將濾波定位軌跡分別與參考軌跡比較，其x、y方向的對(duì)比結(jié)果見(jiàn)圖1、圖2，定位偏差見(jiàn)圖3、圖4；坐標(biāo)分量方向上各歷元定位偏差的均值和標(biāo)準(zhǔn)差見(jiàn)圖5和圖6。

圖1 傳統(tǒng)卡爾曼濾波軌跡

圖2 抗差卡爾曼濾波軌跡

3.4 結(jié)果分析

對(duì)比分析圖1～圖4可知：抗差卡爾曼濾波方法的定位精度比傳統(tǒng)卡爾曼濾波要高，其跟蹤軌跡基本吻合參考軌跡；而傳統(tǒng)卡爾曼濾波方法在濾波過(guò)程中出現(xiàn)多次濾波發(fā)散的現(xiàn)象，定位偏差較大。分析圖5和圖6可知：抗差卡爾曼濾波軌跡與參考軌跡的定位偏差的均值在m級(jí)，標(biāo)準(zhǔn)差的差在10 m內(nèi)；而傳統(tǒng)卡爾曼濾波軌跡的定位偏差均值達(dá)到了3 m以上，標(biāo)準(zhǔn)差達(dá)到80 m。因此相比于傳統(tǒng)卡爾曼濾波，抗差卡爾曼濾波具有良好的抗差效果，魯棒性較強(qiáng)。

圖3 傳統(tǒng)卡爾曼濾波xy方向誤差

圖4 抗差卡爾曼濾波xy方向誤差

圖5 2種濾波方法的定位偏差均值

圖6 2種濾波方法的定位偏差標(biāo)準(zhǔn)差

另外，由圖4發(fā)現(xiàn)，抗差卡爾曼濾波在個(gè)別觀測(cè)歷元也出現(xiàn)了幾十米的定位偏差，這是由于觀測(cè)誤差引起的。車輛在市區(qū)的行駛軌跡極為復(fù)雜，多路徑誤差較嚴(yán)重，且易出現(xiàn)接收機(jī)信號(hào)失鎖的情況；這些都會(huì)導(dǎo)致出現(xiàn)較大的觀測(cè)誤差?？柭鼮V波同時(shí)受到運(yùn)動(dòng)模型誤差和觀測(cè)誤差的影響[15]。本文所提出的抗差濾波方法能有效抵抗運(yùn)動(dòng)模型誤差，但對(duì)觀測(cè)誤差的作用不明顯；因此在觀測(cè)誤差較大時(shí)，該抗差濾波也會(huì)出現(xiàn)較大定位偏差。

4 結(jié)束語(yǔ)

本文提出的抗差卡爾曼濾波利用位置狀態(tài)參數(shù)預(yù)測(cè)狀態(tài)不符值的標(biāo)準(zhǔn)化殘差自適應(yīng)抗差；相比于傳統(tǒng)卡爾曼濾波，該方法能簡(jiǎn)單有效地抵抗運(yùn)動(dòng)模型誤差，具有魯棒性較強(qiáng)、算法簡(jiǎn)單有效的特點(diǎn)，適用于復(fù)雜的運(yùn)動(dòng)場(chǎng)景。需要指出的是，該抗差卡爾曼濾波方法的缺點(diǎn)是抵抗觀測(cè)誤差的效果不理想。如何同時(shí)有效地抑制運(yùn)動(dòng)模型誤差和觀測(cè)誤差對(duì)濾波結(jié)果的影響，是下一步需要研究的問(wèn)題。

[1] 楊元喜，任夏，許艷.自適應(yīng)抗差濾波理論及應(yīng)用的主要進(jìn)展[J].導(dǎo)航定位學(xué)報(bào)，2013，1(1)：9-15.

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

[3] 王宏強(qiáng)，黎湘，劉丹，等.非線性系統(tǒng)中的機(jī)動(dòng)目標(biāo)跟蹤算法[J].國(guó)防科學(xué)技術(shù)大學(xué)學(xué)報(bào)，2002，24(4)：57-60.

[4] ZHOU Hongren，KUMAR K.A “current” statistical model and adaptive algorithm for estimating maneuvering targets[J].AIAA Journal，Guidance，Control and Dynamics，1984，7(5)：1102-1106.

[5] LI Rong，VESSELIN P.Survey of maneuvering target tracking.Part I：Dynamic Models[J].IEEE Transactions on Aerospace and Electronic Systems：2003，39(4)：1333-1362.

[6] 吳富梅，楊元喜.一種兩步自適應(yīng)抗差Kalman濾波在GPS/INS組合導(dǎo)航中的應(yīng)用[J].測(cè)繪學(xué)報(bào)，2010，39(5)：522-523.

[7] 吳富梅，楊元喜，崔先強(qiáng).利用部分狀態(tài)不符值構(gòu)造的自適應(yīng)因子在GPS/INS緊組合導(dǎo)航中的應(yīng)用[J].武漢大學(xué)學(xué)報(bào)·信息科學(xué)版，2010，35(2)：156-157.

[8] KAY S M.統(tǒng)計(jì)信號(hào)處理基礎(chǔ)-估計(jì)與檢測(cè)理論(卷I、卷II合集)[M].羅鵬飛,張文明,劉忠,等譯.北京:電子工業(yè)出版社,2014:289-311.

[9] 楊元喜，吳富梅.臨界值可變的抗差估計(jì)等價(jià)權(quán)函數(shù)[J].測(cè)繪科學(xué)技術(shù)學(xué)報(bào)，2006，23(5)：317-320.

[10]羅笑冰.強(qiáng)機(jī)動(dòng)目標(biāo)跟蹤技術(shù)研究[D].長(zhǎng)沙：國(guó)防科學(xué)技術(shù)大學(xué)，2007：15-23.

[11]謝鋼.GPS原理與接收機(jī)設(shè)計(jì)[M].北京：電子工業(yè)出版社，2009：152-153.

[12]何海波，楊元喜，孫中苗.GPS多普勒頻移測(cè)量速度模型與誤差分析[J].測(cè)繪學(xué)院學(xué)報(bào)，2003，20(2)：79-82.

[13]袁愛(ài)裕，秦紅磊.UKF在GPS/INS偽距、偽距率組合導(dǎo)航中的應(yīng)用[J].雷達(dá)科學(xué)與技術(shù)，2009，7(1)：59-64.

[14]李春華，蔡成林，梁愈高，等.一種高精度的北斗偽距單點(diǎn)定位加權(quán)算法[J].測(cè)繪科學(xué)，2015，40(9)：33-38.

[15]YANG Yuanxi，GAO Weiguang.Comparison of two fading filters and adaptively robust filter[J].Geo-spatial Information Science，2007，10(3)：200-203.

Application of an efficient robust Kalman filtering in navigation

PENG Yue

(Southwest Employee Training Center of Surveying and Mapping，Sichuan Bureau of Surveying Mapping and Geoinformation，Chengdu，Sichuan 610100，China)

Aiming at the problem that the traditional Kalman filter is easily affected by the error of motion models，the paper proposed a robust method of Kalman filter which can resist the motion model error efficiently.It used the standardized residuals of the predictive position state errors as judgment for anti-error of the motion model，whose accuracy was judged then，so as to determine whether use of the predictive information to filter.Experimental results showed that this robust method could effectively weaken the influence of motion model error in complex movement environment.

robust Kalman filtering；motion model error；predictive position errors；standardized residuals

2016-02-18

彭月(1989—)，女(土家族)，四川成都人，講師，研究方向?yàn)闇y(cè)繪、導(dǎo)航等應(yīng)用。

彭月.一種高效抗差卡爾曼濾波的導(dǎo)航應(yīng)用[J].導(dǎo)航定位學(xué)報(bào)，2016，4(4)：104-107.(PENG Yue.Application of an efficient robust Kalman filtering in navigation[J].Journal of Navigation and Positioning，2016，4(4)：104-107.)

10.16547/j.cnki.10-1096.20160420.

P228

A

2095-4999(2016)04-0104-04