本發(fā)明涉及基于容積四元數(shù)估計以及星敏感器與陀螺組合的航天器姿態(tài)估計方法。

背景技術(shù)：

隨著航天器和空間系統(tǒng)復(fù)雜性的不斷加大，需要對航天器的姿態(tài)進(jìn)行更加準(zhǔn)確和魯棒的估計。對每個航天器的功能需求正在增加，而航天器本身的尺寸卻越來越小，且硬件之間是相互獨(dú)立的，這就導(dǎo)致系統(tǒng)的計算能力受到很大限制。這樣的系統(tǒng)常常被稱是操作響應(yīng)空間(ORS)。ORS系統(tǒng)的主要目標(biāo)是為了航天器使用模塊化的類型來建設(shè)加速任務(wù)概念推出的時間。這種模塊化的系統(tǒng)，定制的接口和軟件必須管理傳送到中央處理器的所有數(shù)據(jù)。然而，潛在的危險情況是數(shù)據(jù)傳播的時引入未知擾動，或部分?jǐn)?shù)據(jù)在傳輸時丟失。

實際中，航天器系統(tǒng)姿態(tài)模型是一個非線性模型。在近些年中，利用一系列含有噪聲的觀測值對非線性系統(tǒng)的最優(yōu)估計問題，吸引了眾多學(xué)者們的廣泛關(guān)注和研究。不僅在航天領(lǐng)域，在其他領(lǐng)域內(nèi)對非線性系統(tǒng)的狀態(tài)最優(yōu)估計問題也取得了許多重大的成果?，F(xiàn)有的大部分濾波理論是基于貝葉斯方法的，貝葉斯方法為狀態(tài)估計問題提供了一個通用框架。貝葉斯方法是利用對在所有觀測量作為條件下的系統(tǒng)狀態(tài)的條件概率密度函數(shù)進(jìn)行遞歸計算。在目前的應(yīng)用過程中，通常將系統(tǒng)假設(shè)為一種特殊的情形式，即所有的概率密度函數(shù)為高斯概率密度函數(shù),這種系統(tǒng)稱為高斯非線性系統(tǒng)。針對高斯非線性系統(tǒng)，學(xué)者們提出了多種濾波方法，應(yīng)用最為廣泛的是擴(kuò)展卡爾曼濾波算法(EKF)，EKF具有運(yùn)算速度快，穩(wěn)定性可證等優(yōu)勢，但是該算法存在著一定的缺陷。由于是對非線性系統(tǒng)通過泰勒展開進(jìn)行線性逼近，所以只適用于弱非線性系統(tǒng)。另外，需要求解泰勒展開中的雅克比矩陣，但是有些系統(tǒng)的雅克比矩陣很難求解，有些甚至不存在雅克比矩陣，因此EKF應(yīng)用存在一定的局限性。因此，高斯濾波框架作為一種估計高斯非線性系統(tǒng)的通用方法框架得到了廣泛應(yīng)用。高斯濾波框架通過高斯加權(quán)積分的方式代替對貝葉斯框架下概率密度函數(shù)求解。許多濾波算法都是建立在高斯濾波框架的基礎(chǔ)上，利用不同的對高斯加權(quán)積分的近似求解方式實現(xiàn)的，例如高斯厄爾米特濾波器(GHQF)、無跡卡爾曼濾波算法(UKF)、差分濾波器(DDF)、容積卡爾曼濾波(CKF)。

