基于ARM與MEMS的AHRS設(shè)計(jì)

　　四元數(shù)姿態(tài)矩陣微分方程只要解4個(gè)微分方程，比方向余弦姿態(tài)矩陣微分方程減少了大量的運(yùn)算，便于微處理器的編程實(shí)現(xiàn)。

　　2.1　時(shí)間更新

　　系統(tǒng)的狀態(tài)方程如式(5)所示。

　　其中狀態(tài)變量為四元數(shù)X=[q0,q1,q2,q3]T，Wk-1為四維過(guò)程噪聲。矩陣A可以根據(jù)陀螺儀測(cè)得的三軸角速率[ωX，ωY，ωZ]T得到，如式(6)所示。其中Δt為兩次時(shí)間預(yù)測(cè)更新所流逝的時(shí)間。

　　狀態(tài)變量的時(shí)間更新如式(7)所示。

　　協(xié)方差矩陣P預(yù)測(cè)如式(8)所示，式中Q為四維過(guò)程激勵(lì)噪聲協(xié)方差。

　　2.2　觀測(cè)更新

　　AHRS的觀測(cè)更新是通過(guò)本體坐標(biāo)系上的重力加速度和地磁場(chǎng)的參考矢量旋轉(zhuǎn)至導(dǎo)航坐標(biāo)系上，再與加速度和磁場(chǎng)傳感器比較，得到觀測(cè)變量的殘余。由本體系轉(zhuǎn)換至導(dǎo)航系的轉(zhuǎn)移矩陣由四元數(shù)可以表示為式(9)。

　　三維參考向量v轉(zhuǎn)移至導(dǎo)航系中可由觀測(cè)方程式(10)表示。

　　當(dāng)重力加速度觀測(cè)更新時(shí)參考向量v等于重力加速度參考矢量(可設(shè)置為當(dāng)平臺(tái)靜止水平放置時(shí)，加速度計(jì)測(cè)量得到的三維矢量為：

　　當(dāng)磁場(chǎng)觀測(cè)更新時(shí)v等于磁場(chǎng)參考矢量(可設(shè)置為當(dāng)平臺(tái)靜止水平放置且航向指向正北時(shí)，磁阻計(jì)測(cè)量得到的三維矢量為：

　　H是h對(duì)X求偏導(dǎo)的雅可比矩陣，如式(11)所示。