置信檢驗(yàn)自適應(yīng)聯(lián)邦卡爾曼濾波及其水下機(jī)器人組合導(dǎo)航應(yīng)用

陳帥，王寧*，陳廷凱，楊毅，田嘉禾

1 大連海事大學(xué) 船舶電氣工程學(xué)院，遼寧 大連 116026

2 中車大連電力牽引研發(fā)中心有限公司，遼寧 大連 116085

0 引 言

水下機(jī)器人作為一種智能化設(shè)備，已逐漸成為人類開發(fā)利用海洋資源的重要工具和海工裝備的中堅(jiān)力量。水下機(jī)器人在執(zhí)行任務(wù)時(shí)，精準(zhǔn)定位是極其重要的一環(huán)。因此，提升機(jī)器人組合導(dǎo)航系統(tǒng)的精度有著重大的研究意義。目前，GPS/INS是應(yīng)用最為廣泛的組合導(dǎo)航方式，其本質(zhì)是一種誤差特性互補(bǔ)的組合導(dǎo)航，通常與卡爾曼濾波相結(jié)合。具體而言，一方面利用GPS信息修正INS的導(dǎo)航結(jié)果，以調(diào)整其隨時(shí)間積累的誤差；另一方面，利用INS短時(shí)間內(nèi)的高精度定位優(yōu)勢(shì)，解決GPS信號(hào)在受遮擋、中斷等不利條件下的定位問題[1]。需要注意的是，當(dāng)載體受到外界強(qiáng)擾動(dòng)或量測(cè)信息不可靠情況下，傳統(tǒng)卡爾曼濾波效果將會(huì)受到影響，不可避免地導(dǎo)致GPS/INS組合導(dǎo)航的精度受到影響。

高精度的組合導(dǎo)航技術(shù)引起了國(guó)內(nèi)外學(xué)者的廣泛關(guān)注。Yang等[2]提出了一種基于預(yù)測(cè)殘差協(xié)方差陣的聯(lián)邦卡爾曼濾波自適應(yīng)因子改進(jìn)方法，有效提高了聯(lián)邦卡爾曼濾波的濾波精度；周先林等[3]提出了一種基于新息的自適應(yīng)卡爾曼濾波算法，該算法通過卡方檢驗(yàn)檢測(cè)出量測(cè)異常值，在量測(cè)異常值處調(diào)整量測(cè)噪聲方差陣，有效解決了濾波器中出現(xiàn)不可靠測(cè)量時(shí)濾波發(fā)散的問題；呂建新等[4]將里程計(jì)與GPS/INS組合導(dǎo)航系統(tǒng)相結(jié)合，提出了自適應(yīng)信息分配因子的策略，提升了組合導(dǎo)航系統(tǒng)的抗擾動(dòng)能力；Li等[5]針對(duì)GPS和INS更新速率不同步問題，提出了一種針對(duì)GPS量測(cè)值的觀測(cè)性擴(kuò)展方法，提升了組合導(dǎo)航系統(tǒng)在載體低速率時(shí)的導(dǎo)航精度；Jiang等[6]將澳大利亞的Locata定位系統(tǒng)、精準(zhǔn)點(diǎn)定位全球衛(wèi)星導(dǎo)航系統(tǒng)（PPP-GNSS）和INS相結(jié)合，并利用隨機(jī)向量空間方法設(shè)計(jì)了一種全局最優(yōu)濾波器，有效提升了導(dǎo)航精度；Ma等[7]將電磁羅盤（magnetic compass）、地形輔助導(dǎo)航（terrain aided navigation）、多普勒測(cè)速儀（Doppler velocity log）與INS相結(jié)合，并且利用殘差提出了一種自適應(yīng)聯(lián)邦卡爾曼濾波算法，有效提升了組合導(dǎo)航系統(tǒng)的穩(wěn)定性；Liu等[8]提出了魯棒卡爾曼濾波算法并將其用于聯(lián)邦卡爾曼濾波器，有效抑制了傳感器誤差對(duì)濾波器結(jié)果的影響；Malleswaran等[9]將輸入延遲動(dòng)態(tài)神經(jīng)網(wǎng)絡(luò)與GPS/INS組合導(dǎo)航系統(tǒng)相結(jié)合，在GPS信號(hào)不可用時(shí)利用神經(jīng)網(wǎng)絡(luò)對(duì)INS進(jìn)行補(bǔ)償，有效提升了組合導(dǎo)航系統(tǒng)的導(dǎo)航精度；Sung等[10]將數(shù)字羅盤與GPS相結(jié)合，并提出了一種簡(jiǎn)化卡爾曼濾波器，一定程度上提高了導(dǎo)航精度；Abosekeen等[11]利用簡(jiǎn)化的慣性傳感器系統(tǒng)替代INS，并利用離散余弦變換對(duì)數(shù)據(jù)進(jìn)行處理，提高了GPS信號(hào)被遮擋時(shí)的濾波精度；García等[12]通過構(gòu)造可信度量判斷量測(cè)信息的可靠性，解決了量測(cè)信息不可靠問題對(duì)濾波結(jié)果的影響。需要注意的是，上述方法在量測(cè)值發(fā)生異常時(shí)，無法解決組合導(dǎo)航系統(tǒng)穩(wěn)定性變差和濾波精度降低的問題。