由于航天器上的傳感元件在數(shù)據(jù)傳輸過程中，存在著傳輸時滯、丟包和多種未知外來擾動的問題，其中存在乘性噪聲就是一種常見的問題。例如系統(tǒng)參數(shù)的不確定性可由乘性噪聲描述，量化作用和隨機(jī)產(chǎn)生的傳感器飽和問題可以在建模過程中由乘性噪聲描述。在文獻(xiàn)(F.Yang,Z.Wang,and Y.Hung.Robust Kalman filtering for discrete time-varying uncertain systems with multiplicative noises,IEEE Trans.on Automatic Control,47(7):1179-1183,2002)(J.Jimenez,T.Ozaki,Local linearization filters for non-linear continuous-discrete state space models with multiplicative noise,International Journal of Control,76(12):1159-1170,2003)(F.Carravetta,A.Germani,and N.Raimondi,Polynomial filtering of discrete-time stochastic linear systems with multiplicative state noise,IEEE Trans.on Automatic Control,42(8):1106-1126,1997)中，最小方差濾波器和多項式濾波器可以用來解決具有乘性噪聲的線性系統(tǒng)。對于具有乘性噪聲的非線性系統(tǒng)，文獻(xiàn)(D.Spinello,D.Stilwell,Nonlinear Estimation With State-Dependent Gaussian Observation Noise,IEEE Trans.on Automatic Control,55(6):1358-1366,2010)中設(shè)計了使濾波誤差的協(xié)方差最小的遞歸EKF算法。文獻(xiàn)(X.Hu,Y.Hu,and B.Xu,Generalised Kalman filter tracking with multiplicative measurement noise in a wireless sensor network,IET Signal Processing,8(5):467-474,2014)提出了推廣EKF(GEKF)和推廣UKF(GUKF)算法，該算法不需要迭代非線性優(yōu)化過程，利用條件高斯分布的性質(zhì)去提高傳統(tǒng)EKF和UKF的性能。但是所有算法都基于系統(tǒng)的乘性噪聲和相應(yīng)的加性噪聲不具有相關(guān)性的前提下。

在實際的應(yīng)用中，噪聲相關(guān)也是一種常見的問題。例如，動態(tài)觀測過程在相同的噪聲環(huán)境中，由于受到的擾動影響相同，所以產(chǎn)生的誤差也具有相關(guān)性。在近期的相關(guān)研究中，針對噪聲相關(guān)的問題學(xué)者們提出了一些理論方法，其中通過解耦重新構(gòu)建系統(tǒng)方程的方法，在解決該問題上應(yīng)用的最多，文獻(xiàn)(G.Chang,Marginal unscented kalman filter for cross-correlated process and observation noise at the same epoch,IET Radar Sonar Navigation,8(1):54-64,2014)(A.Hermosocarazo,J.Linarespérez,Unscented Filtering from Delayed Observations with Correlated Noises,Mathematical Problems in Engineering,2009(1024-123X):266-287,2009)就是利用解耦的方法。在文獻(xiàn)(X.Wang,Y.Liang,Q.Pan,and F.Yang,A Gaussian approximation recursive filter for nonlinear systems with correlated noises,Automatica,48(48):2290–2297,2012)中，利用兩步預(yù)測的方法去更新后驗概率密度函數(shù)，成功避開了量測噪聲和系統(tǒng)噪聲耦合的問題，提出了一種新型的高斯濾波框架。但在上述文獻(xiàn)中，都是考慮系統(tǒng)加性噪聲和量測加性噪聲的耦合情況，沒有考慮乘性噪聲和加性噪聲之間的耦合情況。

技術(shù)實現(xiàn)要素：

本發(fā)明的目的是為了解決現(xiàn)有技術(shù)存在乘性噪聲及噪聲相關(guān)問題，導(dǎo)致航天器姿態(tài)估計精度低的缺點(diǎn)，而提的基于容積四元數(shù)估計的航天器姿態(tài)估計方法。

基于容積四元數(shù)估計的航天器姿態(tài)估計方法包括以下步驟：

步驟一：建立航天器姿態(tài)運(yùn)動學(xué)模型和觀測模型；

步驟二：采用高斯濾波算法去除步驟一建立的航天器姿態(tài)運(yùn)動學(xué)模型和觀測模型中的噪聲；

步驟三：采用容積四元數(shù)姿態(tài)估計器對航天器姿態(tài)進(jìn)行估計。

