王夢(mèng)龍， 王 洋， 王 華

(1. 北京航空航天大學(xué) 宇航學(xué)院， 北京 100191； 2. 北京自動(dòng)化控制設(shè)備研究所， 北京 100074)

0 引 言

捷聯(lián)慣性導(dǎo)航系統(tǒng)一般固連在載體上， 直接測(cè)量載體的運(yùn)動(dòng)， 而姿態(tài)實(shí)時(shí)解算是捷聯(lián)慣性導(dǎo)航系統(tǒng)工程應(yīng)用中需要解決的關(guān)鍵技術(shù)[1-6]. 巡飛器在飛行過程中需要經(jīng)常做轉(zhuǎn)彎和盤旋等高動(dòng)態(tài)機(jī)動(dòng)動(dòng)作， 在這種高動(dòng)態(tài)環(huán)境中， 巡飛器姿態(tài)解算必然會(huì)產(chǎn)生不可交換性誤差， 特別是當(dāng)巡飛器做圓錐運(yùn)動(dòng)時(shí)產(chǎn)生的不可交換性誤差最大， 也稱為圓錐誤差[7-10]. 此時(shí)要得到高精度的姿態(tài)角， 必須對(duì)圓錐誤差進(jìn)行補(bǔ)償， 所以高動(dòng)態(tài)環(huán)境下圓錐誤差補(bǔ)償對(duì)巡飛器實(shí)時(shí)姿態(tài)解算精度的提高有重要意義. 為了減小圓錐誤差的影響， Bortz首次提出了等效旋轉(zhuǎn)矢量概念[11]， 此后國內(nèi)外學(xué)者對(duì)此作了大量研究. 文獻(xiàn)[12]提出了一種基于角增量的改進(jìn)高動(dòng)態(tài)捷聯(lián)慣導(dǎo)解算算法， 文獻(xiàn)[13]提出了一種雙速優(yōu)化姿態(tài)編排算法， 文獻(xiàn)[14]提出了一種旋轉(zhuǎn)矢量多迭代捷聯(lián)姿態(tài)計(jì)算誤差補(bǔ)償算法， 文獻(xiàn)[15]采用角速率分析方法推導(dǎo)了一種簡(jiǎn)單新四子樣算法， 這種方法為改進(jìn)角速率輸入下的旋轉(zhuǎn)矢量算法提供了一種新的思路.

本文從工程實(shí)用的角度出發(fā)， 針對(duì)高動(dòng)態(tài)環(huán)境中巡飛器姿態(tài)信息實(shí)時(shí)解算的特點(diǎn)， 提出了一種基于滑動(dòng)三周期旋轉(zhuǎn)矢量的姿態(tài)濾波算法. 最后通過數(shù)字仿真和飛行動(dòng)態(tài)試驗(yàn)驗(yàn)證了該算法的有效性.

1 基于角速率的等效旋轉(zhuǎn)矢量法

傳統(tǒng)的姿態(tài)更新算法往往采用角增量計(jì)算等效旋轉(zhuǎn)矢量， 這種方法應(yīng)用于角速率輸出陀螺儀， 需要先用數(shù)值積分法將角速率信息轉(zhuǎn)換為角增量信息， 這樣必然會(huì)引入積分計(jì)算誤差.

根據(jù)Bortz理論[11,16]， 等效旋轉(zhuǎn)矢量微分方程如下

(1)

由于姿態(tài)解算周期一般較短， |Φ|的數(shù)值很小，Φ的高次冪可以忽略， 因此可得到工程中常用近似的等效旋轉(zhuǎn)矢量微分方程為

(2)

對(duì)式(2)在姿態(tài)更新周期T上進(jìn)行積分， 得到時(shí)間T內(nèi)等效旋轉(zhuǎn)矢量增量為

ΔΦ=α+e,

(3)

在姿態(tài)更新周期T內(nèi)， 假設(shè)載體角速度可以由三次多項(xiàng)式擬合表示如下