為解決上述問題，本文將提出一種置信檢驗(yàn)自適應(yīng)聯(lián)邦卡爾曼濾波算法，通過構(gòu)造高斯分布對(duì)量測(cè)值進(jìn)行可靠性檢驗(yàn)，保證量測(cè)值的有效性；根據(jù)殘差協(xié)方差設(shè)計(jì)局部濾波器的自適應(yīng)調(diào)節(jié)因子，對(duì)系統(tǒng)噪聲協(xié)方差進(jìn)行實(shí)時(shí)調(diào)整，有效提高濾波精度和系統(tǒng)穩(wěn)定性。

1 INS/GPS/EC聯(lián)邦濾波

1.1 聯(lián)邦卡爾曼濾波

聯(lián)邦卡爾曼濾波的狀態(tài)方程和量測(cè)方程為：

式中：X(k+1)為系統(tǒng)k時(shí)刻對(duì)k+1時(shí)刻的狀態(tài)估計(jì)值；F(k)，X(k) 和G(k)分別為系統(tǒng)在k時(shí)刻的狀態(tài)轉(zhuǎn)移矩陣、狀態(tài)變量和控制矩陣；w(k)為系統(tǒng)的白噪聲矩陣；i=1,2,···,n，為第i個(gè)局部濾波器；Zi(k)，Hi(k)和vi(k)分別為系統(tǒng)局部濾波器i在k時(shí)刻的量測(cè)值、量測(cè)矩陣和量測(cè)噪聲。

聯(lián)邦卡爾曼濾波器濾波機(jī)制主要包括4個(gè)步驟：信息分配、信息時(shí)間更新、信息量測(cè)更新和最優(yōu)信息融合[13]。

1） 信息分配。

假設(shè)在初始時(shí)刻全局狀態(tài)的初始值為X0，其協(xié)方差矩陣為P0，系統(tǒng)噪聲方差矩陣為Q0。將這一信息通過信息分配因子按以下規(guī)則分配到各局部濾波器和全局濾波器：

式中：βi為信息分配因子，必須滿足信息守恒原則，即 β1+β2+···+βn+βm=1,0≤βi≤1；下標(biāo)n代表第n個(gè)局部濾波器，下標(biāo)m代表主濾波器。

2） 信息時(shí)間更新。

時(shí)間更新包括2個(gè)部分：狀態(tài)預(yù)測(cè)和協(xié)方差預(yù)測(cè)。

式中：Pi(k+1|k)，X?i(k+1|k)，F(xiàn)i(k+1|k)和Qi(k+1|k)分別為局部濾波器i的一步預(yù)測(cè)誤差協(xié)方差矩陣、狀態(tài)一步預(yù)測(cè)、一步轉(zhuǎn)移矩陣和一步預(yù)測(cè)系統(tǒng)噪聲方差矩陣。

3） 信息量測(cè)更新。

量測(cè)更新包括3個(gè)部分：卡爾曼濾波增益計(jì)算、狀態(tài)最優(yōu)估計(jì)以及協(xié)方差修正。

式中，Ki(k)和Ri(k)分別為局部濾波器i的增益矩陣和量測(cè)噪聲方差矩陣。

4） 最優(yōu)信息融合。

