李開(kāi)龍, 胡柏青, 常路賓

(海軍工程大學(xué)導(dǎo)航工程系, 湖北 武漢 430033)

?

改進(jìn)四元數(shù)無(wú)味卡爾曼濾波算法

李開(kāi)龍, 胡柏青, 常路賓

(海軍工程大學(xué)導(dǎo)航工程系, 湖北 武漢 430033)

摘要：針對(duì)四元數(shù)無(wú)味卡爾曼濾波計(jì)算量較大等問(wèn)題,提出一種改進(jìn)的四元數(shù)無(wú)味卡爾曼濾波算法,通過(guò)四元數(shù)與修正羅德里格斯參數(shù)的線(xiàn)性變換,避免了四元數(shù)無(wú)味卡爾曼濾波中,四元數(shù)與誤差羅德里格斯參數(shù)頻繁切換所帶來(lái)的計(jì)算量增大的問(wèn)題,從而降低了算法的計(jì)算量。推導(dǎo)了修正羅德里格斯參數(shù)出現(xiàn)奇異值時(shí),改進(jìn)四元數(shù)無(wú)味卡爾曼濾波的奇異值避免的轉(zhuǎn)換公式。在捷聯(lián)慣性導(dǎo)航系統(tǒng)/全球衛(wèi)星定位系統(tǒng)(strapdown inertial navigation system / global positioning system,SINS/GPS)直接式位置松組合仿真試驗(yàn)中,對(duì)比了乘性擴(kuò)展卡爾曼濾波、四元數(shù)無(wú)味卡爾曼濾波和改進(jìn)四元數(shù)無(wú)味卡爾曼濾波的估計(jì)性能,證明了所提出算法的有效性。

關(guān)鍵詞：四元數(shù); 姿態(tài)估計(jì); 濾波; 組合導(dǎo)航

0引言

姿態(tài)估計(jì)技術(shù)在航空航天、導(dǎo)航等領(lǐng)域中擁有廣泛的應(yīng)用[1-4]。姿態(tài)表示方法是研究姿態(tài)估計(jì)問(wèn)題的基礎(chǔ),其中有四參量的四元數(shù)表示方法和三參量的旋轉(zhuǎn)矢量、羅德里格斯參數(shù)族以及Euler角等表示方法[5]。其中羅德里格斯參數(shù)族表示方法包含羅德里格斯參數(shù)(Rodrigues parameter, RP)、修正羅德里格斯參數(shù)(modified Rodrigues parameter,MRP)等姿態(tài)表示方法。不同的表示方法具有其特殊性。但是,其中尤為突出的一個(gè)特點(diǎn)就是任何三參量的姿態(tài)表示方法都無(wú)法全局非奇異,而四參量的四元數(shù)表示方法卻具有全局非奇異的優(yōu)勢(shì)[6-7],基于四元數(shù)的優(yōu)越性,其逐漸成為姿態(tài)表示當(dāng)中應(yīng)用最為廣泛的表示方法[8-9]。

姿態(tài)估計(jì)實(shí)現(xiàn)的根本途徑是現(xiàn)代濾波技術(shù),Kalman濾波是現(xiàn)代濾波技術(shù)當(dāng)中最主要的技術(shù)手段,從線(xiàn)性最優(yōu)的經(jīng)典Kalman濾波到應(yīng)用于非線(xiàn)性系統(tǒng)的擴(kuò)展Kalman濾波(extended Kalman filter,EKF)、無(wú)味Kalman濾波(unscented Kalman filter,UKF),Kalman濾波技術(shù)的發(fā)展極大地促進(jìn)了姿態(tài)估計(jì)研究的進(jìn)步。在非線(xiàn)性Kalman濾波中,基于無(wú)味變換(unscented transformation, UT)的UKF具有比模型線(xiàn)性化的EKF更高的濾波精度[10-11],因此,UKF已經(jīng)成為非線(xiàn)性濾波體系中研究的重點(diǎn)與熱點(diǎn)。

