王丁偉，祖家奎*，黃 海

(1.南京航空航天大學(xué)自動(dòng)化學(xué)院，南京210016;2.中創(chuàng)航空技術(shù)有限公司，浙江嘉興314000)

基于MAEKF算法的航姿參考系統(tǒng)設(shè)計(jì)

王丁偉1，祖家奎1*，黃 海2

(1.南京航空航天大學(xué)自動(dòng)化學(xué)院，南京210016;2.中創(chuàng)航空技術(shù)有限公司，浙江嘉興314000)

構(gòu)建了以低成本MEMS陀螺儀、加速度計(jì)和磁傳感器組合的航姿參考系統(tǒng)，提出了一個(gè)乘性自適應(yīng)擴(kuò)展卡爾曼濾波算法。取乘性誤差四元數(shù)和陀螺儀誤差作為狀態(tài)量，基于重力場(chǎng)和磁場(chǎng)構(gòu)造了量測(cè)矢量，用于修正航姿數(shù)據(jù)。并采用準(zhǔn)確量測(cè)法，給濾波器加入了四元數(shù)的歸一化約束，最后給出了基于新息的估計(jì)量測(cè)噪聲方差矩陣的公式。通過(guò)仿真和試飛驗(yàn)證，表明本文設(shè)計(jì)的低成本的航姿參考系統(tǒng)能夠提供比較準(zhǔn)確的航姿信息。與常規(guī)的擴(kuò)展卡爾曼濾波器比較，本文設(shè)計(jì)的乘性自適應(yīng)擴(kuò)展卡爾曼濾波算法有效提高了系統(tǒng)的精度和穩(wěn)定性，并且具有較好的魯棒性。

航姿參考;乘性擴(kuò)展卡爾曼濾波;自適應(yīng);誤差四元數(shù)

航姿參考系統(tǒng)能夠準(zhǔn)確測(cè)量載體的空間姿態(tài)信息。在三維空間中，通常采用俯仰角、滾轉(zhuǎn)角、航向角來(lái)描述其姿態(tài)信息。在軍事、航空航天、航海等領(lǐng)域多采用高精度的慣性測(cè)量元件，但其價(jià)格昂貴。隨著價(jià)格低廉的MEMS慣性傳感器的發(fā)展，低成本的航姿參考系統(tǒng)廣泛應(yīng)用于消費(fèi)類的無(wú)人機(jī)、室內(nèi)導(dǎo)航、智能穿戴設(shè)備、游戲手柄等領(lǐng)域。低成本的航姿參考系統(tǒng)一般由MEMS陀螺儀、加速度計(jì)、磁傳感器組成［1］。但是，MEMS陀螺儀存在嚴(yán)重的零點(diǎn)漂移、隨機(jī)誤差，直接對(duì)原始數(shù)據(jù)進(jìn)行積分得到的角度會(huì)產(chǎn)生很大的累積誤差，并且該誤差會(huì)隨著時(shí)間不斷增大［2］。因此，輔以加速度計(jì)和磁傳感器測(cè)量的重力場(chǎng)、地磁場(chǎng)來(lái)補(bǔ)償MEMS陀螺儀的誤差能夠顯著提高航姿數(shù)據(jù)精度［3］。

目前廣泛采用的航姿融合算法主要分為互補(bǔ)濾波、擴(kuò)展卡爾曼濾波兩大類。從融合效果看，互補(bǔ)濾波的精度不高，且動(dòng)態(tài)響應(yīng)速度不如卡爾曼濾波迅速［4］。文獻(xiàn)［1，5］分別提出各自的互補(bǔ)濾波算法，前者采用PI調(diào)節(jié)器，先使用加速度計(jì)數(shù)據(jù)補(bǔ)償陀螺儀數(shù)據(jù)，然后再融合磁場(chǎng)數(shù)據(jù);后者采用向量叉乘解算陀螺儀誤差，并且能夠直接運(yùn)用于未校準(zhǔn)的傳感器，能夠獲取相對(duì)較好的姿態(tài)數(shù)據(jù)。但是兩者的融合精度都不是很好。文獻(xiàn)［4，6］設(shè)計(jì)了基于四元數(shù)的擴(kuò)展卡爾曼濾波器，前者著重介紹了使用MPU9250作為MEMS九軸姿態(tài)傳感器，實(shí)現(xiàn)EKF算法;后者先用重力場(chǎng)補(bǔ)償陀螺儀誤差，再用磁場(chǎng)修正偏航角，接著構(gòu)造了自適應(yīng)的量測(cè)噪聲方差矩陣，但是其本質(zhì)仍然還是加性擴(kuò)展卡爾曼濾波器，脫離了四元數(shù)歸一化的約束，濾波器極易發(fā)散。而乘性擴(kuò)展卡爾曼濾波器能夠較好的保持四元數(shù)的歸一化約束，文獻(xiàn)［2，7］以乘性誤差四元數(shù)作為狀態(tài)量，設(shè)計(jì)了乘性擴(kuò)展卡爾曼濾波器。其中，文獻(xiàn)［2］僅僅采用磁場(chǎng)信息修正陀螺儀誤差，這樣的確避免了振動(dòng)和載體運(yùn)動(dòng)導(dǎo)致的干擾，但是，磁力計(jì)易受到外部磁場(chǎng)干擾，導(dǎo)致姿態(tài)數(shù)據(jù)錯(cuò)誤;文獻(xiàn)［7－8］則是加入了加速度計(jì)，但是并未給出如何設(shè)計(jì)量測(cè)噪聲方差矩陣。