最優(yōu)信息融合則是利用各個(gè)局部濾波器的協(xié)方差和估計(jì)值，在主濾波器中進(jìn)行最優(yōu)狀態(tài)估計(jì)。

上面4個(gè)步驟即為聯(lián)邦卡爾曼濾波的基本步驟，通過以上步驟，可以實(shí)現(xiàn)多測(cè)量值系統(tǒng)對(duì)狀態(tài)的最優(yōu)估計(jì)。

1.2 組合導(dǎo)航建模

通常而言，選取慣性導(dǎo)航系統(tǒng)的位置、速度和姿態(tài)等15維誤差信息作為狀態(tài)變量，分別為位置誤差、速度誤差、姿態(tài)角誤差、陀螺儀漂移誤差以及加速度計(jì)的零偏誤差[14]。故狀態(tài)變量X(k)為：

式常速度值中 計(jì)漂：ε 零移bx， 偏；εΔb誤yb，x，差εbΔz；b分yw，別(Δkb)z為分=[東別ωg、為x北 ω東g、y、天 ω北g方z、 ω向天ax的方ω陀向ay螺的ωaz儀 加]T為白噪聲矩陣，其中ωgx，ωgy，ωgz分別為東、北、天方向的陀螺儀白噪聲，ωax，ωay，ωaz為東、北、天方向的加速度計(jì)一階馬爾科夫過程白噪聲；狀態(tài)轉(zhuǎn)移矩陣F(k)和 控制矩陣G(k)可參考文獻(xiàn)[15]。

在建立組合導(dǎo)航系統(tǒng)的狀態(tài)方程后，需建立觀測(cè)方程對(duì)組合導(dǎo)航系統(tǒng)的位置、速度、姿態(tài)信息進(jìn)行觀測(cè)。慣性導(dǎo)航系統(tǒng)能夠測(cè)量水下機(jī)器人的位置、姿態(tài)、速度信息，全球定位系統(tǒng)能夠測(cè)量水下機(jī)器人的位置、速度信息，電子羅盤能夠測(cè)量水下機(jī)器人的姿態(tài)信息。以慣性導(dǎo)航系統(tǒng)作為公共系統(tǒng)，分別求取與電子羅盤、全球定位系統(tǒng)的相同信息差值，選取慣性導(dǎo)航系統(tǒng)和全球定位系統(tǒng)的速度信息和位置信息作為觀測(cè)變量建立INS/GPS局部濾波器，選取慣性導(dǎo)航系統(tǒng)和電子羅盤的姿態(tài)信息作為觀測(cè)變量建立INS/EC局部濾波器[16]。

1.2.1 INS/GPS局部濾波器

因慣性導(dǎo)航系統(tǒng)和全球定位系統(tǒng)均可直接測(cè)得水下機(jī)器人在某一時(shí)刻的位置和速度信息，故選取慣性導(dǎo)航系統(tǒng)和全球定位系統(tǒng)的位置和速度測(cè)量值作為量測(cè)值。慣性導(dǎo)航系統(tǒng)和全球定位系統(tǒng)的位置信息可表示為：

式中：X1，Y1，Z1，X2，Y2，Z2分別為慣性導(dǎo)航系統(tǒng)和全球定位系統(tǒng)的經(jīng)度、緯度、高度的測(cè)量值；λ，L，h分別為水下機(jī)器人經(jīng)度、緯度、高度的真實(shí)值；δλ ，δL， δh， δX2，δY2，δZ2分別為慣性導(dǎo)航系統(tǒng)和全球定位系統(tǒng)的經(jīng)度、緯度、高度的測(cè)量誤差；R為地球赤道半徑，約為 6.37×106m。

定義位置觀測(cè)方程為，

式中，O1×13為零矩陣。

慣性導(dǎo)航系統(tǒng)和全球定位系統(tǒng)的速度信息可表示為

式中：vx，vy，vz，vx2，vy2，vz2分別為慣性導(dǎo)航系統(tǒng)和全球定位系統(tǒng)的東、北、天方向速度的測(cè)量值；vxr，vyr，vzr分別為水下機(jī)器人東、北、天方向速度的真實(shí)值；δvx，δvy，δvz， δvx2， δvy2， δvz2分別為慣性導(dǎo)航系統(tǒng)和全球定位系統(tǒng)的東、北、天方向速度的測(cè)量誤差。

定義速度觀測(cè)方程為