(4)

式中：t為當(dāng)前時(shí)刻；τ為到t時(shí)刻的時(shí)間間隔；a、b、c、d為多項(xiàng)式系數(shù).

若將每個(gè)姿態(tài)更新周期T分為三等份， 并在每個(gè)等分時(shí)間點(diǎn)采樣對(duì)應(yīng)的角速率， 則在時(shí)間區(qū)間[t,t+T]內(nèi)得到4個(gè)角速率， 按照時(shí)間從前至后的順序依次對(duì)應(yīng)ω1,ω2,ω3,ω4， 將其代入式(4)可得多項(xiàng)式系數(shù)如下

(5)

α=aT+bT2+cT3+dT4.

(6)

根據(jù)式(4)， 可得t時(shí)刻Φ的各階導(dǎo)數(shù)為

(7)

將式(5)～(7)代入Φ(t+T)在t時(shí)刻的泰勒展開式， 得到基于角速率輸入的等效旋轉(zhuǎn)矢量在時(shí)間區(qū)間[t,t+T]上增量的估計(jì)表達(dá)式為

(8)

算法以4個(gè)連續(xù)的采樣角速率作為輸入計(jì)算一個(gè)姿態(tài)更新周期內(nèi)的等效旋轉(zhuǎn)矢量增量.

設(shè)巡飛器沿OL軸作如圖 1 所示的典型圓錐運(yùn)動(dòng)， 圖中OXYZ為參考坐標(biāo)系；Oxyz是載體坐標(biāo)系， 則其等效旋轉(zhuǎn)矢量為

Φ(t)=[0γcos(ω0t)γsin(ω0t)]T,

(9)

式中：γ為錐半角；ω0為角頻率.

將式(9)轉(zhuǎn)化為四元數(shù)表示， 并通過四元數(shù)微分方程可得典型圓錐運(yùn)動(dòng)下載體坐標(biāo)系相對(duì)于導(dǎo)航坐標(biāo)系的角速率如下

(10)

圖 1 典型圓錐運(yùn)動(dòng)示意圖Fig.1 Diagram of typical coning motion

將式(10)代入式(3)得到時(shí)間T內(nèi)等效旋轉(zhuǎn)矢量增量理論值為

(11)

由于周期震蕩不會(huì)造成誤差在時(shí)間上的累積， 故僅分析x軸上的算法誤差. 將式(9)代入式(3)可以得到典型圓錐運(yùn)動(dòng)中載體x軸圓錐誤差項(xiàng)真實(shí)值e為

e=2sin2(γ/2)(ω0T-sin(ω0T)).

(12)

令λ=ω0td， 則在典型圓錐環(huán)境下有

(13)

由式(13)計(jì)算圓錐誤差計(jì)算值β， 并在t時(shí)刻進(jìn)行Taylor展開

(14)

式中：K1=K12+K23+K34，K2=K13+K24，K3=K14.

同樣對(duì)式(12)圓錐誤差理論值在t時(shí)刻進(jìn)行Taylor展開

(15)

令圓錐誤差計(jì)算值β等于圓錐誤差理論值e， 并忽略λ的高次項(xiàng)可得

(16)

將式(16)代入式(8)即可得到基于角速度三周期圓錐補(bǔ)償旋轉(zhuǎn)矢量算法

(17)

基于角速度三周期圓錐補(bǔ)償旋轉(zhuǎn)矢量誤差估計(jì)值為

(18)

將式(14)，(15)代入式(18)， 并忽略高階小量可得

(19)

2 滑動(dòng)三周期旋轉(zhuǎn)矢量更新算法