然而,如何將四元數(shù)同UKF濾波結(jié)合起來(lái),卻長(zhǎng)期遇到一個(gè)關(guān)鍵性問(wèn)題,即四元數(shù)在非線(xiàn)性濾波當(dāng)中的約束性問(wèn)題。文獻(xiàn)[12-13]提出的四元數(shù)無(wú)味Kalman濾波算法(quaternion unscented Kalman filter,USQUE)成功解決了該問(wèn)題,并將該算法從航空姿態(tài)估計(jì)當(dāng)中引入到組合導(dǎo)航領(lǐng)域中來(lái),受到廣泛地關(guān)注。文獻(xiàn)[14]也提出了一種“分層濾波”的四元數(shù)濾波算法,即采用不存在奇異性的四元數(shù)進(jìn)行外層的姿態(tài)更新,采用不存在約束性的旋轉(zhuǎn)矢量表示姿態(tài)誤差進(jìn)行內(nèi)層濾波算法遞推。另外,文獻(xiàn)[15-16]在文獻(xiàn)[14]的基礎(chǔ)上,采用慣導(dǎo)基本方程作為濾波模型,研究了在低精度微機(jī)械系統(tǒng)(micro electro mechanical systems,MEMS)與衛(wèi)星導(dǎo)航系統(tǒng)(global navigation satellite system,GNSS)緊組合中的應(yīng)用等問(wèn)題。雖然,USQUE算法能夠得到較好的姿態(tài)估計(jì)效果,但是,為了避免四元數(shù)維數(shù)同四元數(shù)濾波方差的維數(shù)不匹配的問(wèn)題,需要利用四元數(shù)與誤差修正羅德里格斯參數(shù)(error-MRP)反復(fù)的切換,并且這種切換是通過(guò)乘性四元數(shù)公式進(jìn)行的,增大了算法的計(jì)算量,使得算法在實(shí)際問(wèn)題中的應(yīng)用受到了限制。雖然,后續(xù)學(xué)者對(duì)此展開(kāi)了卓有成效的研究[17-18],但是總結(jié)來(lái)說(shuō)其算法都是沒(méi)有考慮四元數(shù)維數(shù)同四元數(shù)濾波方差的維數(shù)匹配性問(wèn)題的,為了達(dá)到降低計(jì)算量的效果,將四元數(shù)姿態(tài)表示部分的濾波方差直接設(shè)置為4×4,然而,由于四元數(shù)是沒(méi)有物理意義的參量,因此,這種濾波方差設(shè)置是帶有極強(qiáng)主觀性的。

對(duì)此,本文提出一種改進(jìn)的四元數(shù)無(wú)味卡爾曼濾波算法(modified USQUE, MUSQUE),利用四元數(shù)與MRP線(xiàn)性變換的特點(diǎn),避免了USQUE算法中乘性四元數(shù)變換增大計(jì)算量的問(wèn)題。濾波算法同樣采用“分層濾波”的思想,兼顧考慮到四元數(shù)維數(shù)與四元數(shù)濾波方差維數(shù)匹配性的問(wèn)題,在外層采用四元數(shù)進(jìn)行姿態(tài)更新,在內(nèi)層采用MRP進(jìn)行姿態(tài)的遞推,同時(shí)推導(dǎo)MRP出現(xiàn)奇異值的避免方法。將改進(jìn)算法應(yīng)用到捷聯(lián)慣性導(dǎo)航系統(tǒng)(strapdown inertial navigation system,SINS)/全球衛(wèi)星定位系統(tǒng)(global positioning system，GPS)直接式組合導(dǎo)航問(wèn)題中,與乘性擴(kuò)展卡爾曼濾波(multiplicative extended Kalman filter， MEKF)和USQUE算法進(jìn)行比較,證明了改進(jìn)算法在保持較高精度情況下,具有更好的時(shí)效性和可信性。

1MRP與USQUE算法

1.1MRP

MRP是一種姿態(tài)表示的方法,同四元數(shù)和旋轉(zhuǎn)矢量有式(1a)、式(1b)的變

(1a)

(1b)

另外,作為一種三參量的姿態(tài)表示方法,MRP是存在姿態(tài)奇異點(diǎn)的,通過(guò)式(1a)中MRP與旋轉(zhuǎn)矢量的關(guān)系可知,MRP將在-2π與2π以及其整數(shù)周期點(diǎn)處出現(xiàn)奇異,因此,在采用羅德里格斯參數(shù)族姿態(tài)表示進(jìn)行濾波時(shí),需要考慮姿態(tài)表示的切換問(wèn)題以避免奇異,對(duì)于MRP來(lái)說(shuō)切換判斷點(diǎn)為‖σ‖>1。

1.2USQUE算法