本發(fā)明的有益效果為：

針對現(xiàn)有技術(shù)各種情況的考慮，本發(fā)明針對具有乘性噪聲的非線性系統(tǒng)且系統(tǒng)中乘性噪聲與加性噪聲相關(guān)，提出一種改進(jìn)的高斯濾波框架，作為對該類系統(tǒng)進(jìn)行最優(yōu)估計的統(tǒng)一框架。該框架將對噪聲的相關(guān)性及條件概率密度函數(shù)進(jìn)行充分利用，從而對系統(tǒng)狀態(tài)的估計精度更高。

本發(fā)明針對航天器的姿態(tài)估計時，可能出現(xiàn)系統(tǒng)乘性噪聲，及乘性噪聲和觀測噪聲相關(guān)的問題，設(shè)計了一種基于高斯濾波算法的改進(jìn)高斯濾波框架。同時采用CQE作為改進(jìn)高斯濾波算法的特殊實現(xiàn)形式，提出了CQE-MCNS算法。該算法主要針對航天器的姿態(tài)估計問題，繼承了CQE算法的優(yōu)勢避免四元數(shù)規(guī)范約束和奇異的發(fā)生，同時又能處理乘性噪聲及噪聲相關(guān)的問題。通過與CQE算法進(jìn)行仿真實驗對比，結(jié)果表明，本發(fā)明所提出的CQE-MCNS算法相比CQE算法，角度估計誤差減小0.0002°左右，陀螺漂移估計誤差減小0.002°/h左右，因此更適用于具有乘性噪聲及噪聲相關(guān)的航天器姿態(tài)估計問題。此外，本發(fā)明所提出的改進(jìn)高斯濾波框架可以作為解決具有乘性噪聲及噪聲相關(guān)的系統(tǒng)狀態(tài)估計問題的通用框架應(yīng)用于其他領(lǐng)域。

附圖說明

圖1為CQE角度估計誤差2-范數(shù)圖；

圖2為CQE滾動軸角度估計誤差圖；

圖3為CQE俯仰軸角度估計誤差圖；

圖4為CQE偏移軸角度估計誤差圖；

圖5為CQE陀螺漂移量估計誤差2-范數(shù)圖；

圖6為CQE x軸陀螺漂移量估計誤差圖；

圖7為CQE y軸陀螺漂移量估計誤差圖；

圖8為CQE z軸陀螺漂移量估計誤差圖；

圖9為CQE-MCNS角度估計誤差2-范數(shù)圖；

圖10為CQE-MCNS滾動軸角度估計誤差圖；

圖11為CQE-MCNS俯仰軸角度估計誤差圖；

圖12為CQE-MCNS偏移軸角度估計誤差圖；

圖13為CQE-MCNS陀螺漂移量估計誤差2-范數(shù)圖；

圖14為CQE-MCNS x軸陀螺漂移量估計誤差圖；

圖15為CQE-MCNS y軸陀螺漂移量估計誤差圖；

圖16為CQE-MCNS z軸陀螺漂移量估計誤差圖。

具體實施方式

具體實施方式一：基于容積四元數(shù)估計的航天器姿態(tài)估計方法的具體過程為：

步驟一：建立航天器姿態(tài)運(yùn)動學(xué)模型和觀測模型；

步驟二：采用高斯濾波算法去除步驟一建立的航天器姿態(tài)運(yùn)動學(xué)模型和觀測模型中的噪聲；

步驟三：采用容積四元數(shù)姿態(tài)估計器對航天器姿態(tài)進(jìn)行估計。

具體實施方式二：本實施方式與具體實施方式一不同的是：所述步驟一中建立航天器姿態(tài)運(yùn)動學(xué)模型和觀測模型的具體過程為：

考慮帶有乘性噪聲的非線性離散系統(tǒng)：

x_k+1＝(I_n×n+ζ_kΦ_k)f(x_k)+w_k (1)

y_k＝h(x_k)+v_k (2)

