楊兆，沈作軍

北京航空航天大學(xué) 航空科學(xué)與工程學(xué)院，北京 100191

隨著現(xiàn)代科技的發(fā)展，無(wú)人機(jī)成為一種越來(lái)越受重視的新興飛行器，由于其低風(fēng)險(xiǎn)、低成本、可自主飛行和可重復(fù)利用的特點(diǎn)，被廣泛應(yīng)用于軍事和民用領(lǐng)域。針對(duì)小型無(wú)人機(jī)，由于其制作成本低、體積較小，在配備導(dǎo)航系統(tǒng)時(shí)，主要選擇重量輕、成本低且體積小、易于集成且功耗小的微機(jī)電系統(tǒng)（Micro Electro Mechanical System，MEMS）式慣性導(dǎo)航系統(tǒng)。慣導(dǎo)系統(tǒng)的最大缺點(diǎn)是隨著時(shí)間的推移，其誤差隨著時(shí)間的積分會(huì)愈來(lái)愈大。相對(duì)于高成本高精度的慣導(dǎo)系統(tǒng)，MEMS的精度較低，這會(huì)導(dǎo)致單純的依靠MEMS進(jìn)行姿態(tài)估計(jì)得到的誤差越來(lái)越大并且不能得到修正。衛(wèi)星定位導(dǎo)航系統(tǒng)可以提供不隨時(shí)間積累誤差的導(dǎo)航信息，因此，將這兩種導(dǎo)航系統(tǒng)的數(shù)據(jù)進(jìn)行融合，從而給出一個(gè)較為精確的姿態(tài)信息。不準(zhǔn)確的姿態(tài)信息會(huì)對(duì)無(wú)人機(jī)的飛行控制產(chǎn)生巨大的干擾，不僅會(huì)使得無(wú)法完成規(guī)劃的飛行任務(wù)，嚴(yán)重時(shí)還會(huì)導(dǎo)致控制失穩(wěn)，造成無(wú)人機(jī)墜毀等事故。

本文基于擴(kuò)展卡爾曼濾波理論（Extended Kalman Filter，EKF），結(jié)合MEMS的慣性導(dǎo)航系統(tǒng)和衛(wèi)星定位導(dǎo)航系統(tǒng)的測(cè)量信息，研究一種針對(duì)小型無(wú)人機(jī)的姿態(tài)估計(jì)方法，并對(duì)其進(jìn)行仿真分析。

1 卡爾曼濾波理論

卡爾曼濾波[1]是一種對(duì)系統(tǒng)進(jìn)行最優(yōu)估計(jì)的算法，其原理是利用線(xiàn)性系統(tǒng)狀態(tài)方程，通過(guò)對(duì)系統(tǒng)輸入，輸出觀測(cè)數(shù)據(jù)。由于所得觀測(cè)數(shù)據(jù)包括系統(tǒng)本身所含噪聲以及外界干擾的影響，因此，最優(yōu)估計(jì)的過(guò)程也被視作濾波過(guò)程。卡爾曼濾波具有以下幾個(gè)特點(diǎn)：

（1）利用狀態(tài)空間方程在時(shí)域內(nèi)設(shè)計(jì)一種遞推的濾波器，可以用于估計(jì)多維的隨機(jī)過(guò)程。

（2）卡爾曼濾波理論采用狀態(tài)方程去描述被估計(jì)量的動(dòng)態(tài)變化規(guī)律。被估計(jì)的狀態(tài)量其動(dòng)態(tài)信息由兩方面確定，一是激勵(lì)白噪聲的統(tǒng)計(jì)信息，二是動(dòng)力學(xué)方程。在動(dòng)力學(xué)方程已知并且激勵(lì)白噪聲是一個(gè)平穩(wěn)過(guò)程的情況下，被估計(jì)的狀態(tài)量既可以是平穩(wěn)的，也可以是非平穩(wěn)的。