USQUE算法本質(zhì)上解決了四元數(shù)在非線(xiàn)性濾波當(dāng)中應(yīng)用的兩個(gè)問(wèn)題,其一是解決了四元數(shù)在非線(xiàn)性濾波當(dāng)中約束性的問(wèn)題;其二是采用 “分層濾波”框架,解決了四元數(shù)同濾波方差維數(shù)匹配性問(wèn)題。USQUE算法外層采用四元數(shù)進(jìn)行姿態(tài)更新,內(nèi)層采用error-MRP進(jìn)行姿態(tài)遞推方式進(jìn)行,總結(jié)其算法流程如下：

首先定義一個(gè)離散的非線(xiàn)性模型

(2)

式中,xk∈Rn為k時(shí)刻的狀態(tài)量；yk∈Rm為k時(shí)刻的觀測(cè)量；f(·)和h(·)分別為系統(tǒng)的狀態(tài)方程與觀測(cè)方程；uk-1和vk分別為零均值的系統(tǒng)狀態(tài)白噪聲和零均值的系統(tǒng)觀測(cè)白噪聲,即qk-1～N(0,Qk-1),vk～N(0,Rk)。

(3)

時(shí)間更新產(chǎn)生Sigma點(diǎn)

外層姿態(tài)更新

(4)

內(nèi)層姿態(tài)遞推

(5)

(6)

(7)

量測(cè)更新

如果觀測(cè)方程是非線(xiàn)性的,那么步驟同時(shí)間更新類(lèi)似,如果觀測(cè)方程是線(xiàn)性的,那么可以利用線(xiàn)性Kalman量測(cè)更新過(guò)程進(jìn)行,這里不再贅述。

姿態(tài)更新

(8)

通過(guò)以上USQUE算法流程可知,在每步中USQUE算法都將利用乘性四元數(shù)關(guān)系進(jìn)行多次error-MRP與四元數(shù)的反復(fù)切換,這使得算法的計(jì)算量增大。據(jù)此,提出一種改進(jìn)的四元數(shù)無(wú)味卡爾曼濾波算法,在保證精度的前提下,降低其計(jì)算量。

2改進(jìn)四元數(shù)無(wú)味卡爾曼濾波算法

MUSQUE算法利用四元數(shù)與MRP線(xiàn)性變換特點(diǎn)及“分層濾波”思想進(jìn)行的,其算法流程如下：

時(shí)間更新

產(chǎn)生Sigma點(diǎn):

(9)

(10)

計(jì)算出一步預(yù)測(cè)狀態(tài)與濾波方差如式(6)和式(7)所示。

量測(cè)更新同時(shí)間更新類(lèi)似,這里不再贅述。

對(duì)于‖σ‖>1情況下,采用將四元數(shù)與MRP切換轉(zhuǎn)換為四元數(shù)與error-MRP切換的方法,即:

濾波方差切換用Pσ表示基于MRP的濾波方差,用Pδσ表示基于error-MRP的濾波方差。

采用UT變換的形式來(lái)保證轉(zhuǎn)換精度,δσ與σ變換采用乘性MRP公

(11)

(12)

(13)

(14)式中

(15)

值得指出得是σ(0)是零時(shí)刻的MRP值,這里將零時(shí)刻的MRP值作為參考MRP值來(lái)進(jìn)行乘性MRP公式,另外,對(duì)于參考MRP的選取,也可以采用類(lèi)似文獻(xiàn)[20]中選取參考四元數(shù)的運(yùn)算形式,雖然文獻(xiàn)[20]方法保證了算法的完備性,但是卻增大了計(jì)算量,因此,這里仍采用零時(shí)刻的MRP值作為參考MRP值。

3仿真試驗(yàn)

3.1SINS/GPS直接式組合導(dǎo)航模型

本文采用SINS/GPS直接式位置松組合導(dǎo)航模型進(jìn)行仿真試驗(yàn),其中,直接式是指系統(tǒng)狀態(tài)方程采用捷聯(lián)慣導(dǎo)基本方程,位置松組合是指觀測(cè)方程采用GPS提供的位置信息作輔助校正信息。記SINS的載體坐標(biāo)系 (右-前-上)為b系,地理坐標(biāo)系或?qū)Ш阶鴺?biāo)系 (東-北-天)為n系,地球坐標(biāo)系為e系,慣性坐標(biāo)系為i系?？紤]SINS/GPS直接式組合導(dǎo)航方

(16a)

(16b)

(16c)式中

(16d)

(16e)

另外,觀測(cè)模型考慮SINS/GPS位置松組合模式,則

(17)

式中，vk為零均值的觀測(cè)白噪聲,其方差為Rk。則式(16a)～式(16e)和式(17)構(gòu)成了SINS/GPS直接式位置松組合導(dǎo)航的系統(tǒng)模型。

