戎海龍，彭翠云

1.常州大學(xué) 城市軌道交通學(xué)院，江蘇 常州 213164

2.常州大學(xué) 信息科學(xué)與工程學(xué)院，江蘇 常州 213164

1 引言

慣性-地磁組合一般由三軸MEMS（微機(jī)電系統(tǒng)）陀螺儀、三軸MEMS加速度計(jì)和三軸MEMS地磁傳感器組成，然后通過(guò)多傳感器信息融合技術(shù)獲得運(yùn)動(dòng)體姿態(tài)，在人體運(yùn)動(dòng)分析等領(lǐng)域應(yīng)用很廣泛。這種組合可以看做是兩種姿態(tài)測(cè)量方法的重新組合。眾所周知，單獨(dú)利用三軸陀螺儀也可以實(shí)現(xiàn)姿態(tài)測(cè)量（基于多加速度計(jì)組合的所謂的無(wú)陀螺捷聯(lián)慣導(dǎo)系統(tǒng)也可以歸為此類），這種姿態(tài)測(cè)量方法的短時(shí)測(cè)量精度很高，相應(yīng)的姿態(tài)解算算法包括方向余弦算法、歐拉角法、四元數(shù)微分算法、等效矢量算法等，然而這些算法全部建立在數(shù)值積分的基礎(chǔ)上，因而如果陀螺儀為MEMS陀螺儀，則其自身所具有的幅值較大的隨機(jī)噪聲及從根本上無(wú)法完全消除的轉(zhuǎn)動(dòng)不可交換性誤差很容易使解算結(jié)果快速發(fā)散，因而這種方法并不適合長(zhǎng)時(shí)間的姿態(tài)測(cè)量。利用三軸加速度計(jì)和三軸地磁傳感器也可以獲得運(yùn)動(dòng)體姿態(tài)，這種方法的靜態(tài)精度很高，相應(yīng)的姿態(tài)解算算法包括TRIAD算法、QUEST算法、FORM算法、ESOQ、ESOQ2算法、q-算法、Euler-q算法等，然而加速度計(jì)不可能只測(cè)量運(yùn)動(dòng)體重力加速度，況且人體運(yùn)動(dòng)有一個(gè)不同于機(jī)械運(yùn)動(dòng)（包括飛行器）的特點(diǎn)，即運(yùn)動(dòng)的線加速度復(fù)雜多變，無(wú)規(guī)律可循，采用建模的方法抑制線加速度對(duì)算法本身的影響并不現(xiàn)實(shí)，因而即使加速度計(jì)精度再高，上述算法的動(dòng)態(tài)精度仍然很差。慣性-地磁組合的目的是將上述兩種方法重新進(jìn)行組合，進(jìn)而獲得更好的靜動(dòng)態(tài)精度，采用的姿態(tài)解算算法包括狀態(tài)方程非線性[1]或觀測(cè)方程非線性[2-5]的擴(kuò)展卡爾曼算法、互補(bǔ)濾波算法[6-8]、梯度下降算法[9]，以及經(jīng)作者[10]實(shí)驗(yàn)研究發(fā)現(xiàn)也適用的REQUEST 算法[11]、Optimal-REQUEST 算法[12]、自適應(yīng)Optimal-REQUEST算法[13]、filter-REQUEST算法[14]等，這些算法均為迭代算法，基本思想類似。狀態(tài)方程非線性的卡爾曼算法的觀測(cè)矩陣為單位陣，并假設(shè)載體角速度為一階馬爾科夫過(guò)程，因此對(duì)加速度計(jì)和地磁傳感器組合的依賴較大，這使得該算法的靜態(tài)結(jié)果較高且不發(fā)散，然而動(dòng)態(tài)精度欠佳；觀測(cè)方程非線性的擴(kuò)展卡爾曼算法由于狀態(tài)噪聲取的過(guò)?。ò碝EMS陀螺儀實(shí)際噪聲間接得出）導(dǎo)致卡爾曼增益很小，進(jìn)而使得其對(duì)陀螺儀的依賴較大，因而其靜態(tài)結(jié)果存在緩慢發(fā)散現(xiàn)象，但是其動(dòng)態(tài)精度較高；互補(bǔ)濾波算法和梯度下降算法可以分別通過(guò)調(diào)整其截止頻率和相關(guān)的權(quán)值實(shí)現(xiàn)對(duì)陀螺儀或者加速度計(jì)與地磁傳感器組合的依賴程度，但調(diào)整的依據(jù)沒(méi)有提及，如果調(diào)整不合適，則亦或動(dòng)態(tài)精度較低，亦或靜態(tài)精度較差；REQUEST算法家族隨著其記憶因子的逐步衰減，對(duì)加速度計(jì)和地磁傳感器組合的依賴程度越來(lái)越小，因此一段時(shí)間之后其動(dòng)態(tài)精度比觀測(cè)方程非線性的擴(kuò)展卡爾曼算法精度更高，然而由于過(guò)分依賴陀螺儀，使得其發(fā)散現(xiàn)象比觀測(cè)方程非線性的擴(kuò)展卡爾曼算法更嚴(yán)重。如果能夠根據(jù)運(yùn)動(dòng)體運(yùn)動(dòng)情況對(duì)觀測(cè)噪聲或者過(guò)程噪聲進(jìn)行實(shí)時(shí)估計(jì)，也就是采用自適應(yīng)卡爾曼算法[15]顯然是比較合適的，然而自適應(yīng)卡爾曼算法適用的前提是觀測(cè)噪聲為方差可變的白噪聲，因此該算法目前只在組合導(dǎo)航系統(tǒng)中GPS觀測(cè)噪聲實(shí)時(shí)估計(jì)上獲得應(yīng)用。已有學(xué)者采用下式作為觀測(cè)噪聲調(diào)整依據(jù)：其中ax(k)、ay(k)、az(k)分別為由k時(shí)刻加速度計(jì)測(cè)得的三軸向加速度分量（包括線加速度和重力加速度分量），而后或者采用門技術(shù)[4]調(diào)整觀測(cè)噪聲方差或者直接將上式作為觀測(cè)噪聲并對(duì)其方差進(jìn)行實(shí)時(shí)估計(jì)[3，5]，這些方法盡管簡(jiǎn)單有效，但是有四個(gè)問(wèn)題：（1）很明顯，上式并不能看做噪聲，它甚至根本不是一個(gè)平穩(wěn)過(guò)程；（2）加速度計(jì)存在輸出零偏和隨機(jī)游走，這會(huì)對(duì)上式造成誤差；（3）用上式表示運(yùn)動(dòng)體線加速度大小并不準(zhǔn)確，εo=0時(shí)，上式在三維坐標(biāo)系下可表示為一個(gè)球，球表面上的任何點(diǎn)均滿足該式，這意味著滿足該式并不代表運(yùn)動(dòng)體處于靜止?fàn)顟B(tài)；（4）運(yùn)動(dòng)體線加速度是無(wú)界的，如果過(guò)大，則有可能導(dǎo)致算法過(guò)于依賴陀螺儀而發(fā)散。本文仍然將觀測(cè)方程非線性的擴(kuò)展卡爾曼算法作為姿態(tài)解算算法，然而將給出一種新的觀測(cè)噪聲，這種觀測(cè)噪聲的方差在運(yùn)動(dòng)體運(yùn)動(dòng)時(shí)會(huì)增大，進(jìn)而加大對(duì)陀螺儀的依賴，這將構(gòu)成一種真正意義上的自適應(yīng)擴(kuò)展卡爾曼算法，實(shí)驗(yàn)表明這種算法是比較有效的。

