基于自適應(yīng)并行擴(kuò)展卡爾曼濾波的SITAN匹配算法

王傲明，李?yuàn)檴?，李新星，黃志勇，黃 炎，范 雕

（信息工程大學(xué) 地理空間信息學(xué)院，鄭州 450001）

重力匹配輔助慣性導(dǎo)航具有自主性強(qiáng)、隱蔽性好、不受地域和時(shí)域限制以及定位精度高等諸多優(yōu)點(diǎn)，被越來(lái)越多地應(yīng)用于各類(lèi)水下航行器[1-3]。重力匹配需要在重力場(chǎng)特征豐富的區(qū)域進(jìn)行，匹配前需要進(jìn)行適配區(qū)選擇，才可進(jìn)行匹配定位[4-6]。匹配定位精度主要由慣導(dǎo)精度、重力測(cè)量傳感器精度、重力基準(zhǔn)圖精度、匹配區(qū)域的適配性以及匹配算法所決定。其中，匹配算法是重力匹配導(dǎo)航中的一項(xiàng)關(guān)鍵技術(shù)，主要包括基于相關(guān)分析技術(shù)的地形輪廓匹配（Terrain Contour Matching,TRECOM）算法、迭代最近等值線點(diǎn)（Iterated Closest Contour Point,ICCP）算法和基于遞推濾波技術(shù)的桑迪亞慣性地形輔助導(dǎo)航（Sandia Inertial Terrain Aided Navigation,SITAN）算法[7]。近年來(lái)，相關(guān)學(xué)者提出了矢量匹配算法[8,9]、具有約束條件的匹配算法[10,11]和基于人工蜂群算法的匹配算法[12]。同時(shí)，國(guó)內(nèi)外學(xué)者對(duì)TRECOM 算法[13-16]和ICCP 算法[17,18]進(jìn)行了相應(yīng)改進(jìn)，提高了匹配定位精度。

序列相關(guān)匹配算法可靠性較好，為了達(dá)到較高的匹配精度，一般以一個(gè)觀測(cè)序列進(jìn)行搜索匹配，其運(yùn)算量巨大，匹配速度較慢，影響導(dǎo)航的實(shí)時(shí)性；由于是對(duì)觀測(cè)序列作驗(yàn)后相關(guān)分析，得到的正確位置存在一定的延遲，并且對(duì)基準(zhǔn)圖的精度要求較高。因此，目前國(guó)外主要還是采用基于擴(kuò)展卡爾曼濾波（Extended Kalman Filter,EKF）的SITAN 算法，其優(yōu)點(diǎn)是能滿足系統(tǒng)修正的實(shí)時(shí)性，且在低信噪比的條件下依舊能夠運(yùn)行。SITAN 算法的主要缺陷在于僅僅將非線性觀測(cè)方程在單點(diǎn)附近展開(kāi)，在非線性程度較高的區(qū)域容易導(dǎo)致濾波發(fā)散。文獻(xiàn)[19-20]和文獻(xiàn)[21]針對(duì)非線性觀測(cè)模型的線性化過(guò)程容易引起較大觀測(cè)模型誤差的問(wèn)題，分別提出質(zhì)點(diǎn)濾波和并行濾波以提高位置估計(jì)的精度。文獻(xiàn)[22]研究了基于RAE 量測(cè)噪聲自適應(yīng)的SITAN 算法，提高了定位精度與穩(wěn)定性，但該算法最大的缺陷在于要求載體平穩(wěn)且各歷元觀測(cè)信息同類(lèi)、同維、同分布，難以做到真正的自適應(yīng)，容易造成濾波發(fā)散；文獻(xiàn)[23]研究了基于抗差估計(jì)的SITAN 匹配算法，該算法能夠有效抵抗含有粗差觀測(cè)值的影響，提高了算法的魯棒性，但在濾波過(guò)程中要求航行器處于勻速定深狀態(tài)，限制了算法的應(yīng)用范圍。文獻(xiàn)[24]對(duì)重力匹配輔助慣性導(dǎo)航系統(tǒng)的可觀測(cè)性進(jìn)行了分析，結(jié)果表明僅當(dāng)系統(tǒng)的狀態(tài)量取平面位置誤差時(shí)才是可觀的。由慣導(dǎo)誤差方程可知，位置誤差的變化率與位置誤差和速度誤差有關(guān)。而可觀測(cè)分析表明，僅有平面位置誤差是可觀的，系統(tǒng)的狀態(tài)量?jī)H能取平面位置誤差，因此建立的狀態(tài)方程存在模型誤差。利用Kalman 濾波進(jìn)行最優(yōu)估計(jì)，需要可靠的函數(shù)模型、隨機(jī)模型以及合理的估計(jì)方法，任何信息的失真都會(huì)導(dǎo)致難以獲得最優(yōu)估計(jì)結(jié)果甚至濾波發(fā)散[26]。

