1、四元數完全解析及資料匯總本文原帖出自匡名四軸論壇，附件里的資源請到匿名論壇下載:http:感謝匿名的開源分享，感謝群友的熱心幫助。說什么四元數完全解析其實都是前輩們的解析T,小小真心是一個搬磚的，搬得不好希望大神們洽以批評和指正，在此謝it了。因為本人是小萊烏一枚，對，裝某的那種菜鳥所以對四元教求解姿態角這么一個在大神眼里詢單的算法，小弟我還是費了很大勁才旃微理解了那么一點點,小第我轉整理時也是基于小弟的理解和幫商的，有些太息耳，有些可能錯了，大牛們發現了再罵過我后希望能夠洽與指正哈。好，廢話到此為止，開始說主體。四元數和姿方南怎么說呢？先得給和我一樣的小菜鳥們理一理思路，小鳥我在此叵了一個“

2、忠雄導困”（我承認我畫的丑），四元數解算姿態首先分為兩部分理解：第一部分先理解什么是四無數，四元數與姿志雨閏的關系；第二部分會理解怎么由慣性單元測出的加送度和角it度求出四元教，再由四元救求出歐拉角©圖1渣渣總維導團在講解什么是四元教時，小第的忠雄是順看說的，息由四元敢的定義說起，說到四元敲與委冬雨間的關系。但在講解姿態解算時，小弟的思維是逆向的，就是反推回來的，從歐拉用一步步反推回到慣性器件的測量數據，這樣逆向說是因為便于龍解，因為實際在工程應用時和理論推導有很大差別O實際應用時正確的求解頓序應該為圖1中序號順序，即1->2->3->.但在筆者講解姿態求解時恐略是

3、如圖2的。四元k/求出k/求出數定義7=A姿態角尸入四元數圖2逆向講解芯路大家在看四元數時最好結合看代碼一塊看，小第看的是匿名四軸的代碼，感覺寫的豐常好也本常清晰，粘出來大家一塊觀摩。缸。部分是核心代碼，總共分為八個步碟，和圖1中的八個步.喋是一一對應的。講解介紹時也是和代碼對比起來講解的。代碼可以去匿名官網上下我，都是開源的，不是小第的，所以小弟不方便加在附件中。四元教更新姿態#defineKp2.of/加速度權重，越大則向加it度測至值收斂越快#defineKio.ooif誤是積分增益voidANO_IMU:Quaternion_CF(Vector3fgyro#Vector3facqflo

4、atdelta!)VectorfVgravity.Verror.VerrorI:/!重力加入度歸一日accnormalize();2.提取四元數的等效余弦矩陣中的支力分至Q.vector_gravity(V_gravity);3.向寶叉枳得出第態誤是V_crror=acc%V_gravity;/1對誤差進行積3V_error_l+=V_error*Ki;&互補逐波，姿態誤差補償到角速度上，修正南it度積分漂移Gyro+=V_error*Kp:V_error_l;6.一階龍格庫熔法更新四元數Q.Runge_Kutta_ist(GyrofdeltaT);7四元數歸一化Q.normalize