2 基于觀測(cè)噪聲實(shí)時(shí)估計(jì)的自適應(yīng)EKF算法

卡爾曼算法的k+1時(shí)刻的驗(yàn)后估計(jì)為：

Fk+1為k+1時(shí)刻的觀測(cè)陣（或者線性化后的觀測(cè)陣）；Rk+1為觀測(cè)噪聲；為k+1時(shí)刻的先驗(yàn)估計(jì)的誤差協(xié)方差陣：

Qk為過(guò)程噪聲；Pk為k時(shí)刻的驗(yàn)后估計(jì)的誤差協(xié)方差陣。

REQUEST算法的迭代方程為：

其中bi和ri為k時(shí)刻由三軸加速度計(jì)（三軸地磁傳感器）得到的載體坐標(biāo)系表示的重力矢量（地磁場(chǎng)矢量）觀測(cè)值以及該矢量在參考坐標(biāo)系下的表示值；ρk為記憶因子，取值在0至1之間，用于逐步舍棄最初的歷史測(cè)量信息，避免逐步累積的測(cè)量誤差嚴(yán)重污染K矩陣。k+1時(shí)刻的姿態(tài)四元數(shù)qk+1為與矩陣Kk+1/k+1的最大特征值相對(duì)應(yīng)的歸一化的特征向量。