針對(duì)上述眾多問(wèn)題，本文設(shè)計(jì)了一種可靠的航姿參考系統(tǒng)，該系統(tǒng)采用MAEKF算法進(jìn)行航姿融合，適用于量測(cè)噪聲多變的復(fù)雜環(huán)境，并且通過(guò)仿真和試飛測(cè)試驗(yàn)證了該航姿參考系統(tǒng)的可靠性。

1 系統(tǒng)結(jié)構(gòu)

本文將MEMS陀螺儀、加速度計(jì)、磁傳感器的數(shù)據(jù)作為MAEKF的輸入。其中，加速度計(jì)的原始數(shù)據(jù)需要經(jīng)過(guò)低通濾波器濾除高頻噪聲和震動(dòng)干擾。本文設(shè)計(jì)的航姿參考系統(tǒng)結(jié)構(gòu)和實(shí)物圖，如圖1所示。

圖1 航姿參考系統(tǒng)結(jié)構(gòu)圖和實(shí)物圖

三軸陀螺儀數(shù)據(jù)驅(qū)動(dòng)誤差四元數(shù)的更新，通過(guò)加速度計(jì)和磁傳感器所測(cè)得的重力場(chǎng)和磁場(chǎng)來(lái)修正，從而得到誤差四元數(shù)的最優(yōu)估計(jì)。用該誤差四元數(shù)修正陀螺儀數(shù)據(jù)積分的四元數(shù)，再通過(guò)姿態(tài)解算，得到精度較高的姿態(tài)角數(shù)據(jù)。該系統(tǒng)采用MPU6050和HMC5803作為姿態(tài)傳感器，STM32F429作為核心處理器。

2 航姿融合算法設(shè)計(jì)

2.1 乘性誤差四元數(shù)

姿態(tài)解算算法中，有多種參數(shù)化方法，比如歐拉角、四元數(shù)、改進(jìn)的Rodrigues參數(shù)、等效旋轉(zhuǎn)矢量等［9］。由于四元數(shù)的非奇異性、計(jì)算量小等優(yōu)點(diǎn)，使其廣泛應(yīng)用于姿態(tài)解算算法中。四元數(shù)必須滿足歸一化約束，然而標(biāo)準(zhǔn)EKF方法釋放了四元數(shù)的歸一化約束。為了克服這種缺陷，本文采用乘性誤差四元數(shù)方法。

對(duì)式(1)兩邊求微分得到:

由四元數(shù)運(yùn)動(dòng)學(xué)方程可得:

綜上可得誤差四元數(shù)運(yùn)動(dòng)學(xué)方程:

2.2 系統(tǒng)狀態(tài)方程設(shè)計(jì)

2.2.1 MEMS陀螺儀建模

系統(tǒng)狀態(tài)方程主要描述了陀螺儀輸出的角速率值和誤差四元數(shù)之間的動(dòng)力學(xué)關(guān)系。因此，設(shè)計(jì)狀態(tài)方程之前，首先要完成MEMS陀螺儀的建模。

陀螺儀的誤差主要來(lái)源于隨機(jī)漂移和噪聲，一種廣泛采用的數(shù)學(xué)模型是一階馬爾可夫過(guò)程，其主要形式如下:

式中:ηv和 ηu分別表示具有譜密度σ2vI3×3和 σ2uI3×3的零均值高斯白噪聲過(guò)程;β為偏置向量;珟ω為測(cè)量觀測(cè)值［9］。

