GNSS/SINS定位穩(wěn)健SE2(3)-EKF方法

李 昕,孟碩林,黃觀文,張 勤,李晗旭

長(zhǎng)安大學(xué)地質(zhì)工程與測(cè)繪學(xué)院,陜西 西安 710054

GNSS/SINS組合導(dǎo)航中傳統(tǒng)EKF框架中位置和速度等狀態(tài)誤差定義只考慮了大小差異,忽略了方向上的差異,當(dāng)濾波初值尤其是姿態(tài)失準(zhǔn)角誤差較大時(shí),引起的狀態(tài)誤差坐標(biāo)系定義不一致的問(wèn)題較為突出[1-3]。針對(duì)此問(wèn)題,文獻(xiàn)[4]提出了一種速度變換旨在消去狀態(tài)變換矩陣中的比力項(xiàng),對(duì)應(yīng)的φ角誤差模型保證了速度誤差狀態(tài)定義的坐標(biāo)系一致性。文獻(xiàn)[5]基于狀態(tài)誤差矢量的幾何變換提出了幾何擴(kuò)展卡爾曼濾波(GEKF)算法,在模擬和實(shí)測(cè)數(shù)據(jù)中均取得了優(yōu)于傳統(tǒng)EKF的良好性能。由于EKF是對(duì)非線性狀態(tài)和量測(cè)方程在狀態(tài)初值上進(jìn)行一階近似線性化和離散化處理,一旦狀態(tài)初值即線性化點(diǎn)誤差過(guò)大,直接導(dǎo)致依賴狀態(tài)估計(jì)值的狀態(tài)雅可比(F矩陣)和觀測(cè)雅可比矩陣(H矩陣)出現(xiàn)較大偏差,利用F矩陣和H矩陣?yán)^續(xù)誤差傳遞和量測(cè)更新,又進(jìn)一步放大誤差,整個(gè)誤差傳遞系統(tǒng)形成正反饋,最終導(dǎo)致濾波器發(fā)散。另外EKF在組合導(dǎo)航狀態(tài)估計(jì)中還存在所謂的方差估計(jì)不一致問(wèn)題,即對(duì)本來(lái)不可觀的狀態(tài)也進(jìn)行了濾波估計(jì),直接引起誤差傳遞出現(xiàn)為較大的偏差,以及過(guò)于樂(lè)觀的協(xié)方差估計(jì),這個(gè)現(xiàn)象在地圖同步定位與建圖(SLAM)領(lǐng)域較常見(jiàn),在載體震動(dòng)等不利運(yùn)動(dòng)狀態(tài)下也會(huì)導(dǎo)致IMU相關(guān)狀態(tài)矩陣誤差迅速累積。文獻(xiàn)[6]指出導(dǎo)致GNSS/SINS組合導(dǎo)航系統(tǒng)方差估計(jì)不一致問(wèn)題的主要原因在于系統(tǒng)矩陣中的比力項(xiàng)影響,并在文獻(xiàn)[4]的研究基礎(chǔ)上進(jìn)一步提出了ST-EKF,解決了GNSS/SINS組合導(dǎo)航中因高頻振動(dòng)等引起的方差估計(jì)不一致問(wèn)題[6-7]。