由式（1）和式（4）知，k+1時(shí)刻的姿態(tài)估計(jì)結(jié)果由兩部分組成，第一項(xiàng)為姿態(tài)預(yù)測(cè)，由三軸陀螺儀得到，第二項(xiàng)為姿態(tài)修正（姿態(tài)修正增益分別為Kk+1和1 mk+1），由三軸加速度計(jì)和三軸地磁傳感器得到。由式（1）可知，卡爾曼增益陣Kk+1中的元素值越小，則姿態(tài)修正量越小，估計(jì)結(jié)果越依賴于陀螺儀，反之則越依賴于加速度計(jì)和地磁傳感器組合。狀態(tài)方程非線性的卡爾曼算法由于其觀測(cè)陣為單位陣，其卡爾曼增益陣中的元素?cái)?shù)量級(jí)恒定維持在101左右，這就表明狀態(tài)方程非線性的卡爾曼算法側(cè)重于依賴加速度計(jì)和地磁傳感器組合。觀測(cè)方程非線性的卡爾曼算法的過(guò)程噪聲協(xié)方差陣中的元素與采樣時(shí)間的平方成正比，而通常慣性-地磁組合的采樣時(shí)間在幾十毫秒，因此，由式（2）和（3）不難發(fā)現(xiàn)卡爾曼增益被嚴(yán)重衰減，這表明觀測(cè)方程非線性的卡爾曼算法更傾向于依賴陀螺儀信息。由式（4）可知，隨著時(shí)間的增長(zhǎng)，1 mk+1逐漸衰減至0，mkmk+1逐漸接近1，這就表明REQUEST算法對(duì)陀螺儀的依賴程度更甚于觀測(cè)方程為非線性的卡爾曼算法。以上分析表明狀態(tài)方程非線性的卡爾曼算法的靜態(tài)精度比觀測(cè)方程為非線性的卡爾曼算法和REQUEST算法要高，而動(dòng)態(tài)精度要差。

為了獲得更好的靜動(dòng)態(tài)性能，一個(gè)自然的想法是觀測(cè)噪聲的方差與運(yùn)動(dòng)體線加速度的大小成正比，進(jìn)而通過(guò)觀測(cè)噪聲方差間接調(diào)整姿態(tài)修正增益，使得在運(yùn)動(dòng)體運(yùn)動(dòng)時(shí)加大對(duì)陀螺儀的依賴。由于狀態(tài)方程非線性的卡爾曼算法觀測(cè)矩陣為單位陣，由式（2）不難看出該算法的增益陣幾乎不受過(guò)程噪聲和測(cè)量噪聲的影響，所以通過(guò)干預(yù)過(guò)程噪聲或者測(cè)量噪聲的辦法來(lái)干預(yù)姿態(tài)修正增益幾乎沒(méi)有效果。而由式（4）可以看出，姿態(tài)修正增益1 mk+1隨時(shí)間逐漸衰減，對(duì)其進(jìn)行調(diào)整的結(jié)果只能是減緩其衰減的速度，因此調(diào)整REQUEST算法的姿態(tài)修正增益是沒(méi)有意義的?；谏鲜鲈颍疚牟扇?shí)時(shí)調(diào)整觀測(cè)方程非線性的卡爾曼算法的觀測(cè)噪聲的方法來(lái)達(dá)到此目的。

假設(shè)x=vg×vm，其中“×”表示兩矢量的向量積。對(duì)x求導(dǎo)得到：

取測(cè)量方程

其中C為單位陣。

將式（9）與式（10）組合在一起，并考慮過(guò)程噪聲及觀測(cè)噪聲后形成如下方程組：

之所以考慮過(guò)程噪聲及觀測(cè)噪聲，是由于觀測(cè)矢量vg、vm和ω本身存在測(cè)量噪聲，而且[ ]ω×、u(t)只能采用這些觀測(cè)矢量計(jì)算。將式（11）離散化，得到：

其中，k表示具體的采樣時(shí)刻。

ξ(t)和η(t)可以由式（15）得到：