估計(jì)的角速度為

2.2.2 建立狀態(tài)方程

式中:

2.3 量測(cè)方程設(shè)計(jì)

2.3.1 姿態(tài)誤差的測(cè)量

EKF中的量測(cè)方程能夠起到修正狀態(tài)預(yù)測(cè)值的作用，在許多關(guān)于AHRS的研究中，狀態(tài)方程的建立大多不盡相同，主要區(qū)別在于量測(cè)方程的量測(cè)量的選取中。本文選用符號(hào)p和m表示狀態(tài)量的預(yù)測(cè)值和量測(cè)值，e表示預(yù)測(cè)值和量測(cè)值的外積，θe表示兩個(gè)向量的內(nèi)積，則

式中:向量p和m均為單位向量。

通過(guò)式(12)、式(13)構(gòu)建姿態(tài)誤差向量:

乘性誤差四元數(shù)與姿態(tài)誤差向量之間的關(guān)系為［7］:

本文選用了加速度計(jì)和磁傳感器作為量測(cè)的傳感器。接下來(lái)，分別對(duì)兩個(gè)傳感器設(shè)計(jì)量測(cè)方程。

2.3.2 加速度計(jì)的量測(cè)設(shè)計(jì)

加速度計(jì)的輸出主要包含重力加速度、運(yùn)動(dòng)加速度以及噪聲，可用如下形式表達(dá):

式中:am表示加速度計(jì)輸出，Cbng表示重力向量在機(jī)體軸下的投影，表示運(yùn)動(dòng)加速度，b表示加速度計(jì)的噪聲。

通過(guò)重力加速度在機(jī)體軸下的分量可以計(jì)算出，當(dāng)前載體的滾轉(zhuǎn)角和俯仰角。因此，當(dāng)載體的運(yùn)動(dòng)加速度較小時(shí)，式(16)可以近似為:

式中:gb表示重力加速度在機(jī)體軸下的投影。

顯然，gb=Cbng是成立的。其中:

由式(12)、式(13)得:

式中:am需要?dú)w一化處理。

2.3.3 磁傳感器的量測(cè)設(shè)計(jì)

磁傳感器測(cè)量的是三軸的磁場(chǎng)強(qiáng)度，其輸出形式如下:

式中:mm表示磁傳感器的輸出，m0表示當(dāng)?shù)氐牡卮艔?qiáng)度，b表示磁傳感器的噪聲。

同理，由式(12)、式(13)得:

2.3.4 準(zhǔn)確量測(cè)法的狀態(tài)約束

由于乘性誤差四元數(shù)是嚴(yán)格歸一化的。因此，在濾波過(guò)程中，必須滿足歸一化約束才能有效的防止濾波器的發(fā)散［10］。其歸一化方程為:

將式(23)作為狀態(tài)量的偽量測(cè)方程，引入到系統(tǒng)的量測(cè)中，構(gòu)成系統(tǒng)的量測(cè)方程。

2.3.5 建立量測(cè)方程

由式(15)、式(23)可知，量測(cè)方程中包含有量測(cè)量和狀態(tài)約束兩個(gè)部分。系統(tǒng)的量測(cè)方程為:

式中:

2.4MEKF算法

基于MEKF的姿態(tài)融合算法區(qū)別于AEKF(加性擴(kuò)展卡爾曼濾波)的地方在于:AEKF直接選用姿態(tài)四元數(shù)作為濾波器的狀態(tài)量，姿態(tài)誤差采用加性修正，這種加性修正很容易破壞姿態(tài)四元數(shù)的歸一化約束，降低濾波精度，更嚴(yán)重可能導(dǎo)致濾波器發(fā)散［9，12－13］。而MEKF雖然在形式上比AEKF復(fù)雜一點(diǎn)，但是，卻能夠很好的保持姿態(tài)四元數(shù)的歸一化約束。

狀態(tài)四元數(shù)傳播方程:

狀態(tài)預(yù)測(cè)方程:

協(xié)方差預(yù)測(cè)方程:

卡爾曼增益方程:

信息方程:

狀態(tài)更新:

協(xié)方差更新方程:

狀態(tài)量歸一化:

姿態(tài)四元數(shù)修正:

姿態(tài)四元數(shù)歸一化

注意:MEKF迭代過(guò)程中，每次姿態(tài)四元數(shù)修正結(jié)束后，誤差四元數(shù)需要重置為初始值。

3 航姿融合算法實(shí)現(xiàn)

