px4裡面有兩種姿態估計演算法，一種是基於ekf的，還有一種是基於mahony的，雖然mahony演算法簡單而且計算量少，但效能並不輸ekf多少。這裡我們講一下基於mahony的姿態估計演算法，本來我還想在正式開始前為大家補充一下座標系、四元數、尤拉角和旋轉矩陣等一些基礎知識，不過筆者最近又是期末考試又是啥的，破事挺多，有點忙，也考慮到如果把這些加進去的話會使重心不突出，所以就放棄了這個想法，如果以後有機會的話我們可以再補上。
那我們就正式開始吧。
在講**之前先不做解釋地丟擲一張流程圖和幾個公式，當後面**看不懂時，記得回來翻一下。
基於mahony演算法的姿態估計模組對應於attitude_estimator_q_main.cpp這個原始檔，前面一堆**只是簡單地獲取資料和引數等，並沒有什麼實質性地內容，我們把目光放到演算法的具體實現上，以**注釋作為講解，但是截圖有點小，有時候一些很重要的內容沒辦法展開，我會在圖的後面加上文字做補充。我擷取了兩個函式，乙個是初始化函式，另乙個是姿態更新函式，下面我們分別來討論一下。
首先從初始化開始，這個函式會使用加速度計和磁力計計算出代表飛行器姿態的最初的四元數，具體過程如圖。
接下來是獲取到感測器資料後的姿態更新，update()函式比上面那個稍微複雜一點，也有幾個稍微難理解的地方，我會在下面標註。在這部分開始前，請再回過頭去看一下流程圖和那幾個公式，以方便理解下面的內容。
由四元數微分方程可知，若初始四元數沒有誤差，則四元數（代表姿態）誤差來自
求解四元數導數時所用到的角速率，角速率的微小偏差經過累積之後會形成很大的姿態偏差，所以我們需要借助加速度計、磁力計等對陀螺儀進行校正，經過多次迴圈後，校正後的角速率逐漸趨向真值（類似於加了乙個積分環節）。
下面正式開始這部分內容。
第563-573行**，使用磁力計補償：
若角速率沒有誤差，則表示姿態的四元數也不會誤差，這樣使用該四元數把磁力計的值旋轉到ned座標系中，arctan值補償磁偏角之後將為0，若不為0，則代表四元數（即姿態）有誤差，可以用這個值代表誤差，來補償n系偏航角角速率分量，構造補償向量變換到機體座標系中補償陀螺儀得到的角速度。
第583-589行**，使用加速度計補償：
r'*(0,0,1)'拆開就是上面的一堆，意思是把重力加速度向量從n系旋轉到b系，而加速度計測量值扣除機體自身的加速度就是重力加速度，如果上面的旋轉中四元數沒有誤差，則測得的重力加速度和旋轉得到的重力加速度應該的重合的，即二者之間的叉乘為0，若不為零，則代表四元數（即姿態）有誤差，這樣叉乘得到的值就可以用來表示誤差，取相反數加權重即可得到補償向量。
第593行，_gyro_bias中存放的是陀螺儀偏置，就是對它的補償值，每次執行update()函式，它都會累加本次對陀螺儀的補償值，相當於乙個積分環節，而我們都知道，積分環節能夠消除靜差，所以這樣經過幾次迴圈後，_gyro_bias會逐漸趨向真值，姿態估計誤差也變得非常小。
px4的姿態估計就先講到這裡，講的不是很詳細，不過應該也很容易理解，畢竟就這麼一丟丟。
流程圖是筆者親手畫的，所有的注釋也都是筆者親手加上去的，如果有問題還請多多包涵，另外尊重
原創 哦
這是筆者今天發的第二篇部落格，還有一些積累下來的東西，以後如果有時間的話會繼續分享給大家的
postek二次開發 POSTEK二次開發介面
例項簡介 例項截圖 核心 using system using system.collections.generic using system.componentmodel using system.data using system.drawing using system.text using ...
基於leveldb,levigo做二次開發
leveldb是乙個c 庫，它有提供標準的c介面，標頭檔案在include leveldb c.h中 開發過程，以新增乙個簡單的計算功能為例 修改leveldb原始碼 在include leveldb c.h中新增以下宣告 extern int andre add const int a,const...
mcms二次開發 YzmCMS二次開發手冊
摘要 yzmcms是採用mvc設計模式開發,基於模組和操作的方式進行訪問，採用單一入口模式進行專案部署和訪問，無論訪問任何乙個模組或者功能，只有一.yzmcms是採用mvc設計模式開發,基於模組和操作的方式進行訪問，採用單一入口模式進行專案部署和訪問，無論訪問任何乙個模組或者功能，只有乙個統一的入口...