由式（12）及式（15）可以發(fā)現(xiàn)：（1）過(guò)程噪聲和觀測(cè)噪聲并不為白噪聲；（2）兩種噪聲之間是相關(guān)的；（3）兩種噪聲的方差陣及兩者之間的協(xié)方差陣在不斷變化。為簡(jiǎn)化計(jì)算起見(jiàn)，可以否定上述三條結(jié)論，針對(duì)式（15）的大量仿真計(jì)算也說(shuō)明了這一點(diǎn)?？梢远x

其中Q、R為常量，可以利用式（15）估算，然后根據(jù)卡爾曼濾波效果再做調(diào)整?？柭鼮V波公式限于篇幅略去。

待x(k)估計(jì)出后，可利用式（17）計(jì)算估計(jì)殘差：

其中，ρ＞0殘差調(diào)整因子。值得指出的是，Allan方差分析表明，加速度計(jì)等慣性器件的輸出零偏和隨機(jī)游走屬于低頻成分，卡爾曼濾波器屬于低通濾波器，其本身又是無(wú)偏估計(jì)，所以加速度計(jì)的輸出零偏和隨機(jī)游走會(huì)直接通過(guò)卡爾曼濾波器而疊加到其輸出中，這就使得利用

圖2 靜態(tài)測(cè)試條件下MTi姿態(tài)輸出結(jié)果

式（17）計(jì)算出的估計(jì)殘差中不會(huì)包含任何有關(guān)加速度計(jì)的輸出零偏和隨機(jī)游走的成分。上述結(jié)論表明加速度計(jì)的輸出零偏和隨機(jī)游走并不影響本文算法。

估計(jì)殘差的方差可利用滑窗估計(jì)的方法（式（18））實(shí)時(shí)計(jì)算

其中，n為截取的滑窗長(zhǎng)度。

觀測(cè)方程非線性的卡爾曼算法的觀測(cè)噪聲的協(xié)方差可按照下式實(shí)時(shí)進(jìn)行調(diào)整：

3 實(shí)驗(yàn)結(jié)果

3.1 實(shí)驗(yàn)設(shè)置

如圖1所示。實(shí)驗(yàn)時(shí)將新近購(gòu)得的兩種慣性-地磁組合MTi和ADIS16480通過(guò)扎帶固定于滑臺(tái)上，滑臺(tái)可以用手驅(qū)動(dòng)在滑軌上做往復(fù)直線運(yùn)動(dòng)。MTi的基本噪聲參數(shù)為陀螺儀0.34°/s、加速度計(jì)0.008 m/s2、地磁傳感器0.1 μT，ADIS16480的基本噪聲參數(shù)則分別為0.16 °/s、0.014 7 m/s2及0.045 μT。這兩種慣性地磁組合的采樣率分別為256 Hz和307.5 Hz。本文算法以ADIS16480輸出的原始傳感器數(shù)據(jù)解算姿態(tài)。姿態(tài)用歐拉角表示，并且為了清晰展示，姿態(tài)解算結(jié)果已經(jīng)除去了其均值量。Q=0.01，R=0.1，ρ =100，n=30[3]。

圖1 實(shí)驗(yàn)設(shè)置

3.2 靜態(tài)測(cè)試

圖3 靜態(tài)測(cè)試條件下ADIS16480姿態(tài)輸出結(jié)果

將兩種慣性-地磁組合在滑臺(tái)上靜止一段時(shí)間，以測(cè)試靜態(tài)精度。圖2和圖3分別為MTi和ADIS16480內(nèi)置的卡爾曼算法的姿態(tài)解算結(jié)果，圖4為本文姿態(tài)解算結(jié)果，圖5為REQUEST算法的姿態(tài)解算結(jié)果，其中REQUEST算法的記憶因子取為0.9。

3.3 動(dòng)態(tài)測(cè)試

在手帶動(dòng)下使滑臺(tái)在滑軌上做一段時(shí)間的往復(fù)直線運(yùn)動(dòng)。圖6～8為ADIS16480輸出的傳感器原始數(shù)據(jù)，圖9～12為姿態(tài)解算結(jié)果，圖13為估計(jì)殘差及其方差估計(jì)結(jié)果。

3.4 結(jié)果分析

