姿態(tài)解算理解

1,、姿態(tài)的描述方法

前幾天在論壇里偶爾看到一個帖子,，帖子的內(nèi)容是問的為什么不用傾斜角表示姿態(tài)，我認為他說的傾斜角是指的斜面與斜面的夾角,，或者說是物體與垂線的夾角吧,，這種想法可能來源于我們?nèi)粘Ｉ畹乃季S。

圖1  立方體

比如有一個立方體,，我們放在水平面上的時候它的底面是和水平面平行的,，但是當(dāng)我們把立方體的一個腳墊起一個角度時，這樣一來,，立方體的一條棱與水平面的垂線就有了一定的夾角了,。我們所說傾斜了多少多少度就是指的這個夾角，這是我們直觀的反應(yīng),。我認為這樣直觀的反應(yīng)甚至比歐拉角還要來的直觀,，因為歐拉角是基于旋轉(zhuǎn)的，肯定不會說這個立方體X,、Y軸各旋轉(zhuǎn)了多少度（假設(shè)Z旋轉(zhuǎn)無效）,，我們可能也沒那個概念，我們直觀的反應(yīng)就是它傾斜了一定度數(shù)。

但是我們在姿態(tài)解算的時候為啥不用的這種描述方法呢,，個人認為是雖然方便我們直觀的表達但不適合數(shù)學(xué)上的計算,，還有就是我們僅僅知道這個傾斜角我們怎么施加控制量呢？高中物理學(xué)習(xí)物體運動的時候我們知道,，物體的運動是合運動,，我們可以把它的運動矢量正交分解為幾個運動的合成（不正交也是可以的，但那不是在自找麻煩嗎）,，同樣道理,，我們可以把剛體的旋轉(zhuǎn)分解為三個軸上的旋轉(zhuǎn)，這個旋轉(zhuǎn)的角度就是歐拉角,，如圖2,。

圖2  zxz序規(guī)歐拉角

歐拉角

歐拉角的定義不僅僅和旋轉(zhuǎn)角度有關(guān)系，還和旋轉(zhuǎn)軸的旋轉(zhuǎn)順序有關(guān)系,，任何一種旋轉(zhuǎn)順序都是合法的,。根據(jù)定義，歐拉角有12種旋轉(zhuǎn)順序（維基）,，一個物體通過任意一個旋轉(zhuǎn)順序都可以達到同樣的姿態(tài),，在各個學(xué)科里所以為了統(tǒng)一，航空航天領(lǐng)域規(guī)定XYZ為歐拉角的旋轉(zhuǎn)順序,。

上面已經(jīng)說了歐拉角的定義,。歐拉角的定義也是很直觀而且容易理解的，也利于我們的計算,，因為我們用的慣性器件也是按照單個軸向運動來測量的,。定義上的歐拉角還和我們所說的Yaw、Pitch,、Roll不是一回事,。因為定義上的歐拉角就是剛體繞三個軸的旋轉(zhuǎn)角度，歐拉旋轉(zhuǎn)和外界的東西（參考系）是沒有關(guān)系的,。Yaw,、Pitch、Roll就是載體對于參考系來說的了,，這意味著第一次的旋轉(zhuǎn)不會影響第二,、三次的轉(zhuǎn)軸，因為選的參考系都是地球了（這也是產(chǎn)生Gimbal Lock的原因,，下面會講到）,。

我們現(xiàn)在說的飛機都是在近地表附近飛行，所以我們習(xí)慣是拿地球作為參考系,，我們的飛機總是在一點起飛在另一點降落。所以我們規(guī)定地理方位東、北,、天為參考初始點,，也就是說，我們的飛機頭朝北水平放置時載體坐標(biāo)系和參考坐標(biāo)系是重合的,，那么接下來我們繞飛機的Z軸旋轉(zhuǎn)30°,，這個旋轉(zhuǎn)的歐拉角就是我們所說的Yaw，同樣,，繞飛機的X軸旋轉(zhuǎn)30°,，我們得到Pitch，繞飛機Y軸旋轉(zhuǎn)得到Roll,，如圖3,。

圖3  

這和規(guī)定歐拉角的旋轉(zhuǎn)順序是一樣的，所以說是和歐拉角是一一對應(yīng)的,，要注意歐拉角是基于飛機本身軸旋轉(zhuǎn)得到的,，但是得到的姿態(tài)卻是對于參考坐標(biāo)系而說的，要不然對我們來說也沒有實際的意義,，來個更直觀的：