針對(duì)狀態(tài)方程存在模型誤差這一問(wèn)題，本文在SITAN 算法的基礎(chǔ)上，提出基于自適應(yīng)并行擴(kuò)展卡爾曼濾波（Adaptive Parallel Extended Kalman Filter,APEKF）的SITAN 算法。首先，依據(jù)慣導(dǎo)誤差方程選取慣導(dǎo)平面位置誤差為濾波狀態(tài)量構(gòu)建狀態(tài)方程，并利用重力異常隨機(jī)線性化技術(shù)構(gòu)建量測(cè)方程；其次，利用自適應(yīng)因子調(diào)節(jié)狀態(tài)預(yù)測(cè)信息的協(xié)方差矩陣以減弱失真的狀態(tài)預(yù)測(cè)信息對(duì)濾波解的影響；最后，利用并行濾波降低因?yàn)榱繙y(cè)的不可信引起的濾波發(fā)散。仿真試驗(yàn)結(jié)果表明：本文提出的算法能夠有效地抑制濾波發(fā)散，保證了定位結(jié)果的精度與穩(wěn)定性。

1 基于并行擴(kuò)展卡爾曼濾波的SITAN 算法

基于并行擴(kuò)展卡爾曼濾波（Parallel Extended Kalman filtering,PEKF）的SITAN 算法，由并行濾波、擴(kuò)展卡爾曼濾波和重力異常隨機(jī)線性化技術(shù)組成。能夠通過(guò)實(shí)時(shí)的重力異常量測(cè)值對(duì)載體的位置進(jìn)行修正，且計(jì)算量小。濾波器的狀態(tài)方程、量測(cè)方程以及最優(yōu)濾波器的準(zhǔn)則如下。

1.1 濾波器狀態(tài)方程與觀測(cè)方程的建立

為了確保狀態(tài)量可觀，由前面分析可知，濾波的狀態(tài)量取平面位置誤差X=(δφ,δλ)T，δφ、δλ分別表示水下航行器的緯度誤差和經(jīng)度誤差。由慣導(dǎo)誤差方程可知[25]，平面位置誤差的微分方程如式(1)所示。

式中，δvN、δvE代表北向和東向速度誤差，RM、RN分別為子午圈和卯酉圈曲率半徑，δφ、δλ、hδ分別表示緯度誤差、經(jīng)度誤差和高度誤差，φ為緯度。

忽略式(1)中高度誤差和速度誤差的影響，濾波器的狀態(tài)方程表示為：

假設(shè)濾波狀態(tài)更新周期為T(mén)，由線性系統(tǒng)理論，對(duì)式(3)進(jìn)行離散化處理，可得：

式中，Φk.k-1為狀態(tài)轉(zhuǎn)移矩陣，Xk、Xk-1分別為k、k-1時(shí)刻的狀態(tài)量，Wk-1為狀態(tài)噪聲。T通常取慣導(dǎo)解算的時(shí)間間隔。

重力異常與位置呈非線性關(guān)系。為了構(gòu)建量測(cè)方程，需要對(duì)局部重力異常進(jìn)行線性化處理。對(duì)慣導(dǎo)指示點(diǎn)(φi,λi)處的重力異常在其鄰域內(nèi)進(jìn)行一階泰勒級(jí)數(shù)展開(kāi)：