（3）雖然是在時(shí)域內(nèi)設(shè)計(jì)的一種遞推的濾波器算法，但是卡爾曼濾波有連續(xù)性和離散型兩類(lèi)算法，離散型算法方便在數(shù)字計(jì)算機(jī)上實(shí)現(xiàn)。

卡爾曼濾波理論克服了以往濾波理論的局限性，因此在工程上得到了較為廣泛的應(yīng)用，在航空航天領(lǐng)域的導(dǎo)航制導(dǎo)控制（GNC）方向以及通信領(lǐng)域都起到了重大的作用[2]。

1.1 離散型卡爾曼濾波

由于離散型的卡爾曼濾波算法通過(guò)對(duì)時(shí)間進(jìn)行離散，可以在數(shù)字計(jì)算機(jī)上進(jìn)行編程計(jì)算，所以該方法在對(duì)信號(hào)的線(xiàn)性估計(jì)和處理方面有著廣泛的應(yīng)用。

假設(shè)在tk時(shí)刻，系統(tǒng)噪聲序列Wk-1驅(qū)動(dòng)被估計(jì)的狀態(tài)量Xk，式（1）描述了驅(qū)動(dòng)的原理。式中：Φk，k-1是由tk-1的狀態(tài)進(jìn)入到tk時(shí)刻狀態(tài)的一步轉(zhuǎn)移陣，Γk-1是系統(tǒng)噪聲驅(qū)動(dòng)陣、Wk-1是系統(tǒng)噪聲序列。

Zk為tk時(shí)刻狀態(tài)量Xk的量測(cè)量，它們之間有線(xiàn)性關(guān)系由量測(cè)方程表示：

式中：Hk為量測(cè)陣；Vk為量測(cè)噪聲序列。式（1）和式（2）中的噪聲序列滿(mǎn)足下式：

式中：Qk和Rk分別是系統(tǒng)噪聲序列的方差陣和量測(cè)噪聲序列的方差陣，在這假定Qk是非負(fù)定矩陣，Rk是正定矩陣。根據(jù)線(xiàn)性最小方差的原理推導(dǎo)出離散型卡爾曼濾波方程。

卡爾曼濾波過(guò)程其實(shí)是一個(gè)預(yù)測(cè)ü校正的過(guò)程，根據(jù)系統(tǒng)方程利用上一個(gè)時(shí)刻的狀態(tài)估計(jì)值進(jìn)行預(yù)測(cè)，從而計(jì)算得出當(dāng)前時(shí)刻的一步狀態(tài)估計(jì)值，而當(dāng)前時(shí)間點(diǎn)的狀態(tài)最優(yōu)估計(jì)值是根據(jù)質(zhì)量?jī)?yōu)劣參數(shù)和量測(cè)方程以及利用量測(cè)信息校正狀態(tài)量的一步預(yù)測(cè)估計(jì)值這三個(gè)量得到的。

1.2 擴(kuò)展卡爾曼濾波

對(duì)于一般的線(xiàn)性系統(tǒng)而言，離散型卡爾曼濾波基本方程都可以解決，但是對(duì)于工程應(yīng)用中的實(shí)際系統(tǒng)而言，大部分都是非線(xiàn)性的，所以要求系統(tǒng)方程和量測(cè)方程是非線(xiàn)性的，對(duì)于非線(xiàn)性系統(tǒng)，一般用式（4）表示：

式中：f表示系統(tǒng)的非線(xiàn)性動(dòng)力學(xué)函數(shù)，該函數(shù)與時(shí)間、輸入量以及系統(tǒng)的狀態(tài)量相關(guān)；G用來(lái)表示該系統(tǒng)的噪聲輸入。h表示系統(tǒng)的非線(xiàn)性量測(cè)函數(shù)，該函數(shù)與系統(tǒng)的狀態(tài)和時(shí)間相關(guān)；w和v是兩組零均值白噪聲序列，彼此相互獨(dú)立，其初始狀態(tài)不受系統(tǒng)的初始狀態(tài)X(0)影響，它們具有如下特性：