歐拉角是有很多優(yōu)點的,。但是也有致命的缺點，那就是Gimbal Lock（萬向節(jié)死鎖）,，要理解Gimbal Lock所說的情況,，這可能有點不好理解，讓我們看個現(xiàn)實中的場景,。

假如我們有一個望遠鏡和一個用來放望遠鏡的三腳架,，（我們將）三腳架放在地面上，使支撐望遠鏡的三腳架的頂部是平行于地平面（參考平面）的,，以便使得豎向的旋轉(zhuǎn)軸（記為x軸）是完全地垂直于地平面的?，F(xiàn)在，我們就可以將望遠鏡饒x軸旋轉(zhuǎn)360度,，從而觀察（以望遠鏡為中心的）水平包圍圈的所有方向,。通常將正北朝向方位角度記為0度方位角。第二個坐標(biāo)軸,，即平行于地平面的橫向的坐標(biāo)軸（記為y軸）使得望遠鏡可以饒著它上下旋轉(zhuǎn),，通常將地平面朝向的仰角記為0度，這樣,，望遠鏡可以向上仰+90度指向天頂,，或者向下-90度指向腳底。好了,，萬事俱備?，F(xiàn)在,，天空中（包括地面上）的每個點只需要唯一的一對x和y度數(shù)就可以確定。比如x=90度,y=45度指向的點是位于正東方向的半天空上?，F(xiàn)在,，看看萬向節(jié)死鎖是怎么發(fā)生的。一次,，我們探測到有一個飛行器貼地飛行,，位于望遠鏡的正東方向（x=90度，y=10度）,，朝著我們直飛過來,，我們跟蹤它。飛行器飛行方向是保持x軸角度90度不變,，而y向的角度在慢慢增大,。隨著飛行器的臨近，y軸角增長的越來越快且當(dāng)y向的角度達到90度時（即將超越）,，突然它急轉(zhuǎn)彎朝南飛去,。這時，我們發(fā)現(xiàn)我們不能將望遠鏡朝向南方,，因為此時y向已經(jīng)是90度,，造成我們失去跟蹤目標(biāo)。這就是萬向節(jié)死鎖,！

        為什么說不能將望遠鏡朝向南方呢,，讓我們看看坐標(biāo)變化，從開始的（x=90度,，y=10度）到（x=90度,，y=90度），這個過程沒有問題,，望遠鏡慢慢轉(zhuǎn)動跟蹤飛行器,。當(dāng)飛行器到達（x=90度，y=90度）后,，坐標(biāo)突然變成（x=180度,，y=90度）（因為朝南），x由90突變成180度,所以望遠鏡需要饒垂直軸向x軸旋轉(zhuǎn)180-90=90度以便追上飛行器,但此時,，望遠鏡已經(jīng)是平行于x軸,，我們知道饒平行于自身的中軸線的的旋轉(zhuǎn)改變不了朝向，就象擰螺絲一樣,，螺絲頭的指向不變,。所以望遠鏡的指向還是天頂。而后由于飛行器飛遠,，坐標(biāo)變成（x=180度,，y<90度）時,，y向角減小，望遠鏡只能又轉(zhuǎn)回到正東指向,，望'器'興嘆,。這說明用x,y旋轉(zhuǎn)角（又稱歐拉角）來定向物體有時并不能按照你想像的那樣工作，象上面的例子中從（x=90度,，y=10度）到（x=90度，y=90度）,，按照歐拉角旋轉(zhuǎn)確實可以正確地定向,，但從（x=90度，y=90度）到（x=180度,，y=90度）,，再到（x=180度，y<90度）,按照歐拉角旋轉(zhuǎn)后的定向并非正確,。我的理解是坐標(biāo)值的變化和飛行器空間的位置變化一一對應(yīng),，但是從（x=90度，y=90度）到（x=180度,，y=90度）,，再到（x=180度，y<90度）這個變化,，飛行器位置是連續(xù)的變化,，但坐標(biāo)值的變化卻不是連續(xù)的（從90突變到180），其原因在于（x=90度,，y=90度）和（x=180度,，y=90度）甚至和（x=任意度，y=90度）這些不同的坐標(biāo)值對應(yīng)空間同一個位置,，這種多個坐標(biāo)值對應(yīng)同一個位置的不一致性是造成死鎖的根源,。