近些年,基于李群和流形理論又發(fā)展了一種濾波,即SEk(3)-EKF,其核心思想是將姿態(tài)、速度及位置(AVP)等狀態(tài)及誤差定義在群空間而非傳統(tǒng)歐氏空間,根據(jù)李群的乘法封閉特性以及李群和李代數(shù)之間的仿射特性,重新設(shè)計(jì)顧及姿態(tài)誤差的更緊湊的位置、坐標(biāo)狀態(tài)誤差動(dòng)力學(xué)方程,旨在滿足誤差傳遞F矩陣或GNSS等量測(cè)H矩陣保持不變或緩變,實(shí)現(xiàn)F或H矩陣與狀態(tài)估值獨(dú)立,因此能夠做到“不變”,也稱之為不變EKF[8-10]。不變EKF對(duì)應(yīng)的誤差定義具備較好的自洽性,有效克服了傳統(tǒng)EKF過(guò)于依賴初值的缺陷,具有較好的濾波收斂性和一致性,理論上更為嚴(yán)密,在機(jī)器人姿態(tài)估計(jì)、SLAM及視覺(jué)里程計(jì)(VO)等慣性基導(dǎo)航領(lǐng)域均得到了較好的應(yīng)用[11-14]。文獻(xiàn)[15]研究了不變EKF的理論及在SLAM和SINS初始對(duì)準(zhǔn)等領(lǐng)域的應(yīng)用,文獻(xiàn)[16]研究了SEk(3)的特性及對(duì)應(yīng)的濾波框架,推導(dǎo)了幾種常見(jiàn)坐標(biāo)框架下的SINS左不變和右不變EKF具體數(shù)學(xué)表達(dá),文獻(xiàn)[17]研究了一種顧及陀螺零偏的SE2(3)-EKF的姿態(tài)估計(jì)方法,文獻(xiàn)[18]對(duì)速度誤差進(jìn)行變換實(shí)現(xiàn)了基于不變EKF的準(zhǔn)靜基座大失準(zhǔn)角線性初始對(duì)準(zhǔn)。在基于不變EKF的GNSS/SINS組合導(dǎo)航方面相對(duì)研究較少,尤其是針對(duì)實(shí)測(cè)數(shù)據(jù)和分析驗(yàn)證方面。文獻(xiàn)[19]較早設(shè)計(jì)了一種針對(duì)低成本GPS/磁力計(jì)輔助的SINS誤差動(dòng)力學(xué)模型的右不變EKF,并進(jìn)行仿真試驗(yàn)驗(yàn)證,文獻(xiàn)[16]給出了松組合GNSS坐標(biāo)量測(cè)更新的左不變EKF濾波形式。實(shí)際上,對(duì)于不同的組合導(dǎo)航應(yīng)用,SEk(3)-EKF的狀態(tài)誤差量的構(gòu)建形式并不統(tǒng)一,較為靈活,除了姿態(tài)、速度及位置狀態(tài)還需考慮陀螺零偏等其他待估參數(shù),選擇具體的投影坐標(biāo)系及左不變還是右不變狀態(tài)定義均需要根據(jù)實(shí)際導(dǎo)航器件特性、組合模式、不同載體和觀測(cè)環(huán)境等來(lái)確定。

另外,針對(duì)復(fù)雜觀測(cè)環(huán)境或者載體機(jī)動(dòng)較多的導(dǎo)航場(chǎng)景,GNSS/SINS目前普遍采用的是抗差或自適應(yīng)濾波,這方面已有大量的研究基礎(chǔ)[20-22],如抗差M估計(jì)、Sage-husa濾波及抗差自適應(yīng)濾波等。這類自適應(yīng)濾波一般是在EKF的框架基礎(chǔ)上,考慮歷史信息貢獻(xiàn)或者平衡載體動(dòng)力學(xué)模型和觀測(cè)模型,一般情況下可一定程度提高組合定位的可用性、精度和可靠性[23-24],但當(dāng)慣導(dǎo)預(yù)測(cè)值誤差較大或載體動(dòng)力學(xué)模型誤差和GNSS觀測(cè)異常同時(shí)存在等情況下,常規(guī)EKF由于上述客觀局限性導(dǎo)致其濾波性能面臨著較大的挑戰(zhàn),基于新的SEk(3)-EKF框架下的抗差或自適應(yīng)濾波為緩解這一問(wèn)題提供了可能?？紤]到GNSS/SINS組合定位中姿態(tài)和位置誤差相互耦合,對(duì)于MEMS慣導(dǎo),通常先利用GNSS或磁力計(jì)等輔助求解載體姿態(tài)初值,實(shí)際應(yīng)用中還會(huì)發(fā)生IMU斷電或者類似于旋翼無(wú)人機(jī)大的機(jī)動(dòng),造成較大的姿態(tài)角誤差,該階段傳統(tǒng)方法比較耗時(shí),且常需要載體有加速、轉(zhuǎn)彎等機(jī)動(dòng),此時(shí)如果GNSS信號(hào)異常,又進(jìn)一步影響到姿態(tài)的求解,常規(guī)抗差濾波及組合定位性能急劇下降,甚至直接發(fā)散導(dǎo)致定位失效?；诖?本文針對(duì)復(fù)雜環(huán)境下GNSS/SINS松組合模式中GNSS觀測(cè)異常頻繁發(fā)生的問(wèn)題,以MEMS慣導(dǎo)機(jī)械編排理論和經(jīng)典抗差方法為基礎(chǔ),研究一種基于GNSS位置量測(cè)的15狀態(tài)的左不變穩(wěn)健SE2(3)-EKF方法,并以城市環(huán)境下的車載GNSS/SINS組合導(dǎo)航數(shù)據(jù)為算例,驗(yàn)證了大失準(zhǔn)角誤差和GNSS異常情況下新的濾波方法性能。