在相同動(dòng)態(tài)環(huán)境中， 隨著采樣周期的增加， 多周期旋轉(zhuǎn)矢量更新算法的精度會(huì)增加， 但算法姿態(tài)更新的頻率卻降低了， 影響了整個(gè)系統(tǒng)的實(shí)時(shí)性. 針對(duì)巡飛器這類對(duì)飛行器姿態(tài)更新頻率要求較高的飛行器， 本文提出了一種滑動(dòng)三周期旋轉(zhuǎn)矢量更新算法， 有效結(jié)合了單周期算法更新速率快和三周期算法計(jì)算精度高的優(yōu)點(diǎn). 算法以連續(xù)兩個(gè)單周期算法作為啟動(dòng)項(xiàng)， 在第三個(gè)周期后采用三周期算法解算巡飛器的姿態(tài)角， 從而實(shí)現(xiàn)姿態(tài)的快速更新.

假設(shè)系統(tǒng)一個(gè)姿態(tài)更新周期T內(nèi)的采樣點(diǎn)個(gè)數(shù)為N， 則載體的角速率采樣周期td=T/N. 單周期旋轉(zhuǎn)矢量更新算法、 三周期旋轉(zhuǎn)矢量更新算法以及滑動(dòng)三周期旋轉(zhuǎn)矢量更新算法的載體角速率采樣周期和姿態(tài)更新周期如圖 2 所示.

圖 2 姿態(tài)更新周期與采樣周期關(guān)系示意圖Fig.2 Diagram of relationship between attitude update period and sampling period

由圖 2 可知， 滑動(dòng)三周期旋轉(zhuǎn)矢量算法的姿態(tài)角更新頻率是普通三周期旋轉(zhuǎn)矢量算法的3倍. 滑動(dòng)三周期旋轉(zhuǎn)矢量算法采用單周期旋轉(zhuǎn)矢量法作為啟動(dòng)算法， 在前2個(gè)姿態(tài)采樣周期采用單周期旋轉(zhuǎn)矢量法， 可以由式(20)得到第1個(gè)采樣周期末t1時(shí)刻的姿態(tài)變化四元數(shù).

(20)

(21)

(22)

式中：Q(t0)為初始時(shí)刻t0的初始姿態(tài)四元數(shù).

同理， 可由單周期旋轉(zhuǎn)矢量法得出t2時(shí)刻的姿態(tài)更新四元數(shù)Q(t2)

(23)

(24)

(25)

此時(shí)， 利用前2個(gè)采樣周期的計(jì)算結(jié)果及采樣數(shù)據(jù)， 在第3個(gè)采樣周期末t3時(shí)刻， 采用三周期旋轉(zhuǎn)矢量法計(jì)算t3時(shí)刻的姿態(tài)更新四元數(shù)Q(t3)， 采用三周期旋轉(zhuǎn)矢量法時(shí)，T=3td.

(26)

(27)

(28)

類似的， 按照式(26)～(28)可計(jì)算t3時(shí)刻之后各時(shí)刻的姿態(tài)更新四元數(shù)

(29)

由上述推導(dǎo)可以得出， 本文提出的滑動(dòng)三周期旋轉(zhuǎn)矢量姿態(tài)更新算法需要在前2個(gè)陀螺儀采樣周期內(nèi)采用單周期旋轉(zhuǎn)矢量算法啟動(dòng)， 算法啟動(dòng)后的每個(gè)陀螺儀采樣周期末都可以利用之前連續(xù)3個(gè)陀螺儀采樣周期內(nèi)的測(cè)量和計(jì)算數(shù)據(jù)， 按照上述提出的滑動(dòng)三周期旋轉(zhuǎn)矢量姿態(tài)更新算法依次計(jì)算出各個(gè)陀螺儀采樣周期內(nèi)的姿態(tài)更新四元數(shù)， 從而實(shí)時(shí)更新巡飛器的姿態(tài)信息. 由理論推導(dǎo)可知， 本文提出的滑動(dòng)三周期旋轉(zhuǎn)矢量算法在更新頻率上與單周期算法持平， 高于雙周期與三周期算法.