如式（5）所描述的非線(xiàn)性系統(tǒng)，離散型卡爾曼濾波的基本方程將不再能解決該類(lèi)問(wèn)題。對(duì)于非線(xiàn)性較弱的系統(tǒng)，通過(guò)對(duì)式（4）中的非線(xiàn)性部分進(jìn)行泰勒展開(kāi)，保留一次項(xiàng)，省略掉高階項(xiàng)，從而得到線(xiàn)性系統(tǒng)。用這種方法可以繼續(xù)使用卡爾曼濾波基本方程對(duì)該線(xiàn)性系統(tǒng)狀態(tài)進(jìn)行估計(jì)。但是在對(duì)系統(tǒng)進(jìn)行線(xiàn)性化時(shí)，需要找到一個(gè)系統(tǒng)的參照點(diǎn)，也就是一條參考的標(biāo)稱(chēng)軌跡。假如選擇的參考軌跡是系統(tǒng)噪聲為0時(shí)所解算得到的軌跡，便會(huì)出現(xiàn)以下兩個(gè)問(wèn)題：其一，參考軌跡的狀態(tài)量，量測(cè)值的參考值都預(yù)先存儲(chǔ)在計(jì)算機(jī)中，需要較大的存儲(chǔ)單元，在工程應(yīng)用上，特別是對(duì)小型固定翼無(wú)人機(jī)而言，實(shí)現(xiàn)起來(lái)十分麻煩；其二，在實(shí)際的工程系統(tǒng)中，由于受到多方面的影響，隨著時(shí)間的積累，實(shí)際軌跡與參考軌跡之間的誤差將越來(lái)越大，此時(shí)如果還使用預(yù)存的參考軌跡，那么系統(tǒng)的非線(xiàn)性部分誤差將十分大，從而難以忽略，因此會(huì)直接影響到系統(tǒng)的準(zhǔn)確性。

在工程應(yīng)用中，需要對(duì)系統(tǒng)進(jìn)行線(xiàn)性化時(shí)，通常采用的參考軌跡是最優(yōu)狀態(tài)估計(jì)軌跡，把這種基于最優(yōu)估計(jì)進(jìn)行線(xiàn)化的軌跡，再采用卡爾曼濾波進(jìn)行估計(jì)的方法稱(chēng)為擴(kuò)展卡爾曼濾波[3]。

1.3 離散型擴(kuò)展卡爾曼濾波方程

真實(shí)軌跡和標(biāo)稱(chēng)軌跡之間的偏差定義用式（6）和式（7）表示：

式中為標(biāo)稱(chēng)量測(cè)值，表達(dá)式為：

從式（8）可以看出，在遞推的過(guò)程中，若要求得到狀態(tài)量估計(jì)值 ，那么必須已知量測(cè)值 ，但是量測(cè)值又需要從狀態(tài)量估計(jì)值所計(jì)算得到，為了解決以上問(wèn)題，采取上一時(shí)刻的狀態(tài)量估計(jì)值來(lái)求解量測(cè)量的標(biāo)稱(chēng)值：

到標(biāo)稱(chēng)軌跡后，則可以對(duì)系統(tǒng)進(jìn)行線(xiàn)化，線(xiàn)化后的標(biāo)稱(chēng)軌跡有：

式中：F(t)和H(t)為雅可比矩陣，表達(dá)式為：

在對(duì)系統(tǒng)完成線(xiàn)化后，還需要對(duì)系統(tǒng)進(jìn)行離散，以便于在計(jì)算機(jī)上實(shí)現(xiàn)，離散后的系統(tǒng)為：

其中：

式中：T為時(shí)間步長(zhǎng)。根據(jù)離散系統(tǒng)方程和上一節(jié)所提到的離散型卡爾曼濾波基本方程，可以推導(dǎo)得出擴(kuò)展離散性卡爾曼濾波方程：

（1）狀態(tài)一步預(yù)測(cè)：

（2）一步預(yù)測(cè)均方差計(jì)算：

（3）卡爾曼增益計(jì)算：