1.2.2 INS/EC局部濾波器

通過慣性導(dǎo)航系統(tǒng)和電子羅盤可直接獲得水下機(jī)器人的三軸姿態(tài)角信息，故以兩者之差作為觀測(cè)量，慣性導(dǎo)航系統(tǒng)和電子羅盤的姿態(tài)信息可表示為

式中：φx，φy，φz，φx2，φy2，φz2分別為慣性導(dǎo)航系統(tǒng)和電子羅盤的東、北、天方向姿態(tài)的測(cè)量值；φxr，φyr，φzr分別為水下機(jī)器人東、北、天方向姿態(tài)的真實(shí)值；δφx，δφy，δφz， δφx2， δφy2， δφz2分別為慣性導(dǎo)航系統(tǒng)和電子羅盤的東、北、天方向姿態(tài)的測(cè)量誤差。

姿態(tài)觀測(cè)方程為

2 置信檢驗(yàn)自適應(yīng)聯(lián)邦卡爾曼濾波

將本文提出的置信檢驗(yàn)自適應(yīng)聯(lián)邦卡爾曼濾波器應(yīng)用于水下機(jī)器人的INS/GPS/EC組合導(dǎo)航系統(tǒng)中，構(gòu)成2個(gè)局部濾波器和1個(gè)主濾波器，INS/GPS/EC組合導(dǎo)航系統(tǒng)結(jié)構(gòu)圖如圖1所示。

傳統(tǒng)聯(lián)邦濾波器的各子濾波器因?yàn)椴捎昧讼嗤臓顟B(tài)方程，若狀態(tài)方程出現(xiàn)擾動(dòng)會(huì)影響子濾波器的性能，在載體發(fā)生擾動(dòng)異?；虼嬖谳^大動(dòng)力學(xué)模型誤差時(shí)，濾波的效果往往不太理想。由圖1可知，在本文設(shè)計(jì)的聯(lián)邦濾波器中，INS，GPS和羅盤的量測(cè)信息需先經(jīng)過置信檢驗(yàn)，可靠的量測(cè)值Zi被送入局部濾波器，基于INS/GPS和INS/EC的自適應(yīng)局部濾波器并行運(yùn)行，利用自適應(yīng)調(diào)節(jié)因子調(diào)節(jié)系統(tǒng)噪聲協(xié)方差，得到預(yù)測(cè)誤差協(xié)方差矩陣Pi和 局部估計(jì)值X?i，并將其送入主濾波器并進(jìn)行融合，得到系統(tǒng)狀態(tài)的最優(yōu)估計(jì)值Pg和X?g。同時(shí)，為進(jìn)一步提升濾波精度，本系統(tǒng)選用了有重置結(jié)構(gòu)的聯(lián)邦卡爾曼濾波器，X?g和被放大為β?i1Pg的最優(yōu)估計(jì)值被反饋到局部濾波器，對(duì)局部濾波器的估計(jì)值進(jìn)行重置。

圖1 INS/GPS/EC組合導(dǎo)航系統(tǒng)結(jié)構(gòu)圖Fig.1 Structure diagram of INS/GPS/EC integrated navigation system

2.1 置信檢驗(yàn)

由卡爾曼濾波器的結(jié)構(gòu)可知，量測(cè)值對(duì)下一階段的估計(jì)值具有直接影響，而對(duì)于聯(lián)邦卡爾曼濾波器而言，當(dāng)任意一個(gè)傳感器出現(xiàn)故障或不可靠測(cè)量時(shí)，主濾波器輸出的最優(yōu)估計(jì)值的估計(jì)精度都會(huì)受損。針對(duì)這一問題，本文提出CC-AFKF框架，對(duì)每一時(shí)刻的量測(cè)值均進(jìn)行一次置信檢驗(yàn)，滿足條件的量測(cè)值認(rèn)為是可靠測(cè)量，可用于進(jìn)行下一時(shí)刻的預(yù)測(cè)；而不滿足條件的量測(cè)值利用量測(cè)方程進(jìn)行更改，保證最優(yōu)估計(jì)值的精度。

對(duì)任何一個(gè)局部濾波器的任意一個(gè)時(shí)刻而言，下一時(shí)刻狀態(tài)的預(yù)測(cè)值為