1 方法介紹

SEk(3)-EKF相對(duì)傳統(tǒng)EKF最大的區(qū)別就是坐標(biāo)框架一致性問(wèn)題,重新定義了待估狀態(tài)的數(shù)學(xué)形式,針對(duì)GNSS/SINS松組合經(jīng)典的15狀態(tài)模型,即姿態(tài)(A)、速度(V)、位置(P)、陀螺零偏(bg)、加速度計(jì)零偏(ba)及形成李群(AVP)-歐氏空間(bg/ba)混合狀態(tài)誤差模型,進(jìn)而在誤差EKF的基礎(chǔ)上推導(dǎo)狀態(tài)誤差微分方程和GNSS量測(cè)更新方程。標(biāo)稱狀態(tài)采用MEMS慣導(dǎo)機(jī)械編排進(jìn)行時(shí)間更新,與傳統(tǒng)EKF一致[25-26]。本文重點(diǎn)就穩(wěn)健SE2(3)-EKF中的狀態(tài)誤差定義、誤差傳遞和GNSS量測(cè)更新展開(kāi)介紹。

1.1 誤差定義

選擇ECEF系(e系)作為組合導(dǎo)航投影坐標(biāo)系,定義投影到e系下的AVP狀態(tài)為一個(gè)群(矩陣)χ,bg/ba狀態(tài)記為矢量θ

(1)

(2)

表1 AVP群誤差定義形式