5、();8.四元教轉歐拉角好的，下面救磚開始！。00嚅咻嘿咻！Q.to_euler(&angle.x#&angle.yfRangle.z);一.什么是四元教？關于四元數的定義摘自落永元的慣性導抗，里面有非帝好的講解，大家可以直接看結論和第九幸就可以。下面我粘貼了部分原文，粘貼的比較多比較詳細，應為本人比較笨還愛較真，所以按本人的風格就妥詳盡一點，大牛們都可以自動忽咯。四元數定義、表達方式及運算方法摘自慣性導航-秦永元P289-2929.2姿態更新計算的四元數算法設由運載體的機體軸確定的坐標系為6,慣導系統所采用的導航坐標系為小則由兒系到系的坐標變換矩陣C稱為運載體的姿態矩陣，姿態

6、更新是指根據慣性器件的輸出實時計算出C；矩陣。由于系和系均為直角坐標系，各軸之間始終保持直角，所以可將坐標系理解成剛體，當只研究兩個坐標系間的角位置關系時，可對一個坐標系作平移，使其原點與另一個坐你系的原點重合。因此，兩坐標系間的空間角位置關系可理解成剛體的定點轉動。從這一基本思想出發，可獲得姿態更新的四元數算法及旋轉矢根算法。本節詳細介紹四元數更新算法。9. 2.1四元數1 .四元數定義顧名思義，四元數是由四個元構成的數：（23或1&?。?q。+qj+q3k（9.2.1）其中,農、41、92、會是實數一、人人既是互相正交的單位向量，又是虛單位i,具體規定體現在如下四元數乘法關系中：=

7、-1,j®j=-1,fc0a=-n1 /=無，j®k=i,k®i=j卜（922）j0i=-k90J=-1»iA=-jJ式中,表示四元數乘法。上述關系可敘述為：相同單位向量作四元數乘時呈虛單位特性;相異單位向量作四元數乘時呈單位向樂叉乘特性。所以四元數既可看作四維空間中的一個向所，又可看作一個超復數C2.四元數的表達方式(1)矢量式Q=Q0+q(9.2.3)其中,如稱四元數。的標量部分,g稱四元數Q的矢量部分。對照式(9.2.D,可看出,是三維空間中的一個向植。(2)復數式Q=9o+qj+q?J+Qik(9.2.4)可視為一個超復數,Q的共腕復數記為C*=

8、q。-Qiiqzjq3A(9.2.5)。稱為Q的共匏四元數。(3)三角式8 8Q=cos爹+tfsin5(926)式中/為實數，“為單位向量6(4)指數式9Q=er2(9.2.7)8和同上。289(5)矩陣式(92.8)3 .西元數的大小范敷四元數的大小用四元數的范數來表示，IIQII=或+d+5+ql(9.29)若IIQII=1,則Q稱為規他化四元數。4 .日元數的運算加減來除1)加法和減法設Q=qo+。"+QiJ+gA尸=%+力】i+Pzj+力3A。土產=(qo±%)+(71±Pi)i+(72土p2)j+(qa±p3)k2)乘法(9.2.11)aQ-

9、+aqIi+aq2j+aq3k其中,a為標量.尸Q=(po+P】i+P2J+%卜)®(7o+q】i+qij+%比)=(%<7o-P0一夕2<72一戶內3)+(9皿1+力闖0+生93-03g2”+(20伙+p泗0+%q3)j+(2國3+。30+P3-Pzqlk(9.2.12)=+尸1+r2j+r3k上式寫成矩陣形式:一力1-/>2-P'%-PiPl戶3PqP-PzP0。-Qi-Qz-(13q。93-qzq、7o磯<72-qi?o-po9】=M(P)Q以-見-Dop=M'(Q)PPz4(9.2-13)(9.2.14)290其中,M(P)的構成形式為

10、：第一列是四元數P本身，第一行是P的共規四元數P的轉置，劃去第一行和第一列余下的部分。PQ-P3PtVp=pi%(92-15)LPiPlPo-稱為M(P)的核，是由四元數P的元構成的反對稱矩陣。同理(Q)的核為9。如一會V'q=-</3qoQ-(12-qi-可見”(。)與M(Q)構成相似，但核不同.由式(9.2.13)和式(9.2.14).得四元數乘法的矩陣表示形式:P®Q=M(P)Q=MYQP由于M(P)和M"P)的核不同，所以PQ=M(P)Q工M'(P)Q=fi®P上式說明四元數乘法不滿足交換律。四元數乘法滿足分配律和結合律，p0(e+/

11、?)=p0e-f-p0/?/QK=(P®。)區。=P<S(Q®R)(9.2.16)(9.2.17a)(9.2.17b)(9.2.18).3)除法求逆如果齒*=1,則稱R為P的逆，記為/?=PT,或稱P為R的逆，記為P=根據范數定義和式(9.2.12)P0P'=(/>()+p2j+p3k)®(/>opxipzj-/>3k)=加+/+用+H=IIPII所以，Pg療丁=1，根據上述關于逆的定義，帝即為P的逆即I=皆(92,22)好，關于四元教定義就撤這么多，其他的大家去附件下載慣性導航的pdf自己看吧下面開始搬四元數與姿冬解算關系的。oo

12、ooO嘿、咻嘿咻一圖9.21剛體的等效旋轉二、四元數與姿態陣間的關系從上面我們知道了四元教的定義，用這四個數和我們安求的三個姿態角有什么關系呢？下面是詳細的推導，同樣插自慣性導抗-泰永元P292-297。四元數與姿看陣間的關系摘自慣性導航-泰永元P292-2979. 2.2四元數與姿態陣間的關系設有參考坐標系R，坐標軸為西、的、N0,坐標軸方向的單位向量為記、丸、燈.剛體相對K系作定點轉動，定點為。取坐標系6與剛體固聯北系的坐標軸為/、外Z,坐標軸方向的單位向嫌為i、j、k.假設初始時刻6系與R系重合.為了便于分析剛體的空間角位置.在剛體上取一點4轉動點。至該點引位置向量OA,如圖9.2.1所

13、示。則該位置向量的空間位置實際上描述了剛體的空間角位置。設剛體以3=3/+叱/+叫A相對R系旋轉，初始時刻位置向過處于OA=i，經過時間t后位置向量處于。A'=，.根據歐拉定理，僅考慮剛體在0時刻和t時刻的角位置時剛體從A位置轉到4位置的轉動可等效成繞瞬軸u(單位向量)轉過,角一次完成。這樣，位置向量做圓錐運動，A和4位于同一圓上"和，位于同一圓錐面上.下面分析，與，的關系。在圓上取一點以使/472=90。，由圖得OO'="U)MO'A=r-OO'=，一(ruyuO'B=uXO'A=uXr(ru)uXu=uXrO1A1=O，A

14、cosd+OB%xd=rcos6(ru)ucos0+uX/sin，所以rf=OOf+O'”=rcosO+(1cosO)X(ru)u+Xrsin由三重矢積計算公式：«X(«Xr)="(ur)<u«)r=(ru)ur(r=r-FwX<wXr)所以/=rcostf+(1-cos夕)r+“X(uXr)J+uXrsintf=r+uX，sin£+(1cos。)”X(wXr)將上式向R系內投影，r,R=/+(“X，尸sin夕+(1c()sG)"X(uXr)*記又根據叉乘關系表達式:(9.2.23)(9.2.24)(9.2.25)

15、'R=DrR記0-nmU=n0ILmI0.則(uX，)"=VrCuX(«Xr)代=UU*所以，火=尸+VrinO+(1-3陰UUrR=(74-2Usin-cos-+2sin2guU1rx令O=，+2Usin5cos方+2sin2與UU則式(92.24)可寫成：(9.2.26)記初始時刻的剛體固聯坐標系為伉，由于初始時刻剛體固聯坐標系與參考坐標系重合,所以rR=(9.2.27)而在轉動過程中，位置向量和力系都同剛體固聯，所以位置向量和6系的相對角位置始終不變，即有4=小(9.2.28)將式(92.28)代入式(9.2.27)，得d=d(9.2.29)將式(9.2,29

16、)代入式(9,2.26),得1=6"該式說明。即為%系至A系的坐標變換矩陣，根據式<9.2.25）和式（9.2.23）Cf=D=7+2l7sinycosy+Zsin24UV乙乙乙10.0+2cos一(mj+n2)sin2%bnsin-/wsin2-yJ8nsin5.0向n萬.0msin5Zwsin2-(Z2+n2)sirfwnsin24Zsin9msin,.0Zsin-乙Zwsin2與.2emnsin(mz+/2)sin2,y乙(9.2.30b)q。(9.2.31)91=，sin.0Qz=加s】n5.e如=wsin-并以電卬:、92、央構造四元數：Q=<7o+q/o+q3

17、k0=cos-+(li0+mj0+*sin“sincor則可得如下結論:<1）四元數。=cos5+dsin3描述r剛體的定點轉動，即當只關心6系相對R系的角位置時，可認為6系是由R系經過無中間過程的一次性等效旋轉形成的.Q包含了這種等效旋轉的全部信息:，為旋轉瞬軸和旋轉方向。為轉過的角度.（2）四元數可確定出6系至R系的坐標變換矩陣。將式（9.2.31）代入式（9：230）,得1-2(或+冠)2(<7i<72+9o<73).2(qQoQ?)2ag-<7o93>1-2(才十底)+QoQ：)2(q】qs+9o<?2>2s溝3-9o<7i)1-2

18、+由(9.2.33)由于IIQII=qW+q：+q；+d=cos2£+（/2+“/+MXil?£=1,所以可進一步推得如下結論：（1）描述剛體旋轉的四元數是規范化四元數。（2）式（9,233）可寫成：+<71-92-Qi2（g】q2-1。如）2"聞3+qg"Ci=2Sq+q。的）<7oQi4-qi2（q閡§gql24闖3倒佻）2（9293+9）加一垃+居（9.2.34）如果參考坐標系K是導航坐標系,剛體固聯坐標系6為機體坐標系，則坐標變換陣c£就是姿態矩陣a,而由姿態矩陣可計算出航向角和姿態角。設運載體的航向角為w（習慣上

19、以北偏東為正），俯仰角為。，橫滾角為人取地理坐標系g為導航坐標系，并規定H、刈、匕的指向依次為東、北、天,則機體坐標系b與導航坐標系（即地理坐標系g）的關系如圖1.2.3所示。由該圖可得三次基本旋轉對應的坐標變換陣為sinV/0cosV001.C9.2.37)cosVC：nC；=sinW0cos/0q=*=01.sin/0"cos/sinsin/cossin/"(9.2.38)0cos。sinG.sin/sincos?coscos/-C-c：cos/cos+sin7sinsin一cos/sin+sin/cossinsin/cos"=sinWcos。cosWcqs。

