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

彭 月

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

?

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

彭 月

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

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

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

0 引言

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

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

1 卡爾曼濾波

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

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

(1)

Lk=AkXk+ek。

(2)

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

2 抗差卡爾曼濾波

2.1 抗差卡爾曼濾波原理

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

(3)

(4)

(5)

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

(6)

2.2 抗差判斷量

(7)

(8)

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

(9)

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

3 實測與結(jié)果分析

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

3.1 觀測方程

(10)

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

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

(11)

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

V=AX-L。

(12)

3.2 狀態(tài)方程

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

(13)

其中：

(14)

(15)

3.3 實驗結(jié)果

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

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

圖2 抗差卡爾曼濾波軌跡

3.4 結(jié)果分析

對比分析圖1～圖4可知：抗差卡爾曼濾波方法的定位精度比傳統(tǒng)卡爾曼濾波要高，其跟蹤軌跡基本吻合參考軌跡；而傳統(tǒng)卡爾曼濾波方法在濾波過程中出現(xiàn)多次濾波發(fā)散的現(xiàn)象，定位偏差較大。分析圖5和圖6可知：抗差卡爾曼濾波軌跡與參考軌跡的定位偏差的均值在m級，標(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)，抗差卡爾曼濾波在個別觀測歷元也出現(xiàn)了幾十米的定位偏差，這是由于觀測誤差引起的。車輛在市區(qū)的行駛軌跡極為復(fù)雜，多路徑誤差較嚴(yán)重，且易出現(xiàn)接收機(jī)信號失鎖的情況；這些都會導(dǎo)致出現(xiàn)較大的觀測誤差?？柭鼮V波同時受到運(yùn)動模型誤差和觀測誤差的影響[15]。本文所提出的抗差濾波方法能有效抵抗運(yùn)動模型誤差，但對觀測誤差的作用不明顯；因此在觀測誤差較大時，該抗差濾波也會出現(xiàn)較大定位偏差。

4 結(jié)束語

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

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

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

[3] 王宏強(qiáng)，黎湘，劉丹，等.非線性系統(tǒng)中的機(jī)動目標(biāo)跟蹤算法[J].國防科學(xué)技術(shù)大學(xué)學(xué)報，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].測繪學(xué)報，2010，39(5)：522-523.

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

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

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

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

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

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

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

[14]李春華，蔡成林，梁愈高，等.一種高精度的北斗偽距單點定位加權(quán)算法[J].測繪科學(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—)，女(土家族)，四川成都人，講師，研究方向為測繪、導(dǎo)航等應(yīng)用。

彭月.一種高效抗差卡爾曼濾波的導(dǎo)航應(yīng)用[J].導(dǎo)航定位學(xué)報，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