3 基于滑動(dòng)三周期旋轉(zhuǎn)矢量的姿態(tài)濾波算法

3.1 狀態(tài)方程

擴(kuò)展卡爾曼濾波器的誤差狀態(tài)量選取如下

(30)

值得注意的是，在我國當(dāng)時(shí)的民事法律及司法解釋中尚無隱私權(quán)保護(hù)的規(guī)定，因此當(dāng)事人在民事法律上主張住宅方面的隱私權(quán)受侵害，并要求法律予以保護(hù)，缺乏充分的立法依據(jù)。二審法院大膽援用我國憲法關(guān)于住宅不受侵犯的規(guī)定，從中引申出住宅隱私權(quán)的內(nèi)容，從而對(duì)當(dāng)事人的隱私權(quán)加以保障。二審法院對(duì)新興權(quán)利的論證思路是，首先從我國憲法第39條第一句關(guān)于住宅不受侵犯的規(guī)定出發(fā)，認(rèn)定住宅體現(xiàn)了個(gè)人領(lǐng)域，進(jìn)而認(rèn)定住宅的私密性屬于隱私權(quán)，應(yīng)受法律保護(hù)。

(31)

式中：α為機(jī)動(dòng)頻率；w(t)為白噪聲.

姿態(tài)角誤差方程忽略地球表觀運(yùn)動(dòng)角速度， 以式(30)為狀態(tài)量的狀態(tài)方程為

(32)

3.2 量測(cè)方程

選取加速度計(jì)的輸出值與加速度估計(jì)值的差值以及地磁傳感器的輸出值與地磁向量在載體坐標(biāo)系中的投影的差值作為觀測(cè)量. 其中， 加速度估計(jì)值包含載體機(jī)動(dòng)加速度估計(jì)值和重力加速度向量在載體坐標(biāo)系中的投影， 系統(tǒng)的觀測(cè)方程為

Z(t)=H(t)X(t)+V(t),

(33)

在每個(gè)采樣周期內(nèi)， 利用滑動(dòng)三周期旋轉(zhuǎn)矢量算法更新航向姿態(tài)四元數(shù)， 系統(tǒng)誤差狀態(tài)及協(xié)方差時(shí)間更新過程如下

(34)

量測(cè)更新方程為

(35)

4 試驗(yàn)結(jié)果

為驗(yàn)證本文基于滑動(dòng)三周期旋轉(zhuǎn)矢量的姿態(tài)濾波算法在工程實(shí)踐中的有效性， 分別進(jìn)行了數(shù)字仿真試驗(yàn)和動(dòng)態(tài)飛行試驗(yàn).

4.1 數(shù)字仿真試驗(yàn)

定點(diǎn)盤旋是巡飛器在執(zhí)行偵查任務(wù)過程中一種常見的飛行狀態(tài)， 這種飛行狀態(tài)與典型的圓錐運(yùn)動(dòng)類似. 本組數(shù)字仿真通過模擬巡飛器在空中的定速定點(diǎn)盤旋飛行狀態(tài)來驗(yàn)證本文基于滑動(dòng)三周期旋轉(zhuǎn)矢量的姿態(tài)濾波算法的有效性. 仿真盤旋半徑為600 m， 高度為200 m， 飛行速度20 m/s， 滾轉(zhuǎn)角為20°， 俯仰角為0°， 總仿真時(shí)間為250 s. 分別采用算法1： 單周期旋轉(zhuǎn)矢量濾波算法； 算法2： 四元數(shù)四階龍格庫塔濾波算法； 算法3： 基于滑動(dòng)三周期旋轉(zhuǎn)矢量的姿態(tài)濾波算法3種算法估計(jì)巡飛器飛行過程中的姿態(tài)角誤差.

如圖 3 所示， 巡飛器首先向東勻速直線飛行60 s， 隨后切入勻速盤旋飛行模式. 仿真計(jì)算結(jié)果如圖 4， 表 1 和表 2 所示.

