在px4中，採用三軸陀螺，三軸加計，三軸磁力計採用ekf進行姿態解算。
（現在在examples/attitude_estimator_ekf裡面，在cmake裡面並沒有開啟，nuttx_px4fmu-v2_default裡面用的是ekf2）
該演算法中估計的是機體系下的量。
定義機體座標系（前右下），導航座標系（北東地）。
一、 概念定義
卡爾曼濾波，可以簡單理解為先通過運動狀態方程求得先驗狀態，再根據觀測方程求得後驗狀態。與貝葉斯概率中的先驗概率和後驗概率有些相像。
貝葉斯概率更新有關的兩個概念。假如某一不確定事件發生的主觀概率 因為某個新情況的出現 而發生了改變，那麼改變前的那個概率就被叫做先驗概率，改變後的概率就叫後驗概率。舉個簡單的更新概率的例子。
想象有 a、b、c 三個不透明的碗倒扣在桌面上，已知其中有（且僅有）乙個瓷碗下面蓋住乙個雞蛋。此時請問，雞蛋在 a 碗下面的概率是多少？答曰 1/3。現在發生一件事：有人揭開了 c 碗，發現 c 碗下面沒有蛋。此時再問：雞蛋在 a 碗下面的概率是多少？答曰 1/2。注意，由於有「揭開c碗發現雞蛋不在c碗下面」這個新情況，對於「雞蛋在 a 碗下面」這件事的主觀概率由原來的 1/3 上公升到了1/2。這裡的先驗概率就是 1/3，後驗概率是 1/2。
也就是說「先」和「後」是相對於引起主觀概率變化的那個新情況而言的。
二、 演算法解析
1 狀態矩陣：
其中： w為機體角速度； c為機體角加速度（取acceleration的第二個字母）； a為機體加速度； m為機體磁力計；
2 量測矩陣：
其中： 為三軸陀螺儀輸出； 為三軸加計輸出； 為三軸磁力計輸出；
3 狀態方程：
該演算法中，主要解決狀態方程的問題。先寫出狀態方程如下：
其中：
上式中比較難理解的是加速度和磁力兩項的狀態方程是怎麼得到的。以加計為例進行分析。
根據方向余弦矩陣的微分方程，加速度的微分方程可以表示為：
因此，在當對 求導時就會存在 和 矩陣。對 求導時會存在 和 矩陣。
（微分方程的理解：機體系下加計和磁力計的變化有兩部分，即方向變化和大小變化。這也對應了 中的內容，即對角速度的偏導和對自身的偏導兩部分。）
則一步轉移矩陣為：
4 量測方程：
5 姿態計算：
根據上述ekf計算得到狀態矩陣之後。
在提取狀態資訊這裡需要說明的是：
1、 加速度向量（7:9）反應的是姿態轉移矩陣的z列是沒有問題的；
2、 按理說，如果定義導航座標係為北東地，那麼只有xn、zn向是有磁力資訊的，yn向為零。那麼由姿態轉移矩陣的z列與機體磁力計叉乘出來的應該是姿態轉移矩陣的x列，但下面公式裡卻是y列，沒想明白。不知道是不是定義的導航座標系是東北天。但由後面尤拉角的計算來看，座標系應該是北東地才對。
(犯懶截圖)
LPMS IMU姿態解算
參考文章 ahrs姿態解算說明 加速度 陀螺儀 磁力計原理及原始資料分析 ahrs俗稱航姿參考系統，ahrs由加速度計，磁場計，陀螺儀構成，ahrs的真正參考來自於地球的重力場和地球的磁場 他的靜態終精度取決於對磁場的測量精度和對重力的測量精度 而則陀螺決定了他的動態效能。如何用加速度計和陀螺儀的資...
姿態解算解析
用四元素法進行姿態解算，其步驟如下 1.四元素初始化 第一次解算時計算 靜止狀態下由加速度，磁力計值求取初始 gamma theta psi gamma frac theta frac psi frac 初始化四元素 left begin end right left begin cos frac ...
Pixhawk學習7 位置解算
pixhawk的位置解算分為兩部分，第一部分主要為感測器的資料獲取，而該部分最主要的就是gps資料的提取。第二部分為與慣性器件之間的組合導航。組合導航的好處我就不用多說了。pixhawk 中目前主要有兩處組合導航的 1 firmware src examples ekf att pos estima...