對(duì)比圖2～5可以發(fā)現(xiàn)，在靜止情況下ADIS16480內(nèi)置卡爾曼算法和本文算法的姿態(tài)解算精度相當(dāng)且較高，均傾向于依賴加速度計(jì)和地磁傳感器信息，而MTi內(nèi)置的卡爾曼算法的姿態(tài)解算結(jié)果出現(xiàn)了發(fā)散現(xiàn)象，這說(shuō)明MTi更傾向于依賴陀螺儀信息，REQUEST算法對(duì)陀螺儀信息依賴更嚴(yán)重，使得其姿態(tài)解算結(jié)果發(fā)散現(xiàn)象更嚴(yán)重，這也是為何REQUEST算法能夠應(yīng)用于廣泛使用光纖、激光或者靜電陀螺的航空航天領(lǐng)域而無(wú)法在普遍使用MEMS陀螺的民用領(lǐng)域獲得推廣的根本原因。

圖4 靜態(tài)測(cè)試條件下本文姿態(tài)解算結(jié)果

圖5 靜態(tài)測(cè)試條件下REQUEST算法姿態(tài)結(jié)算結(jié)果

圖6 動(dòng)態(tài)測(cè)試條件下ADIS16480陀螺儀輸出結(jié)果

圖7 動(dòng)態(tài)測(cè)試條件下ADIS16480加速度計(jì)輸出結(jié)果

圖8 動(dòng)態(tài)測(cè)試條件下ADIS16480地磁傳感器輸出結(jié)果

圖9 動(dòng)態(tài)測(cè)試條件下MTi姿態(tài)輸出結(jié)果

圖10 動(dòng)態(tài)測(cè)試條件下ADIS16480姿態(tài)輸出結(jié)果

圖11 本文姿態(tài)解算結(jié)果

圖12 動(dòng)態(tài)測(cè)試條件下REQUEST算法姿態(tài)解算結(jié)果

圖13 估計(jì)殘差及其方差

由圖6～8所示，如果在某段時(shí)間內(nèi)（圖中的1.85～23.9 s）使滑臺(tái)做直線往復(fù)運(yùn)動(dòng)而保持姿態(tài)不變，那么各種算法的姿態(tài)解算結(jié)果如圖9～12所示，很明顯，由于姿態(tài)保持不變，那么如果某種算法姿態(tài)解算結(jié)果出現(xiàn)變化，此變化量即為誤差，ADIS16480仍然傾向于依賴加速度計(jì)和地磁傳感器信息，這使得其姿態(tài)解算結(jié)果出現(xiàn)大幅擺動(dòng)，俯仰角擺幅近似在±10°以上，而MTi傾向于依賴陀螺儀信息，所以其姿態(tài)解算結(jié)果的擺動(dòng)較小，俯仰角擺幅近似在±1°左右，REQUEST算法給出的姿態(tài)解算結(jié)果也出現(xiàn)擺動(dòng)，但是擺動(dòng)逐步衰減，運(yùn)動(dòng)初始時(shí)刻俯仰角擺幅近似為±4°，而運(yùn)動(dòng)終止時(shí)近似在±0.75°左右，REQUEST算法對(duì)陀螺儀的依賴程度是一個(gè)逐步增大的過(guò)程，因此，如果在使滑臺(tái)運(yùn)動(dòng)之前使其處于靜止?fàn)顟B(tài)更長(zhǎng)一段時(shí)間，那么其動(dòng)態(tài)精度會(huì)更好一些。由圖13可以看到估計(jì)殘差在滑臺(tái)運(yùn)動(dòng)時(shí)會(huì)增大，本文利用這一特點(diǎn)實(shí)現(xiàn)的自適應(yīng)擴(kuò)展卡爾曼算法的姿態(tài)解算結(jié)果如圖11所示，很明顯其俯仰角擺幅只有±0.2°左右，是上述所有算法中擺幅最小的，這說(shuō)明在滑臺(tái)運(yùn)動(dòng)時(shí)，通過(guò)自適應(yīng)調(diào)整觀測(cè)噪聲，使算法在此時(shí)加大對(duì)陀螺儀的依賴，從而減少動(dòng)態(tài)誤差，但同時(shí)也應(yīng)看到，由于對(duì)陀螺儀誤差加大，算法出現(xiàn)緩慢發(fā)散現(xiàn)象。

4 結(jié)論