（4）狀態(tài)偏差及狀態(tài)估計(jì)：

（5）估計(jì)均方誤差：

2 傳感器誤差模型

在實(shí)際的工程應(yīng)用中，由于環(huán)境的溫度、濕度，地球地磁效應(yīng)、周?chē)姶怒h(huán)境以及傳感器本身的測(cè)量誤差和轉(zhuǎn)換誤差等影響，在仿真的過(guò)程中需要對(duì)傳感器建模過(guò)程中加入誤差模型，使得仿真過(guò)程更加與實(shí)際工程應(yīng)用相一致，才能在此基礎(chǔ)上設(shè)計(jì)出更合適的算法來(lái)對(duì)這些系統(tǒng)的誤差進(jìn)行補(bǔ)償，從而得到一個(gè)更為準(zhǔn)確的測(cè)量和估計(jì)。

2.1 MEMS慣導(dǎo)模型

慣性導(dǎo)航系統(tǒng)器件在實(shí)際應(yīng)用中會(huì)受到很多環(huán)境因素的影響，例如，加速度計(jì)和陀螺儀在實(shí)際應(yīng)用中會(huì)受到類(lèi)似測(cè)量噪聲、偏差等因素的影響。針對(duì)陀螺儀，一種常見(jiàn)的陀螺儀誤差模型[4]可用式（21）表示：

在式（21）中，陀螺儀量測(cè)得到的角速率分別為[pmqmrm]；實(shí)際的角速率分別為[p q r]；加入的誤差項(xiàng)[bpbqbr]包含了陀螺儀接通電源后所產(chǎn)生的常值漂移b0和隨機(jī)漂移bR

[5]，可用式（22）表示；高斯白噪聲分別為 [εpεqεr]，其中[p q r]分別代表飛行器飛行過(guò)程中的滾轉(zhuǎn)角速度、俯仰角速度、偏航角速度。

陀螺儀在每次啟動(dòng)時(shí)都會(huì)產(chǎn)生一個(gè)常值漂移和隨機(jī)漂移，常值漂移一般用b0表示，而隨機(jī)漂移一般用一階馬爾科夫[6]隨機(jī)過(guò)程來(lái)描述，用來(lái)表示陀螺的誤差在運(yùn)行過(guò)程中隨時(shí)間的變化，用式（23）表示，τ為高斯一階馬爾科夫過(guò)程中的時(shí)間常數(shù)，εbR為加入的高斯白噪聲。

加速度計(jì)相比于陀螺儀產(chǎn)生的誤差較小，所以加速度計(jì)產(chǎn)生的誤差用式（24）表示，只考慮高斯白噪聲。式中加度計(jì)量測(cè)值用[AxmAymAzm]表示，實(shí)際的加速度信息用[AxAyAz]表示，高斯白噪聲用 [εAxεAyεAz]表示，其中[AxAyAz]表示飛行器三個(gè)方向的加速度。

2.2 衛(wèi)星定位系統(tǒng)模型

目前，衛(wèi)星定位系統(tǒng)有美國(guó)全球定位系統(tǒng)（Global Position System，GPS）、俄羅斯的格洛納斯系統(tǒng)、中國(guó)的北斗定位系統(tǒng)。本文中采用的仿真模型是GPS，GPS有兩大功能應(yīng)用比較廣泛，一是位置測(cè)量，一是速度測(cè)量。GPS根據(jù)偽距、多普勒頻移和載波相位這三種原始測(cè)量值通過(guò)不同的計(jì)算方式派生出單點(diǎn)、相位差分、偽距差分等多種定位方式。GPS速度測(cè)量可以通過(guò)多普勒頻移獲取，由于和衛(wèi)星距離無(wú)關(guān)，所以GPS的測(cè)速精度遠(yuǎn)遠(yuǎn)高于定位精度，精確度可以達(dá)到0.01m/s[7，8]。GPS的誤差包含多種，如衛(wèi)星速度誤差、衛(wèi)星軌道誤差、對(duì)流層和電離層時(shí)延變化率等。為了保證誤差的一般性并且方便仿真的進(jìn)行，本文根據(jù)式（25）建立了以下模型，假設(shè)速度信息無(wú)漂移，只受到一個(gè)高斯白噪聲的影響。