3.2對(duì)比試驗(yàn)

對(duì)比試驗(yàn)中應(yīng)用SINS/GPS直接式組合導(dǎo)航模型,比較MUSQUE與USQUE和MEKF算法對(duì)于姿態(tài)、速度與位置估計(jì)的精度如圖1～圖3所示,仿真軌跡如圖4所示,記錄運(yùn)動(dòng)軌跡的姿態(tài)trj(qk)、速度trj(vk)和位置trj(pk)信息作為參考,仿真步長(zhǎng)Δt為0.1 s,運(yùn)動(dòng)軌跡的仿真時(shí)間T為1 300 s。

圖1　姿態(tài)估計(jì)結(jié)果

特別地,選取如圖4所示的仿真軌跡,旋轉(zhuǎn)矢量是大于2π的,MRP將會(huì)產(chǎn)生奇異,如果未采取奇異值避免方法,則會(huì)產(chǎn)生濾波結(jié)果的發(fā)散。如圖5所示,采用3σ準(zhǔn)則來(lái)衡量未進(jìn)行奇異值避免的基于MRP濾波估計(jì)結(jié)果的可信性,σ是標(biāo)準(zhǔn)差,3σ準(zhǔn)則是指理論上濾波估計(jì)結(jié)果將以99.73%的概率落在±3σ曲線(xiàn)范圍內(nèi),并且±3σ越接近估計(jì)曲線(xiàn),那么濾波算法越可信。在未進(jìn)行奇異值避免情況下,基于MRP的濾波估計(jì)結(jié)果將很快發(fā)散,并且3σ準(zhǔn)則也證明了濾波結(jié)果將不再可信,也就是已經(jīng)發(fā)散。由于MUSQUE是基于四元數(shù)與MRP切換的,會(huì)遇到奇異性問(wèn)題。因此,考慮仿真全面性,在該情況下來(lái)驗(yàn)證MUSQUE具有較好的估計(jì)效果和更小的計(jì)算量。

圖2　速度估計(jì)結(jié)果

圖3　位置估計(jì)結(jié)果

圖4　仿真軌跡圖

圖5　奇異值對(duì)MRP的估計(jì)影響

設(shè)置初始化參數(shù)如下所示:

圖1～圖3是3種濾波的姿態(tài)、速度與位置估計(jì)誤差的結(jié)果。從圖中可以發(fā)現(xiàn)基于UKF濾波框架的USQUE和MUSQUE算法精度均高于基于EKF濾波框架的MEKF。另外,從圖中可以發(fā)現(xiàn)MUSQUE算法的估計(jì)精度同USQUE算法精度是基本相當(dāng)?shù)?但是算法計(jì)算量卻降低,這里用算法仿真計(jì)算時(shí)間來(lái)衡量計(jì)算量大小,在該次仿真試驗(yàn)中USQUE、MUSQUE與MEKF計(jì)算時(shí)間分別為172.183 s、136.564 s、105.048 s。MEKF比USQUE和MUSQUE計(jì)算時(shí)間都要少,這是由于UKF濾波是基于UT變換的,在直接式組合導(dǎo)航中,UT變換產(chǎn)生的每個(gè)Sigma點(diǎn)本質(zhì)上都將進(jìn)行一次導(dǎo)航解算過(guò)程,狀態(tài)維數(shù)越大,產(chǎn)生的Sigma點(diǎn)越多,導(dǎo)航解算的次數(shù)也就越多,算法占用的時(shí)間也就比模型線(xiàn)性化處理的EKF濾波要大很多。但是,從濾波精度上考慮,USQUE與MUSQUE算法要明顯優(yōu)于MEKF,基于這一點(diǎn),MUSQUE在精度基本保持不變情況下,降低了算法的計(jì)算量,相比USQUE更具有時(shí)效性。另外,考慮大失準(zhǔn)角度情況下,濾波算法可信性與有效性,設(shè)置初始角度為真實(shí)角度的30倍,比較USQUE和MUSQUE算法姿態(tài)估計(jì)精度如圖6所示。

圖6　大初始誤差角情況下算法比較

圖6是在大初始誤差角情況下USQUE與MUSQUE算法估計(jì)效果比較,由圖中可知,USQUE與MUSQUE均落在±3σ區(qū)域內(nèi),但顯然,MUSQUE算法的3σ更接近估計(jì)曲線(xiàn),說(shuō)明MUSQUE比USQUE算法更為可信的。