式中，φi、λi分別代表慣導(dǎo)指示點(diǎn)處的緯度和經(jīng)度；vi為高階截?cái)嗾`差； ΔgM(φi,λi)為慣導(dǎo)指示點(diǎn)對(duì)應(yīng)基準(zhǔn)圖上的重力異常。

在真實(shí)位置進(jìn)行重力測(cè)量，經(jīng)過(guò)改化后可以得到觀測(cè)重力異常，表示為：

式中，Δgs(φt,λt)為實(shí)際觀測(cè)重力異常，vs為觀測(cè)噪聲。由式(5)和式(6)可得濾波觀測(cè)方程：

觀測(cè)量為慣導(dǎo)指示點(diǎn)對(duì)應(yīng)基準(zhǔn)圖上的重力異常與實(shí)際觀測(cè)的重力異常之差。

建立觀測(cè)方程的關(guān)鍵是實(shí)時(shí)獲得偏導(dǎo)數(shù)?ΔgM/?φi、?ΔgM/?λi。上述兩個(gè)偏導(dǎo)數(shù)求取的理論依據(jù)是斯托克斯公式，并利用移去-恢復(fù)技術(shù)實(shí)現(xiàn)，但是該方法計(jì)算量大難以滿足實(shí)時(shí)性的要求，因此采用局部重力異常的擬合逼近求取上述兩個(gè)偏導(dǎo)數(shù)。

假設(shè)在匹配點(diǎn)(φi,λi)的鄰域Ω內(nèi)，Δg(φ,λ)存在直到n+1 階的連續(xù)偏導(dǎo)數(shù)，那么 Δg(φ,)λ在Ω內(nèi)可表示成：

式中，0＜θ＜1，右端第一項(xiàng)為泰勒級(jí)數(shù)展開(kāi)逼近，第二項(xiàng)為逼近截?cái)嗾`差。

式(8)中的已知量是等式左邊一些離散點(diǎn)值，顯然當(dāng)N=1 時(shí)為平面擬合，當(dāng)N=2 時(shí)，為雙二次曲面擬合。如有多余觀測(cè)，則采用最小二乘法求解。

1.2 并行濾波器與最優(yōu)濾波器選取準(zhǔn)則