上面是2維坐標(biāo)系中的例子，同樣,，對于3維的也一樣,。比如有一個平行于x軸的向量，我們先將它饒y旋轉(zhuǎn)直到它平行于z軸,，這時,，我們會發(fā)現(xiàn)任何饒z的旋轉(zhuǎn)都改變不了向量的方向，即萬向節(jié)死鎖,，所以說傳統(tǒng)的歐拉角是不能做到全姿態(tài)解析的,。（抄的）

方向余弦

一個向量在坐標(biāo)系中的位置也可以用方向余弦表示，也就是這個向量分別到三個坐標(biāo)軸的夾角余弦值,，實際上就是這個向量到各個坐標(biāo)軸的投影啦,，角度范圍是0~π（維基）,。所以推廣到載體坐標(biāo)系和參考坐標(biāo)系當(dāng)中，我們就有了載體坐標(biāo)軸xyz分別與參考軸XYZ的方向余弦,，這里就是所說的方向余弦矩陣了,，它是由兩組不同的標(biāo)準(zhǔn)正交基的基底向量之間的方向余弦所形成的3x3矩陣。方向余弦矩陣可以用來表達一組標(biāo)準(zhǔn)正交基與另一組標(biāo)準(zhǔn)正交基之間的關(guān)系,。余弦矩陣的列表示載體坐標(biāo)系中的單位矢量在參考坐標(biāo)系中的投影,。分量形式如下:

第 i 行、 j 列的元素表示參考坐標(biāo)系 i 軸和載體坐標(biāo)系 j 軸夾角的余弦,。

其實方向余弦和歐拉角沒有本質(zhì)上的區(qū)別,，因為這是用歐拉角表示的方向余弦。一個坐標(biāo)系到另一個坐標(biāo)系的變換,，在《捷聯(lián)慣性導(dǎo)航技術(shù)》,、和《慣性導(dǎo)航》中都是有介紹的，特別是《慣性導(dǎo)航》有推導(dǎo)的過程,。

推廣到三軸的單次旋轉(zhuǎn),，我們用矩陣表示為：

這里要說一下矩陣的含義，C21表示坐標(biāo)系1到坐標(biāo)系2的變換矩陣,，那么,，

這是繞Z軸的基本旋轉(zhuǎn)，我們可以看到X2,、Y2投影為0,，Z2投影為1，這是為什么呢,？自己想想,。。,。咱們看下面的單位1,，這是什么？所以說這就是上面說的坐標(biāo)系2到坐標(biāo)系1軸的三個方向余弦,！對吧,。。,。這在大家熟知的imu.c里面就有直接的計算,。那么單獨旋轉(zhuǎn)各個軸，我們得到：

實際上,，兩坐標(biāo)系任何復(fù)雜的角位置關(guān)系都可以看做有限次基本旋轉(zhuǎn)的組合,，變換矩陣等于基本旋轉(zhuǎn)確定的變換矩陣的連乘，要是不知道矩陣和矩陣乘法,，那就看線性代數(shù)吧,，連乘的基本順序依據(jù)基本旋轉(zhuǎn)的順序向右排列,。之所以有順序是因為矩陣有“左乘”和“右乘”之分。那么我們得到：

按上面推論,，此矩陣的下面三個就是三軸對應(yīng)的方向余弦,，γ、θ,、ψ就是歐拉角,，現(xiàn)在明白了歐拉角和方向余弦矩陣的關(guān)系了吧。

四元數(shù)