本文在觀測(cè)方程非線性卡爾曼算法的基礎(chǔ)上給出了一種新的自適應(yīng)擴(kuò)展卡爾曼算法，該方法利用線性卡爾曼算法實(shí)時(shí)估計(jì)‖，并將估計(jì)殘差作為用于姿態(tài)解算的擴(kuò)展卡爾曼算法的觀測(cè)噪聲，原因在于該估計(jì)殘差在運(yùn)動(dòng)體靜止和運(yùn)動(dòng)時(shí)方差并不相同，這樣就使得該算法可以兼具更好的靜動(dòng)態(tài)性能。由于采用線性卡爾曼算法，本文提出的方法并不會(huì)大幅度提高擴(kuò)展卡爾曼算法的運(yùn)行時(shí)間。

未來(lái)將在以下兩個(gè)方面繼續(xù)開(kāi)展研究：（1）事實(shí)上，估計(jì)殘差雖然為零均值且平穩(wěn)，但并不為白噪聲，這將使得本文提出的自適應(yīng)擴(kuò)展卡爾曼算法并不是最優(yōu)的，因此需要進(jìn)一步做白化處理；（2）殘差調(diào)整因子ρ的取值是根據(jù)實(shí)驗(yàn)調(diào)整的，并沒(méi)有根據(jù)某種準(zhǔn)則做最優(yōu)選取。

[1]Yun X P，Bachmann E R.Design，implementation，and experimental results of a quaternion-based Kalman filter forhuman body motion tracking[J].IEEE Transactions on Robotics，2006，22（6）：1216-1227.

[2]Sabatelli S，Galgani M，F(xiàn)anucci L，et al.A double stage Kalman filter for sensor fusion and orientation tracking in 9D IMU[C]//Proceedings of IEEE Symposium on Sensors Applications，Brescia，Italy，2012：1-5.

[3]Sessa S，Zecca M，Lin Z H，et al.A methodology for the performance evaluation of inertial measurement units[J].Journal of Intelligent&Robotic System，2013，71（2）：143-157.

[4]Sabatini A M.Kalman-filter-based orientation determina-tion using inertial/magnetic sensors：Observability analysis and performance evaluation[J].Sensors，2011，11（10）：9182-9206

[5]Ren H L，Kazanzides P.Investigation of attitude tracking using an integrated inertial and magnetic navigation system for hand-held surgical instruments[J].IEEE/ASME Transactions on Mechatronics，2012，17（2）：210-217.

[6]Gallagher A，Matsuoka Y，Ang W T.An efficient realtime human posture tracking algorithm using low-cost inertial and magnetic sensors[C]//Proceedings of IEEE/RSJ International Conference on Intelligent Robots and Systems，Sendai，Japan，2004：2967-2972.

[7]Harms H，Amft O，Winkler R，et al.ETHOS：Miniature orientation sensor for wearable human motion analysis[C]//Proceedings of IEEE Conference on Sensors，Kona，USA，2010：1037-1042.

[8]Campolo D，Taffoni F，F(xiàn)ormica D.Inertial-magnetic sensors for assessing spatial cognition in infants[J].IEEE Transactions on Biomedical Engineering，2011，58（5）：1499-1503.

[9]Madgwick S O H，Harrison A J L，Vaidyanathan R.Estimation of IMU and MARG orientation using a gradient descent algorithm[C]//Proceedings of IEEE International Conference on Rehabilitation Robotics，Zurich，Switzerland，2011：1-7.

[10]戎海龍，戴先中，劉信宇.烹飪過(guò)程中鍋具運(yùn)動(dòng)姿態(tài)測(cè)量方法[J].中國(guó)慣性技術(shù)學(xué)報(bào)，2009，17（4）：419-423.

[11]Bar-itzhack I Y.Request：A recursive QUEST algorithm for sequential attitude determination[J].Journal of Guidance，Control and Dynamics，1996，19（5）：1034-1038.

[12]Choukroun D，Bar-itzhack I Y，Oshman Y.Optimal-REQUEST algorithm for attitude determination[J].Journal of Guidance，Control and Dynamics，2004，27（3）：418-425.

[13]Choukroun D.Adaptive optimal-request algorithm for attitude determination[C]//Proceedings of AIAA Guidance，Navigation，and Control Conference，Hilton Head，USA，2007：4624-4647.

[14]Shuster M D.Filter QUEST or REQUEST[J].Journal of Guidance，Control and Dynamics，2009，32（2）：643-645.

[15]卞鴻巍，金志華，王俊璞，等.組合導(dǎo)航系統(tǒng)新息自適應(yīng)卡爾曼濾波算法[J].上海交通大學(xué)學(xué)報(bào)，2006，40（6）：1000-1003.