在姿態解算入門系列裡 Sugar 以初中數學為基礎展開了用加速度計的姿態解算方法。上一篇《姿態解算進階之理解旋轉矩陣(也稱方向餘弦矩陣)》進行了一次理論進階。本篇 Sugar 用進階的理論重寫一下用加速度計做姿態解算的方法，然後與入門級算法對比一下結果有哪些改進。
姿態角定義的悄然改變在《MPU6050 姿態解算系列一：加速度姿態解算》一文中 Sugar 有說明：
1、橫滾角 roll 指：重力加速度與「X-Z 平面」的夾角；
2、俯仰角 pitch 指：重力加速度與「Y-Z 平面」的夾角。
通過我們本文的算法升級，這樣的定義也會有所改變，具體怎麼個變法讀者掌握本文算法後自然會知道。這裡 Sugar 放前面先說，目的是起到一個思維導向作用，防止思維陷在之前的定義裡出不來從而影響對本文算法的理解。
本文姿態角的定義將採用飛控上常用的定義方法：繞 X 軸轉是橫滾角 Roll、繞 Y 軸轉是俯仰角 Pitch、繞 Z 軸轉是偏航角 Yaw。
在《姿態解算進階之理解旋轉矩陣(也稱方向餘弦矩陣)》中 Sugar 介紹了旋轉矩陣。既然用旋轉矩陣能對三維空間內的向量進行任意旋轉，那麼我們不難知道：加速度計在各個軸上的分量就是通過在「豎直的加速度向量」上施加旋轉而得到的。
在姿態解算上我們關心的是向量角度而非向量模長，為了能夠順利的使用反三角函數做姿態解算，我們必須首先對加速度計數據進行單位化，保證反三角函數有實數解。假設上圖的紅色箭頭(直線的那個)是通過對單位向量[0;0;1]施加三維旋轉得到的。為了便於表示，我們做如下約定：
% cr:cos(roll) sr:sin(roll)% cp:cos(pitch) sp:sin(pitch)% cy:cos(yaw) sy:sin(yaw)
from_euler_xyz = [ cp*cy, cy*sp*sr - cr*sy, sr*sy + cr*cy*sp][ cp*sy, cr*cy + sp*sr*sy, cr*sp*sy - cy*sr][ -sp, cp*sr, cp*cr]在這樣的約定下，我們有：
[加速度計單位化後的讀數] = from_euler_xyz * [0;0;1];
記加速度計單位化後的讀數為[Ax;Ay;Az]，將上面方程的右側展開，可得：
Ax = sr*sy + cr*cy*sp Ay = cr*sp*sy - cy*sr Az = cp*cr這個解告訴我們一個現象：在這個旋轉矩陣的作用下，單位向量在 Z 軸上的投影只與橫滾角 roll和俯仰角 pitch有關。深刻理解這個現象是本篇算法的一個很重要的關鍵點。下面 Sugar 以roll 30 度、pitch 30 度為例，用幾張動圖形象地表述一下這個現象：
對於這個 3D 旋轉我們看一下X-Y 方向的投影，如下：
我們看到一個形狀不變的三角形在轉動，說明：旋轉過程過中所有姿態的 roll 和 pitch 都是相同的。我們再來看一下X-Z 方向的投影，如下：
上圖表明上述 Ax、Ay、Az 的解所描述的是：一組法向量與豎直方向夾角相等的姿態。下面 Sugar 以劍影分光術的形式更清楚地表現一下這個現象，為了不讓眾多劍影的出現太突然，先用一成功力弄出兩個劍影：
然後提高功力，輸出一圈劍影：
已經眼花繚亂了，但紅色的各個姿態的法向量有一個明顯特徵：傘骨狀。為了體現量的特點，Sugar 選用了 pitch 60 的一組姿態且法向量在姿態的兩側都是單位向量，我們看一下X-Z視圖表現出的量的特點：
又是動圖又是劍影分光術的，Sugar 要表達的一個重點就是：在 from_euler_xyz 旋轉矩陣的作用下，各個偏航角不同的姿態對應的橫滾角 roll 和俯仰角 pitch 都是跟偏航角為 0 的姿態是一樣的。因為用 IMU 無法估算偏航角 Yaw，不妨隨意標定一個姿態是 Yaw 為 0 的姿態，這樣就有 sy = 0、cy = 1，這樣 Ax、Ay 和 Az 就是：
Ax = cr*sp Ay = -sr Az = cp*cr至此，我們就得到了用加速度計解算姿態的新公式：
r = -asin(Ay)p = atan2(Ax,Az)進階算法效果有什麼改善Sugar 在 MATLAB 同時繪出進階算法和入門級算法的姿態對比圖，主要代碼如下：
效果對比如下：
為什麼 pitch 是正負 180 度我們知道反正切函數atan()的值域是-90 度 ~ 90 度，這個公式算出的怎麼會超過這個值域範圍的？原因是：這裡我們使用的是四象限反正切。
四象限反正切函數的特點：四象限反正切並不是 MATLAB 獨有的，ArduPilot 代碼裡也是用四象限反正切函數做姿態解算的。