GPS本身并不具備有測(cè)量載體加速度的功能，但是我們可以通過(guò)速度差分來(lái)計(jì)算得出加速度信息，通過(guò)這種方法對(duì)載體加速度進(jìn)行計(jì)算，在參考文獻(xiàn)[9]和參考文獻(xiàn)[10]中已經(jīng)得到了應(yīng)用，而且其計(jì)算精度能達(dá)到0.01m/s2。該種方法不受重力、溫度等影響。利用式（26）可以對(duì)GPS測(cè)得的速度信息進(jìn)行差分便可以得到加速度信息：

式中：k表示GPS的運(yùn)行過(guò)程中第k步的可用速度信息；δt表示速度更新時(shí)間間隔。

3 構(gòu)建擴(kuò)展卡爾曼濾波過(guò)程

卡爾曼濾波方程構(gòu)建包括預(yù)測(cè)和校正兩個(gè)過(guò)程，校正過(guò)程是根據(jù)量測(cè)數(shù)據(jù)對(duì)預(yù)測(cè)得到的數(shù)據(jù)進(jìn)行校正。下面主要介紹根據(jù)卡爾曼濾波算法進(jìn)行姿態(tài)估計(jì)的系統(tǒng)方程建立過(guò)程和測(cè)量方程的推導(dǎo)過(guò)程。

飛行器的姿態(tài)一般用兩種方式描述，即姿態(tài)角和四元數(shù)。在本文中選擇使用姿態(tài)角，也就是繞大地坐標(biāo)系形成的三個(gè)歐拉角（滾轉(zhuǎn)角、俯仰角、偏航角）。其微分方程的關(guān)系可在飛行力學(xué)相關(guān)書(shū)籍的運(yùn)動(dòng)學(xué)方程建立中找出，將式（21）代入得到：

為了能夠在陀螺儀啟動(dòng)時(shí)可以估計(jì)出常值漂移，因此在選定卡爾曼濾波方程的狀態(tài)量時(shí)，選擇三個(gè)姿態(tài)角和三個(gè)陀螺儀的誤差量作為系統(tǒng)的狀態(tài)矢量：

針對(duì)非線(xiàn)性系統(tǒng)方程，在擴(kuò)展卡爾曼濾波中要對(duì)其進(jìn)行線(xiàn)化，從而得到線(xiàn)化點(diǎn)的雅可比矩陣，利用該雅可比矩陣進(jìn)行狀態(tài)量的一步迭代預(yù)測(cè)計(jì)算。根據(jù)GPS計(jì)算得到的加速度信息和MEMS測(cè)量的加速度信息相結(jié)合，可以得到飛行器的姿態(tài)信息。具體過(guò)程如下：

（1）在固定翼飛行器飛行過(guò)程中，為了保證飛行安全，一般選擇在側(cè)滑角較小或者為0的情況下飛行，因此本文針對(duì)側(cè)滑角為0的情況下，根據(jù)GPS測(cè)量的速度信息計(jì)算偏航角：

（2）慣導(dǎo)系統(tǒng)中加速度計(jì)是測(cè)量體軸系下的加速度A，但是式（26）中，由GPS所得到的加速度aGPS是大地坐標(biāo)系的加速度，關(guān)系如下：

式中：g為重力加速度，DCMgb為體軸系和大地坐標(biāo)系之間的轉(zhuǎn)換矩陣。根據(jù)式（31）中已經(jīng)計(jì)算好的偏航角，可以將DCMgb改為下述形式：

令：

（3）進(jìn)一步推導(dǎo)得出俯仰角θ、滾轉(zhuǎn)角φ的表達(dá)式：