為了方便計(jì)算機(jī)編程實(shí)現(xiàn),本文選擇誤差定義到標(biāo)稱狀態(tài)對(duì)應(yīng)的“估計(jì)”系。關(guān)于選擇左不變還是右不變定義,比較靈活,通常右不變可保證狀態(tài)矩陣F不變或緩變,載體震動(dòng)或機(jī)動(dòng)較多有一定優(yōu)勢(shì),針對(duì)GNSS觀測(cè),有研究表明左不變能夠?qū)崿F(xiàn)量測(cè)矩陣H不變或緩變[16]。限于篇幅,本文主要針對(duì)左不變誤差形式展開(kāi)討論,根據(jù)式(1)和式(2),左不變?chǔ)羌傲闫`差ζ的具體表達(dá)式為

(3)

式(3)中,η∈SE2(3),根據(jù)李群仿射變換理論,η轉(zhuǎn)化為李代數(shù)ξ的具體形式為

(4)

(5)

(6)

式中,θ=|φ|,為旋轉(zhuǎn)軸角。

記t時(shí)刻的基于左不變定義的李群(χt)-歐氏空間(θt)混合狀態(tài)誤差為et

(7)

記zt為觀測(cè)誤差,則基于混合狀態(tài)誤差模型的系統(tǒng)方程為

(8)

(9)

1.2 誤差傳遞

李群空間的速度和位置誤差δv(φ)、δp(φ)和傳統(tǒng)歐氏空間對(duì)應(yīng)的誤差滿足式(10)

(10)

(11)

(12)

(13)

(14)

式(3)零偏誤差項(xiàng)(ζ)通?？山殡S機(jī)游走過(guò)程

(15)

(16)

式中,wbg、wba為過(guò)程噪聲,同樣可通過(guò)Allan方差分析確定。

式(7)混合狀態(tài)誤差變換后的向量形式記δx,系統(tǒng)噪聲項(xiàng)w,則SE2(3)-EKF狀態(tài)方程為

(17)

式中,F為狀態(tài)矩陣;G為系統(tǒng)噪聲分配矩陣,由式(12)—式(16)可得

(18)

(19)

IMU更新時(shí)間間隔為τ,當(dāng)τ較小時(shí),離散化后的狀態(tài)矩陣為Φ,過(guò)程噪聲矩陣為Q,狀態(tài)誤差δxt,t-1及方差-協(xié)方差Pt,t-1遞推公式為

δxt,t-1=Φt,t-1δxt-1

(20)

(21)

Φt,t-1=I+τ·Ft-1

(22)

(23)

式中,δxt,t-1≡0,因?yàn)檎`差每一次閉環(huán)修正后都重置為0;q為是白噪聲w對(duì)應(yīng)的方差強(qiáng)度矩陣,量綱單位與功率譜密度的單位一致。

1.3 量測(cè)更新

不考慮GNSS/SINS二者之間的空間桿臂,速度觀測(cè)方程為

(24)

速度觀測(cè)誤差方程可以變形為

(25)

構(gòu)造新的誤差量ev,使之包含SE2(3)-EKF中采用的誤差ξ

(26)

如果考慮桿臂,記GNSS/SINS之間的桿臂為lb,滿足以下觀測(cè)方程

(27)

(28)

本文以位置觀測(cè)為例,考慮零偏項(xiàng),t時(shí)刻誤差方程為

(29)

(30)

(31)

δet,t-1=et-Htδxt,t-1=Htδxt+Nt-Htδxt,t-1=

(32)

由于短時(shí)間內(nèi)IMU具有較高的遞推精度,可根據(jù)新息殘差δe可檢測(cè)出GNSS異常,考慮到松組合冗余觀測(cè)較少,本文采用不歸零的等價(jià)權(quán)函數(shù),噪聲調(diào)節(jié)系數(shù)為λ

(33)

(34)

由于ξ為群誤差,ζ為矢量誤差,前者采用“指數(shù)”更新,后者采用“加法”更新

(35)

(36)

(37)

式(37)和常規(guī)EKF誤差更新形式一致,說(shuō)明增益矩陣Kt也與常規(guī)EKF形式完全相同,根據(jù)最小均方根誤差準(zhǔn)則,結(jié)合式(34)可直接推導(dǎo)出增益矩陣Kt及后驗(yàn)方差-協(xié)方差矩陣Pt

(38)

(39)

根據(jù)式(36)和式(37)更新所有狀態(tài)誤差后,進(jìn)行狀態(tài)閉環(huán)修正,得到更新后的狀態(tài)為

(40)

(41)

1.4 濾波對(duì)比

基于左不變的LIEKF以EKF算法為基礎(chǔ),兩者的區(qū)別主要在于狀態(tài)向量的誤差定義不同,傳統(tǒng)誤差定義中,姿態(tài)誤差定義在特殊正交群而其他狀態(tài)誤差均定義于歐氏空間中。LIEKF將姿態(tài)、速度及位置引入一個(gè)群中,構(gòu)成特殊歐氏群SE2(3)。圖1為基于群空間狀態(tài)誤差傳遞示意圖,由于采用了指數(shù)映射,李代數(shù)形式的線性狀態(tài)誤差可以很好地表征李群上的非線性誤差,誤差傳遞后仍滿足高斯分布特性,能夠兼容常規(guī)EKF線性高斯分布的假設(shè),解決了傳統(tǒng)包含姿態(tài)誤差的狀態(tài)誤差坐標(biāo)框架定義不一致性問(wèn)題。