本文構(gòu)建了兩組量測(cè)向量，Murrell對(duì)這種濾波器進(jìn)行了改進(jìn)［9］。該算法每次迭代過(guò)程僅需要處理一組3×1的量測(cè)向量。對(duì)于本文中設(shè)計(jì)的算法，一次完整的修正過(guò)程需要連續(xù)處理兩組量測(cè)向量。

從圖2可以看出，當(dāng)系統(tǒng)中設(shè)計(jì)了n個(gè)量測(cè)向量，該算法采用序貫方式一個(gè)一個(gè)處理量測(cè)向量。這樣處理的優(yōu)點(diǎn)在于:降低了量測(cè)方程的維度，在進(jìn)行增益解算時(shí)，矩陣逆的計(jì)算復(fù)雜度大大降低，能夠有效提高算法實(shí)時(shí)性。另外，由于不同傳感器的采樣頻率不同，因此這種序貫處理方式使得只有傳感器采樣到新值時(shí)才進(jìn)行量測(cè)更新，避免了舊值的重復(fù)計(jì)算。

4 自適應(yīng)量測(cè)噪聲R陣設(shè)計(jì)

卡爾曼濾波器的公式中主要有兩個(gè)噪聲矩陣，分別是系統(tǒng)噪聲矩陣Q和量測(cè)噪聲矩陣R。就系統(tǒng)噪聲而言，其決定于系統(tǒng)內(nèi)部機(jī)理，噪聲參數(shù)相對(duì)穩(wěn)定，可以通過(guò)大量實(shí)驗(yàn)來(lái)實(shí)現(xiàn)精確測(cè)試或建模;但是量測(cè)噪聲不僅僅來(lái)源于MEMS傳感器的誤差和噪聲，還受到外部環(huán)境以及載體運(yùn)動(dòng)狀態(tài)等情況影響［14］。因此量測(cè)噪聲是比較容易變化的，有太多的不確定性，難以建模。因此，需要算法對(duì)量測(cè)噪聲進(jìn)行實(shí)時(shí)的自適應(yīng)估計(jì)，這樣才能保證濾波器的精度，防止濾波器發(fā)散。

Sage-Husa自適應(yīng)擴(kuò)展卡爾曼濾波算法(SHAEKF)在理論上實(shí)現(xiàn)了在進(jìn)行狀態(tài)估計(jì)的同時(shí)還可在線自適應(yīng)估計(jì)系統(tǒng)噪聲和量測(cè)噪聲統(tǒng)計(jì)特性的功能。其Q、R陣的自適應(yīng)算法如下:

在實(shí)際運(yùn)用中，SHAEKF算法所估計(jì)出來(lái)的系統(tǒng)噪聲和量測(cè)噪聲方差序列分別與真實(shí)噪聲方差存在差值誤差，特別是系統(tǒng)噪聲的估計(jì)偏離更大，極易導(dǎo)致對(duì)量測(cè)值的權(quán)重過(guò)高，等效于狀態(tài)方程失效，大大降低了濾波精度，甚至?xí)?dǎo)致濾波器發(fā)散［14］。

本文采用改進(jìn)型的SHAEKF算法估計(jì)R陣，其公式如下:

式中:b為漸消因子，一般取b=0.9～0.999。

5 仿真與試飛驗(yàn)證

5.1 仿真條件

本文使用MATLAB對(duì)MIMU的源數(shù)據(jù)進(jìn)行處理。MIMU傳感器由MPU6050和HMC5803組合而成，處理器選用了意法半導(dǎo)體(ST)公司Cortex－M4架構(gòu)的STM32F429(圖1)。設(shè)置MPU6050采樣頻率500 Hz，HMC5803為75 Hz，數(shù)據(jù)上傳頻率為100 Hz。因此，在MATLAB下仿真時(shí)，算法解算周期為0.01 s。

5.2 仿真結(jié)果

根據(jù)上述條件，對(duì)MIMU靜置1 h，在MATLAB下對(duì)采集到的數(shù)據(jù)進(jìn)行了MAEKF算法和傳統(tǒng)EKF算法的姿態(tài)解算，取中間50 s數(shù)據(jù)，如圖3和圖4所示。

圖3 俯仰角靜態(tài)圖

圖中，橫軸為時(shí)間軸，單位是s，縱軸表示航姿數(shù)據(jù)，單位是(°)。從圖中可以看出，本文設(shè)計(jì)的MAEKF算法的靜態(tài)精度比傳統(tǒng)的EKF算法精度要高。MAEKF算法的俯仰角和滾轉(zhuǎn)角靜態(tài)精度分別達(dá)到了0.016°和0.013°。

