狀態(tài)切換UKF的飛行器姿態(tài)確定算法

吳勃， 徐歡， 喬相偉

(1．哈爾濱理工大學(xué)自動(dòng)化學(xué)院，黑龍江哈爾濱 150080;2．北京環(huán)球信息應(yīng)用開(kāi)發(fā)中心，北京 100094;3．西安航天精密機(jī)電研究所系統(tǒng)工程事業(yè)部，陜西西安 710100)

0 引言

飛行器姿態(tài)確定是飛行器控制系統(tǒng)的關(guān)鍵組成部分。常用的姿態(tài)描述參數(shù)有方向余弦矩陣、歐拉角、姿態(tài)四元數(shù)(quaternion)、羅德里格參數(shù)(rodrigues parameters，RPs)、修正羅德里格參數(shù)(modi-fied rodrigues parameters，MRPs)、凱萊 － 哈密爾頓參數(shù)［1］等。其中姿態(tài)四元數(shù)因?yàn)橛?jì)算量小、非奇異性、可全姿態(tài)工作而廣泛應(yīng)用于實(shí)際系統(tǒng)當(dāng)中［2－3］。但由于四元數(shù)的4個(gè)參數(shù)存在冗余，4個(gè)變量并不獨(dú)立，使他必須滿(mǎn)足規(guī)范化限制。如何克服四元數(shù)的規(guī)范化限制，讓他與先進(jìn)的濾波算法相結(jié)合成為目前國(guó)內(nèi)外研究的熱點(diǎn)問(wèn)題［3］。