20、sindLsin/coscos7sin中sin。sinYsinWcos/cossincos7cos6J(9.2.39)T22733，由于，系至占系的旋轉過程中坐標系始終保持直角坐Ti27'"標系,所以C7為正交矩陣比較式式.2.39）和式（9.2.40）,得0=arcsin(T32>'主=arctan(史)3*主=arctanj-2j(9.2.41)航向角和橫滾角的真值按表9.2.1和表9.2.2確定表9.21航向角W真值表O表9.2.2演浪角y真值表TuTnY-0+90.-0一90°+h-+中蕓.+“780。+空主+180。h一18(r/1+18CT

21、上述分析說明：如果表征舞系至6系的旋轉四元數Q已畫定，則按式（9.2.33）或式（9.2.34）可計算出姿態陣仁,再按式（9.241）和表921及表9.2,2可確定出運載體的航向角、俯仰角和橫滾角，因此，四元數。包含了所有的姿態信息，捷聯慣導中的姿態更新實質上是如何計算四元數Q9o+q；g；2(602+如。3)2(9闖3-gm)呃，粘了這么多其實就是為了想知道推導過程小伙伴好理解，真正有用的就是（9"34）這個公式。這個公式把四元數轉換成了方向余弦矩陣中的幾個元素，再用這幾個元素轉換為歐拉南。就求解除了姿態！2(qq4)2(qg3+QMz)加一qf十或一熄2(q四3qgi)24曲3+

22、一闖】)就一詔一城+gl(9.2.34)先從四元數qOf3轉成方向余弦矩陣:Spo+Qi-Q2-<73=2(4通2+q0q3)I29遇3-QoQ2)2(qig2-QoQ3)Qoq1+q；一后23%+QoQi)2(<7iq3+QoQ2)2(q2Q3-QoQi)Qo-Qi-q；+q；.*nyn4再從方向余弦矩陣椅換為歐拉角:0=sin-1(T32)俯仰角pitc/i0=tan-1）橫滾角廠。6=tail_1航向角yaw好的，公式原理都講清楚了，下面來看一下匿名的代碼:四元救轉歐拉角，這里四元數是qlf4和公式里q0f3相對應。voidQuatemion:to_euler(float*r

23、ollffloat*pitch,float*yaw)Iif(roll)*roll=degrees(atan2f(2.of*(qi*q2+q3*q4)fi-2.of*(q2*q2+q3*qj)；/*roll=degrees(atan2f(-2.of*(q2*q4-qi*q3)fi-2.of*(qz*q2+q3*q3)；1if(pitch)(使用safe_asin()來處理pitch接近臾/9。時的奇點*pitch=degrees(safe_asin(2.of*(qi*q3-qz*q4);/*pitch=degrees(safe_asin(2.of*(q3*q4.qi*qz);)if(yaw)*y

24、aw=degrees(atan2f(2.of*(q2*q3-qi*q4)f2.of*(qi*qi*q2*qz)-1);/*yaw=degrees(atan2f(2.of*(q2*q3-qi*q4)f2.of*(qi*qi+q3*q3)-1);)對比余弦矩陣轉換為歐拉角的公式很容易理解了吧，注意一下，紅已是區名原版的代碼，和公式還是有出入的，自己細心現察一下吧。被注擇了的黑己代碼是我根據上面的公式寫的。但足己的求斛出來的歐拉角反映出來的姿態是不對的，具體表現為俯仰(pitch)和橫滾(roll)是相反的，為啥根據公式寫的艮不對的？科里的小伙伴給與了我熱心的解答O這個錯誤主妥是由于方向余弦矩陣的旋

25、轉順序不一樣，也就是公式(9.2.39)不一樣，這是由于旋轉的先后順序不同引起的，具體大家參考慣性導抗緒論來看就能明白，因為這一點小弟還有點混亂，就點到這為止。c干ycos/cos+sin/sinsincos/sin+sinYcosWsin。sin7cos夕=sinWcosdcosWcos。sin。Lsin/coscos/sinVrsinsinsinWcos/cossincosYcqsOJ(9.2.39)以上就是四元數求解歐拉角的方法©逝過公式可以看外，妥求歐拉角需妥求得四元數的方向余弦矩陣；妥求得四元數方向余弦矩陣，需妥求得四元教qOF3,那么如何求得qOq3?接下耒詳如介紹

26、69;三、四元數微分及龍格庫塔求Q0Q3首先我們先來看一下在程序里如何求解的q0-q3：一階龍格庫塔法更新四元致voidQuaternionRunge_Kutta_lst(VectorSf&g,floatdelta?)一一ql-05*(-q2*gx-q3*gy-q4*g.z)*deltaT.q2+=05*(ql*gx+q3*gz-q4*gy)*deltaT;q3-05*(ql*gy-q2*gz+q4*gx)*deltaT;q44=05*(ql*gz+q2*gy-q3*gx)*deltaT;)這就是一階龍格庫塔法求解q的微分方程，傳入參數只需妥這個周期的角速度gx、gy、gz和冏期時間d

27、eltaTo下面一張是從某位大神的貼吧上盜的圖，描繪的是一階龍格庫熔的計算式。其一階此格-岸塔法計算式為«(，7),q(i)式中的n(t)式式(7.2.19)所示。將式(7.3.25)展開成元素的表達式，有r)=一B,r(，)/(，)一wy(t)Pj(t)-Wg(I)Pj(I)Pl(t+r)=P|(l)u»zU)P2(l)-a,r(t)P3(l)+D=Pj(/)y(a/r(<)A(t)-片(，)=P3(oy<oz(r)A(r)卬£，)片(力-/SK3Lmaj相信很多人和我一樣，羊看上圖很難理解其中的意思和其由來，于是我又找了很多帖子，感謝前人做出的貢獻

28、，小弟在這里再次整理大神的四元數微分方程推導公式，便于大家理解。摘自附件中推沿四元救pdf雖然在下也不是很懂，不過粘出來還是能起到理解的作用，這樣大家就不會覺得這是憑空變出來的，本人教學功底薄弱，沒有對推導進行過驗證，如果有不對的地方歡迎指正。接著使用一階龍格庫塔(Runge-Kutta)發求出q0q3,這一點很多人不知道一階龍格庫熔怎么推導的，下面也是這位網友的推導，大家參考看理解吧。3 .四元數微分四元畋微分，已知一四元敷Q=Cosl+n-Si響，那耀微分dQ1QdOdn。八1Gded7="/叫卜充+市.SE另+n-2COS2,dFA已知；:=-L黎=0,黑=31b.E朗t理座模

29、系,b舄刑亍器坐棒系等=臉(Cos|自+1Smg)=；溫Q因懸陀螂倭在歡行器上測到的向速度內C3=3j+3“+3/A,故棒換成曾較懸方便JW察=：Q溫展陰整理焉其中4 .更新四元舅C使用一隋Rungc-Kutta更新四元敬，假有一微分方程期其解檢Xt+4t=Xt+zk.fxttu)t其中At23取煤調期，4尋套用至四元數Qt+At=(?t+dt-nt)(?t展滯上式(qQ/qQ/fq1一如q2一包q?QiQi|At/+3q。-3yq3+3/q?Qaj-IQz/+2*1+3q3+3yq()gq1<?3/e>Ac73/七一5Qz+%Qi+%q。只需利用角速度即T史新四7t鼓這里的角速度

30、以、4,、%是由捷聯陀螺的揄出（射機械轉子陀螺必須經過誤是補償，將在下面介紹）0對比看匿名四軸的代碼看一看（gx、gy、gz是捷朕陀場的輸出），代碼的意思就比較清楚了。在往上一步步推，我們就會求陀螺揄出了，并且還委對數據進行互補遮波處理。四、慣性單元測量值破合這部分看似很同單，但是也有讓筆者難以理解的地方，行望后人能補充修正進行更好的講解。有了上一步的龍格庫熔方程，我們現在需妥的就是南速度的測至值。在四軸上安裝陀螺儀，可以測至四軸簡料的角速度，由于陀埸儀揄出的是四軸的南速度，不會受到四軸振動影響。因此該信號中噪聲很小。四軸的角度又是通過對角速度積分而得，這T進一步平滑信號，從而使得角度信號更加

31、穩定。因此四軸控制所需妥的角度和角速度可以使用陀螺儀所得到的信號。由于從陀螺儀的角it度獲得南虎信息.需會經過積分運算。如果角速度信號存在微小的偏差，經過積分運髡之后，變化形成枳系誤差。這個誤差會甌看時間延長逐步增加，最終導致電路飽和，無法形成正確的南虎信號。如何消除這個累積誤差呢？于以通過上面的加速度傳感器獲得的角度信息對此進行校正。利用加速度計所獲得的角度信息為與陀螺儀積分后的角/Ih進行比較，將比較的誤差信號經過比例Tg放大之后與陀螺儀輸出的角速度信號要加之后再進行積分。對于加速度計洽定的南度2,經迂比例、積分環節之后產生的角度h必，然最終等于“。由于加速度計獲得的南度信息不會存在積累誤

32、差，所以戊終將輸出角度H中的積累誤差消除了。加速度計所產生的角度信息Hg中會要加很強的有四軸運動加速度噪聲信號。為了遛免該信號對于角度H的影響，因此比例系數Tg應該等韋小。這樣，加it度的喙聲信弓經迂比例、枳分后，在輸出角度信息中就會*有小了。由-丁存在積分環節，所以無論比例Tg多么小，最終輸出雨度h必然與加速度計測至的角度均相等，只是這個調節迂積會隨看Tg的減小而延長。先把這個迂程的代碼粘出來，看看代碼一步步理解：defineKp2.Of加速度權更，越大則向加速度測費值收斂越快#defineKi0OOlf誤差積分增益1.重力加連度歸一化accnormalize。，2.提取四元數的等效余弦矩陣

33、中的重力分至Q.ve:tor_gravity（V_gravity）,3向螢叉枳得出姿態誤差V_error=acc%V_gravity,時誤是進行枳分V_error_IV_error*Ki,彳互補區波，逐東誤差補償到南運皮上，修正角速度和分漂移Gyro4Vcrror*Kp+VcrrorI,1 .重力加送度歸一化：加速度計數揖歸一化，把加迫度計的三維向至轉換為單位向童，因為是單位矢金到參考性的投影，所以妥把加速度計數據單位化，其實歸一化改變的只是這三個向主的長度，也就是只改變了相同的侍數，方向并沒有改變，也是為了與單位四元數對應。2提取四元數的等效余弦矩陣中的重力分貢:/返回該四元數的等效余弦矩陣

34、中的重力分至voidQuaternionvector_gravity(Vector3f&v)(vx=2*(q2*q4-ql*q3);vy=2*(ql*q2+q3*q4),v.z=l-2*(q2*q2+q3*q3).將當前姿態的重力在三個軸上的分至分離出來，把四元數換算成方向余弦中的第三行的三個元素，根據余弦矩陣和歐拉角的定義，就是地理坐標系（參考坐標系）的Z軸的變力向至。當我讀究這句話腦子挺怪的，鬧不明白啊，于是又找到了下面的資料，可以進行解擇了。利用四元數耨地理的重力加速度旋樽至來行器上面，再典加速度舒ifi出的值（已端一化的）做外稹，得出塞用此差封角速度做校正融合重力加速度g=gz

35、做端一化grg=z旋斡至來行器上的重力加速度懸gb兼行器上的加速度升測得的加速度Qb做一化Mq=Qo+qi-qf-Qi2(qiq2+Q0Q3).2(%-QoQ2)2(4142-Q0Q3)Qo-Qi+q；-q；2(%布+q。%)2(Qiq3+Q0Q2)2(<72Q3-QoQi)Q0-Q1-92+q；.別忘了這是個正交矩陣哦！這樣就知道代碼怎么來的了吧？好繼續。3.向至叉枳得出姿態誤差：哎呀，又來棘手問題了，這個我也不太明白怎么講啊，還是把大神的講解粘過來吧，大家看看是不是這么回事：acc是機體坐標參呼.系上，加速度計測出來的重力向主，也就是實際測出來的重力向童。acc是測妄得到的重力向量，

36、V-gravity是陀螺積分后的姿態來推算出的重力向至，它們都是機體坐標參照系上的重力向主。那它們之間的誤是向至，就是陀埸積分后的姿態和加計測出來的姿態之間的誤差。向童間的誤差，可以用向主叉積（也叫向至外積、叉乘）來表示，V_error就是兩個重力向至的叉枳。這個叉枳向至仍舊是位于機體坐標系上的，而陀螺積分誤差也是在機體坐標系，而且叉枳的大小與陀埸枳分誤是成正比，正好拿來到正陀埸。（你可以自己拿東西想象一下）由于陀螺是對機體扎接積分，所以對陀場的糾正至會直按體現在對機體坐標系的到正。看了上面的話，小弟一直對這個誤差向柔感到莫名其妙，后來又找到大種的一F一段話：這里誤是沒說清楚，不是指向主屋。這

37、個叉枳誤差是指將帶有誤甚的加計向至轉動外與重力向受重合，需妥燒什么軸，轉多少南度。逆向推理一下，這個叉積在機體三軸上的投影，就是加計和重力之間的角度誤差。也就是說，如果陀埸按這個叉枳誤差的軸，轉動叉積誤差的角度（也就是轉動三軸投影的角度）那就能把加計和篁力向主的誤是消除掉。（具體可看向主叉枳的定義）如果完全按叉枳誤是轉過去，那就是完全信任加計。如果一點也不轉，邠就是完全信任陀場。那么把這個叉枳的三軸乘以x%,加用陀螺的枳分角度上去，就是這個X%互補系數的互補算法了。這個看了好像終于理解點了，再看下代碼：3向云叉枳得出姿態誤差Verror=acc%Vgravty;這里“”已經支定義為叉來的算法了

38、。4對誤差進行積分：積分求誤差,關于當前姿態分離出的重力分玄，與當前加達度計測得的重力分主的差值進行積分消除誤是VerrorI+=Verror*Ki,5互補濾波，姿態誤差補償到角速度上，修正角速度枳分漂移系數不停地被沱場積分更折，也不件地被誤差修正，它和公式所代表的姿態也在不斷更新。將積分誤差反情幻陀塔儀上，修正陀場儀的值。將該誤差V_ero揄入PI控制器后與本次姿態更新周期中陀螺儀測得的南運度相加，最終得到一個修正的南速度值,將其揄入四元數微分方程,更新四元教。Gyro+=Verror*Kp+VerrorI,Gyr。就走得到的修正角述度值，丁以用于求解四元數q0q3了。到這里回顧一下八個步腺還漏了一個第七步:7.四元數歸一化：規范化四元數作用：1 .表征旋轉的四元數應該是規范化的四元教，但是由于計算誤差等因素，計算迂程中四元教會逐漸失去規范化特性，因此必須對四元數做規范化處理。2 .意義在于單位化四元數在空間旋轉時是不會拄伸的,僅有旋轉角度.這類似與線性代數里面的正交變換。3 .由于誤差的引入，使得計算的變換四元數的模不再等于T,變換四元數失去規