PX4裡面有兩種姿態估計演算法，一種是基於EKF的，還有一種是基於mahony的，雖然mahony演算法簡單而且計算量少，但效能並不輸EKF多少。這裡我們講一下基於mahony的姿態估計演算法，本來我還想在正式開始前為大家補充一下座標系、四元數、尤拉角和旋轉矩陣等一些基礎知識，不過筆者最近又是期末考試又是啥的，破事挺多，有點忙，也考慮到如果把這些加進去的話會使重心不突出，所以就放棄了這個想法，如果以後有機會的話我們可以再補上。

    那我們就正式開始吧。

    在講程式碼之前先不做解釋地丟擲一張流程圖和幾個公式，當後面程式碼看不懂時，記得回來翻一下。

    基於mahony演算法的姿態估計模組對應於attitude_estimator_q_main.cpp

初始化：AttitudeEstimatorQ::init()

    首先從初始化開始，這個函式會使用加速度計和磁力計計算出代表飛行器姿態的最初的四元數，具體過程如圖。

姿態更新：AttitudeEstimatorQ::update()

   接下來是獲取到感測器資料後的姿態更新，update()函式比上面那個稍微複雜一點，也有幾個稍微難理解的地方，我會在下面標註。在這部分開始前，請再回過頭去看一下流程圖和那幾個公式，以方便理解下面的內容。

    由四元數微分方程可知，若初始四元數沒有誤差，則四元數（代表姿態）誤差來自求解四元數導數時所用到的角速率，角速率的微小偏差經過累積之後會形成很大的姿態偏差，所以我們需要藉助加速度計、磁力計等對陀螺儀進行校正，經過多次迴圈後，校正後的角速率逐漸趨向真值（類似於加了一個積分環節）。

    下面正式開始這部分內容。

第563-573行程式碼，使用磁力計補償：
  若角速率沒有誤差，則表示姿態的四元數也不會誤差，這樣使用該四元數把磁力計的值旋轉到NED座標系中，arctan值補償磁偏角之後將為0，若不為0，則代表四元數（即姿態）有誤差，可以用這個值代表誤差，來補償N系偏航角角速率分量，構造補償向量變換到機體座標系中補償陀螺儀得到的角速度。

第583-589行程式碼，使用加速度計補償：
  R'*(0,0,1)'拆開就是上面的一堆，意思是把重力加速度向量從N系旋轉到B系，而加速度計測量值扣除機體自身的加速度就是重力加速度，如果上面的旋轉中四元數沒有誤差，則測得的重力加速度和旋轉得到的重力加速度應該的重合的，即二者之間的叉乘為0，若不為零，則代表四元數（即姿態）有誤差，這樣叉乘得到的值就可以用來表示誤差，取相反數加權重即可得到補償向量。

第593行，_gyro_bias中存放的是陀螺儀偏置，就是對它的補償值，每次執行update()函式，它都會累加本次對陀螺儀的補償值，相當於一個積分環節，而我們都知道，積分環節能夠消除靜差，所以這樣經過幾次迴圈後，_gyro_bias會逐漸趨向真值，姿態估計誤差也變得非常小。

哦

    這是筆者今天發的第二篇部落格，還有一些積累下來的東西，以後如果有時間的話會繼續分享給大家的