目前最常用的飛行器姿態(tài)確定算法是狀態(tài)估計(jì)算法。然而該算法將不可避免地要涉及到非線性濾波問(wèn)題。而基于泰勒展開(kāi)的擴(kuò)展卡爾曼濾波［4］(extended Kalman filter，EKF)，其本質(zhì)是 Kalman濾波在非線性系統(tǒng)中的推廣，該算法比較簡(jiǎn)單，只達(dá)到了對(duì)非線性系統(tǒng)的一階估計(jì)精確度。針對(duì)此，之后的學(xué)者又提出了二階EKF算法［5］，濾波精確度得到較大提高，但是因?yàn)樵撍惴ㄉ婕暗紿assian矩陣的計(jì)算，使得計(jì)算負(fù)擔(dān)也大為增加，實(shí)際應(yīng)用反而沒(méi)有EKF廣泛?？紤]到對(duì)非線性函數(shù)后驗(yàn)均值和方差的近似遠(yuǎn)比對(duì)非線性函數(shù)本身的近似簡(jiǎn)單，近年來(lái)，Julier等提出了無(wú)跡卡爾曼濾波算法［6］。該算法首先以系統(tǒng)狀態(tài)的先驗(yàn)均值和方差為基準(zhǔn)，選取一組確定性采樣點(diǎn)，稱(chēng)為Sigma點(diǎn)，使得這組采樣點(diǎn)的統(tǒng)計(jì)特性與狀態(tài)先驗(yàn)統(tǒng)計(jì)特性相一致，然后將利用系統(tǒng)非線性傳遞函數(shù)得到一組新的采樣點(diǎn)，最后采用加權(quán)統(tǒng)計(jì)線性回歸技術(shù)得到這組樣本點(diǎn)的統(tǒng)計(jì)特性，作為非線性函數(shù)的后驗(yàn)統(tǒng)計(jì)分布。然而早期的無(wú)跡卡爾曼濾波(unscented Kalman filter，UKF)算法并沒(méi)有考慮系統(tǒng)噪聲對(duì)濾波精確度和穩(wěn)定性的影響。為此，武元新和M．Briers等從非線性系統(tǒng)噪聲出發(fā)，分析了系統(tǒng)噪聲對(duì)UKF算法的濾波性能的影響，指出忽略噪聲的非線性傳遞特性的標(biāo)準(zhǔn)UKF算法會(huì)一定程度地降低濾波的精確度和穩(wěn)定性，并從理論上給出了擴(kuò)維UKF(augmented UKF，AUKF)算法可以提高濾波精確度的證明［7］。在此基礎(chǔ)上，Merwe在2004年給出了3種形式的擴(kuò)維UKF算法［8］，包括只將過(guò)程噪聲或者量測(cè)噪聲納入擴(kuò)維狀態(tài)的單邊擴(kuò)維UKF算法和噪聲全部納入擴(kuò)維狀態(tài)的全維 UKF算法(all－dimensional UKF，AUKF)。其中全維UKF算法最為典型，應(yīng)用也最廣。該算法通過(guò)擴(kuò)維將過(guò)程噪聲和量測(cè)噪聲全部納入狀態(tài)變量中參與Sigma點(diǎn)選取，提高了UT變換的后驗(yàn)逼近精確度，進(jìn)而提高了濾波的精確度和穩(wěn)定性。但該算法同時(shí)也帶來(lái)了狀態(tài)維數(shù)倍增，計(jì)算負(fù)擔(dān)加重的問(wèn)題。而事實(shí)上在多數(shù)情況下，系統(tǒng)的量測(cè)與過(guò)程是不相關(guān)的，即傳感器的量測(cè)精確度并不受載體機(jī)動(dòng)變化的影響。為此，提出一種過(guò)程和量測(cè)不相關(guān)的狀態(tài)切換UKF濾波(state switching UKF，SSUKF)算法，并將其應(yīng)用到飛行器姿態(tài)確定中去。針對(duì)姿態(tài)確定過(guò)程中四元數(shù)均值計(jì)算問(wèn)題和協(xié)方差奇異性問(wèn)題［9－11］，采取將四元數(shù)參數(shù)和修正羅德里格斯參數(shù)相結(jié)合的方法，即在均值和協(xié)方差計(jì)算時(shí)將四元數(shù)參數(shù)切換為修正羅德里格斯參數(shù)，而在狀態(tài)傳遞時(shí)采取四元數(shù)形式，這樣既避免了修正羅德里格斯參數(shù)積分帶來(lái)的高計(jì)算復(fù)雜度問(wèn)題，又解決了四元數(shù)的規(guī)范化問(wèn)題。針對(duì)大姿態(tài)誤差角的SINS/CCD飛行器姿態(tài)仿真結(jié)果表明，該算法與全維UKF算法相比，估計(jì)精確度相當(dāng)，但估計(jì)時(shí)間縮短了約1/3。

1 飛行器姿態(tài)確定狀態(tài)空間模型

1.1 飛行器狀態(tài)空間模型

本文以飛行器姿態(tài)參數(shù)和陀螺漂移為狀態(tài)變量，建立飛行器姿態(tài)確定系統(tǒng)狀態(tài)方程，其微分形式為

式中:Ωd［ω(t)］為姿態(tài)更新函數(shù);q(t)為姿態(tài)四元數(shù);ω(t)=(t)－β(t)－ηv(t);(t)為角速度量測(cè)值;β(t)為一階馬爾科夫過(guò)程;ηv(t)，ηu(t)均為零均值白噪聲。

以星敏感器四元數(shù)輸出作為量測(cè)輸出，則姿態(tài)確定系統(tǒng)測(cè)量數(shù)學(xué)模型為

式中:q為真實(shí)姿態(tài)四元數(shù);qn為星敏感器測(cè)量噪聲的四元數(shù)表示形式。

1.2 飛行器姿態(tài)動(dòng)力學(xué)方程

以近地衛(wèi)星為例，建立飛行器姿態(tài)動(dòng)力學(xué)方程［12］為

式中:J為飛行器本體的轉(zhuǎn)動(dòng)慣量;hw為飛行器內(nèi)部運(yùn)動(dòng)部件的總動(dòng)量;Tc為控制力矩;Td為干擾力矩。