圖 3 盤旋飛行軌跡Fig.3 Flight path of circling flight

圖 4 航向姿態(tài)角估計(jì)誤差Fig.4 Estimated error of heading and attitude angle

算法最大滾轉(zhuǎn)角誤差/(°)最大俯仰角誤差/(°)最大航向角誤差/(°)算法112.16.337.4算法25.42.18.3算法31.91.12.5

表 2 各算法姿態(tài)更新周期

通過圖 4 可以看出， 在巡飛器由勻速水平飛行切入到勻速定點(diǎn)盤旋飛行模式后， 由于巡飛器的運(yùn)動(dòng)狀態(tài)類似于典型的圓錐運(yùn)動(dòng)， 導(dǎo)致航向姿態(tài)角解算誤差有一定幅度的增加， 尤其是受圓錐運(yùn)動(dòng)影響最大的航向角通道. 由表 1， 表 2 數(shù)據(jù)可以看出算法3的航向姿態(tài)角最大誤差約為算法2的1/2， 僅為算法1的1/10， 且算法3的姿態(tài)更新周期與算法1相同， 僅為算法2的1/2.

4.2 飛行動(dòng)態(tài)試驗(yàn)

為了驗(yàn)證本文基于滑動(dòng)三周期旋轉(zhuǎn)矢量的姿態(tài)濾波算法的飛行動(dòng)態(tài)性能， 本文選用荷蘭XSENS公司的MTI微慣性測(cè)量單元作為航向姿態(tài)角的參考基準(zhǔn). MTI的性能參數(shù)如下： 采樣頻率為100 Hz， 滾轉(zhuǎn)角及俯仰角靜態(tài)測(cè)量精度<0.5°， 航向角靜態(tài)測(cè)量精度<1°， 動(dòng)態(tài)測(cè)量精度為2° RMS.

利用巡飛器試驗(yàn)平臺(tái)搭載MTI模擬執(zhí)行偵察任務(wù)， 巡飛器在空中做定點(diǎn)盤旋飛行， 飛行實(shí)驗(yàn)平臺(tái)及飛行軌跡如圖 5， 圖 6 所示. 利用MTI加速度計(jì)、 陀螺儀和地磁傳感器原始數(shù)據(jù)， 采用同數(shù)字仿真試驗(yàn)中相同的3個(gè)算法解算巡飛器航向姿態(tài)角， 最后將解算結(jié)果與MTI航向姿態(tài)角輸出值做比對(duì)得到誤差角. 飛行動(dòng)態(tài)試驗(yàn)結(jié)果如表 3 和圖 7 所示.

圖 5 巡飛器試驗(yàn)平臺(tái)Fig.5 Loitering vehicle test platform

圖 6 試驗(yàn)飛行軌跡Fig.6 Flight path of dynamic flight test

圖 7 航向姿態(tài)角估計(jì)誤差Fig.7 Estimated error of heading and attitude angle

算法最大滾轉(zhuǎn)角誤差/(°)最大俯仰角誤差/(°)最大航向角誤差/(°)算法138.126.440.1算法212.38.716.2算法34.63.58.9

巡飛器做定點(diǎn)盤旋時(shí)一直處于高動(dòng)態(tài)環(huán)境中， 此時(shí)巡飛器的飛行狀態(tài)類似于典型的圓錐運(yùn)動(dòng). 由圖7和表3數(shù)據(jù)可已看出， 算法1由于采用了對(duì)圓錐誤差抑制效果較弱的單周期旋轉(zhuǎn)矢量算法， 從而導(dǎo)致其航向姿態(tài)角解算誤差最大， 特別是受圓錐運(yùn)動(dòng)影響最大的航向角通道， 其解算誤差達(dá)到了40.1°. 采用龍格庫塔法的算法2的航向角解算誤差為16.2°， 采用滑動(dòng)三周期旋轉(zhuǎn)矢量算法的算法3的航向角解算誤差為8.9°. 此時(shí)算法3的航向姿態(tài)角解算誤差約為算法2的1/2， 且僅約為算法1的1/7. 上述動(dòng)態(tài)飛行試驗(yàn)結(jié)果驗(yàn)證了本文基于滑動(dòng)三周期旋轉(zhuǎn)矢量的姿態(tài)濾波算法的穩(wěn)定性及有效性， 在動(dòng)態(tài)飛行過程中具有較好的航向姿態(tài)角解算精度.