仿真中，對(duì)量測(cè)噪聲方差矩陣R取了二范數(shù)，根據(jù)本文的自適應(yīng)算法，其曲線如圖5所示。圖5中R陣初始值給的是單位矩陣，最終，其二范數(shù)收斂至0.166 7。

圖4 滾轉(zhuǎn)角靜態(tài)圖

圖5 R陣估計(jì)結(jié)果圖

5.3 試飛驗(yàn)證與分析

試飛驗(yàn)證的飛行器為一個(gè)三軸六槳飛行器，如圖6所示。該飛行器搭載有本文設(shè)計(jì)的AHRS傳感器，該傳感器中同時(shí)運(yùn)行了標(biāo)準(zhǔn)擴(kuò)展卡爾曼濾波算法(EKF)和本文設(shè)計(jì)的乘性自適應(yīng)擴(kuò)展卡爾曼濾波算法(MAEKF)，通過(guò)uCOS－Ⅱ來(lái)調(diào)度，算法解算頻率均為100 Hz。另外，飛行器上還搭載了一款參考IMU，型號(hào)為IMU520，能夠提供精確的滾轉(zhuǎn)角和俯仰角。

圖6 飛行器實(shí)物圖

如圖7、圖8給出參考IMU的輸出、EKF算法的輸出、文中設(shè)計(jì)的MAEKF算法的輸出曲線。

圖7 試飛俯仰角曲線圖

圖8 試飛滾轉(zhuǎn)角曲線圖

圖9給出了兩種算法的誤差對(duì)比曲線圖。本文設(shè)計(jì)的航姿融合算法一般的擴(kuò)展卡爾曼濾波算法精度要高很多，能夠較好的適應(yīng)不確定的量測(cè)噪聲。

圖9 MAEKF與EKF誤差比較曲線

表1中統(tǒng)計(jì)了兩種算法角度誤差的最大值、平均值和標(biāo)準(zhǔn)差。與EKF算法相比，MAEKF算法產(chǎn)生的最大誤差和標(biāo)準(zhǔn)差更小，較小的最大誤差說(shuō)明了該算法的具有更好的精度，而較小的標(biāo)準(zhǔn)差則說(shuō)明了在具有量測(cè)擾動(dòng)的情況下，該算法更加穩(wěn)定，具有更好的魯棒性。

圖10給出了飛行過(guò)程中，飛行器機(jī)體軸3個(gè)軸向的加速度信息?？梢钥闯?，飛行過(guò)程中有無(wú)刷電機(jī)導(dǎo)致的高頻振動(dòng)。常規(guī)的EKF算法直接采用姿態(tài)四元數(shù)作為狀態(tài)量，且采用常值R陣，不能適應(yīng)噪聲多變的飛行環(huán)境;而本文設(shè)計(jì)的MAEKF算法，采用自適應(yīng)R陣，根據(jù)新息估計(jì)量測(cè)噪聲的方差，同時(shí)，采用了直接觀測(cè)法引入了四元數(shù)的歸一化約束，提高了航姿融合算法的濾波精度和魯棒性。

表1 EKF和MAEKF比較

圖10 試飛加速度曲線圖

6 結(jié)論

本文從實(shí)際應(yīng)用角度出發(fā)，設(shè)計(jì)了一個(gè)低成本航姿參考系統(tǒng)。其中包括MAEKF算法，該算法通過(guò)采用約束濾波和自適應(yīng)的量測(cè)噪聲矩陣，與常規(guī)的EKF算法比較，仿真與試飛驗(yàn)證的結(jié)果表明，MAEKF算法不僅有更高的精度，而且對(duì)不確定的量測(cè)噪聲具有更好的魯棒性。在航姿參考應(yīng)用領(lǐng)域具有很好的應(yīng)用前景。

［1］ 杜彬彬，吳昊，張繼文，等.一種面向AHRS的改進(jìn)互補(bǔ)濾波融合算法［J］.國(guó)外電子測(cè)量技術(shù)，2015，34(3):13－18.

［2］ 薛亮，李天志，李曉瑩，等.基于MEMS傳感器的微型姿態(tài)確定系統(tǒng)研究［J］.傳感技術(shù)學(xué)報(bào)，2008，21(3):457－460.

［3］ 趙代弟.MARG姿態(tài)測(cè)量系統(tǒng)抗干擾算法研究［D］.2015.