其中，x_k∈Rⁿ是系統(tǒng)k時刻的狀態(tài)量，y_k∈R^m是系統(tǒng)k時刻的觀測值；f(x_k)和h(x_k)是非線性方程；Φ_k是已知的常系數(shù)矩陣，ζ_k∈R是乘性噪聲，w_k∈Rⁿ和v_k∈R^m是系統(tǒng)加性噪聲和量測加性噪聲；R為實數(shù)，Rⁿ為n維實數(shù)集，R^m為m維實數(shù)集；

假設(shè)1：w_k和v_k都是白噪聲，且滿足

其中δ_t,k是克羅內(nèi)克函數(shù)，即當(dāng)t＝k時，值為1，其他情況值為0。

假設(shè)2：ζ_k和v_k是同步相關(guān)的，且滿足

其中S_k≠0是乘性噪聲和加性噪聲的互協(xié)方差；Q_ζ,k是ζ_k的方差，R_k是v_k的方差；另外，由于是同步相關(guān)，所以只有在t＝k時ζ_k和v_k才存在相關(guān)性；

假設(shè)3：初始值x₀與噪聲ζ_k、w_k和v_k都是不相關(guān)的，且E[x₀]＝0,

在上述的假設(shè)條件下傳統(tǒng)的高斯濾波框架是無法實用的，例如EKF、UKF和CKF。所以本文的目的是設(shè)計一種改進(jìn)的高斯濾波框架，可以有效地對上述系統(tǒng)進(jìn)行狀態(tài)最優(yōu)估計。注意1:從式(4)中可以看出，由于相關(guān)性的存在，乘性噪聲和量測噪聲的統(tǒng)計特性已經(jīng)發(fā)生變化。

采用四元數(shù)描述的航天姿態(tài)運(yùn)動學(xué)模型，f(x_k)具體形式如下：

其中，q為姿態(tài)四元數(shù)，是q的導(dǎo)數(shù)；ω＝[ω₁ ω₂ ω₃]^T為角速度在體坐標(biāo)系下的表示，[ω×]表示由ω生成的反對稱矩陣表示為：

觀測模型(包括陀螺輸出模型和星敏感器輸出模型)的建立：

(一)陀螺輸出模型

假設(shè)陀螺固連在航天器上，并且陀螺的安裝方向與航天器本體坐標(biāo)系重合，可直接測量航天器的角速度；則陀螺模型表示為：

其中，表示實際的陀螺輸出值，β表示陀螺漂移值，ω表示理想情況下的陀螺輸出值，η_v和η_u分別表示不相關(guān)的零均值高斯白噪聲，且其協(xié)方差分別表示為和將陀螺輸出和陀螺漂移方程離散化后的形式如下：

其中，△t表示步長，N_v和N_u分別表示離散后不相關(guān)的零均值高斯白噪聲；

(二)星敏感器輸出模型

星敏感器是通過觀測天體的方向?qū)φ招菤v表來確定航天器的姿態(tài)，假設(shè)星敏感器的安裝方向與航天器本體坐標(biāo)系重合，則星光矢量在航天器本體坐標(biāo)系下的觀測方程為：

其中，r表示星光矢量在慣性系下的單位矢量方向，可查詢星歷表獲得，表示從慣性系到航天器本體坐標(biāo)系的變換矩陣；v表示敏感器的觀測誤差，這里認(rèn)為是高斯白噪聲。假設(shè)有m個星敏感器同時進(jìn)行觀測，則在第k時刻，用四元數(shù)描述的矢量觀測模型為：

其中，b_m和r_m表示第m個參考矢量分別在航天器本體坐標(biāo)系和慣性坐標(biāo)系下的分量；A(q)表示姿態(tài)轉(zhuǎn)移矩陣，其四元數(shù)形式的描述為：

其中，姿態(tài)四元數(shù)q＝[q₁,q₂,q₃,q₄]^T，q可分解為標(biāo)量q₄和矢量ρ，ρ＝[q₁,q₂,q₃]^T；