2 狀態(tài)切換UKF算法

2.1 全維UKF算法

考慮如下非線性離散系統(tǒng)，即

式中:xk∈Rn為k時(shí)刻的系統(tǒng)狀態(tài)向量;zk∈Rm為k時(shí)刻的系統(tǒng)量觀測(cè)向量;ωk∈Rn，vk∈Rm分別為服從N(0，Q)和N(0，R)分布的高斯白噪聲。

選取擴(kuò)維系統(tǒng)狀態(tài)變量xa=［xTwTvT］T。其中，x，w，v分別為系統(tǒng)狀態(tài)變量，過(guò)程噪聲變量和量測(cè)噪聲變量。

1)初始化

式中:x0為系統(tǒng)狀態(tài)初始值;P0，Q0，R0分別為系統(tǒng)狀態(tài)初始誤差方差陣，過(guò)程噪聲初始誤差方差陣和量測(cè)噪聲初始誤差方差陣。

2)時(shí)間更新

時(shí)間更新階段Sigma點(diǎn)及權(quán)值選取為

上述Sigma點(diǎn)對(duì)應(yīng)的均值權(quán)值和方差權(quán)值分別為

式中:L=2n+m;λ=α2(n+κ)－n為尺度因子，α決定了這些Sigma點(diǎn)集到均值點(diǎn)的距離，通常設(shè)置為一個(gè)很小的正數(shù)，κ通常設(shè)置為零;β用于融入隨機(jī)變量的驗(yàn)前信息［8］。

3)量測(cè)更新

量測(cè)更新階段Sigma點(diǎn)選取為

通過(guò)非線性量測(cè)函數(shù) Zk|k－1(i)=］，可得k時(shí)刻系統(tǒng)量測(cè)一步預(yù)測(cè)均值，方差及互協(xié)方差分別為

4)狀態(tài)更新

由上式可得系統(tǒng)狀態(tài)增益矩陣，估計(jì)值及狀態(tài)誤差方差值分別為

由擴(kuò)維UKF算法可知，量測(cè)噪聲參與了時(shí)間更新過(guò)程，而過(guò)程噪聲也參與了量測(cè)更新過(guò)程。而在實(shí)際系統(tǒng)中，大多數(shù)情況下量測(cè)與過(guò)程是不相關(guān)的。此時(shí)，量測(cè)噪聲在時(shí)間更新過(guò)程中只是單純地增加了濾波的計(jì)算量，對(duì)狀態(tài)預(yù)測(cè)更新起不到太大作用，另外由整個(gè)濾波過(guò)程可知，噪聲向量并沒(méi)有得到更新。這是因?yàn)橄到y(tǒng)假設(shè)的噪聲為零均值高斯白噪聲，所以噪聲的最優(yōu)估計(jì)值為零，只需要在每次濾波開(kāi)始前賦值為零即可。

2.2 狀態(tài)切換UKF算法

由擴(kuò)維UKF算法可知，通過(guò)擴(kuò)維狀態(tài)維數(shù)由n維增加為2n+m維，對(duì)應(yīng)的Sigma點(diǎn)也由2n+1個(gè)增加到4n+2m+1個(gè)。這無(wú)疑將極大增加UKF算法的計(jì)算量。對(duì)此，本文提出一種過(guò)程與量測(cè)不相關(guān)的狀態(tài)切換UKF算法。下面給出該算法的具體步驟。

1)初始化

選取擴(kuò)維狀態(tài)變量 xa=［xTwTvT］T。其中，x為待估計(jì)的系統(tǒng)狀態(tài)向量，w和v分別為過(guò)程噪聲向量和量測(cè)噪聲向量。

擴(kuò)維狀態(tài)初始估計(jì)值和誤差方差值如式(5)所示。

2)時(shí)間更新

