基于UKF的水下裝備軌跡估計濾波算法
2025-09-30 00:00:00陳子越季福武周偉
海洋開發與管理 2025年1期
中圖分類號：P715.9 文獻標志碼：A 文章編號：105-9857（2025）01-052-09
（1.ScholofOceanandEarthScience，TongjiUniversity，Shanghai，China; 2.ScholofElectronicandInformationEnginering，TongjiUniversity，Shanghai，China）
Abstract：TheunderwaterequipmentcariedbytheEastChinaSeaseabedobservationnetwork isoftenlostduetohumanactivities.Whensolvingthetrajectoryofunderwaterequipmentbyinertialnavigation，thenoisecausedbythecomplexoceanenvironmentandsudenmotionleads tolargecalculationerors.Aimingatthelimitationthattheclasicunscented Kalmanfilter nedsacuratenoisemodelanddynamicmodeltooutputbeterfilteringresultsinthetrajectory estimationproces，anadaptivefilteringalgorithmbasedonSage-HusaunscentedKalmanfilter isproposedforsolvingthetrajectoryofunderwaterequipmentbeingtowed.Firstly，theimplementationproces oftheclasicunscentedKalmanfilterisexplained，andtheideaofSage-Husa adaptiveadjustmentofnoiseisintroduced;then，onthisbasis，thepredictionresidualvectoris introducedtoreducetheinfluenceofgros eroronthefilteringresult，andatrackingfactoris introducedtoimrovetheadatabilit ofthefiltertosudenmotion;final ，theefectivenesof thealgorithmisverifiedbysimulationexperimentsandtowingexperiments.Theexperimentalresults showthatheadaptivefilteringalgorithmbasedonSage-HusaunscentedKalmanfilterefectively reducesthedivergenceerorcausedbytheunderwateracousticenvironmentandsudenmotion， andimprovesthepositioningacuracyandpositioningstabilityofunderwaterequipmentwhen beingtowed.
Keywords：Unscentedkalmanfilter，Adaptivefilteringalgorithm，Underwaterequipmenttowing，Advancedalgorithm
0 引言
隨著科技的發展和建設海洋強國戰略目標的提出，東海海底觀測網作為我國首個智能化、一體化、開放式的海底觀測網，現已取得良好的成果[1]。但東海頻繁的人為活動對布放于海底觀測網的水下裝備構成了較大的威脅[2-4]，如漁網極易將坐底觀測平臺等裝備拖行[5]，導致資產丟失。水下裝備被拖行時的運動不規則，且包含較多的突變運動，如磕碰礁石等。尋找丟失的水下裝備時，需要用到多種定位技術，其中慣性導航得益于其不易受外界干擾、具有自主性、隱蔽性好的優點，成為水下導航定位的基礎技術[6]。慣性導航敏感元件進行姿態解算時常用的數據融合技術有互補濾波技術、粒子濾波技術、卡爾曼濾波技術等[7-8]。其中互補濾波雖然計算結構簡單、計算量小，但通常只能用于姿態角解算而非位置解算，位置估算需要更復雜的動態模型和誤差修正。粒子濾波技術雖然在強非線性和非高斯噪聲的系統中表現出色，但其對計算資源要求高，且計算復雜，不適用于較為有限的計算資源環境。卡爾曼濾波（KalmanFilter，KF）在慣性導航系統數據融合處理的過程中極其常見，是一種線性最小方差估計算法[9]。其具有位置解算實時性強、數據融合能力較強、計算復雜程度適中的優點。卡爾曼濾波器需要滿足系統噪聲和量測噪聲的概率密度服從高斯分布且系統動力學模型已知這兩個條件，才能獲得最優估計[10]。但由于水下裝備的運動模型為非線性模型，所以需要使用非線性的卡爾曼濾波算法。Julier等于20世紀90年代提出的無跡卡爾曼濾波（UnscentedKalmanFilter，UKF）是當下較為常用的非線性卡爾曼濾波算法，該算法通過無跡變換選取采樣點來近似非線性概率密度分布的均值和方差，因為其采用了 KF 的框架而非引入線性化誤差，所以具有計算量小且精度較高的優點[11-14]。由于 UKF和 KF都是基于最小方差估計原理的濾波算法，所以都需要在已知準確的系統模型和噪聲的前提下才能具有良好的估計值。但在實際水下應用中，噪聲具有極大的不確定性，且完全準確的動力學模型也很難獲取[15-16]。慣性器件誤差、水下流體變化和復雜的海洋環境等因素都會影響動力學模型和噪聲預測的結果。
眾多學者提出了一系列的自適應 KF 算法，通過對量測噪聲估計值的實時調整，來克服 KF 算法由于動態和測量模型中噪聲數據不準確導致濾波器發散的問題。WANG 等[17]在Sage-Husa濾波器的基礎上提出了一種自適應穩健 UKF 算法，提升了自主水下航行器（AutonomousUnderwaterVe-hicle，AUV）導航的精度和穩定性的同時，也提升了模型抵抗粗大誤差影響的能力。但面對系統噪聲和測量噪聲同時出現異常的情況時，濾波器還是存在發散的問題。WANG 等[18]提出的Sage-Husa自適應卡爾曼濾波器通過模糊聚類算法更新聚類中心的方法，實現實時估計噪聲的功能并提升了濾波算法的精度。但濾波器無法處理高頻脈沖干擾和混合噪聲。文勝等[19]針對Sage-Husa濾波器存在精度低和可靠性差的問題，通過指數漸消記憶加權估計方法提升了算法的自適應能力，并結合漸消因子提升了算法抗干擾的能力。肖鵬飛等[20]針對傳統 無法實時跟蹤突變狀態的問題，提出了自適應 UKF算法，在保持濾波精度不變的情況下提高了傳統UKF的適應能力，并且減少了算法計算量，提高了系統的濾波效率。然而，目前國內外對慣性導航中KF算法的研究通常聚焦在提升濾波精度和長航時穩定性上，對載體存在被動運動姿態突變的航程中如何保持濾波算法穩定性的研究仍存在一定空白。因此，本文在前人研究的基礎上，通過針對粗誤差計算殘差預測值并引入跟蹤因子的方式，提出一種在滿足常規運動姿態及位置解算準確性的基礎上，能夠減小突變運動導致的導航粗大誤差并提升穩定性的自適應UKF算法。
1 UKF算法
UKF算法的核心思想是通過產生一組sigma點并使用加權統計線性回歸方法即無損變換來線性化非線性函數。UKF的非線性濾波模型如下：
Xk=f（Xk-1）+wk-1
Zk=h（Xk）+Vk
式和式中： Xk 和 Zk 分別為狀態初始向量和量測向量; f（?） 和 h（?） 為非線性函數， f（?） ）是狀態轉移函數， h（?） 是觀測函數; wk 和 Vk 分別為高斯狀態噪聲和量測噪聲;式（3）和式（4）中： Qk 和 Rk 分別為系統（過程）噪聲協方差和測量（觀測）噪聲協方差; q 和 r 分別為 wk 和 Vk 的非零均值。
UKF產生sigma點并預測sigma點下一幀狀 態，估計系統的均值和協方差的過程如下：
步驟1，初始化并生成sigma點：
λ=α2（n+κ）-n
式中： 
為初始狀態; P0∣0 為協方差矩陣。 λ 是比例因子，通常 α 取值很小，如 10-3 ， κ 通常取 0;β 通常取2; W0（m） 和 W0（c） 為sigma點的權重，前者為均值權重，后者為協方差權重。
步驟2，sima點的傳播：
（1）將sima點通過狀態轉移函數傳播到預測 時刻：
式中： 
為狀態預測均值; Pk∣k-1 為狀態預測協方差。
將 點通過觀測函數傳播到觀測空間：
式中： h（?） 為觀測函數; 
為觀測預測均值;PZZ，k 為觀測預測協方差矩陣; PXZ，k 為狀態和觀測之間的互協方差矩陣。
步驟3，濾波結果和噪聲更新：
計算卡爾曼增益并更新狀態估計：
Kk=Pxz，kPzz，k-1
Pk|k=Pk|k-1-KkPZZKkT
式中： 
為狀態估計值; 
為狀態一步預測值; 
為量測一步預測值; Pk∣k 為狀態估計誤差協方差矩陣; Pk∣k-1 為狀態一步預測誤差協方差矩陣。
2 濾波算法改進
2.1 Sage-Husa濾波算法
Sae-Husa算法在經典UKF算法基礎上，類比線性模型在給定觀測數據下模型的似然函數最大化所采用的極大似然估計統計方法，引入了自適應算法來計算非線性模型實時的狀態噪聲估計值和量測噪聲估計值。在線性模型中，狀態噪聲和量測噪聲分別為：
線性模型的狀態預測協方差矩陣和觀測預測協方差矩陣分別表示為：
由式（12）、式（16）可以得到：
式中： 
為狀態噪聲估計值; 
為量測噪聲估計值; 
為狀態噪聲協方差估計值; 
為量測噪聲協方差估計值; Fj 為線性模型的狀態轉移矩陣;Hj 為線性模型的量測矩陣。綜上，式（ ）在非線性模型中可以改寫為：
式中： b 為遺忘因子，遺忘因子越小，濾波器對變化噪聲的適應能力越強，但當 b 過小，上述噪聲參數更新結果會出現不穩定的現象。一般來說， 0.90[21-22] 。
2.2 殘差預測和跟蹤因子
粗差是指在觀測值中存在的誤差較大的偏差。考慮到海洋環境的復雜性以及水下裝備被拖動時存在的不規則突變運動，粗差不可避免地會給濾波結果帶來影響，并輸出偏差較大的參數 
。為了減少粗差帶來的影響，可以使用預測殘差來約束增益矩陣。預測殘差是觀測值與預測值之間的差值，可用于識別粗差并限制粗差的影響。
式中： εk 為粗差; Gk 為粗差干擾矩陣; νk 為無粗差的預測殘差。
在經典 UKF 的計算中，將式（26）移項后帶有粗差的預測殘差 
表示為：
由于預測殘差本質上是觀測值與預測值之間的差值，所以預測殘差 νk 和 
可以通過式（13）到式（ ）表示為：
若通過殘差預測來反應粗差，那么式（19）中的狀態估計向量 
可以寫為：
在非線性模型的觀測中，殘差預測 νk 包含粗差并通過卡爾曼增益矩陣 Kk 影響狀態估計向量 
。因此，利用預測殘差來約束增益矩陣可以減小粗誤差對狀態估計的影響。此外，類比用于狀態估計的強跟蹤濾波方法，還可以引入一個額外的狀態變量來增強對目標狀態的跟蹤能力，從而提高濾波器對不穩定環境和快速變化的適應性，具體實現方式為引入一個跟蹤因子。
γ=max（cγ，∣Sc∣）
Kk=γ-1Kk
式中：γ 為卡爾曼增益 Kk 的跟蹤因子，通過γ 的變化實時調整系統對增益的信任程度; Ψc 為常數，視水下裝備所處海域的海況而定，一般取 
為可調參數， ΣP 和 Ψc 越大，增益會相應減小，從而減小瞬時粗差對狀態估計的影響; sc 是第 k 個預測狀態的標準殘差，為可調參數 ΣP 和帶有粗差的預測殘差 
乘積除 
的中位數。將式（31）代入式（30），卡爾曼增益 Kk 由式（32）表示。
實驗及結果分析
3.1 仿真實驗結果與分析
為驗證改進算法的有效性，使用 MATLAB 作為平臺進行仿真，導航坐標系參考水下裝備在海底的運動選用東北參考系。借助西北工業大學的高精度 捷 聯 慣 導 算 法 （Precise StrapdownInertialNaviationSstem，PSINS）工具箱[23]生成仿真數據。根據水下裝備的應有運動特征生成仿真慣性器件數據，其中水下裝備運動包含勻速、加速、減速、左轉、右轉、上升、下降等多種運動模式，并于其中插入與被拖行裝備類似的突變運動，包含突變轉向和突變彈起。水下裝備平面拖行的理想軌跡如圖 所示，五角星為起始點，初始速度設置為 0m/s 。
圖1 水下裝備平面拖行仿真軌跡 Fig.1 Simulationofunderwaterequipmentflat towingtrajectory
仿真總時長為 ，模擬慣性導航器件采樣頻率為 2Hz ，共生成了1294 組慣性測量單元數據。傳感器仿真具體參數如表1所示。
表1 傳感器仿真參數
Table1 Sensorsimulationparameters
為驗證改進算法的準確性與魯棒性，分別使用經典UKF濾波、Sage-HusaUKF濾波（以下簡稱為SH-UKF）以及前文提出的基于Sage-Husa的改進UKF濾波（以下簡稱為 AD-UKF）對水下裝備仿真得到的慣性數據進行濾波和解算。首先通過繪制軌跡圖對比不同算法的準確性，上述算法對水下裝備軌跡的估計如圖2所示。
圖2 不同算法水下裝備仿真軌跡估計對比 Fig.2 Comparisonofunderwaterequipmenttrajectory estimationusingdiferentalgorithms
由圖 可以看出，由于海洋環境的復雜性以及水下裝備面對的被動不規則運動，在估計水下裝備拖行軌跡時濾波后位置解算結果常常發散。使用經典UKF濾波無法有效抑制發散的軌跡預測，呈嚴重發散狀。SH-UKF在經典 UKF的基礎上通過引入自適應算法實時計算和調整噪聲估計值，相較于經典收斂些許，但距離真實軌跡仍有較大距離。和真實軌跡最接近，未有嚴重發散。
通過計算SH-UKF算法和AD-UKF算法濾波后水下裝備運動速度和姿態角的均方根誤差（RotMeanSuareEror，RMSE）以驗證改進算法的穩定性。
式中： k 為有效時間內的姿態數據總數; 
為插入噪聲和器件誤差后通過濾波得到的導航信息; Xi 為純慣性導航仿真的理想導航數據。
上述兩種算法在仿真場景下的速度均方根誤差如圖 所示。
上述兩種算法在仿真場景下的俯仰角、橫滾角和偏航角的均方根誤差如圖 所示。
由圖3可以看出，在此仿真場景的647s中，解算東向和北向的速度時誤差會隨加速度計誤差逐漸累積。SH-UKF可抑制速度誤差累積，但抑制時長較短，無法控制較長時間的累積。其東向速度均方根誤差和北向速度均方根誤差于約 300s 時突破抑制，逐漸累積至約 15m/s 。而 AD-UKF相較SH-UKF可有效抑制速度誤差累積，且抑制時長較長。其東向速度均方根誤差和北向速度均方根誤差分別為 2m/s 和1.96m/s ，較SH-UKF的速度誤差抑制能力提升約86.67% 。兩種算法的速度誤差如表2所示。
圖3 仿真實驗速度均方根誤差
圖4 仿真實驗姿態角誤差
Fi.4 Simulationexerimentatitudeanleeror
表2 速度最大均方根誤差
Table2 Maximumspedrotmeansquareeror
由圖4可以看出，解算姿態角時誤差會隨陀螺儀誤差逐漸累積。SH-UKF可抑制姿態角的誤差累積，但抑制時長較短，無法控制較長時間的累積，且無法在姿態角突變時保持穩定，易產生較大波動。其俯仰角、橫滾角和偏航角均有不同程度的波動，其中俯仰角在647s內最大均方根誤差均達到了約 0.015rad ;橫滾角均方根誤差最大值為 0.006rad ;偏航角均方根誤差最大，絕對值超過了 
。AD-UKF 相較SH-UKF極大限度地抑制姿態角的誤差累積，俯仰角、橫滾角和偏航角的均方根誤差均穩定在 附近，誤差隨時間增量較小。兩種算法的姿態角誤差如表3所示。
表3 姿態角最大均方根誤差
3.2 拖曳實驗結果與分析
為進一步驗證改進算法的有效性，使用維特智能制造公司的JY62慣性導航實物進行了拖車拖曳實驗。JY62的慣性參數如表4所示，實驗持續約 562s ，拖車起始緯度為 31.27807° ，經度為 121.49504° ，高度約為 2.19m 。為體現水下裝備被拖行面臨的突變運動，于第 260s 和第 380s 拖行拖車經過地面凸起的路障從而使拖車產生突變運動。
表4 主要慣性參數
Table4 Maininertialparameters
分別使用經典 UKF 濾波算法、SH-UKF 算法以及AD-UKF算法對拖曳實驗得到的慣性數據進行濾波和解算。首先通過繪制軌跡圖對比不同算法的準確性，上述算法對拖曳載體軌跡的估計如圖5所示。
圖5 不同算法拖曳實驗軌跡估計對比 Fig.5 Comparisonoftrajectoryestimationoftowing experimentsusingdiferentalgorithms
由圖 可知，經典 算法和 算法在拖曳剛開始時對目標有較好的跟蹤能力，但在多次轉彎后較大程度地失去對目標的跟蹤能力。而AD-UKF算法相比于前兩種算法有較強的位置解算能力，解算出的軌跡和原始軌跡更為接近。SH-UKF算法和AD-UKF算法在仿真場景下的姿態角均方根誤差如圖6所示。
圖6 拖曳實驗姿態角誤差
Fig.6 Towingexperimentatitudeangleeror
由圖6 可見，AD-UKF 算法相比于 SH-UKF算法在姿態角突變時有更強的穩定性，AD-UKF雖然在姿態角突變時也會有一定程度的波動，但是可以快速穩定并收斂于真實值。圖 中俯仰角、橫滾角和偏航角的均方根誤差均只有小幅度的波動，可見 AD-UKF 算法對誤差的累積也有一定的抑制作用。
4 結語
本文針對普通 UKF 算法在水下裝備拖行軌跡估計解算中存在的精度低、穩定性差的缺點。設計了一種結合Sae-HusaUKF 的改進算法，通過計算殘差預測值及引入跟蹤因子的方法，對不規則和帶有突變的運動軌跡進行解算濾波。經過仿真實驗和拖曳實驗驗證，該改進算法能在純慣性導航的應用中有效實時調整預測噪聲并提升系統在不準確環境中的姿態、速度、位置解算的準確性，并在載體發生突變運動時保持較強的穩定性。
參考文獻（References）：
[1] 陳建冬，張達，王瀟，等.海底觀測網發展現狀及趨勢研究[J].海洋技術學報，2019，38（6）：95-103. CHENJiandnog，ZHANGDa，WANGXiao，etal.Researchonthestate-of-the-artandtrendsofSeaflorObservatory[J].JournalofOceanTechnology，2019，38（6）：95-103.
[2] 朱德高，陳奕鈧，高東明，等.基于大數據的海底電纜安全運行影響因素多維度分析[J].自動化應用，2019（12）：128-131. ZHU Degao，CHEN Yikang，GAODongming，etal.Multi-dimensionalanalysisofinfluencingfactorsofsafeoperationofsubmarinecablesbasedonbigdata[J].AutomationAplication，2019（12）：128-131.
[3] 王紅紅，劉國恒.中國海油海底管道事故統計及分析[J].中國海上油氣，2017，29（5）：157-160. WANG Honghong，LIU Guoheng.StatisticsandanalysisofsubseapipelineacidentsofCNOOC[J].ChinaOfshoreOilandGas，2017， 29（5）：157-160.
[4] 羅楚軍，李健，吳慶華，等.高壓海底電纜錨害事故風險評估[J].南方能源建設，2021，8（1）：67-73. LUOChujun，LIJian，WU Qinghua，etal.Riskasesmentmethodforanchoringacidentofhighvoltagesubmarinecable[J].Southern EnergyConstruction，2021，8（1）：67-73.
[5] 張巍.漁船拖網板對水下結構物防護裝置的撞擊損傷和能量研究[D].哈爾濱：哈爾濱工程大學，202. ZHNAG Wei.Researchonimpactdamageandenergyofsubseaprotectionstructuresbytrawlboard[D].Harbin：HarbinEngineringUniversity，202 .
[6] 張世童，張宏偉，王延輝，等.自主水下航行器導航技術發展現狀與分析[J].導航定位學報，2020，8（2）：1-7. ZHANGShitong，ZHANG Hongwei.WANG Yanhui，etal.Developmentstatusandanalysisofnavigationtechnologyforautonomous underwatervehicles[J].JournalofNavigationandPositioning，2020，8（2）：1-7.
[7] 劉穎，楊媛，徐金濤.融合互補濾波和卡爾曼濾波的姿態解算算法[J].西安郵電大學學報，2021，26（4）：98-104. LIUYing，YANGYuan，XUJintao.AtitudecalculationalgorithmforfusioncomplementaryfilteringandKalmanfiltering[J].Journalof Xi'anUniversityofPostsandTelecommunications，2021，26（4）：98-104.
[8] WUDongjin，XIALinyuan，GENGJijun.HeadingestimationforpedestriandeadreckoningbasedonrobustadaptiveKalmanfiltering[J]. Sensors，2018，18（6）：1970.
[9] 劉賢俊.大深度載人潛水器用組合導航系統關鍵技術研究[D].南京：東南大學，202. LIU Xianjun.Researchonkeytechnologiesofintegratednavigationsystemfordephumanocupiedvehicle[D].Nanjing：SoutheastUniversity，202 .
[10] 孫淑光，陳建達.改進調節因子的GPS/SINS組合導航自適應濾波算法[J].航天控制，202，40（4）：53-60. SUNShuguang，CHENJianda.GPS/SINSintegratednavigationadaptivefilteringalgorithm withimprovedadjustmentfactor[J].AerospaceControl，202 ，40（4）：53-60.
[1] JULIERSJ，UHLMANNJK.AnewextensionoftheKalmanfiltertononlinearsystems[J].ProcedingsofSPIE-TheInternational SocietyforOpticalEnginering，199，3068：182-193.
[12] MUS OC，OUDJANEN.Particlemethodsformultimodalfiltering[J].A plicationtoTerainNavigation.199，090（7）：6-8.
[13] 遲鳳陽.水下航行器組合導航定位技術研究[D].哈爾濱：哈爾濱工程大學，2017. CHIFengyang.ResearchonAUVIntegtatednavigationpositioningtechnology[D].Harbin：HarbinEngineringUniversity，2017.
[14] 嚴恭敏，嚴衛生，徐德民.簡化 UKF濾波在SINS大失準角初始對準中的應用[J].中國慣性技術學報，208（3）：253-264. YAN Gongmin，YAN Weisheng，XUDemin.AplicationofsimplifiedUKFinSINSinitialalignmentforlargemisalignmentangles[J]. JournalofChineseInertialTechnology，208（3）：253-264.
[15] 葉澤浩，宋亞偉，陳傳生，等.基于噪聲估計的自適應無跡卡爾曼濾波算法[J].空天預警研究學報，202 ，36（4）：242-246. YEZehao，SONG Yawei，CHENChuansheng，etal.AdaptiveuntracedKalmanfilteringalgorithmbasedonnoiseestimation[J].JournalofAiramp; SpaceEarlyWarningResearch，202 ，36（4）：242-246.
[16] 何東旭.AUV 水下導航系統關鍵技術研究[D].哈爾濱：哈爾濱工程大學，2014. HEDongxu.ResearchonkeytechologiesofINS/TAN/GANintegratednavigationsystemforunderwatervehicles[D].Harbin：Harbin EngineringUniversity，2014.
[17] WANGJunting，XU Tianhe，WANGZhenjie.AdaptiverobustunscentedkalmanfilterforAUVacousticnavigation[J].Sensors，2019， 20（1）：60.
[18] WANGXingyu，WANGAna，WANGDazhi，etal.AmodifiedSage-HusaadaptiveKalmanfilterforstateestimationofelectricvehicle servocontrolsystem[J].EnergyReports，202 ，8：20-27.
[19] 文勝，劉彩云，欒添添，等.改進Sage-Husa算法在SINS/GPS組合導航中的應用[J].哈爾濱理工大學學報，2024，29（3）：37-4 . WENSheng，LIUCaiyun，LUANTiantian，etal.AplicationofimprovedSage-HusaalgorithminSINS/GPSintegratednavigation[J]. JournalofHarbinUniversityofScienceandTechnology，2024.29（3）：37- 4.
[20] 肖鵬飛，許至尊，白虎林，等.自適應無跡卡爾曼濾波算法在水下組合導航系統中的應用[J].廣東海洋大學學報，2024， 4（4）： 121-128. XIAOPengfei，XUZhizun，BAIHulin，etal.AplicationofadaptiveunscentedKalmanFilterinunderwaterintegratednavigationsystem[J].JournalofGuangdongOceanUniversity，2024，4（4）：121-128.
[21] AHMAD MS，KUKRERO，HOCANINA.TheefectoftheforgetingfactorontheRIadaptivealgorithminsystemidentification[J]. IEEE，201 ，1-4.
[2] 張欣.多旋翼無人機的姿態與導航信息融合算法研究[D].長春：中國科學院研究生院（長春光學精密機械與物理研究所），2015. ZHANGXin.Researchontheatitudeandnavigationinformationfusionalgorithmofmulti-rotorUAV[D].Changchun：ChangchunInstituteofOptics，FineMechanicsandPhysics，ChineseAcademyofSciences，2015.
[23] 嚴恭敏.高精度捷聯慣性導航系統 Matlab工具箱[EB/OL]. （2013-09-16） ）.htps：/www.psins.org.cn/kydm. YANGongmin.Precisestrapdowninertialnavigationsystem MATLABtolbox[EB/OL].（2013-09-16）.htps：/ www.psins.org.cn/ kydm.