［4］ 劉春陽(yáng)，徐軍領(lǐng)，程洪濤，等.MPU9250傳感器的姿態(tài)檢測(cè)與數(shù)據(jù)融合［J］.河南科技大學(xué)學(xué)報(bào)，2015，36(4):14－17.

［5］ Hamel T，Mahony R.Attitude Estimation on SO(3)Based onDirect Inertial Measurements［J］.IEEE International Conference on Robotics and Automation，2006:2170－2175.

［6］ 劉興川，張盛，李麗哲，等.基于四元數(shù)的MARG傳感器姿態(tài)測(cè)量算法［J］.清華大學(xué)學(xué)報(bào)，2012，52(5):627－631.

［7］ Mohammad A，Alireza K，Paknush K.Orientation Estimation by Error-State Extended Kalman Filter in Quaternion vector space［J］.SICE，2007 Annual Conference，2007:60－67.

［8］ 葉锃鋒，馮恩信.基于四元數(shù)和卡爾曼濾波的兩輪姿態(tài)穩(wěn)定方法［J］.傳感技術(shù)學(xué)報(bào)，2012，25(4):524－528.

［9］ John L Crassidis，John L Junkins.動(dòng)態(tài)系統(tǒng)最優(yōu)估計(jì)［M］.2版.北京:國(guó)防工業(yè)出版社，2016，1:357－365.

［10］薛亮，姜澄宇，常洪龍，等.基于狀態(tài)約束的MIMU/磁強(qiáng)計(jì)組合姿態(tài)估計(jì)濾波算法［J］.中國(guó)慣性技術(shù)學(xué)報(bào)，2009，17(3): 338－343.

［11］ James K Hall，Nathan B Knoebel，Timothy W McLain.Quaternion Attitude Estimation for Miniature Air Vehicles Using a Multiplicative Extended Kalman Filter［J］.IEEE Position，Location and Navigation Symposium，2008:1230－1237.

［12］李翔.基于MARG傳感器的AHRS關(guān)鍵技術(shù)研究［D］.2013.

［13］李建國(guó)，催祜濤，田陽(yáng)，等.基于乘性四元數(shù)和約束濾波的飛行器姿態(tài)估計(jì)算法［J］.系統(tǒng)工程與電子技術(shù)，2013，35(5): 1031－1036.

［14］張欣.多旋翼無(wú)人機(jī)的姿態(tài)與導(dǎo)航信息融合算法研究［D］.2015.

王丁偉(1991－)，男，江蘇揚(yáng)州人，碩士研究生，主要研究方向?yàn)闊o(wú)人機(jī)飛行控制，812724408@qq.com;

祖家奎(1974)，男，江蘇徐州人，副研究員，主要從事無(wú)人直升機(jī)飛行控制技術(shù)、多變量控制技術(shù)及其應(yīng)用、系統(tǒng)仿真與建模技術(shù)、嵌入式系統(tǒng)開(kāi)發(fā)等方向，zujiakui@nuaa.edu.cn。

The Design of AHRS Based on MAEKF

WANG Dingwei1，ZU Jiakui1*，HUANG Hai2
(1.College of Automation Engineering，Nanjing University of Aeronautics and Astronautics，Nanjing 210016，China; 2.ZhongChuang Aviation Technology Co.Ltd，Jiaxing Zhejiang 314000，China)

An attitude and heading reference system(AHRS)is designed which is composed of low cost MEMS gyroscopes，accelerometers and magnetometer and a multiplicative adaptive extended kalman filter algorithm(MAEKF)is proposed based on the gravity field and magnetic field constructed measurement vector.In order to correct attitude and heading data，the MAEKF algorithm selects quaternion error and bias of gyroscopes as state vector.Then，the accurate measurement method is adopted to provide the filterwith normalized constraint.Furthermore，this paper presents an innovation-based formula which is used to estimate measurement noise covariance matrix.Finally，simulation and flight-test show that the designed AHRS can provide accurate attitude and heading data.Compared with traditional extended kalman filter(EKF)，the MAEKF algorithm effectively improves the accuracy，stability and robustness of the system.

attitude and heading reference system;multiplicative extended kalman filter;adaptive;quaternion error EEACC:2575;7230;7630 doi:10.3969/j.issn.1004－1699.2017.02.026

10.3969/j.issn.1004－1699.2017.02.026

TJ765

A

1004－1699(2017)02－0319－07

2016－08－03 修改日期:2016－09－22