時(shí)間更新過(guò)程因?yàn)橛^測(cè)噪聲沒(méi)有參與，選取擴(kuò)維狀態(tài)變量為狀態(tài)向量和過(guò)程噪聲。k時(shí)刻濾波狀態(tài)變量及方差選取為

式中:xk，wk分別為k時(shí)刻的狀態(tài)向量和過(guò)程噪聲;Pk，Qk則分別為k時(shí)刻的狀態(tài)向量和過(guò)程噪聲對(duì)應(yīng)的誤差方差矩陣。

k時(shí)刻時(shí)間更新Sigma點(diǎn)選取為

3)量測(cè)更新

量測(cè)更新過(guò)程中，由于過(guò)程噪聲不參與更新，切換狀態(tài)維數(shù)L2=n+m，此時(shí)擴(kuò)維狀態(tài)變量選取為狀態(tài)向量和量測(cè)噪聲，即

量測(cè)更新階段Sigma點(diǎn)選取為

通過(guò)非線性量測(cè)函數(shù) Zk|k－1(i)=h［(i)，］，其中，)分別為量測(cè)更新Sigma點(diǎn)的狀態(tài)向量部分和量測(cè)噪聲部分，可得k時(shí)刻系統(tǒng)量測(cè)一步預(yù)測(cè)均值，方差及互協(xié)方差為

4)狀態(tài)更新

狀態(tài)更新與全維UKF相同，這里不再贅述。

由上面狀態(tài)切換算法可知，在時(shí)間更新和量測(cè)更新濾波過(guò)程中狀態(tài)維數(shù)分別由2n+m維減小為2n維和n+m維，對(duì)應(yīng)的Sigma點(diǎn)分別由4n+2m+1個(gè)點(diǎn)減小為4n+1個(gè)點(diǎn)和2n+2m+1個(gè)點(diǎn)。狀態(tài)維數(shù)的減小不僅影響了Sigma點(diǎn)選點(diǎn)的減少，而且在濾波時(shí)間更新和量測(cè)更新階段也大大減小了計(jì)算量。在不影響濾波精確度的前提下，節(jié)省了濾波的時(shí)間，提高了系統(tǒng)的實(shí)時(shí)性。

3 飛行器姿態(tài)UKF算法

3.1 四元數(shù)規(guī)范化問(wèn)題

相比于其他姿態(tài)描述參數(shù)，四元數(shù)因?yàn)橛?jì)算量小、非奇異性和可全姿態(tài)工作成為姿態(tài)描述的首選。然而，四元數(shù)規(guī)范化要求卻限制了其在非線性濾波中的應(yīng)用。四元數(shù)規(guī)范化問(wèn)題的核心是四元數(shù)加權(quán)均值計(jì)算問(wèn)題，四元數(shù)加權(quán)均值ˉqsimple最直接的方法［13］為

但是這樣就會(huì)帶來(lái)兩個(gè)問(wèn)題，其一，均值四元數(shù)ˉqsimple將不再是一個(gè)規(guī)范的四元數(shù);其二，q和－q表示相同的矢量旋轉(zhuǎn)，也就是說(shuō)改變四元數(shù)的符號(hào)并不應(yīng)該影響四元數(shù)的加權(quán)均值，但是由式(21)所得的均值四元數(shù)顯然不能滿(mǎn)足這一點(diǎn)。

解決四元數(shù)規(guī)范化問(wèn)題的一個(gè)方法就是姿態(tài)參數(shù)切換算法，具體來(lái)講就是在涉及四元數(shù)均值及協(xié)方差計(jì)算時(shí)，將四元數(shù)通過(guò)姿態(tài)參數(shù)切換轉(zhuǎn)化為3參數(shù)的MRPs，利用MRPs進(jìn)行均值及協(xié)方差計(jì)算就避免了四元數(shù)協(xié)方差的奇異性問(wèn)題。用四元數(shù)表示的MRPs參數(shù)為