四元數(shù)要說的實在太多,，因為它的優(yōu)點很多,，利用起來很方便，但是理解起來就有點蹩腳了,。我們百度四元數(shù)，一開始看到的就是四元數(shù)來歷,，還有就是四元數(shù)的基本計算,。對于來歷，還是想說一下,，四元數(shù)（Quaternions）是由威廉·盧云·哈密爾頓(William Rowan Hamilton,1805-1865）在1843年愛爾蘭發(fā)現(xiàn)的數(shù)學(xué)概念（百度百科）,。

將實數(shù)域擴充到復(fù)數(shù)域，并用復(fù)數(shù)來表示平面向量,，用復(fù)數(shù)的加,、乘運算表示平面向量的合成、伸縮和旋,，這就是我們熟知的復(fù)數(shù)的二維空間含義,，所以人們會繼續(xù)猜想，利用三維復(fù)數(shù)不就可以表達三維空間的變換了嗎,，歷史上有很多數(shù)學(xué)家試圖尋找過三維的復(fù)數(shù),，但后來證明這樣的三維復(fù)數(shù)是不存在的。有關(guān)這個結(jié)論的證明,，我沒有查到更明確的版本,，據(jù)《古今數(shù)學(xué)思想》中的一個理由，三維空間中的伸縮旋轉(zhuǎn)變換需要四個變量來決定：兩個變量決定軸的方向,，一個變量決定旋轉(zhuǎn)角度,，一個變量決定伸縮比例。這樣,，只有三個變量的三維復(fù)數(shù)無法滿足這樣的要求,。但是歷史上得到的應(yīng)該是比這個更強的結(jié)論，即使不考慮空間旋轉(zhuǎn),，只從代數(shù)角度來說,，三維的復(fù)數(shù)域作為普通復(fù)數(shù)域的擴張域是不存在的,。并且，據(jù)《古今數(shù)學(xué)思想》敘述,，即使像哈密爾頓后來引入四元數(shù)那樣,，犧牲乘法交換律，這樣的三維復(fù)數(shù)也得不到,。經(jīng)過一些年的努力之后,， Hamilton 發(fā)現(xiàn)自己被迫應(yīng)作兩個讓步,，第一個是他的新數(shù)包含四個分量，而第二個是他必須犧牲乘法交換律。（ 《古今數(shù)學(xué)思想》第三冊 177 頁 ）但是四元數(shù)用作旋轉(zhuǎn)的作用明顯,，簡化了運算，而且避免了Gimbal Lock,，四元數(shù)是最簡單的超復(fù)數(shù),，我們不能把四元數(shù)簡單的理解為 3D 空間的矢量，它是 4 維空間中的的矢量,，也是非常不容易想像的（抄的）,。

1、四元數(shù)的表示方式：

2,、四元數(shù)的計算：

關(guān)于四元數(shù)的基本運算,，加減乘除，大家從別的地方看看吧,，維基百科http://zh./wiki/%E5%9B%9B%E5%85%83%E6%95%B0

要注意的是,，四元數(shù)的元的平方定義為-1，這點等同于復(fù)數(shù),，四元數(shù)不同元相乘得到另外一個,，這點有點像向量叉乘?？梢?，四元數(shù)相乘不是虛數(shù)相乘也不是向量相乘，這一運算特點是由四元數(shù)的性質(zhì)決定的,。四元數(shù)由一個實數(shù)和三個虛數(shù)構(gòu)成,，所以是一個四維空間的向量，但是它的三個虛數(shù)又有三維空間的性質(zhì),，因此,，三維空間中的一個矢量，可以看作一個實部為0的四元數(shù),，這個四元數(shù)是這個三維空間的一個矢量在四維空間里的“映像”,，或者叫做這個矢量的“四元數(shù)映像”。這樣，我們就把三維空間和一個四維空間聯(lián)系起來,，用四維空間中四元數(shù)的性質(zhì)和運算規(guī)律來研究三維空間中剛體定點轉(zhuǎn)動的問題,。

四元數(shù)的乘法不可逆性，這里說的是四元數(shù)的外積,，定義兩個四元數(shù):

它們相乘得到：

（看明白這個,！既得到了平行又得到了垂直分量）

四元數(shù)乘法的非可換性，pq并不等于qp,。我們發(fā)現(xiàn),，兩個四元數(shù)相乘，我們不僅僅的得到內(nèi)機,，而且還得到外積,！這是由四元數(shù)運算性質(zhì)決定的。qp乘積的向量部分是:

,，四元數(shù)乘法不可逆,。

下面要說四元數(shù)是如何表示旋轉(zhuǎn)的，這個確實有點不好理解,，我看到的資料有的說的還是有點出入的,，或許只有自己理解了才能明白吧?！耙粋€單位四元數(shù)可以表示一個旋轉(zhuǎn)”，這句話的信息量確實夠大的,。它基于的思路是:一個坐標(biāo)系到另一個坐標(biāo)系的變換可以通過繞一個定義在參考坐標(biāo)系中的矢量 μ 的單次轉(zhuǎn)動來實現(xiàn),，四元數(shù)提供了這種數(shù)學(xué)描述。我們直觀的想一下,，我們可以把剛體僅僅旋轉(zhuǎn)一次就能達到任意的姿態(tài),，前提是確定了旋轉(zhuǎn)軸線和旋轉(zhuǎn)的角度，大家想一想,，感覺也是可以實現(xiàn)的吧,。

設(shè)Q = a + bi + cj +dk ，我們可以寫成[w,，v],其中w=a,，v=bi + cj +dk。

那么,，v 是矢量,，表示三維空間里的旋轉(zhuǎn)軸。w 標(biāo)量,，表示旋轉(zhuǎn)角度,。所以，一個四元數(shù)可以表示一個完整的旋轉(zhuǎn)。但要注意只有單位四元數(shù)才可以表示旋轉(zhuǎn),，至于為什么,，那是因為這就是四元數(shù)表示旋轉(zhuǎn)的約束條件。

就如上面說的我們利用角度和旋轉(zhuǎn)軸構(gòu)造了一個四元數(shù),，下面就是要滿足的這個關(guān)系：

定義μ 的大小和方向是使參考坐標(biāo)系繞 μ 轉(zhuǎn)動一個角度 μ ,，就能與載體坐標(biāo)系重合。這可能不好理解,，為什么說繞矢量μ還轉(zhuǎn)動μ,，(μx/μ)就相當(dāng)于方向余弦，這里并不矛盾,。個人認為,，也可以旋轉(zhuǎn)載體坐標(biāo)系和參考坐標(biāo)系重合。

這樣定義也是一樣的：

其中是繞旋轉(zhuǎn)軸旋轉(zhuǎn)的角度,，為旋轉(zhuǎn)軸在x,y,z方

向的分量,，就是方向余弦（由此確定了旋轉(zhuǎn)軸，為啥,？）,。

3、利用四元數(shù)進行矢量變換

那么利用四元數(shù)代表旋轉(zhuǎn)是如何實現(xiàn)的,，在載體系定義的一個矢量 r b 可以直接利用四元數(shù)將其在參考系中表示為 r n ,。首先定義一個四元數(shù) rb' ，它的虛部等于r b的相應(yīng)分量,，標(biāo)量分量為零：

參考系中的 r n’ 表示為,，其中q = a + bi + cj +dk，q*的復(fù)共軛,。因此有：

寫成矩陣式：

這個矩陣就對應(yīng)方向余弦矩陣了,，從而對應(yīng)歐拉角了。

肯定還有沒回過神的,，上面僅僅說明了一個“結(jié)果”：,。要直觀說清楚四元數(shù)如何工作的確實有點抽象。我們把三維空間里的點用實部為0的四元數(shù)表示成q = bi + cj +dk,，我們可以理解為虛部就是表示我們生活的三維空間,，那么我們把q左乘一個標(biāo)準(zhǔn)四元數(shù)后，我們能得到什么,？

為了更清楚地看到兩個四元數(shù)乘積到底是什么樣子,，我們把上節(jié)用到的向量空間的觀點拿過來，四元數(shù)的全體構(gòu)成的集合F是實數(shù)域上的四維向量空間,，可以把四元數(shù) q = a + bi + cj +dk看成四維實數(shù)元組（a,，b，c，d）,。然后用另外一個四元數(shù)Q左乘q,，這個運算就相當(dāng)于q在四維空間F上的線性變換，這就如同兩個虛數(shù)相乘（1+i）*（2+i）=1+3i,，（2+i）在復(fù)數(shù)坐標(biāo)系上的一個線性變換——大小和方向都發(fā)送了變化,。如果Q是個純實數(shù)（虛部為0的四元數(shù)），那么Q*q就是q簡單的伸縮比例,，方向什么的沒有發(fā)生變化,；如果Q是個虛部不為0的四元數(shù)，同樣道理q不僅僅是做了伸縮,，而且方向也發(fā)生了旋轉(zhuǎn),，不僅如此，所有的向量長度都伸縮相同的倍數(shù),。根據(jù)線性代數(shù)理論,，一個等距線性變換要么是單純的旋轉(zhuǎn)，要么是單純的對稱變換,，要么是二者的復(fù)合,。而四維空間上這樣的線性變換必有兩個垂直的二維不變子空間（就是后三相），也就是說,，可以在四維空間中找到兩個相垂直的平面,，在每個平面上的向量經(jīng)過變換之后還是在這個平面上。

4,、歐拉角,、方向余弦、四元數(shù)的關(guān)系

上面已經(jīng)說的很清楚了,，看一下：

通過比較上述等式的各個元素，四元數(shù)可以直接用歐拉角或方向余弦表示,。同樣

地,，歐拉角也可以用方向余弦或四元數(shù)表示。

用方向余弦表示四元數(shù)：

用歐拉角直接表示四元數(shù)：

用方向余弦表示歐拉角：

可以按下述方法直接由方向余弦推導(dǎo)出歐拉角,。當(dāng) 0 ,！= 90 。時,，歐拉角可由下式計算:

這個公式是在imu.c里有直接應(yīng)用的,。

5、傳感器

關(guān)于傳感器,，價格貴的肯定性能就好點,。還有就是傳感器的安裝方向，個人喜好X模式，飛行更靈活,，適合航拍,。

6、數(shù)據(jù)融合

數(shù)據(jù)融合也算是姿態(tài)解算的核心了,，好的解算算法可以更好的補償傳感器誤差,，目前就研究了論壇里的imu.c濾波思想，感覺很巧妙,。

// Description:

//

// Quaternion implementation of the 'DCM filter' [Mayhony et al].

//

// User must define 'halfT' as the (sample period / 2), and the filter gains 'Kp' and 'Ki'.

//

// Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elements representing the estimated

// orientation.  See my report for an overview of the use of quaternions in this application.

//

// User must call 'IMUupdate()' every sample period and parse calibrated gyroscope ('gx', 'gy', 'gz')

// and accelerometer ('ax', 'ay', 'ay') data.  Gyroscope units are radians/second, accelerometer

// units are irrelevant as the vector is normalised.

//

//=====================================================================================================

#define Kp 10.0f  //proportional gain governs rate of convergence to accelerometer/magnetometer

#define Ki 0.008f           // integral gain governs rate of convergence of gyroscope biases

#define halfT 0.001f        // half the sample period采樣周期的一半

float q0 = 1, q1 = 0, q2 = 0, q3 = 0;   // quaternion elements representing the estimated orientation

float exInt = 0, eyInt = 0, ezInt = 0;  // scaled integral error

void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az)