展開形式是：

q＝[q₁,q₂,q₃,q₄]^T，ρ＝[q₁,q₂,q₃]^T。

注：

慣性坐標(biāo)系

原點(diǎn)位于地心O_I，O_IX_I軸指向平春分點(diǎn)方向(天球黃道和天球赤道在天球上交叉點(diǎn)為春分點(diǎn)和秋分點(diǎn))，O_IZ_I軸平行于平均地球自轉(zhuǎn)軸方向，O_IY_I軸與另兩軸構(gòu)成右手系。

航天器本體坐標(biāo)系

航天器本體坐標(biāo)系與航天器固連，其原點(diǎn)位于航天器質(zhì)心O_b，三軸與航天器慣量主軸一致，固聯(lián)于星體上。O_bX_b軸指向航天器飛行方向，稱為滾動軸，O_bZ_b軸指向地心方向，稱為偏航軸，O_bY_b軸與另兩軸構(gòu)成右手直角坐標(biāo)系，稱為俯仰軸。

其它步驟及參數(shù)與具體實施方式一相同。

具體實施方式三：本實施方式與具體實施方式一或二不同的是：所述步驟二中采用高斯濾波算法去除步驟一建立的航天器姿態(tài)運(yùn)動學(xué)模型和觀測模型中的噪聲的具體過程為：

用一種改進(jìn)的高斯濾波框架處理噪聲相關(guān)的情況，本文中所設(shè)計的濾波算法采用一步更新后驗概率密度。改進(jìn)的高斯濾波框架與經(jīng)典高斯濾波算法類似，也分為一步預(yù)測和量測更新兩個步驟，分別通過定理1和定理2描述如下。

引理1：假設(shè)向量X和Y的聯(lián)合高斯分布如下

則X在Y＝y(tǒng)情況下的條件概率滿足如下高斯分布

步驟二一：一步預(yù)測；

考慮非線性系統(tǒng)形式為式(1)和(2)，假設(shè)后驗概率密度函數(shù)P_k-1|k-1和狀態(tài)估計值是已知的，則x_k的一步預(yù)測均值和方差形式如下：

其中

其中是x_k的一步預(yù)測值，I_n×n為n×n的單位矩陣，P_k|k-1為x_k的一步預(yù)測方差，為變量a的高斯概率密度，b為a的期望，c為a的方差；為y_k-1的方差；M_k-1表示ζ_k-1的條件均值，U_k-1表示ζ_k-1的條件方差，D_k-1表示測量值的集合；

證明：首先將表示k-1時刻的式(2)代入?yún)f(xié)方差的定義中可得式(11)，然后將式(1)代入一步關(guān)于狀態(tài)量x_k的一步預(yù)測均值的定義中，可得：

由于乘性噪聲ζ_k-1和D_k-1中的y_k-1觀測值中的v_k-1相關(guān)，因此E[ζ_k-1|D_k-1]的值不為0。ζ_k-1和y_k-1在以k-1時刻的狀態(tài)估計值作為條件的聯(lián)合分布如下：

根據(jù)引理1和v_k-1與D_k-2相互獨(dú)立可知式(9)成立，同理可得式(10)也成立。將式(9)代入式(13)中可以得到式(7)。最后將式(1)代入P_k|k-1定義中，可得到如下

將式(15)的等式右邊展開可得式(16)：

由于w_k-1和ζ_k-1不相關(guān)，且與狀態(tài)x_k-1中的w_k-2也不相關(guān)，因此，可得：

最后將式(9)、(10)、(13)和(17)代入式(16)，可得到式(8)。

步驟二二：量測更新；

考慮非線性系統(tǒng)形式為式(1)和(2)，假設(shè)狀態(tài)量x_k的預(yù)測均值和方差已經(jīng)得到，則它的后驗均值和方差P_k|k的形式如下：

