實現了基於四元數的姿態外環PID控制器（實際上是P+前饋）。
類的定義及實現存放在：mc_att_control/AttitudeControl
控制器輸入為NED坐標系下四元數表示的當前姿態q和期望姿態qd，輸出為機體坐標系下三軸的角速度。
如何由NED坐標系下表示的q和q_d得到在機體坐標系下的姿態誤差是關鍵，本來這個誤差由q.inversed() * qd就可以得到，但是考慮了優先控制roll和pitch，就有壹些復雜，下面進行解釋。
註：網上查到的這部分的源碼解讀大部分是舊版本的，本文基於V1.10.1版本。
作用是給P參數賦值。
由於控制策略是優先控制roll和pitch，所以並不是直接把proportional_gain賦值給私有變量_proportional_gain。
具體方式為:
給控制器輸出的角速度設置limit
PID控制器的實現。
將q和qd單位化
q.dcm_z()得到的是將q轉化為方向余弦矩陣，並取第三列形成的向量。
所以根據方向余弦的定義，e_z和e_z_d即為當前機體坐標系和期望機體坐標系的z軸在NED坐標系下的表示。
第三行調用了Quatf類的其中壹種構造函數，qd_red代表的是從e_z旋轉到e_z_d的四元數。
可以這樣理解：當前姿態坐標系經過qd_red代表的旋轉之後，兩個坐標系（當前姿態坐標系和期望姿態坐標系）的z軸是對齊的。我們把這個z軸對齊的狀態定義為壹個中間狀態。可以認為 qd_red包含了roll和pitch的誤差信息 。
註意此時qd_red只是代表了壹個旋轉（q->qd），並不像q和qd代表了姿態（NED->q和NED->qd）。
由於於e_z和e_z_d是在NED坐標系下的向量，所以得到的qd_red也是在NED下表示的。
接下來是對壹種極端情況的考慮
q_mix代表從中間狀態qd_red到期望姿態qd的旋轉四元數 在qd_red系 下的表示，可以表征yaw的誤差。下面進行解釋。
對q_mix規範化。
因為同壹個旋轉可以由互為相反數的兩個四元數表示。為了避免奇異，進行統壹表示。
由前面我們知道，q_mix代表從中間狀態qd_red到期望姿態qd的旋轉四元數在qd_red系下的表示，可以表征yaw的誤差。，由於中間狀態qd_red坐標系與期望姿態qd坐標系的z軸是重合的，所以q_mix在qd_red系下的表示應該是 ， 為旋轉角，即只有q_mix(0)和q_mix(3)的值是我們需要的。
所以經過了_yaw_w參數作用後的，在反推回來得到的qd與最開始給定的qd以經有所變化，即最後算出來的roll和pitch的期望角速度相同，但計算出來的yaw的期望角速度根據_yaw_w的大小會有所減小，這樣就實現了優先控制roll和pitch。
qe即為從q旋轉到qd的的四元數在機體坐標系下的表示，即誤差。
使用sin(alpha/2)縮放的旋轉軸作為姿態誤差(參見軸角定義的四元數)
還要考慮對映單元四元數的歧義性
得到期望的三軸角速度（用P參數乘以誤差），並且加上yaw的前饋。
yawspeed_feedforward是繞NED系z軸旋轉的前饋增益，但是我們需要在機體坐標系中應用它。
因此，我們通過取R的最後壹列來推斷出世界z軸(在body frame中表示)。將其取逆(== q.inversed)並將其乘以偏航設定值率(yawspeed_feedforward)。
這就產生了壹個在機體系下表示的前饋的向量。
限幅，返回期望角速度。
整個過程大致為：
這樣壹系列操作，可以達到優先控制roll&pitch的效果。
有慣性坐標系 ，以及表示在 下的兩個單位四元數 ，此時若存在 ，滿足：
那麽 有什麽幾何意義?
初次接觸，如有錯誤歡迎批評指正。