5 結(jié) 論

本文從工程實(shí)用的角度出發(fā)， 對(duì)現(xiàn)有幾種姿態(tài)更新算法進(jìn)行分析和比較， 詳細(xì)推導(dǎo)得出了角速率輸入的等效旋轉(zhuǎn)矢量算法， 并提出了一種基于滑動(dòng)三周期旋轉(zhuǎn)矢量的姿態(tài)濾波算法. 通過仿真及試驗(yàn)將本算法與其他兩種常用的姿態(tài)更新算法對(duì)比， 證實(shí)了基于滑動(dòng)三周期旋轉(zhuǎn)矢量的姿態(tài)濾波算法的合理性， 有效地解決了計(jì)算精度與計(jì)算周期的矛盾.

數(shù)字仿真及試驗(yàn)結(jié)果表明：

1) 該算法有效地補(bǔ)償了系統(tǒng)的圓錐誤差， 其誤差角僅為單周期旋轉(zhuǎn)矢量算法的1/7， 且明顯優(yōu)于四元數(shù)四階龍格庫塔算法.

2) 該算法的姿態(tài)更新周期僅為普通三周期旋轉(zhuǎn)矢量法的1/3， 彌補(bǔ)了常規(guī)旋轉(zhuǎn)矢量姿態(tài)更新算法存在的更新頻率較低的問題.

3) 該算法的動(dòng)態(tài)性能可以滿足巡飛器等高動(dòng)態(tài)飛行器的應(yīng)用需求.

[1]李海濤， 曹詠弘， 祖靜. 等效旋轉(zhuǎn)矢量法在旋轉(zhuǎn)彈姿態(tài)解算中的應(yīng)用[J]. 測(cè)試技術(shù)學(xué)報(bào)， 2011， 25(4)： 287-291. Li Haitao, Cao Yonghong, Zu Jing. Application of rotation vector algorithm to attitude updating for spinning projectiles[J]. Journal of Test and Measurement Technology, 2011, 25(4)： 287-291. (in Chinese)

[2]程承， 潘泉， 李漢舟. 一種新的捷聯(lián)慣導(dǎo)系統(tǒng)圓錐誤差補(bǔ)償算法研究[J]. 彈箭與制導(dǎo)學(xué)報(bào)， 2014， 34(1)： 1-4. Cheng Cheng, Pan Quan, Li Hanzhou. The research on new coning error compensation algorithm for strapdown inertial navigation system[J]. Journal of Projectiles, Rockets, Missiles and Guidance, 2014, 34(1)： 1-4. (in Chinese)

[3]Maley J M. Efficient attitude estimation for a spin-stabilized projectile[J]. Journal of Guidance Control and Dynamics, 2016, 39(2)： 339-350.

[4]Weibel D, Lawrence D, Palo S. Small unmanned aerial system attitude estimation for flight in wind[J]. Journal of Guidance Control and Dynamics, 2015, 38(7)： 1300-1305.

[5]Chen J Z, Yuan J P, Fang Q. Flight vehicle attitude determination using the modified rodrigues parameters[J]. Chinese Journal of Aeronautics, 2008, 21(5)： 433-440.

[6]Gebre-Egziabher D, Hayward R C, Powell J D. Design of multi-sensor attitude determination systems[J]. IEEE Transactions on Aerospace and Electronic Systems, 2004, 40(2)： 627-649.