圖1 群空間狀態(tài)誤差傳遞Fig.1 The error propagation in group space

由于誤差定義在李群中,濾波中的狀態(tài)轉(zhuǎn)移矩陣F、量測(cè)矩陣H的推導(dǎo)以及反饋校正方式顯然與EKF有所區(qū)別,穩(wěn)健REKF與RLIEKF又是在EKF與LIEKF的基礎(chǔ)上增加了量測(cè)矩陣R的抗差因子調(diào)節(jié)。本文試驗(yàn)部分將對(duì)比分析EKF、REKF、LIEKF及RLIEKF 4種濾波性能,本文總結(jié)了幾種濾波方法的區(qū)別,見(jiàn)表2,其中表中“常規(guī)”可參考文獻(xiàn)[22]。

表2 4種濾波算法對(duì)比

2 試驗(yàn)與分析

2.1 姿態(tài)角收斂性能分析

由第1節(jié)理論部分可知,左不變EKF(LIEKF)顧及了姿態(tài)誤差的影響,為了驗(yàn)證大失準(zhǔn)角下的LIEKF性能,考慮到姿態(tài)角的確定主要難點(diǎn)是航向角,限于篇幅,本文以航向角為例,在車載試驗(yàn)初始階段模擬了150°、120°、90°、60°及30°的航向失準(zhǔn)角誤差,GNSS差分定位結(jié)果作為量測(cè)信息,噪聲設(shè)置為2 m。圖3給出了兩種失準(zhǔn)角對(duì)應(yīng)的LIEKF和EKF航向角誤差序列?？梢钥闯?初始航向失準(zhǔn)角為150°,傳統(tǒng)EKF姿態(tài)無(wú)法收斂,因?yàn)榇蟮淖藨B(tài)誤差導(dǎo)致IMU遞推位置誤差過(guò)大,二者相互耦合,無(wú)法通過(guò)GNSS量測(cè)更新來(lái)實(shí)現(xiàn)航向收斂,而LIEKF則表現(xiàn)出極快的收斂速度。

表3統(tǒng)計(jì)了5種航向失準(zhǔn)角誤差對(duì)應(yīng)的兩種濾波收斂(誤差小于2°)時(shí)間,可以看出即使相對(duì)較小的失準(zhǔn)角誤差,EKF仍需要5 min左右收斂,而LIEKF收斂時(shí)間都在半分鐘左右,基本與失準(zhǔn)角誤差大小無(wú)關(guān),這是由于LIEKF在誤差傳遞和量測(cè)更新階段顧及了姿態(tài)誤差,尤其是量測(cè)更新H矩陣保持不變的特性,獨(dú)立于狀態(tài)估計(jì)值,第1節(jié)中的線性模型即可精確反推出李群上的非線性誤差,即對(duì)于初始狀態(tài)估計(jì)遠(yuǎn)遠(yuǎn)偏離真實(shí)值的情形,利用LIEKF線性模型同樣可以做到濾波很快收斂。本文從試驗(yàn)結(jié)果進(jìn)一步驗(yàn)證了LIEKF在姿態(tài)角收斂方面的絕對(duì)優(yōu)勢(shì)。

表3 不同失準(zhǔn)角兩種濾波收斂時(shí)間

2.2 定位性能分析

組合導(dǎo)航在市區(qū)環(huán)境通常都會(huì)面臨GNSS觀測(cè)異常,初始階段或者中途IMU斷電等異常還會(huì)存在大的姿態(tài)失準(zhǔn)誤差。由于LIEKF在大失準(zhǔn)角誤差時(shí)很快能夠收斂,意味著實(shí)際應(yīng)用中GNSS/SINS定位可以迅速地進(jìn)入組合定位模式,從而更好地應(yīng)對(duì)GNSS信號(hào)短暫缺失和多徑干擾等異常不利影響,增加了對(duì)環(huán)境的抗干擾性。