為了進(jìn)一步說(shuō)明算法的有效性,采用蒙特卡羅仿真方法,初始條件不變,蒙特卡羅仿真50次,并分別求取姿態(tài)、速度、位置的估計(jì)誤差均方根值及仿真50次3種算法計(jì)算時(shí)間的平均值,總結(jié)如表1所示(CPUofIntelcorei3-2328M, 2.20GHz,MatlabR2011a)。

表1　平均估計(jì)精度及運(yùn)行時(shí)間比較

4結(jié)論

USQUE算法為了保持四元數(shù)狀態(tài)維數(shù)同四元數(shù)濾波方差維數(shù)匹配性問(wèn)題,頻繁將四元數(shù)與error-MRP進(jìn)行切換,增大了算法的計(jì)算量。針對(duì)USQUE算法計(jì)算量較大的問(wèn)題,在兼顧估計(jì)精度與匹配性問(wèn)題的基礎(chǔ)上,提出了一種改進(jìn)的MUSQUE算法,通過(guò)四元數(shù)與MRP進(jìn)行線(xiàn)性切換,避免了USQUE算法中通過(guò)乘性四元數(shù)關(guān)系進(jìn)行切換計(jì)算量大的問(wèn)題,保持了算法的估計(jì)精度。同時(shí),推導(dǎo)了在MRP出現(xiàn)奇異時(shí),奇異值避免的解決途徑。仿真結(jié)果表明,MUQUE相比USQUE在估計(jì)精度相當(dāng)?shù)那闆r下,具有更低的計(jì)算量和更高的可信度,并相比MEKF具有更高的估計(jì)精度。

參考文獻(xiàn)：

[1] Crassidis J L, Markley F L. Unscented filtering for spacecraft attitude estimation[J].JournalofGuidance,Control,andDynamics, 2003, 26(4): 536-542.

[2] Li J G, Cui H T, Tian Y. Space attitude estimation algorithm abased on multiplicative quaternion and constrained filter[J].SystemsEngineeringandElectronics,2013,35(5):1032-1036.(李建國(guó),崔祜濤,田陽(yáng).基于四元數(shù)與約束濾波的飛行器姿態(tài)估計(jì)算法[J].系統(tǒng)工程與電子技術(shù),2013,35(5):1032-1036.)

[3] Suh Y S. Attitude estimation by multiple-mode Kalman filters[J].IEEETrans.onIndustrialElectronics, 2006, 53(4): 1386-1389.

[4] Markley F L, Mortari D. Quaternion attitude estimation using vector observations[J].JournaloftheAstronauticalSciences, 2000, 48(2): 359-380.

[5]Markley F L, Crassidis J L.Fundamentalsofspacecraftattitudedeterminationandcontrol[M]. New York:Springer, 2014.

[6] O’Keefe S A, Schaub H. Shadow set considerations for modified rodrigues parameter attitude filtering[J].JournalofGuidance,Control,andDynamics, 2014: 1-5.

[7] Karlgaard C D, Schaub H. Nonsingular attitude filtering using modified rodrigues parameters[J].JournaloftheAstronauticalSciences, 2009, 57(4): 777-791.

[8] Marins J L, Yun X, Bachmann E R, et al. An extended Kalman filter for quaternion-based orientation estimation using MARG sensors[C]∥Proc.oftheIEEE/RSJInternationalConferenceonIntelligentRobotsandSystems, 2001: 2003-2011.

[9] Markley F L. Fast quaternion attitude estimation from two vector measurements[J].JournalofGuidance,Control,andDynamics, 2002, 25(2): 411-414.

[10] Zhou W D,Qiao X W, Ji Y R, et al. SINS/GPS tightly integrated navigation system based on quaternion square root unscented Kalman filter[J].SystemsEngineeringandElectronics,2010,32(12):2643-2647.(周衛(wèi)東,喬相偉,吉宇人,等.基于四元數(shù)平方根UKF算法的SINS/GPS緊組合導(dǎo)航系統(tǒng)研究[J].系統(tǒng)工程與電子技術(shù),2010,32(12):2643-2647.)

[11] Chang L, Hu B, Chang G, et al. Huber-based novel robust unscented Kalman filter[J].IETScience,Measurement&Technology, 2012, 6(6): 502-509.

[12] Crassidis J L. Sigma-point Kalman filtering for integrated GPS and inertial navigation[J].IEEETrans.onAerospaceandElectronicSystems, 2006, 42(2): 750-756.