（4）根據(jù)上述的三個(gè)計(jì)算順序依次求出飛行器的三個(gè)姿態(tài)角的表達(dá)式，通過(guò)加速度計(jì)和GPS可以聯(lián)合求出姿態(tài)信息，將該信息作為卡爾曼濾波方程中的量測(cè)值。

（5）由于通過(guò)上述計(jì)算可以直接得到姿態(tài)角信息，所以卡爾曼濾波方程中的量測(cè)方程可以得到很大的簡(jiǎn)化，其量測(cè)方程如式（37）所示：

4 仿真算例

在第三小節(jié)中建立了卡爾曼濾波的系統(tǒng)方程和量測(cè)方程，本文基于離散型擴(kuò)展卡爾曼濾波的基本方程，建立姿態(tài)估計(jì)的遞推算法，利用Matlab數(shù)學(xué)仿真軟件，對(duì)Cessna182飛機(jī)進(jìn)行氣動(dòng)建模并且進(jìn)行全機(jī)的控制律設(shè)計(jì)，在此基礎(chǔ)上，對(duì)該算法進(jìn)行仿真驗(yàn)證。飛行軌跡設(shè)計(jì)包含對(duì)稱(chēng)和非對(duì)稱(chēng)運(yùn)動(dòng)。為了更好地模擬真實(shí)的飛行數(shù)據(jù)，在虛擬傳感器信息中需要加入噪聲和漂移量，見(jiàn)表1，根據(jù)參考文獻(xiàn)[11]和參考文獻(xiàn)[12]選擇出適合的噪聲量和漂移量。

表1 傳感器噪聲和偏移量Table 1 Sensor noise and offset

則基于傳感器的噪聲誤差參數(shù)可以得到系統(tǒng)噪聲方差陣。由于量測(cè)信息是由解析計(jì)算得到，所以噪聲方差沒(méi)有直接的計(jì)算依據(jù)，本文中假設(shè)量測(cè)信息即姿態(tài)歐拉角的標(biāo)準(zhǔn)差為0.5°，在R取上述值時(shí)，估計(jì)器有較好的收線(xiàn)效果。

圖1 三維飛行軌跡圖Fig.1 3D trajectory

圖1是飛行器的三維飛行軌跡，圖2是加速度計(jì)和GPS加入白噪聲和漂移量后，飛行器未加入卡爾曼濾波時(shí)的姿態(tài)角變化的真實(shí)值和估計(jì)值，圖3是飛行器未加入卡爾曼濾波時(shí)姿態(tài)角真實(shí)值和估計(jì)值的誤差。從圖3可以看出，當(dāng)反饋的姿態(tài)角信息有誤差時(shí)，隨著時(shí)間的推移，控制器不能很好地消除誤差，導(dǎo)致姿態(tài)角的誤差越來(lái)越大，滾轉(zhuǎn)角最大誤差達(dá)到20°、俯仰角誤差達(dá)到5°、偏航角的較大誤差使得飛行器已經(jīng)脫離了飛行任務(wù)的航跡。

圖2 未經(jīng)過(guò)卡爾曼濾波的姿態(tài)角變化Fig.2 Attitude variation without EKF

圖3 未加入濾波姿態(tài)角真實(shí)值與估計(jì)值間的誤差Fig.3 Attitude deviation without EKF between estimation and true value

圖4是加入卡爾曼濾波后的姿態(tài)估計(jì)值和真實(shí)值，圖5是加入濾波姿態(tài)角真實(shí)值與估計(jì)值間的誤差。從圖5中可以看出，在飛行過(guò)程中誤差比較小，滾轉(zhuǎn)角的誤差在2°以?xún)?nèi)，而俯仰角的誤差也在2.5°以?xún)?nèi)，偏航角在有最大2°左右的誤差后逐漸收斂為0。

對(duì)比之后可以看出，利用離散型擴(kuò)展卡爾曼濾波對(duì)姿態(tài)角進(jìn)行估計(jì)的算法，對(duì)飛行器導(dǎo)航系統(tǒng)的準(zhǔn)確性有了較大的提升。