為了驗(yàn)證本文提出的穩(wěn)健左不變EKF(RLIEKF)定位性能,在車載試驗(yàn)實(shí)測(cè)數(shù)據(jù)基礎(chǔ)上進(jìn)行了航向失準(zhǔn)和GNSS異常進(jìn)行了模擬。整個(gè)路段按照每隔約20 min分3個(gè)時(shí)段(T1、T2、T3),分別對(duì)應(yīng)了3種典型車載導(dǎo)航場(chǎng)景,其中T1時(shí)段需要進(jìn)行姿態(tài)初始對(duì)準(zhǔn),初始航向失準(zhǔn)角誤差模擬為90°,并考慮到建筑物等多徑干擾,模擬了500 s GNSS隨機(jī)粗差;T2時(shí)段姿態(tài)已經(jīng)收斂,但還是會(huì)存在GNSS粗差,模擬了500 s GNSS隨機(jī)粗差;T3時(shí)段開(kāi)始時(shí)刻由于IMU異?；蛘叽蟮臋C(jī)動(dòng),航向失準(zhǔn)角誤差模擬為90°,途經(jīng)隧道遮擋以及剛出隧道時(shí)樹(shù)木林立,模擬了10 s GNSS信號(hào)中斷以及隨后的1 min的GNSS觀測(cè)異常。

2.2.1 場(chǎng)景一

圖4給出了EKF、LIEKF以及具備抗差功能的REKF和RLIEKF 4種濾波算法在場(chǎng)景1中的定位誤差對(duì)比情況,可以看出LIEKF和RLIEKF很快就達(dá)到了較高的定位精度,前期EKF定位誤差較大,因?yàn)樽藨B(tài)還未收斂到一定精度,此時(shí)GNSS異常出現(xiàn),REKF則表現(xiàn)出更差的性能,因?yàn)榛贛EMS慣導(dǎo)推算的新息誤差仍較大,利用式(33)—式(34)反而放大了GNSS異常影響,因此不具備抗差功能。RLIEKF在整個(gè)階段則均較為穩(wěn)定,具備“既快又強(qiáng)”的抗GNSS粗差能力。

圖4 T1時(shí)段不同濾波方法的性能對(duì)比Fig.4 Comparison of four filtering methods in T1 period

2.2.2 場(chǎng)景二

場(chǎng)景二由于EKF和LIEKF姿態(tài)均已很好的收斂,意味著式(10)—式(11)中φ角影響已經(jīng)較小,由姿態(tài)誤差帶來(lái)的狀態(tài)誤差坐標(biāo)系定義不一致性也較小,若φ=0,此時(shí)LIEKF理論上便退化為常規(guī)EKF。圖5給出了此時(shí)段對(duì)應(yīng)的4種濾波方法的X方向定位誤差序列?？梢钥闯鯡KF與LIEKF誤差曲線基本一致,針對(duì)出現(xiàn)的GNSS異常,REKF和RLIEKF也表現(xiàn)出一致的抗差能力,從試驗(yàn)上驗(yàn)證了常規(guī)場(chǎng)景下LIEKF和RLIEKF同樣適用于常規(guī)場(chǎng)景,計(jì)算效率上,EKF、REKF基本相當(dāng),經(jīng)統(tǒng)計(jì),二者單歷元平均耗時(shí)為0.027 1及0.027 0 s,都具備較好的實(shí)時(shí)性。

圖5 T2時(shí)段4種濾波方法X方向定位誤差Fig.5 X direction positioning error of four filtering methods in T2 period

2.2.3 場(chǎng)景三