式中:g為MRPs參數(shù);a，b均為標(biāo)量;ρ為四元數(shù)的向量部分;q4為四元數(shù)標(biāo)量部分。

用MRPs表示的四元數(shù)參數(shù)為

3.2 飛行器姿態(tài)UKF算法

本文在濾波過(guò)程中采取四元數(shù)和修正羅德里格斯參數(shù)實(shí)時(shí)切換的方法，有效解決了四元數(shù)的規(guī)范性問(wèn)題。同時(shí)考慮到姿態(tài)確定中噪聲非線性傳遞的特性，將前面的狀態(tài)切換UKF算法應(yīng)用到姿態(tài)確定中去。下面給出基于狀態(tài)切換UKF的飛行器姿態(tài)確定算法的具體流程。

對(duì)于低年級(jí)孩子來(lái)說(shuō)，作文評(píng)語(yǔ)有時(shí)可能被孩子忽略了，有時(shí)孩子還不是很懂老師的意思。所以，只寫(xiě)評(píng)語(yǔ)是不夠的。每次習(xí)作我都堅(jiān)持及時(shí)面批，讓孩子看著老師改他的作文，具體地進(jìn)行指導(dǎo)。面批，讓孩子直觀了解自己的作文，哪些地方這樣寫(xiě)是好的，哪些地方怎樣改會(huì)更好。要讓孩子體會(huì)到老師的鼓勵(lì)，老師對(duì)他作文的熱情和重視。低年級(jí)學(xué)生的作文篇幅不長(zhǎng)，老師若利用好零散的課余時(shí)間，是可以做到人人面批的。

選取擴(kuò)維狀態(tài)變量為 xa=［xT，wT，vT］T=［pT，其中 x=［pT，βT］T，w=［，p 為 MRPs形式的姿態(tài)變量，β 為陀螺漂移，ηv，ηu為過(guò)程噪聲向量，ηs為量測(cè)噪聲。

1)初始化

2)時(shí)間更新

時(shí)間更新階段，沒(méi)有量測(cè)噪聲參與，此時(shí)擴(kuò)維狀態(tài)變量選為

式中p為用修正羅德里格斯參數(shù)形式描述的飛行器姿態(tài)向量。

時(shí)間更新階段Sigma點(diǎn)及權(quán)值選擇，在已知k－1時(shí)刻狀態(tài)分布的基礎(chǔ)上，選取k時(shí)刻狀態(tài)Sigma點(diǎn)為

由于狀態(tài)變量在時(shí)間更新時(shí)涉及到四元數(shù)更新，首先將上述Sigma點(diǎn)分為MRPs和非MRPs兩部分，即

將MRPs參數(shù)點(diǎn)轉(zhuǎn)換為四元數(shù)Sigma點(diǎn)，即

為了計(jì)算姿態(tài)預(yù)測(cè)均值需要將四元數(shù)一步預(yù)測(cè)值轉(zhuǎn)換為MRPs形式的一步預(yù)測(cè)值，即

陀螺漂移的時(shí)間更新為

從而得到狀態(tài)預(yù)測(cè)均值為

對(duì)應(yīng)的狀態(tài)預(yù)測(cè)誤差方差值為

(3)量測(cè)更新

測(cè)量更新部分過(guò)程噪聲未參與其中，切換擴(kuò)維系統(tǒng)狀態(tài)為

測(cè)量更新過(guò)程中Sigma點(diǎn)選取為

由于量測(cè)更新中只有姿態(tài)觀測(cè)矢量，此時(shí)需要用四元數(shù)形式進(jìn)行更新，首先將上述Sigma點(diǎn)分為MRPs部分和非MRPs部分，即

將MRPs點(diǎn)轉(zhuǎn)換為四元數(shù)Sigma點(diǎn)，即

四元數(shù)量測(cè)一步預(yù)測(cè)值為