為判定量測(cè)值的可靠度，將預(yù)測(cè)向量中第t維的預(yù)測(cè)值X?it(k+1|k)表 示為。計(jì)算G(k)w(k)第t維的方差。G(k)w(k)第t維參數(shù)的均值 μ為

由系統(tǒng)噪聲w(k)的高斯特性，可認(rèn)為下一時(shí)刻斯的分最布優(yōu)，即估X?計(jì)it(k值|k均)～服N從(Xˉ均it,σ值2)為。X引ˉit，入方一差個(gè)為置σ2信的概高率α， 為使得Zti(k)處于置信區(qū)間內(nèi)，根據(jù)標(biāo)準(zhǔn)正態(tài)分布的分布函數(shù) Φ(x)，量測(cè)值的邊界取值、均值、方差和置信概率的關(guān)系可表示為

因傳感器每次測(cè)量時(shí)，真實(shí)值均服從正態(tài)分布[17]，所以根據(jù)傳感器的測(cè)量特性，x1和x2應(yīng)滿足x1+x2=2Xˉit，當(dāng)Zti(k)∈(x1,x2]時(shí)，可認(rèn)為下一時(shí)刻的Zti(k)屬于可靠測(cè)量，能夠用于接下來的預(yù)測(cè)環(huán)節(jié)；當(dāng)Zti(k)?(x1,x2]時(shí)，則認(rèn)為量測(cè)值屬于不可靠測(cè)量，量測(cè)值利用觀測(cè)方程Z(k)=HX(k)+v(k)獲得。

同時(shí)，根據(jù)正態(tài)分布的“3σ”法則，當(dāng)α設(shè)置為0.27%時(shí)，置信檢驗(yàn)可以過濾掉傳感器在出廠誤差范圍之外的量測(cè)值。但在實(shí)際應(yīng)用場(chǎng)合中，為提高融合精度，可通過重復(fù)試驗(yàn)合理選取α值。

2.2 自適應(yīng)局部濾波器

在對(duì)量測(cè)值進(jìn)行了置信檢驗(yàn)后，確保了每次用于下一階段預(yù)測(cè)的量測(cè)值的可靠性，保證了濾波器的穩(wěn)定性，進(jìn)而考慮設(shè)計(jì)自適應(yīng)調(diào)節(jié)因子對(duì)濾波器中的相關(guān)變量進(jìn)行實(shí)時(shí)調(diào)整，提高濾波器的濾波精度。

式中，P(k+1|k)為預(yù)測(cè)誤差協(xié)方差矩陣在計(jì)算區(qū)間內(nèi)的估計(jì)值。kt?k0一般取20，當(dāng)殘差的實(shí)際協(xié)方差與理論協(xié)方差保持一致時(shí)，卡爾曼濾波算法的精度最高[18]。根據(jù)殘差的實(shí)際協(xié)方差和理論協(xié)方差，可用兩者的跡tr的比值η來衡量真實(shí)值與理論值的偏差程度：

考慮到濾波器結(jié)構(gòu)中包含多個(gè)局部濾波器，調(diào)整Ri(k)會(huì)不可避免地增加系統(tǒng)的運(yùn)算量，又因?yàn)镻i(k+1|k)=Fi(k+1|k)Pi(k|k)FiT(k+1|k)+Qi(k)，故調(diào)整殘差的實(shí)際協(xié)方差可通過調(diào)整帶有調(diào)節(jié)因子 τ的Qi(k)來實(shí)現(xiàn)：

式中，e為自然常數(shù)，每次迭代時(shí)令Qi(k)=τQi(k?1)，不斷調(diào)整每次更新過程中的系統(tǒng)噪聲協(xié)方差，實(shí)現(xiàn)自適應(yīng)狀態(tài)估計(jì)。

本文所提出的置信檢驗(yàn)自適應(yīng)聯(lián)邦卡爾曼濾波算法流程圖如圖2所示。

圖2 置信檢驗(yàn)自適應(yīng)聯(lián)邦卡爾曼濾波算法流程圖Fig.2 Flow process of confidence check-adaptive federated Kalman filter algorithm