由式(5)可知，構(gòu)建量測(cè)方程時(shí)僅僅取局部重力異常泰勒展開(kāi)的一階近似。為了使量測(cè)方程具有足夠高的精度，需要慣導(dǎo)指示點(diǎn)與真實(shí)點(diǎn)的位置足夠靠近，才可忽略高階項(xiàng)帶來(lái)的截?cái)嗾`差。因此，僅用觀測(cè)重力異常與慣導(dǎo)指示點(diǎn)對(duì)應(yīng)基準(zhǔn)圖上的重力異常之差作為唯一的量測(cè)值，可能因?yàn)榱繙y(cè)的失真導(dǎo)致濾波發(fā)散。為增加算法的魯棒性，在濾波過(guò)程中建立并行濾波器。所謂并行濾波是根據(jù)慣導(dǎo)位置誤差確定置信區(qū)間，在置信區(qū)間內(nèi)沿經(jīng)緯向以相同的位置間隔設(shè)計(jì)一組并行的卡爾曼濾波器。將重力傳感器觀測(cè)得到的重力異常值與每個(gè)濾波器中心位置處的重力異常之差作為量測(cè)值，分別進(jìn)行卡爾曼濾波，然后將各濾波器的濾波結(jié)果組成平滑算子，根據(jù)一定的準(zhǔn)則進(jìn)行優(yōu)化選擇。

使用平滑加權(quán)殘差平方（Smoothed Weighted Residual Squared,SWRS）最小作為選取最佳濾波器的準(zhǔn)則[22]。設(shè)k時(shí)刻第j個(gè)濾波器的觀測(cè)向量的預(yù)測(cè)殘差為：

定義預(yù)測(cè)殘差加權(quán)平方：

則平滑加權(quán)殘差平方為：

式中，α為平滑加權(quán)因子，SWRS的值越小，則濾波器結(jié)果與模型的吻合程度越好，選做最優(yōu)濾波器。

2 基于自適應(yīng)并行擴(kuò)展卡爾曼濾波的SITAN算法

基于自適應(yīng)PEKF（APEKF）的SITAN 算法，是在基于PEKF 的SITAN 算法的基礎(chǔ)上，對(duì)系統(tǒng)的狀態(tài)預(yù)測(cè)信息進(jìn)行自適應(yīng)調(diào)整，其基本思想是：當(dāng)狀態(tài)方程存在模型異常誤差時(shí)，將狀態(tài)方程預(yù)測(cè)信息作為一個(gè)整體，采用統(tǒng)一的自適應(yīng)因子調(diào)整狀態(tài)方程預(yù)測(cè)信息對(duì)狀態(tài)參數(shù)的整體貢獻(xiàn)[26,27]。由式(1)和式(2)可知，濾波器的狀態(tài)方程存在模型誤差，因此可利用自適應(yīng)因子調(diào)節(jié)狀態(tài)方程預(yù)測(cè)信息，即利用自適應(yīng)因子對(duì)狀態(tài)預(yù)測(cè)信息的協(xié)方差矩陣進(jìn)行膨脹，進(jìn)而達(dá)到重用觀測(cè)信息，減弱預(yù)測(cè)信息的使用以抑制濾波發(fā)散的目的。使用自適應(yīng)并行擴(kuò)展卡爾曼濾波要解決兩個(gè)問(wèn)題：一是利用模型誤差判別統(tǒng)計(jì)量對(duì)狀態(tài)方程預(yù)測(cè)信息異常進(jìn)行識(shí)別，二是利用模型誤差判別統(tǒng)計(jì)量構(gòu)造自適應(yīng)因子。

模型誤差判別統(tǒng)計(jì)量包括狀態(tài)不符值統(tǒng)計(jì)量、方差分量比統(tǒng)計(jì)量、預(yù)測(cè)殘差統(tǒng)計(jì)量[26]。三種統(tǒng)計(jì)量中，狀態(tài)不符值與方差分量比統(tǒng)計(jì)量要求有多余觀測(cè)，而預(yù)測(cè)殘差統(tǒng)計(jì)量對(duì)觀測(cè)個(gè)數(shù)沒(méi)有任何要求。由濾波的觀測(cè)方程式(7)可知，在同一歷元中，觀測(cè)值個(gè)數(shù)為1，狀態(tài)未知數(shù)為2，不存在多余觀測(cè)。因此，模型誤差判別統(tǒng)計(jì)量只能取預(yù)測(cè)殘差統(tǒng)計(jì)量。設(shè)k時(shí)刻的觀測(cè)向量Lk的預(yù)測(cè)殘差為：

式中，Hk為量測(cè)方程系數(shù)矩陣，為k時(shí)刻狀態(tài)方程的預(yù)測(cè)信息，Lk為量測(cè)值。

以預(yù)測(cè)殘差構(gòu)造的預(yù)測(cè)殘差統(tǒng)計(jì)量表示為[27]：

式中，為k時(shí)刻量測(cè)預(yù)測(cè)殘差的協(xié)方差矩陣，tr(·)代表取跡。

自適應(yīng)因子模型分為歸零函數(shù)和非歸零函數(shù)[26]，包括三段函數(shù)自適應(yīng)因子和兩段函數(shù)自適應(yīng)因子。

三段函數(shù)自適應(yīng)因子[27]：

式中，c0、c1為常數(shù)，可分別取c0=1～1.5、c1=1.0～8.5。

兩段函數(shù)自適應(yīng)因子[27]：

式中，c為常數(shù)，可取c=1.0～2.5。

三段函數(shù)自適應(yīng)因子和兩段函數(shù)自適應(yīng)因子的區(qū)別在于三段函數(shù)自適應(yīng)因子存在零段，表示對(duì)狀態(tài)預(yù)測(cè)信息棄置不用，當(dāng)觀測(cè)個(gè)數(shù)小于狀態(tài)未知參數(shù)個(gè)數(shù)時(shí)，會(huì)導(dǎo)致法方程系數(shù)矩陣出現(xiàn)秩虧，無(wú)法求解狀態(tài)未知數(shù)。因此，在基于自適應(yīng)并行擴(kuò)展卡爾曼濾波的匹配算法中，自適應(yīng)因子模型取兩段函數(shù)自適應(yīng)因子，模型誤差判別統(tǒng)計(jì)量取預(yù)測(cè)殘差統(tǒng)計(jì)量。

綜上，基于自適應(yīng)并行擴(kuò)展卡爾曼濾波的SITAN算法的流程圖如圖1所示。

圖1 算法流程圖Fig.1 Flow chart of algorithm

3 試驗(yàn)驗(yàn)證與分析

為了驗(yàn)證匹配算法的有效性，在南海海域，選取重力異常變化劇烈程度不同的兩片海域進(jìn)行仿真試驗(yàn)。利用自編程序，設(shè)置慣導(dǎo)數(shù)據(jù)采樣率為100 Hz，生成兩片海域航跡上各采樣點(diǎn)的姿態(tài)、速度、位置、慣性元器件輸出。重力異?；鶞?zhǔn)圖數(shù)據(jù)為丹麥科技大學(xué)（Technical University of Denmark,DTU）發(fā)布的DTU17 海洋重力異常，分辨率為1′× 1′，中誤差為2～3 mGal，重力異常觀測(cè)數(shù)據(jù)為在重力異常基準(zhǔn)圖數(shù)據(jù)上添加中誤差為3 mGal 的隨機(jī)觀測(cè)噪聲所得。兩條航跡的時(shí)長(zhǎng)均為24 h，航跡一與航跡二的初始速度和姿態(tài)相同，均為（0 m/s 0 m/s 0 m/s）和（0° 0° 0°），航跡一的起始坐標(biāo)：（7°N 113.5°E-100 m），航跡二的起始坐標(biāo)：（9.5°N 106°E-100 m）。兩條航跡的運(yùn)動(dòng)過(guò)程也是相同的，先以0.0086 m/s2的加速度，勻加速600 s，速度達(dá)到10 節(jié)，然后勻速直線運(yùn)動(dòng)7 h 后，左轉(zhuǎn)90°，勻速運(yùn)動(dòng)8 h1500 s，最后，右轉(zhuǎn)90°，勻速運(yùn)動(dòng)8 h1500 s。上述兩條航跡作為參考航跡。利用生成的慣性元器件輸出以及水下航行器運(yùn)動(dòng)的初始條件，結(jié)合現(xiàn)有慣性元器件水平，設(shè)置陀螺儀與加速度計(jì)誤差參數(shù)如表1所示，采用雙子樣算法進(jìn)行慣導(dǎo)解算獲得載體的慣導(dǎo)指示位置作為慣導(dǎo)航跡。試驗(yàn)海域一和海域二的范圍、航跡重力異常的變化、以及參考航跡和慣導(dǎo)航跡如圖2和圖3所示。選取六個(gè)重力場(chǎng)特征參數(shù)，分別為標(biāo)準(zhǔn)差、粗糙度、相關(guān)系數(shù)、坡度及標(biāo)準(zhǔn)差、偏態(tài)系數(shù)和峰態(tài)系數(shù)，統(tǒng)計(jì)兩片海域的重力特征參數(shù)，并基于主成分分析對(duì)海域一和海域二的適配性進(jìn)行評(píng)價(jià)。海域一和海域二的計(jì)算值分別為0.096和0.002，因此海域一具有更好的適配性。

表1 慣性器件誤差設(shè)置Tab.1 Inertial device error setting

圖2 試驗(yàn)海域一Fig.2 First test sea area

圖3 試驗(yàn)海域二Fig.3 Second test sea area

重力匹配輔助慣性導(dǎo)航要求，兩次重力匹配間水下航行器的航行距離要超過(guò)半個(gè)重力異常格網(wǎng)[22]。結(jié)合重力異?；鶞?zhǔn)圖數(shù)據(jù)的分辨率和載體的航行速度，濾波過(guò)程中量測(cè)更新時(shí)間間隔取為180 s。由于慣導(dǎo)數(shù)據(jù)頻率為100 Hz 并采用雙子樣算法進(jìn)行姿態(tài)解算，因此慣性導(dǎo)航解算時(shí)間間隔為0.02 s，即狀態(tài)更新時(shí)間間隔為0.02 s，并沿經(jīng)緯度方向構(gòu)造了9 個(gè)并行濾波器，分別利用基于PEKF 和APEKF 的SITAN 算法對(duì)兩片試驗(yàn)海域開(kāi)展重力匹配仿真試驗(yàn)。為了敘述方便，以后對(duì)上述兩種算法分別簡(jiǎn)稱(chēng)為PEKF算法和APEKF算法。

3.1 仿真試驗(yàn)一

試驗(yàn)海域一慣導(dǎo)航跡的經(jīng)、緯向誤差及兩種匹配算法匹配結(jié)果的經(jīng)、緯向誤差如圖4和5所示。由圖4和5 可知，重力匹配能夠有效降低慣導(dǎo)隨著時(shí)間增長(zhǎng)累積的誤差。對(duì)比PEKF 算法誤差和APEKF 算法的經(jīng)、緯向誤差可以發(fā)現(xiàn)，雖然PEKF 算法能夠減弱慣導(dǎo)的累積誤差，但是在12 h 以后濾波逐漸發(fā)散，導(dǎo)航精度逐漸減低，而APEKF 匹配算法不存在發(fā)散情況，能夠穩(wěn)定運(yùn)行且導(dǎo)航精度高。這是由于濾波器的狀態(tài)方程存在模型誤差，導(dǎo)致PEKF 算法濾波發(fā)散，而APEKF 算法利用自適應(yīng)因子調(diào)節(jié)狀態(tài)預(yù)測(cè)信息的權(quán)重，以減弱失真的狀態(tài)預(yù)測(cè)信息對(duì)濾波結(jié)果的影響，有效地綜合狀態(tài)預(yù)測(cè)信息與量測(cè)信息對(duì)濾波結(jié)果的貢獻(xiàn)，使得濾波器的魯棒性增強(qiáng)，進(jìn)而系統(tǒng)的定位精度也得到提高。

圖4 緯向誤差Fig.4 Errors of latitude

圖5 經(jīng)向誤差Fig.5 Errors of longitude

PEKF 算法和APEKF 算法，在濾波過(guò)程中的濾波增益如圖6和圖7所示。由圖6可知，在濾波過(guò)程中，PEKF 算法的濾波增益隨著時(shí)間的增加逐漸收斂至零，這表明隨著時(shí)間增加量測(cè)值對(duì)濾波結(jié)果將不起任何作用，濾波結(jié)果為失真的狀態(tài)預(yù)測(cè)信息，導(dǎo)致濾波發(fā)散。由圖7可知，在濾波過(guò)程中，APEKF 算法的濾波增益隨著時(shí)間的增加不斷的變化且不會(huì)收斂至零，因此量測(cè)值會(huì)對(duì)濾波器的預(yù)測(cè)結(jié)果不斷修正，保證了濾波結(jié)果的可靠性。

圖6 PEKF 濾波增益Fig.6 The filter gain of PEKF

圖7 APEKF 濾波增益Fig.7 The filter gain of APEKF

將PEKF 算法和APEKF 算法匹配結(jié)果的經(jīng)、緯向誤差進(jìn)行統(tǒng)計(jì)，并與INS 的經(jīng)、緯向誤差進(jìn)行對(duì)比，誤差統(tǒng)計(jì)結(jié)果見(jiàn)表2。由表2可知，APEKF 算法的定位誤差在緯度方向上最大值為1.22 n mile，在經(jīng)度方向上最大值為1.14 n mile，與重力異常圖網(wǎng)格分辨率相當(dāng)。并且兩方向上的精度均優(yōu)于INS 的精度和PEKF匹配算法的精度。

表2 誤差統(tǒng)計(jì)（單位：海里）Tab.2 Error statistics (Unit:n mile)

3.2 仿真試驗(yàn)二

試驗(yàn)海域二的慣導(dǎo)航跡的經(jīng)、緯向誤差及兩種匹配算法匹配結(jié)果的經(jīng)、緯向誤差如圖8和圖9所示。由圖8和圖9可知，重力匹配能夠有效降低慣導(dǎo)隨著時(shí)間增長(zhǎng)累積的誤差。對(duì)比PEKF 算法和APEKF 算法的經(jīng)、緯向誤差可以發(fā)現(xiàn)，雖然PEKF算法和APEKF算法均能夠減弱慣導(dǎo)的累積誤差，且APEKF 算法的估計(jì)精度優(yōu)于PEKF 算法，但是在緯度和經(jīng)度方向上兩種濾波均會(huì)逐漸發(fā)散，導(dǎo)航精度逐漸減低。雖然APEKF 算法使用自適應(yīng)因子減弱失真的狀態(tài)預(yù)測(cè)信息對(duì)濾波結(jié)果的影響，但是由于所處區(qū)域過(guò)于平緩，量測(cè)信息失真，APEKF 算法也會(huì)逐漸發(fā)散引起定位精度下降。

圖8 緯向誤差Fig.8 Errors of latitude

圖9 經(jīng)向誤差Fig.9 Errors of longitude

將PEKF 算法和APEKF 算法結(jié)果的經(jīng)、緯向誤差進(jìn)行統(tǒng)計(jì)，并與INS 的經(jīng)、緯向誤差進(jìn)行對(duì)比，誤差統(tǒng)計(jì)結(jié)果見(jiàn)表3。由表3可知，雖然APEKF 算法的估計(jì)精度優(yōu)于PEKF 算法，也遠(yuǎn)優(yōu)于INS 的精度，但隨著時(shí)間的增加濾波逐漸發(fā)散。對(duì)比試驗(yàn)一和試驗(yàn)二可知，為了獲得更高的定位精度，需要在重力特征變化劇烈的區(qū)域進(jìn)行重力匹配。

表3 誤差統(tǒng)計(jì)（單位：海里）Tab.3 Error statistics (Unit:n mile)

3.3 仿真試驗(yàn)三

由于水下航行器在運(yùn)動(dòng)的過(guò)程中，無(wú)法保證其一直處于適配區(qū)，由仿真實(shí)驗(yàn)二可知，此時(shí)重力匹配失效，位置誤差將逐漸增大。為了驗(yàn)證在位置誤差較大的情況下，處于適配區(qū)的航行器能否通過(guò)重力匹配以獲得高精度的定位信息，取適配性好的試驗(yàn)海域一前八小時(shí)的航跡上慣導(dǎo)數(shù)據(jù)和重力異常觀測(cè)數(shù)據(jù)進(jìn)行匹配試驗(yàn)。為了模擬初始位置誤差較大的情況，水下航行器的經(jīng)緯向位置誤差均設(shè)置為3 n mile，同時(shí)為了使濾波器搜索范圍能夠包含真正的位置，以慣導(dǎo)指示位置為中心，以三個(gè)基準(zhǔn)圖格網(wǎng)分辨率長(zhǎng)度為半徑，共設(shè)置49 個(gè)濾波器，分別進(jìn)行慣性導(dǎo)航解算和基于APEKF 算法的匹配導(dǎo)航解算。慣導(dǎo)航跡的經(jīng)、緯向誤差及APEKF 匹配算法匹配結(jié)果的經(jīng)、緯向誤差如圖10和圖11所示。由圖10和圖11可知，在位置誤差較大的情況下，通過(guò)APEKF 匹配算法進(jìn)行匹配導(dǎo)航，仍能獲得高精度的位置信息，且僅需要20 個(gè)濾波周期即一小時(shí)，濾波器便會(huì)收斂。

圖10 緯向誤差Fig.10 Errors of latitude

圖11 經(jīng)向誤差Fig.11 Errors of longitude

將APEKF 算法結(jié)果的經(jīng)、緯向誤差進(jìn)行統(tǒng)計(jì)，并與INS 的經(jīng)、緯向誤差進(jìn)行對(duì)比，誤差統(tǒng)計(jì)結(jié)果見(jiàn)表4。由表4可知，當(dāng)初始位置誤差較大時(shí)，在重力特征變化劇烈的區(qū)域進(jìn)行重力匹配，仍能獲得較高的定位精度，經(jīng)、緯向的位置誤差均優(yōu)于基準(zhǔn)圖分辨率。

表4 誤差統(tǒng)計(jì)（單位：海里）Tab.4 Error statistics (Unit:n mile)

4 結(jié) 論

本文針對(duì)SITAN 匹配算法中狀態(tài)方程存在模型誤差的問(wèn)題，研究了基于APEKF 的SITAN 算法。在重力異常特征不同的海域開(kāi)展了基于PEKF和APEKF的SITAN 匹配算法的仿真試驗(yàn)，試驗(yàn)結(jié)果表明利用自適應(yīng)因子調(diào)節(jié)預(yù)測(cè)信息的協(xié)方差矩陣的APEKF 算法能夠有效地提高重力匹配輔助慣性導(dǎo)航系統(tǒng)的定位精度和穩(wěn)定性，并且在重力適配區(qū)能夠有效地抑制濾波發(fā)散且定位精度與重力異常基準(zhǔn)圖網(wǎng)格分辨率相當(dāng)?；贏PEKF 的SITAN 匹配算法是一種根據(jù)當(dāng)前濾波器的相關(guān)信息自適應(yīng)的調(diào)節(jié)濾波增益，不需要借助外部信息，對(duì)于實(shí)現(xiàn)水下航行器長(zhǎng)航時(shí)、隱蔽、高精度的自主導(dǎo)航定位具有重要意義。由于APEKF 算法在重力異常特征變化明顯的海域定位精度高，在接下來(lái)的研究工作中，將利用高精度的位置信息對(duì)慣導(dǎo)系統(tǒng)的陀螺零漂、姿態(tài)誤差、速度誤差進(jìn)行反饋校正。