類(lèi)似于時(shí)間更新，此時(shí)將四元數(shù)形式的姿態(tài)量測(cè)預(yù)測(cè)值轉(zhuǎn)換為MRPs形式的姿態(tài)預(yù)測(cè)值，即

由于量測(cè)量中沒(méi)有陀螺漂移，所以狀態(tài)量測(cè)預(yù)測(cè)均值為

從而可得狀態(tài)量測(cè)誤差方差陣及互協(xié)方差值分別為

4)狀態(tài)更新

狀態(tài)增益矩陣更新為

從而狀態(tài)誤差值及狀態(tài)估計(jì)值分別更新為

狀態(tài)估計(jì)誤差方差陣為

4 仿真實(shí)驗(yàn)研究

本文以近地衛(wèi)星為例，建立SINS/CCD姿態(tài)估計(jì)系統(tǒng)仿真平臺(tái)，為各種姿態(tài)確定算法提供統(tǒng)一的仿真實(shí)驗(yàn)平臺(tái)，該系統(tǒng)軟件開(kāi)發(fā)語(yǔ)言主要是Matlab和C++。

1)飛行器參數(shù)設(shè)置

飛行器軌道為典型的近地圓軌道，軌道高度為800 km，傾角為60°，飛行器飛行軌道角速度 Ωo=0.001 rad/s，飛行器初始姿態(tài)角速度分別為0.01°/s，0.01°/s，0.05°/s。衛(wèi)星三軸初始姿態(tài)角分別為 1°，2°，10°。飛行器的轉(zhuǎn)動(dòng)慣量矩陣為

2)傳感器參數(shù)設(shè)置

光纖陀螺測(cè)量白噪聲為0.5°/h，驅(qū)動(dòng)白噪聲為0.02°/h，陀螺輸出采樣頻率為100 Hz，星敏感器采用兩個(gè)光軸垂直安裝，其輸出頻率為4 Hz，飛行器初始姿態(tài)誤差設(shè)定:橫滾角，俯仰角，偏航角分別為1°，2°，10°。初始陀螺漂移在三軸上分別為 1°/h，1°/h，1°/h。星敏感器運(yùn)動(dòng)速率為 0.05°/s，測(cè)量白噪聲標(biāo)準(zhǔn)差為20″，輸出頻率為4 Hz。卡爾曼濾波器中的姿態(tài)和陀螺漂移估計(jì)值均設(shè)定為零。

依據(jù)上述初始條件，分別采用全維UKF和狀態(tài)切換UKF算法進(jìn)行仿真，仿真結(jié)果如圖1～圖3和表1所示。

由圖1和2可知，當(dāng)初始姿態(tài)誤差角較小取為1°和2°時(shí)，兩種算法精確度相當(dāng)且都以較快的速度收斂;而當(dāng)初始姿態(tài)誤差角為大角度時(shí)，由圖3可知，兩種算法在精確度相當(dāng)?shù)那闆r下，SUKF能以更快的速度收斂，收斂時(shí)間與AUKF相比。這是由于SUKF算法通過(guò)實(shí)時(shí)切換系統(tǒng)狀態(tài)減小了狀態(tài)的維數(shù)，降低了運(yùn)算的復(fù)雜度。另外由表1平均一次迭代濾波所用時(shí)間也可以看出，SUKF算法與UKF算法相比濾波時(shí)間減少了大約1/3，這也再次驗(yàn)證了該算法在提高系統(tǒng)實(shí)時(shí)性方面的有效性。

圖1 橫滾角姿態(tài)誤差對(duì)比Fig．1 Comparison of roll angle errors

圖2 俯仰角姿態(tài)誤差對(duì)比Fig．2 Comparison of pitch angle errors

圖3 偏航角姿態(tài)誤差對(duì)比Fig．3 Comparison of yaw angle errors