首先設(shè)定好濾波器的初值，包括狀態(tài)初值、系統(tǒng)噪聲協(xié)方差和量測(cè)噪聲協(xié)方差，其次根據(jù)預(yù)測(cè)值和置信概率進(jìn)行量測(cè)值的置信檢驗(yàn)，當(dāng)量測(cè)值屬于不可靠測(cè)量時(shí)，利用量測(cè)方程修正量測(cè)值，然后計(jì)算自適應(yīng)調(diào)節(jié)因子實(shí)時(shí)調(diào)節(jié)系統(tǒng)噪聲協(xié)方差矩陣，利用調(diào)節(jié)后的系統(tǒng)噪聲協(xié)方差進(jìn)行狀態(tài)估計(jì)，最后得到卡爾曼濾波結(jié)果。

3 試驗(yàn)結(jié)果分析

為驗(yàn)證本文所提出算法的有效性和優(yōu)越性，將本文所提出的CC-AFKF框架與經(jīng)典的KF，F(xiàn)KF框架在融合精度層面相比較。本次試驗(yàn)地點(diǎn)為大連市凌海港，試驗(yàn)所用的組合導(dǎo)航模塊信息如表1所示，水下機(jī)器人試驗(yàn)平臺(tái)如圖3所示。

圖3 水下機(jī)器人試驗(yàn)平臺(tái)Fig.3 Test platform of underwater vehicle

表1 組合導(dǎo)航模塊器件清單Table 1 Parts list of integrated navigation module

試驗(yàn)初始參數(shù)設(shè)置如下：本次試驗(yàn)僅考慮東向和北向速度，并忽略水下機(jī)器人的天向速度[19]，水下機(jī)器人的初始位置為東經(jīng)121.546°，北緯38.872°，初始東向和北向速度均為0 m/s，試驗(yàn)將第三方SBG Ekinox 2的組合導(dǎo)航輸出結(jié)果（理論定位精度為0.02 m）作為參考值，試驗(yàn)分析的各項(xiàng)定位測(cè)速誤差都是以該參考值為基準(zhǔn)得出。水下機(jī)器人真實(shí)運(yùn)動(dòng)軌跡如圖4所示。

圖4 水下機(jī)器人的真實(shí)運(yùn)動(dòng)軌跡Fig.4 Real trajectory of underwater vehicle

根據(jù)GPS，INS和EC所測(cè)得的數(shù)據(jù)，得到GPS，INS和使用本文算法CC-AFKF融合后的經(jīng)緯度和速度數(shù)據(jù)，分別如圖5和圖6所示。

圖5 原始經(jīng)緯度信息對(duì)比圖Fig.5 Comparison of original latitude and longitude information

圖6 原始北、東向速度信息對(duì)比圖Fig.6 Comparison of original information of eastward and northward speed

從圖5和圖6中可以看出，本文所提出的CC-AFKF算法在位置和速度信息融合方面具備有效性。其中，圖5顯示INS框架定位誤差隨著時(shí)間的增加而增加，在經(jīng)度為121.546 8°時(shí)其測(cè)量結(jié)果就已開始明顯偏離實(shí)際參考值，選取圖5中INS發(fā)生明顯偏差的一部分進(jìn)行放大，通過觀察可知，本文所提出的CC-AFKF算法所得到的經(jīng)、緯度基本上與參考值保持一致；由于試驗(yàn)海域不存在遮擋物，因此GPS單獨(dú)導(dǎo)航的效果與參考值相比也相差不大，但限于GPS單系統(tǒng)的不穩(wěn)定性，可以發(fā)現(xiàn)GPS的數(shù)據(jù)存在較大波動(dòng)。同理，圖6也明顯展示出無論是原始東向速度還是北向速度，INS框架的速度誤差也在隨時(shí)間累積；可以發(fā)現(xiàn)，在INS運(yùn)行一段時(shí)間后，速度開始偏離參考值，而CC-AFKF框架和GPS框架在速度信息上與參考值進(jìn)行對(duì)比時(shí)，展現(xiàn)的特性與經(jīng)緯度信息幾乎完全一致。

由此可知，本文提出的CC-AFKF算法在位置和速度的融合方面具備一定的可靠性，為證明置信檢驗(yàn)環(huán)節(jié)的有效性，在150 s時(shí)對(duì)GPS的測(cè)量數(shù)據(jù)施加一定的擾動(dòng)，得到AFKF和CC-AFKF的經(jīng)緯度和速度對(duì)比圖（圖7和圖8）。