{

  float norm;

  float vx, vy, vz;// wx, wy, wz;

  float ex, ey, ez;

//先把這些用得到的值算好

  float q0q0 = q0*q0;

  float q0q1 = q0*q1;

  float q0q2 = q0*q2;

  float q1q1 = q1*q1;

  float q1q3 = q1*q3;

  float q2q2 = q2*q2;

  float q2q3 = q2*q3;

  float q3q3 = q3*q3;

  if(ax*ay*az==0)

                 return;

//normalise the measurements               

  norm = sqrt(ax*ax + ay*ay + az*az);      //加計數(shù)據(jù)歸一化,把加計的三維向量轉(zhuǎn)換為單位向量,。因為是單位矢量到參考性的投影，所以要把acc單位化

  ax = ax / norm;                                                                         //其實歸一化改變的只是這三個向量的長度,，也就是只

  ay = ay / norm;                                                                         //改變了相同的倍數(shù),，方向并沒改變，也是為了與單位

  az = az / norm;                                                                         //四元數(shù)對應(yīng),。

//estimated direction of gravity and flux (v and w),，估計重力方向和流量/變遷

//這是把四元數(shù)換算成方向余弦中的第三行的三個元素，根據(jù)余弦矩陣和歐拉角的定義,，地里坐標(biāo)系的Z軸重力向量,，

//轉(zhuǎn)到機體坐標(biāo)系，正好是這三個元素,，所以這里的vx,vy,vz其實就是上一次的歐拉角（四元數(shù)）的機體坐標(biāo)參考系

//換算出來的重力的單位向量,。(是帶有誤差的姿態(tài)。)

  vx = 2*(q1q3 - q0q2);                                                                //參考系Z軸與載體系x軸之間方向余弦向量                                

  vy = 2*(q0q1 + q2q3);                                                                //參考系Z軸與載體系y軸之間方向余弦向量

  vz = q0q0 - q1q1 - q2q2 + q3q3 ;                                    //參考系Z軸與載體系z軸之間方向余弦向量

//error is sum of cross product between reference direction of fields and direction measured by sensors

//ax,，ay,，az是機體坐標(biāo)參考系上，加計測出來的重力向量,，也就是實際測出來的重力向量（這明顯就是重力在三個軸

//上的“分擔(dān)”）,。那他們之間的誤差向量就是陀螺積分后的姿態(tài)和加計測出來

//的姿態(tài)之間的誤差。向量間的誤差可以用向量的叉積（外積）來表示,，ex,，ey，ez就是兩重力方向的叉積,，注意這個

//叉積向量仍是在機體坐標(biāo)系上的,，而陀螺的積分誤差也是在機體坐標(biāo)系上的，而且叉積的大小和陀螺的積分誤差成正

//比,，正好用來修正陀螺,，這是因為，由于陀螺是對機體直接積分的,，所以對陀螺的誤差修正會直接會直接體現(xiàn)在對機

//體坐標(biāo)系的修正,，就是對機體修正,。說白了，就是用加計測量標(biāo)定陀螺積分,。

//這里誤差沒說清楚,，不是指向量差。這個叉積誤差是指將帶有誤差的加計向量轉(zhuǎn)動到與重力向量重合,，需要繞什么軸,，

//轉(zhuǎn)多少角度。逆向推理一下,，這個叉積在機體三軸上的投影,，就是加計和重力之間的角度誤差。也就是說,，如果陀螺

//按這個叉積誤差的軸,，轉(zhuǎn)動叉積誤差的角度（也就是轉(zhuǎn)動三軸投影的角度）那就能把加計和重力向量的誤差消除掉。

//（具體可看向量叉積的定義）如果完全按叉積誤差轉(zhuǎn)過去,，那就是完全信任加計,。如果一點也不轉(zhuǎn)，那就是完全信任

//陀螺,。那么把這個叉積的三軸乘以X%,，加到陀螺的積分角度上去，就是這個x%互補系數(shù)的互補算法了,。

//ps：實際上叉積的length是兩向量夾角的正弦,，而且必須在±90度以內(nèi)，并不完全與誤差角度成線性正比,。如果轉(zhuǎn)成三

//軸夾角,，按歐拉角的轉(zhuǎn)動順序分解到三軸上去，會很麻煩,。這里的叉積算法把sin誤差當(dāng)成角度誤差,，并無視歐拉角的

//轉(zhuǎn)動順序，在誤差較小的時候,，并不會有影響,。因為這個修正對誤差來說，是收斂的,，正常情況下誤差只會越來越小,。

//為了解釋方便,，所以上面就把叉積等同于角度誤差了,。

  ex = ay*vz - az*vy ;                                                 //上面說了，vx,vy,vz就是方向余弦,，

  ey = az*vx - ax*vz ;

  ez = ax*vy - ay*vx ;

//對誤差進行積分

  exInt = exInt + ex * Ki;                                                   

  eyInt = eyInt + ey * Ki;

  ezInt = ezInt + ez * Ki;

// adjusted gyroscope measurements

  gx = gx + Kp*ex + exInt;                                                       //將誤差PI后補償?shù)酵勇輧x,，即補償零點漂移

  gy = gy + Kp*ey + eyInt;                                                    //修正陀螺輸出

  gz = gz + Kp*ez + ezInt;                                                           //這里的gz由于沒有觀測者進行矯正會產(chǎn)生漂移,，表現(xiàn)出來的就是積分自增或自減

// integrate quaternion rate and normalise                        //四元素的微分方程

  q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT;

  q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT;

  q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT;

  q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT;

這是四元數(shù)的微分方程，用于更新四元數(shù),，

所以說halfT定義為0.001,，因為我們的姿態(tài)解算的周期是2ms，這里有個0.5系數(shù),。

// normalise quaternion

//由于誤差的引入,，使計算的變換四元數(shù)的模不再等于1，變換四元數(shù)失去規(guī)范性,，因此在利用更新四元數(shù)

//計算歐拉角時必須要對四元數(shù)規(guī)范化處理,。

  norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);

  q0 = q0 / norm;

  q1 = q1 / norm;

  q2 = q2 / norm;

  q3 = q3 / norm;

//由計算四元數(shù)計算歐拉角

// Q_ANGLE.Z = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2*q2 - 2 * q3* q3 + 1)* Rad; // yaw

  Q_ANGLE.Y  = asin(-2 * q1 * q3 + 2 * q0* q2)* Rad; // pitch

  Q_ANGLE.X = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1) * Rad; // roll

  if(Q_ANGLE.X>90||Q_ANGLE.X<-90)

  {

  if(Q_ANGLE.Y>0)

        Q_ANGLE.Y=180-Q_ANGLE.Y;

  if(Q_ANGLE.Y<0)

        Q_ANGLE.Y=-(180+Q_ANGLE.Y);

  }

}