場(chǎng)景三在T2開(kāi)始階段出現(xiàn)了GNSS信號(hào)中斷以及多徑等干擾,這在市區(qū)環(huán)境中頻繁出現(xiàn),是一種較為典型的復(fù)雜觀測(cè)環(huán)境。圖6給出了T3時(shí)段4種濾波方法的定位性能對(duì)比。可以看出EKF和REKF對(duì)應(yīng)的X方向定位結(jié)果在2500歷元附近出現(xiàn)了大的誤差,通過(guò)軌跡圖(相對(duì)基站的E/N坐標(biāo))可以進(jìn)一步發(fā)現(xiàn),由于GNSS信號(hào)中斷了10 s,此時(shí)還存在姿態(tài)誤差,REKF對(duì)應(yīng)的平面軌跡迅速漂移,出隧道后又面臨樹(shù)木遮擋,存在較大多徑干擾,REKF由于隧道內(nèi)慣導(dǎo)位置遞推精度較差,同樣造成新息誤差較大,無(wú)法較好地識(shí)別GNSS異常觀測(cè),定位性能進(jìn)一步變差。由于后續(xù)觀測(cè)條件較好,二者均姿態(tài)收斂,定位誤差和軌跡也趨于相同。針對(duì)此場(chǎng)景,RLIEKF則基本不受GNSS信號(hào)短暫缺失和多徑等干擾,保持了良好的定位精度和穩(wěn)健性。

圖6 T3時(shí)段不同濾波方法的性能對(duì)比Fig.6 Comparison of four filtering methods in T3 period

3 結(jié) 論

針對(duì)GNSS/SINS組合導(dǎo)航經(jīng)常面臨大的姿態(tài)失準(zhǔn)和GNSS異常,傳統(tǒng)EKF及抗差算法由于在誤差傳遞和量測(cè)更新中未考慮由姿態(tài)誤差引起的位置、速度誤差定義坐標(biāo)框架不一致性和線性化誤差較大問(wèn)題,導(dǎo)致其定位性能受到影響。本文提出了SE2(3)-EKF框架下的穩(wěn)健GNSS/SINS組合定位方法,通過(guò)對(duì)狀態(tài)及誤差形式重新定義,并在此基礎(chǔ)上研究了相應(yīng)的誤差傳遞和GNSS量測(cè)更新的穩(wěn)健定位方法,通過(guò)車載試驗(yàn)和幾種常見(jiàn)的場(chǎng)景模擬,對(duì)比分析了EKF、REKF、LIEKF及RLIEKF的性能,得出了以下結(jié)論。

(1) LIEKF采用了群-向量混合誤差模型,基于李代數(shù)的線性狀態(tài)誤差可以很好地表征李群上的非線性誤差,兼容了EKF濾波線性假設(shè),由于顧及了姿態(tài)誤差,具有更高精度的動(dòng)力學(xué)模型和量測(cè)更新精度,H矩陣獨(dú)立于狀態(tài)預(yù)測(cè)值保持不變,因此對(duì)于大失準(zhǔn)角有著極快的收斂速度,對(duì)于150°、120°、90°、60°及30° 5種航向失準(zhǔn)角,都能夠在半分鐘左右收斂。

(2) 當(dāng)存在大失準(zhǔn)角誤差和GNSS異常時(shí),傳統(tǒng)的REKF由于H矩陣存在較大誤差,新息誤差不準(zhǔn)確導(dǎo)致其性能反而不如EKF,不具備抗粗差能力。而RLIEKF由于采用了新的新息誤差定義,不受姿態(tài)誤差的影響,可以很快地收斂并進(jìn)入組合導(dǎo)航模式,具備應(yīng)對(duì)GNSS信號(hào)短暫缺失和較好抗GNSS粗差能力;當(dāng)姿態(tài)收斂到一定精度,LIEKF便趨于退化為常規(guī)EKF,性能也趨于相同;在計(jì)算效率方面,LIEKF上和EKF均是計(jì)算維度相同的線性矩陣,效率相當(dāng)。因此RLIEKF是EKF較好的拓展,具備更強(qiáng)的抗GNSS異常能力,在城市等復(fù)雜環(huán)境下具備更好的穩(wěn)健性和實(shí)用性。