圖4 加入濾波姿態(tài)角變化圖Fig.4 Attitude variation with EKF

圖5 加入濾波姿態(tài)角真實(shí)值與估計(jì)值間的誤差Fig.5Attitude deviation with EKF between estimation and true value

5 結(jié)束語(yǔ)

本文針對(duì)低成本的小型固定翼無(wú)人機(jī)，利用擴(kuò)展卡爾曼濾波理論，結(jié)合MEMS慣導(dǎo)系統(tǒng)和GPS導(dǎo)航系統(tǒng)，建立了進(jìn)行姿態(tài)角估計(jì)的卡爾曼濾波方程，根據(jù)MEMS和GPS的誤差模型建立系統(tǒng)方程，根據(jù)加速度計(jì)和GPS速度差分計(jì)算得到的加速度計(jì)算姿態(tài)角得到量測(cè)方程，并將該方法帶入飛行器的數(shù)學(xué)仿真中，進(jìn)行閉環(huán)仿真驗(yàn)證?？梢钥闯?，加入濾波比不加入濾波姿態(tài)角的誤差要小一個(gè)量級(jí)，取得了很好的姿態(tài)估計(jì)精度結(jié)果，說(shuō)明該姿態(tài)估計(jì)方法是針對(duì)成本較低的固定翼無(wú)人機(jī)是可靠的。

[1] Kalman R E. A new approach to linear filtering and prediction problems[J]. Journal of Basic Eng （ASME）， 1960（82D）： 35-46.

[2] Schmidt S F. The Kalman filter： Its recognition and development for aerospace application[J]. Journal of Guidance and Control，1981， 4（1）： 4-7.

[3] 秦永元，張洪鉞，汪叔華.卡爾曼濾波與組合導(dǎo)航原理[M].西安：西北工業(yè)大學(xué)出版社，2015：33-35.QIN Yongyuan， ZHANG Hongyue， WANG Shuhua. Kalman filtering and combined navigation principle[M]. Xi’an： Northwestern Polytechnical University Press， 2015：33-35.（in Chinese）

[4] Farrenkopf R L. Analytic steady-state accuracy solution for two common spacecraft attitude estimators[J]. Journal of Guidance and Control， 1978， 1（4）： 282-284.

[5] Gleason S， Egziabher G. GNSS application and methods[M].Boston： Artech House Publishers， 2009： 105-107.

[6] John L C， Markley F L， Cheng Y. Survey of nonlinear attitude estimation methods[J]. Journal of Guidance， Control and Dnamics， 2007（30）： 14-16.

[7] Luis S， Donghyun K， Ricichard B L. A GPS velocity sensor：How accurate can it be?-a first look[C]//ION NTM 2004， San Diego， 2004：875-885.

[8] 王甫紅，張小紅，黃勁松. GPS單點(diǎn)測(cè)速的誤差分析及精度評(píng)價(jià) [J].武漢大學(xué)學(xué)報(bào)，2007（6）：515-519.WANG Fuhong， ZHANG Xiaohong， HUANG Jinsong. Error analysis and accuracy evaluation of GPS single point velocity measurement[J]. Journal of Wuhan University， 2007（6）： 515-519.（in Chinese）

[9] Psiaki M L， Powell S P， Kintner P M. The accuracy of the GPS-derived acceleration vector， a novel attitude reference[C]//Proceedings of Guidance， Navigation， and Control Conference，Portland， 1999：751-760.

[10] Pinder S D， Crowe T G， Nikiforuk P N. Application of the global positioning system in determination of vehicular acceleration[J]. AIAA Aircraft， 2001（38）： 856-859.

[11] Li Y， Dempster A， Li B， et al. A low-cost attitude heading reference system by combination of GPS and magnetometers and MEMS inertial sensors for mobile applications[J]. Journal of Global Positioning System， 2006（6）： 88-95.

[12] Godha S. Performance evaluation of low cost MEMS-based IMU integrated with GPS for land vehicle navigation application[D].Canada： The University of Calgary， 2006.