在看《歐拉角、旋轉矩陣、四元數合輯 》，就之前所學做點筆記，以便以後再次複習。
先複習先基本概念
坐标系
我們為了能夠科學的反映物體的運動特性，會在特定的坐标系中進行描述，經常要用到以下幾種坐标系：
- 大地坐标系統
- 地心固定坐标系統
- 本地北東地坐标系統
- 機載北東地坐标系統
- 機體軸坐标系統
其中3、4、5都是我們模組化、設計控制律時經常使用的坐标系，描述物體（剛體）位姿資訊的6個自由度資訊都是在這三個坐标系中産生的。
機體軸坐标系的原點固連于飛行器重心，X軸指向機頭，Z軸指向機腹，X軸和Z軸都位于縱向對稱面内，而Y軸指向機身右側，與X、Z軸構成右手系，該坐标系我們用body的首字母b表示。
機載NED坐标系的原點也位于飛行器重心，其X軸指向橢球模型地理北極，Y軸指向橢球模型地理東方，Z軸沿橢球面法線向下，在絕大多數了解上都可以了解成通常的北向、東向、地向，通常該坐标系用n表示。
本地NED坐标系與機載NED坐标系唯一的不同就是原點坐标位于地面上任意一點，在分析物體的旋轉運動時一般不會用到。
旋轉關系
描述清楚姿态資訊的方式有三種：歐拉角、旋轉矩陣、四元數。
歐拉角
歐拉角是歐拉引入用來描述剛體姿态的三個角。歐拉角有靜态和動态兩種，靜态的是繞靜止的慣性坐标系三個軸進行旋轉，而動态的在旋轉過程中旋轉坐标軸會發生變化，除了第一次旋轉是繞慣性系的坐标軸進行之外，後續兩次旋轉都是動态的，并且前面旋轉的角度對後面的旋轉軸是有影響的，按照不同的軸順序進行旋轉得到的歐拉角也是不同的，旋轉變換可以歸結為若幹個沿着坐标軸旋轉的組合，組合個數不超過三個并且兩個相鄰的旋轉必須沿着不同坐标軸，總共有12種旋轉方式，分别是XYZ、XZY、XYX、XZX、YXZ、YZX、YXY、YZY、ZXY、ZYX、ZXZ、ZYZ。雖然存在12種旋轉方式，但是每一種旋轉方式都存在萬向鎖現象。
航空領域通常采用的旋轉方式是ZYX順序，下圖是一架飛機按照ZYX組合進行旋轉産生歐拉角的過程，其中，ψ為偏航角，θ為俯仰角，φ為滾轉角。
旋轉矩陣
旋轉矩陣反映了一個坐标系中的坐标在另一個坐标系中表示的轉換關系。矩陣的乘法，本質是一種運動—矩陣的實質就是将坐标整體線性變換——矢量的前移相當于坐标系的後移。這裡推薦閱讀《透析矩陣，由淺入深娓娓道來—高數-線性代數-矩陣》
關于平面向量的逆時針旋轉：
已知任意一個向量OA=(x,y),把向量OA繞其起點O沿逆時針方向旋轉α角得到向量OB=(xcosα-ysinα,xsiaα+ycosα)。推導如下：
歐拉ZYX(ψ為偏航角，θ為俯仰角，φ為滾轉角，{B}是繞着{B}的軸旋轉的)變換拆解。隻要θ≠±90°，歐拉角可以描述清楚任何剛體的姿态以及角運動資訊
若使用X-Y-Z固定角法對姿态進行描述，首先将坐标系{B}和一個已知參考坐标系{A}重合。先将{B}繞XA旋轉γ角，再繞YA旋轉ψ角，最後繞ZA旋轉θ角。（{B}是繞着{A}的軸旋轉的）
這兩種方法得到的最終旋轉矩陣一緻，即有對偶關系，記這樣得到的旋轉矩陣為:[r11,r21,r31;r12,r22,r32,r13,r23,r33]
=>
其中atan2(y,x)表示在計算arctan(y/x)時，根據x和y的符号判别角度所在象限。注意當ψ=90°時 ,θ=0º，Ψ=90º，γ=atan2(r12,r22)
四元數
四元數是由愛爾蘭數學家Hamilton發明的。四元數是由1個實數加上3個複數組合而成，通常可以表示成w+xi+yj+zk或者（w，（x，y，z）），其中w、x、y、z都是實數，而i^2 = j^2 =k^2 = -1， i^0 = j^0 = k^0 = 1。那複數與四元數之間的關系、以及如何從複數這樣一個概念擴充到四元數呢？
空間中的子空間：一般而言，空間（次元>2）都存在更低維的子空間，比如二維空間中一維子空間，也就是直線；三維空間中的一維子空間和二維子空間，也就是直線和面。當超過三維的概念我們就很難去想象是什麼樣子，但四維空間一定會存在三維子空間或二維子空間。我們這裡會加一個字首超（hyper）來形容這些空間，比如高維空間中的平面我們稱為超平面。
空間和子空間的映射：我們将二維空間表示為(x,y)，當y=0時，其實可以看成是一維的，隻不過它表示成(x,0)這種形式。推到四維，(w,x,y,z)，當w=0時，(0,x,y,z)就是一個三維子空間，這也是為什麼我們可以用機關四元數對三維向量進行操作，其實我們是将三維向量映射到四維的三維子空間（w=0，這種形式也成純四元數），然後對其進行旋轉，最終得到的向量結果依然是這個三維子空間中的，因而可以映射回三維空間。
廣義球：這裡的球是廣義上的。我們在二維平面上，廣義球其實指代circle，三維空間上就是我們認知上的球，稱為two-sphere，而四維空間中廣義球其實是一個超球（hyper-sphere），又稱為three-sphere。機關向量其實就是廣義球上面的點，而機關四元數也就是three-sphere上面的點。
限制與特征向量：空間中的一點由x, y, z等參數來表示，一般來說參數的數量與維數相等，二維空間的點用{x, y}參數，四維空間的點用{x, y, z, w}參數。但是對于空間的點加以限制，則該會減少參數的數量，比如三維空間的點在某一機關球面上，原本三個參數{x, y, z}才能表達的點現在隻需要兩個參數{u, v}就可以表達。如果{u, v}是機關向量，也可以稱{u, v}是{x, y, z}的特征向量。
四元數特性
- 四元數（以後不特指四元數=機關四元數）是四維空間中一個超球上面的點，滿足w²+x²+y²+z²=1；而純四元數是四維空間在w=0時的一個子空間的點，形式為{0, q}，特别注意的是純四元數與四元數是不同的概念。
- 四元數是複數虛部擴充的結果，複數的虛部為1個，而四元數虛部有3個，且兩兩互相正交，其中實部是cosθ/2，而虛部為一個単位軸乘以sinθ/2。
- 四元數自由度并沒有四個次元，由于存在w²+x²+y²+z²=1這個限制，它的自由度其實隻有3，且每個四元數可以對應一個特征向量，即n。但請記住四元數并不是與特征向量一一對應的，後文會有說。
由于四元數存在于四維空間，是以如何利用低維資訊去了解高維資訊就顯得尤為重要。我們這裡先用三維舉個例子，三維的球用代數表示為x²+y²+z²=1，雖然球上面的點是由x，y，z三個參數來确定，但實際上我們隻需要兩個。假設取x和z表示，其中y可以通過x和z進行求解。那麼，我們将y軸資訊給隐去，隻看投影平面，如下圖所示。這張圖的意思是，整個球在XOZ平面上投影是一個圓，當球面一點投影在圓上時，y=0；投影的位置位于圓内時，則分别兩種情況，y>0處于北半球，y<0處于南半球。是以我們僅通過投影後的圓即可還原出整個球體。
讓我們推廣到四維，w²+x²+y²+z²=1中取x、y和z來表示超球。如下圖所示，四維空間投影到三維超平面（w=0）可能是一個two-sphere。當投影點在整個two-sphere的邊緣時，w一定為0，值得一提的是在這個空間内的四元數是一個純四元數。當投影點落在two-sphere的内部時，也分為兩種情況，w>0和w<0。但是我們可以發現這兩種情況下對應的特征向量是一樣的，是以我們将旋轉矩陣向四元數轉換時，是有兩個對應值的，四元數的範圍是2倍覆寫于3D旋轉（2:1 mapping）。
基本運算規律
對于四元數的運算法則，我們要清楚的有以下幾個，假設有兩個四元數分别為q1=(w1,(x1,y1,z1))和q2=(w2,(x2,y2,z2))，令v1 = (x1,y1,z1),v2= (x2,y2,z2)，則
四元數的姿态表示
了解了四元數的基本運算規律後，我們來看下它如何表征姿态，假設存在一根旋轉軸u，有一個繞u軸旋轉σ角度的這麼一個旋轉存在，那這時候代表這個旋轉的四元數是這樣子的：q=[cos(σ/2),u·sin(σ/2)]。其中u是旋轉軸的機關向量，q是一個機關四元數。
那上述這個四元數有什麼用呢，它對任何向量施加以下算子運算後可以得到該向量繞u軸旋轉σ角度後的向量：w=qvq*
其實四元數表征旋轉關系，跟旋轉矩陣的表示方法類似，隻不過它隻需要4個元素，而旋轉矩陣需要9個元素。
四元數相比其他形式變換的優點：
- 解決萬向節死鎖（Gimbal Lock）問題
- 僅需存儲4個浮點數，相比矩陣更加輕量
- 四元數無論是求逆、串聯等操作，相比矩陣更加高效
現在主流遊戲或動畫引擎都會以縮放向量+旋轉四元數+平移向量的形式進行存儲角色的運動資料。看看旋轉矩陣和四元數
首先将坐标系{B}和一個已知參考坐标系{A}重合，将{B}繞機關矢量 [公式] 旋轉θ角。旋轉矩陣
=>
其中vθ=1-cosθ
Def. 設$i^2=j^2=k^2=ijk=-1$，将ijk表示為直角坐标系，稱q=t+xi+yj+zk為一般四元數，其共轭複數 q=t-xi-yj=zk=½·(t²+x²+y²+z²)=-(q+iqi+jqj+kqk)/2
設q0=cos(θ/2) ，q1=kxSin(θ/2)，q2=kysin(θ/2),，q3=kzsin(θ/2),那麼：q0² + q1² + q3² = 1，且
具體檢視《機器人運動學：旋轉，D-H表示法與雅可比矩陣》
歐拉角、旋轉矩陣、四元數之間互相轉換
先回顧一下這三個角運動的微分方程。
歐拉角角運動方程：
旋轉矩陣角運動方程：
四元數角運動方程：
推薦閱讀《歐拉角、旋轉矩陣、四元數角運動微分方程》
四元數轉旋轉矩陣
已知四元數：q=(q1,q2,q2,q3)
旋轉矩陣為(利用羅德裡格斯公式即可)：
旋轉矩陣轉四元數
已知旋轉矩陣：[r11,r21,r31;r12,r22,r32,r13,r23,r33]
則求解四元數時根據的方法就是從四元數轉旋轉矩陣的公式中得到：
四元數轉歐拉角
歐拉角為：
但是當β角度為90度時，四元數反向計算歐拉角時會出現奇點，是無法計算的。
因為這時候簡化後的四元數是這樣的：[.707·cos[(α-λ)/2]，.707·[(α-λ)/2]，.707·cos[(α-λ)½]，.707·cos[(α-λ)½]]
是以atan2中後面那一項就變成了0：1-2×(q1²^2+q2²)=0
這時候我們通常令α=0，然後解出歐拉角的值。
歐拉角轉四元數：
已知歐拉角：α、β、γ
四元數為：