[7]嚴(yán)恭敏， 楊小康， 翁浚， 等. 一種求解不可交換誤差補(bǔ)償系數(shù)的通用方法[J]. 宇航學(xué)報(bào)， 2017， 38(7)： 723-727. Yan Gongmin, Yang Xiaokang, Weng Jun, et al. A general method to obtain noncommutativity error compensation coefficients for strapdown attitude algorithm[J]. Journal of Astronautics, 2017, 38(7)： 723-727. (in Chinese)

[8]Savage P G. Strapdown inertial navigation integration algorithm design part 1： attitude agorithms[J]. Journal of Guidance Control and Dynamics, 1998, 21 (1)： 19-28.

[9]嚴(yán)恭敏， 楊小康， 翁浚， 等. 捷聯(lián)慣導(dǎo)中求解圓錐誤差系數(shù)的通用算法[J]. 導(dǎo)航定位學(xué)報(bào)， 2017， 5(3)： 1-5. Yan Gongmin, Yang Xiaokang, Weng Jun, et al. A general numerical method to obtaining optimized coning compensation coefficients for strapdown attitude algorithm[J]. Journal of Navigation and Positioning, 2017, 5(3)： 1-5. (in Chinese)

[10]Lee J G, Yoon Y J, Mark J G, et al. Extension of strapdown attitude algorithm for high-frequency base motion[J]. Journal of Guidance Control and Dynamics, 1990, 13 (4)： 738-743.

[11]Bortz J E. A new mathematical formulation for strapdown inertial navigation[J]. Journal of IEEE Transactions on Aerospace and Electronic Systems, 1971, 7(1)： 61-66.

[12]趙欣， 王仕成， 楊東方， 等. 一種改進(jìn)的高動(dòng)態(tài)捷聯(lián)慣導(dǎo)解算算法[J]. 中國慣性技術(shù)學(xué)報(bào)， 2011， 19(2)： 163-169. Zhao Xin, Wang Shicheng, Yang Dongfang, et al. Improved algorithm for high dynamic strapdown inertial navigation system[J]. Journal of Chinese Inertial Technology, 2011, 19(2)： 163-169. (in Chinese)

[13]邢麗， 熊智， 劉建業(yè)， 等. 針對(duì)高動(dòng)態(tài)載體應(yīng)用的高精度捷聯(lián)慣導(dǎo)姿態(tài)算法優(yōu)化方法[J]. 中國慣性技術(shù)學(xué)報(bào)， 2014， 22(6)： 701-706. Xing Li, Xiong Zhi, Liu Jianye, et al. Optimized method of high-precision attitude algorithm for high dynamic vehicles[J]. Journal of Chinese Inertial Technology, 2014, 22(6)： 701-706. (in Chinese)

[14]王真， 高鳳岐， 高敏， 等. 旋轉(zhuǎn)矢量多迭代捷聯(lián)姿態(tài)解算誤差補(bǔ)償算法[J]. 中國測(cè)試， 2016， 42(8)： 113-117. Wang Zhen, Gao Fengqi, Gao Min, et al. Research on error compensation improved algorithm of strapdown attitude & heading measurement based on multiple iteration rotation vector[J]. China Measurement & Test, 2016, 42(8)： 113-117. (in Chinese)

[15]黃昊， 鄧正隆. 角速率輸入下的航姿算法研究[J]. 中國慣性技術(shù)學(xué)報(bào)， 2002， 10(8)： 9-14. Huang Hao, Deng Zhenglong. Study of navigation attitude algorithms for angular rate input[J]. Journal of Chinese Inertial Technology, 2002, 10(8)： 9-14. (in Chinese)

[16]Wu Y X, Hu X P, Hu D W, et al. Strapdown inertial navigation system algorithms based on dual quaternions[J]. Journal of IEEE Transactions on Aerospace and Electronic Systems, 2005, 41(1)： 110-132.