其中為y_k的一步預(yù)測值，K_k為濾波增益，為x_k和y_k的一步預(yù)測協(xié)方差，為y_k的一步預(yù)測方差。

證明：首先將式(2)代入y_k的預(yù)測均值定義中可得

由于E[v_k|D_k-1]＝0，可直接得到式(21)。然后將式(2)代入?yún)f(xié)方差的定義中可得：

將式(25)的右側(cè)展開可得

由于式(22)可以直接獲得。最后將式(2)代入?yún)f(xié)方差的定義中可得：

將式(27)的等式右邊展開可得：

由于可知式(23)成立。

其它步驟及參數(shù)與具體實施方式一或二相同。

具體實施方式四：本實施方式與具體實施方式一至三之一不同的是：所述步驟三中采用容積四元數(shù)姿態(tài)估計器對航天器姿態(tài)進(jìn)行估計的具體過程為：

本發(fā)明利用容積卡爾曼濾波器(CKF)作為GF框架的實現(xiàn)形式，與四元數(shù)相結(jié)合估計衛(wèi)星姿態(tài)，提出新的算法為乘性相關(guān)噪聲系統(tǒng)的容積四元數(shù)姿態(tài)估計器(cubature quaternion estimator for multiplicative correlated noises system,CQE-MCNS)。為了避免采用四元數(shù)描述姿態(tài)時存在的冗余而導(dǎo)致濾波過程中協(xié)方差陣出現(xiàn)奇異的狀況，將狀態(tài)向量選為x＝[δp^T β^T]^T，其中δp為與誤差四元數(shù)對應(yīng)的修正羅德里格斯參數(shù)，形式如下：

其中0≤a≤1，b為尺度參數(shù)；當(dāng)a＝0,b＝1式(38)退化為羅德里格參數(shù)，a＝1,b＝1時式(38)退化為修正的羅德里格參數(shù)，廣義羅德里格參數(shù)的奇異點(diǎn)在180^°到360^°間。

(一)時間預(yù)測

已知濾波器的初值q₀，β₀，P₀，由k-1時刻(前一時刻)的狀態(tài)估計和協(xié)方差陣估計P_k-1獲得k時刻(下一時刻)的容積點(diǎn)為：

其中，δp_i,k-1和β_i,k-1分別表示姿態(tài)角誤差和陀螺漂移，ξ_i表示容積點(diǎn)集合{ξ_i}的第i列向量。容積點(diǎn)集合{ξ_i}可定義為即

e_i表示單位向量，且該單位向量中第i個元素為1。

在濾波算法更新過程中，為了避免誤差廣義羅德里格參數(shù)描述姿態(tài)發(fā)生的奇異現(xiàn)象，將誤差廣義羅德里格參數(shù)轉(zhuǎn)換成容積誤差四元數(shù)為：

其中，δρ_i,k-1表示第k-1時刻誤差四元數(shù)δq_i,k-1的矢量部分，表示第k-1時刻誤差四元數(shù)δq_i,k-1的標(biāo)量部分。

由容積誤差四元數(shù)獲得一步預(yù)測估計的容積四元數(shù)點(diǎn)集為：

容積四元數(shù)由姿態(tài)運(yùn)動學(xué)方程(29)獲得，采用常用的解析形式為：

其中：

陀螺的角速度估計為：

其中，i表示取值為1,2,…,n的數(shù)，表示第k-1時刻陀螺輸出值，β_i,k-1表示第k-1時刻陀螺漂移值。

獲得容積四元數(shù)一步預(yù)測值后，再將其轉(zhuǎn)換為容積誤差羅德里格參數(shù)；先計算一步預(yù)測容積誤差四元數(shù)得：

再將容積誤差四元數(shù)轉(zhuǎn)換為容積誤差羅德里格參數(shù)為：

在由步驟二所設(shè)計的高斯濾波算法，得到一步預(yù)測均值