[13] Crassidis J L, Markley F L, Cheng Y. Survey of nonlinear attitude estimation methods[J].JournalofGuidance,Control,andDynamics, 2007, 30(1): 12-28.

[14] Shin E H, El-Sheimy N. An unscented Kalman filter for in-motion alignment of low-cost IMUs[C]∥Proc.oftheIEEEPositionLocationandNavigationSymposium, 2004: 273-279.

[15] Yang Y, Zhou J, Loffeld O. Quaternion-based Kalman filtering on INS/GPS[C]∥Proc.ofthe15thIEEEInternationalConferenceonInformationFusion, 2012: 511-518.

[16] Zhou J, Edwan E, Knedlik S, et al. Low-cost INS/GPS with nonlinear filtering methods[C]∥Proc.ofthe13thIEEEConferenceonInformationFusion, 2010: 1-8.

[17] Qian H M, Huang W, Sun L. Cubature Kalman filter with quaternion constraint and its application[J].JournalofHarbinInstituteofTechnology,2014,46(7):41-46.(錢(qián)華明,黃蔚,孫龍.四元數(shù)約束的容積卡爾曼濾波及其應(yīng)用[J].哈爾濱工業(yè)大學(xué)學(xué)報(bào),2014,46(7):41-46.)

[18] Zhou W D, Qiao X W, Ren L, et al. Quaternion unsecnted Kalman filter and its application to initial alignment of SINS[J].ChineseJournalofScientificInstrument,2010(2):264-269.(周衛(wèi)東,喬相偉,任蕾,等.QUKF算法及其在SINS初始對(duì)準(zhǔn)中的應(yīng)用[J].儀器儀表學(xué)報(bào),2010(2):264-269.)

[19] Soken H E, Hajiyev C. REKF and RUKF for pico satellite attitude estimation in the presence of measurement faults[J].JournalofSystemsEngineeringandElectronics,2014,25(2):288-297.

[20] Chang L, Hu B, Chang G. Modified unscented quaternion estimator based on quaternion averaging[J].JournalofGuidance,Control,andDynamics, 2013, 37(1): 305-309.

[21] Chang L, Hu B, Li A, et al. Strapdown inertial navigation system alignment based on marginalised unscented Kalman filter[J].IETScience,Measurement&Technology, 2013, 7(2): 128-138.

李開(kāi)龍(1988-),男,博士研究生,主要研究方向慣性導(dǎo)航技術(shù)及應(yīng)用。

E-mail:lee_kailong@163.com,

胡柏青(1964-),男,教授,博士研究生導(dǎo)師,博士,主要研究方向?yàn)閼T性導(dǎo)航技術(shù)及應(yīng)用。

E-mail:hubaiqing2005@163.com

常路賓(1987-),男,講師,博士,主要研究方向?yàn)閼T性導(dǎo)航技術(shù)及應(yīng)用。

E-mail:changlubin@163.com

Modified quaternion unscented Kalman filter

LI Kai-long, HU Bai-qing, CHANG Lu-bin

(DepartmentofNavigationEngineering,NavalUniversityofEngineering,Wuhan430033,China)

Abstract:Aiming at the problem of larger calculation burden of the quaternion unscented Kalman filter (USQUE), a modified USQUE (MUSQUE) is proposed. Based on the transformation between the quaternion and the modified rodriguez parameter (MRP), the novel method can reduce calculation amount by avoiding frequent transformation between the quaternion and the error-MRP for the USQUE. Meanwhile, the singularity avoidance method is also derived in case when the MRP is singularity. By simulating strapdown inertial navigation system/global positioning system (SINS/GPS) direct integrated navigation, the results prove the validation of the novel method comparing with the multiplicative extended Kalman filter and USQUE.

Keywords:quaternion; attitude estimation; filter; integrated navigation

收稿日期：2015-04-04；修回日期：2015-11-17；網(wǎng)絡(luò)優(yōu)先出版日期：2016-02-15。

基金項(xiàng)目：國(guó)家重點(diǎn)基礎(chǔ)研究發(fā)展計(jì)劃(973計(jì)劃)(2012CB719902)；國(guó)家自然科學(xué)基金(61304241,61374206,41374082)資助課題

中圖分類(lèi)號(hào)：U 666.12

文獻(xiàn)標(biāo)志碼：A

DOI:10.3969/j.issn.1001-506X.2016.06.28

作者簡(jiǎn)介：

網(wǎng)絡(luò)優(yōu)先出版地址：http://www.cnki.net/kcms/detail/11.2422.TN.20160215.1436.006.html