MPU6050主要包含陀螺儀和加速度計。陀螺儀主要測量角速度，即可以測出某一時間段物體轉過的角度。加速度計測量的是物體的加速度，重力加速度即物體受重力作用的情況下具有的加速度，物體靜止時，加速度計測出來的加速度等於重力加速度g，約等於9.8米每平方秒，重力加速度總是豎直向下，通過計算重力加速度在X,Y的分量，則可以計算出物體相對於水平面的傾斜角度。

我們先來看看如何用尤拉角描述旋轉(座標變換)：

繞Z軸旋轉即滾轉角，繞Y軸旋轉即偏航角，繞X旋轉即俯仰角。

這裡需要注意繞X,Y,Z軸方向旋轉的先後順序不一樣，得出餘弦矩陣的順序（也就是公式）也不一樣。並且在一組旋轉裡X,Y,Z不可交換(Euler旋轉

Gimbal Lock總結就是，其源自Euler旋轉原理，此原理旋轉變數不可交換，所以有先後之分，所以可以改變後兩個軸而第一軸方向不變，所以產生軸共線，即Gimbal Lock。

上圖中得出旋轉後的尤拉角公式，但是無人機的姿態結算中不能用尤拉角公式計算，一方面是因為尤拉角微分方程中包含了大量的三角運算，這給實時結算帶來了一定的困難。而且當俯仰角為90度時方程式會出現神奇的“Gimbal Lock”。所以尤拉角方法只適合水平姿態變化不大的情況，而不適用于飛行器的姿態確定。

下面對四元數姿態進行結算：

1. 重力加速度歸一化。即將加速度計的三維向量（ax,ay,az）轉換為單位向量，因為是單位向量到參考性的投影，所以要把加速度計資料單位化。歸一化只是改變這三個向量的長度，方向不改變，也就是隻改變了相同的倍數，只是為了與單位四元數對應。ax,ay,az是機體座標參照系上，加速度測出來的重力向量。
2. 四元數換成方向餘弦中的第三行的三個元素。將當前姿態重力在三個軸的分量分離出來，把四元數換算成方向餘弦中的第三行的三個元素(vx,vy,vz)。慣性測量器件測量的都是關於b系的值。vx,vy,vz是陀螺儀的值積分後的姿態來推算出的重力向量。
3. 向量叉積得出姿態誤差。將ax,ay,az和vx,vy,vz對應的進行向量叉積（向量外積、叉乘）。分別得出ex,ey,ez。這個叉積向量仍然是位於機體座標系，並和積分誤差成正比，正好矯正陀螺。
4. 對誤差進行積分。將ax,ay,az和vx,vy,vz的誤差進行積分消除誤差。
5. 進行濾波。陀螺儀測的值不斷的進行更新，相應的積分誤差也不斷的修正，最後將積分誤差反饋給陀螺儀，修正陀螺儀的值。
6. 四元數微分方程。通過一階龍格庫塔法更新四元數。
7. 四元數歸一化。對四元數的單位化，單位化的四元數可以表示一個完整的旋轉，只有單位四元數才可以表示旋轉，這就是四元數表示旋轉的約束條件。
8. 四元數轉尤拉角。

在加速度計矯正的基礎上+磁力計：
在三維空間中，根據重力加速度，加速度為我們提供一個水平位置的參考，但是無法獲得方向的參考，這時就需要磁力計，它能給人們提供一個正北方向的絕對參考。

在上述姿態結算的基礎上進行磁力計矯正：

1. 把磁力計的資料進行歸一化處理（進行量化）。
2. 根據當前四元數姿態值估算各重力分量vx,vy,vz，再根據vx,vy,vz預估磁場的方向wx,wy,wz。
3. 再根據wx,wy,wz對磁力計測出的值進行誤差矯正。
4. 把加速度計和磁力計修正後的陀螺儀資料整合到四元數中。
5. 最後進行角度運算。

以上僅為本人的理解，僅供參考，如有細節上的錯誤希望大神指點指點！