容積點(diǎn)誤差一步預(yù)測均值為和方差陣P_k|k-1為：

其中，是容積點(diǎn)傳播值。

(二)量測更新

計算容積點(diǎn)

星敏感器生成觀測容積估計點(diǎn)Z_i,k為：

通過以上容積點(diǎn)得到量測容積點(diǎn)均值

求取容積濾波增益K_k：

其中，協(xié)方差陣和互協(xié)方差陣P_xz,k分別表示如下：

再由式(49)得到狀態(tài)量的量測更新為：

其中，

計算更新容積誤差四元數(shù)和陀螺漂移；容積誤差四元數(shù)δq_k更新為：

得到容積點(diǎn)四元數(shù)

協(xié)方差陣P_k更新為：

其它步驟及參數(shù)與具體實施方式一至三之一相同。

實施例一：

在本發(fā)明中，通過對近地軌道航天器的姿態(tài)估計來驗證所設(shè)計的CQE-MCNS算法的有效性。假設(shè)航天器的本體坐標(biāo)系和軌道坐標(biāo)系重合，這樣衛(wèi)星體系相對于慣性系角速度與軌道坐標(biāo)系相對于慣性系角速度相等。同時，假設(shè)航天器上裝配有星敏感器，用于測量角度矢量。角速度的測量利用三軸陀螺。星敏感器對姿態(tài)矢量測量帶有標(biāo)準(zhǔn)差為2角秒的高斯白噪聲。陀螺采樣周期為1/s，星敏感器的采樣周期為10/s，仿真時間設(shè)為3個軌道周期。假設(shè)三軸角度初值有較大估計誤差，分別為-45°,45°,155°，陀螺漂移初始值為0，初始方差陣P₀＝[(40°)²I_3×3(0.1°/h)²I_3×3]^T。乘性噪聲ζ_k-1的均值為0，方差為Q_ζ,k-1＝0.1。相關(guān)噪聲的互協(xié)方差為S_k＝0.05。下面給出文獻(xiàn)(魏喜慶,宋申民.基于容積卡爾曼濾波的衛(wèi)星姿態(tài)估計[J].宇航學(xué)報,2013,34(2):193-200)提出的CQE和所設(shè)計的CQE-MCNS的姿態(tài)估計結(jié)果。

(一)CQE估計結(jié)果

圖1和圖5分別給出的是角度估計誤差和陀螺漂移量估計誤差的2-范數(shù)曲線，圖2-圖4和圖6-圖8則給出三軸角度估計誤差和陀螺漂移量估計誤差曲線。

從仿真中可以看出，CQE的角度估計誤差在0.002°左右，陀螺漂移估計精度達(dá)到了0.01°/h。

(二)CQE-MCNS估計結(jié)果

圖9和圖13分別給出的是角度估計誤差和陀螺漂移量估計誤差的2-范數(shù)曲線，圖10-圖12和圖14-圖16則給出三軸角度估計誤差和陀螺漂移量估計誤差曲線。

從仿真中可以看出，CQE-MCNS的角度估計誤差在0.0005°左右，陀螺漂移估計精度達(dá)到了0.005°/h。在估計精度上，CQE-MCNS相比于CQE有了顯著的提高，這是由于乘性噪聲和噪聲相關(guān)性的存在對CQE的估計精度產(chǎn)生了一定的影響。從而表明本文所設(shè)計的CQE-MCNS算法可以有效估計具有乘性狀態(tài)噪聲及乘性噪聲與觀測噪聲相關(guān)的系統(tǒng)狀，且有較高的估計精度。

本發(fā)明還可有其它多種實施例，在不背離本發(fā)明精神及其實質(zhì)的情況下，本領(lǐng)域技術(shù)人員當(dāng)可根據(jù)本發(fā)明作出各種相應(yīng)的改變和變形，但這些相應(yīng)的改變和變形都應(yīng)屬于本發(fā)明所附的權(quán)利要求的保護(hù)范圍。