從圖7和圖8可知，施加擾動(dòng)前，AFKF框架下的融合結(jié)果與CC-AFKF和參考值幾乎完全重合；但在施加擾動(dòng)后，無論是位置信息還是速度信息，均發(fā)生了較大偏差；由此可見，本文所提出的具備置信檢驗(yàn)環(huán)節(jié)的CC-AFKF框架具備一定的抗擾動(dòng)性能。

圖7 受擾動(dòng)后不同框架的經(jīng)緯度對(duì)比圖Fig.7 Comparison of latitude and longitude between different frameworks due to perturbation

圖8 受擾動(dòng)后不同框架的北、東向速度對(duì)比圖Fig.8 Comparison of northward and eastward speed between different frameworks after perturbation

現(xiàn)有組合導(dǎo)航方式多為運(yùn)用KF的GPS/INS組合導(dǎo)航模式[20]，為充分體現(xiàn)本文算法的優(yōu)越性，將本文所提出的CC-AFKF算法與經(jīng)典的KF和FKF算法相比較，結(jié)果如圖9至圖14所示。其中，圖9至圖11分別是經(jīng)緯度對(duì)比、誤差對(duì)比和均方根誤差對(duì)比；圖12至圖14分別是東北向速度對(duì)比、誤差對(duì)比和均方根誤差對(duì)比。

圖9 不同框架的經(jīng)緯度對(duì)比圖Fig.9 Comparison of latitude and longitude between different frameworks

圖10 不同框架的經(jīng)緯度誤差對(duì)比圖Fig.10 Comparison of longitude and latitude errors between different frameworks

圖11 不同框架的位置均方根誤差對(duì)比圖Fig.11 Comparison of root mean square error of position between different frameworks

圖12 不同框架的北、東向速度對(duì)比圖Fig.12 Comparison of northward and eastward speed between different frameworks

圖13 不同框架下的北、東向速度誤差對(duì)比圖Fig.13 Comparison of northward and eastward speed errors between different frameworks

圖14 不同框架的速度均方根誤差對(duì)比圖Fig.14 Comparison of root mean square error of speed between different frameworks

從圖9至圖14可以看出，在KF框架和FKF框架下速度和位置信息相較于參考值不會(huì)發(fā)生明顯偏差，但本文所提出的CC-AFKF框架在速度和位置融合精度方面能夠?qū)崿F(xiàn)更優(yōu)越的性能。從圖9和圖12的細(xì)節(jié)放大圖可知，KF框架下的數(shù)據(jù)波動(dòng)較大，KFK框架雖然相較于KF框架穩(wěn)定度有所提高，但提升并不明顯，CC-AFKF框架下的位置和速度信息與參考值最為接近，誤差圖對(duì)比也證明了這一點(diǎn)。在位置融合方面，經(jīng)度和緯度誤差能夠穩(wěn)定在 [?1,1]區(qū)間，與KF框架相比較，本文所提出的CC-AFKF框架在緯度和經(jīng)度的融合精度上分別提升了71.4%和68.5%，相較于FKF則分別提升了32.5%和36.9%。在速度的融合方面，本文所提出的CC-AFKF框架使用自適應(yīng)系統(tǒng)噪聲協(xié)方差，能夠更加精準(zhǔn)地實(shí)現(xiàn)速度融合。具體而言，本文提出的CC-AFKF框架在北向速度和東向速度的融合精度上相較于KF分別提升了46.9%和59.2%，相較于FKF則分別提升了29.4%和42.7%。綜上所述，由于KF和FKF缺少對(duì)系統(tǒng)噪聲的跟隨性，故其難以實(shí)現(xiàn)與本文所提出的CC-AFKF框架相一致的融合性能。

4 結(jié) 語

本文設(shè)計(jì)了一種基于GPS/INS/EC的聯(lián)邦卡爾曼濾波器框架，利用系統(tǒng)狀態(tài)真實(shí)值的高斯分布特性對(duì)量測(cè)值進(jìn)行置信檢驗(yàn)，有效剔除系統(tǒng)中的不可靠測(cè)量值；根據(jù)殘差設(shè)計(jì)自適應(yīng)分配因子，對(duì)系統(tǒng)噪聲協(xié)方差自適應(yīng)進(jìn)行調(diào)整，提高了系統(tǒng)精度。大量的試驗(yàn)和比較證明了本文所提出的CC-AFKF框架的有效性和優(yōu)越性。