表1 平均一次迭代濾波時(shí)間對(duì)比Table 1 Time consumption comparison of average one time iteration filter

5 結(jié)語(yǔ)

為解決全維姿態(tài)UKF算法帶來(lái)的維數(shù)倍增，計(jì)算負(fù)擔(dān)加重的問(wèn)題，本文提出了一種過(guò)程和量測(cè)不相關(guān)的狀態(tài)切換UKF算法，通過(guò)在預(yù)測(cè)和量測(cè)階段選取不同的狀態(tài)變量，減小了實(shí)時(shí)濾波的維數(shù)，有效降低了濾波計(jì)算量。針對(duì)四元數(shù)非線性濾波算法中四元數(shù)規(guī)范化的限制，給出了參數(shù)切換UKF算法，通過(guò)濾波過(guò)程中四元數(shù)與修正羅德里格斯參數(shù)的實(shí)時(shí)切換，解決了四元數(shù)均值計(jì)算和協(xié)方差奇異性問(wèn)題。以實(shí)驗(yàn)室SINS/CCD組合姿態(tài)估計(jì)系統(tǒng)為平臺(tái)，進(jìn)行了仿真實(shí)驗(yàn)，實(shí)驗(yàn)結(jié)果驗(yàn)證了所提算法的有效性。

［1］MARKLEY F L．Attitude error representations for Kalman filtering［J］．Journal of Guidance，Control，and Dynamics，2003，63(2):311－317．

［2］CHUNG D Y，LEE J G．Strap-down INS error model for multi-position alignment［J］．IEEE Transactions on Aerospace and Electronic Systems，1996，32(4):1362 －1366．

［3］SHUSTER M D．Constraint in attitude estimation part I:constrained estimation［J］．Journal of the Astronautical Sciences，2003，51(1):51－74．

［4］MARKLEY F Landis，CRASSIDIS J L，CHENG Yang．Nonlinear attitude filtering methods［C］//Collection of Technical Papers-AIAA Guidance，Navigation，and Control Conference，August 15－18，2005，San Francisco，USA．2005，1:753－784．

［5］PSIAKI M L．The super-iterated extended Kalman filter［C］//Collection of Technical Papers-AIAA Guidance，Navigation，and Control Conference，August 16 － 19，2004，Providence，USA．2004，5:3384－3398．

［6］WU Yuanxin，HU Dewen，WU Meiping，et al．Unscented Kalman filtering for additive noise case:augmented vs．non-augmented［C］//Proceedings of the American Control Conference，June 8－10，2005，Portland，USA．2005，6:4051－4055．

［7］JULIER S J，UHLMANN J K．Unscented filtering and nonlinear estimation［J］．Proceedings of the IEEE，2004，92(3):401－422．

［8］MERWE R．Sigma-point Kalman Filters for Probabilistic Inference in Dynamic State-space Models［D］．OGI School of Science and Engineering at Oregon Health and Science University，2004．

［9］MARKLEY F L．Attitude determination using vector observations and the singular value decomposition［J］．Journal of the Astronautically Sciences，1988，36(3):245 －258．

［10］CHOUKROUN D，BAR-ITZHACK I Y，OSHMAN Y．Novel quaternion Kalman filter［J］//IEEE Transactions on Aerospace and Electronic Systems，2006，42(1):174 －190．

［11］CRASSIDIS J L，MARKLEY F L．Unscented filtering for spacecraft attitude estimation［J］．Journal of Guidance，Control and Dynamics，2003，26(4):536 －542．

［12］姜雪原．衛(wèi)星姿態(tài)確定及敏感器誤差修正的濾波算法研究［D］．哈爾濱:哈爾濱工業(yè)大學(xué)控制科學(xué)與工程系，2006．

［13］MARKLEY F L，CHENG Yang，CRASSIDIS J L，et al．Averaging quaternions ［J］．Journal of Guidance，Control，and Dynamics，2007，30(4):1193 －1197．