知乎使用者2016-02-21 09:56:19

多圖！

題主沒有說明是原生韌體還是APM版的韌體，原生韌體已經取消EKF姿態解算了，官方解釋的原因是佔RAM太大。估且就認為你問的是原生韌體EKF姿態解算演算法的步驟吧，簡單的介紹下，看不明白，以及相關理論不清楚的查閱介紹慣性導航的書籍即可，應該沒多少人需要真正研究演算法的吧~

首先是建模，設狀態變數，

其中w為體系角速度，wa為體系角加速度，ze為重力向量，mu為磁場向量。如果使用慣性矩陣修正當前角加速度，那麼：

則機體系角速度為：

再根據機體座標系相對地理座標系的旋轉角速度變換矩陣：

如果採用指數函式的一階泰勒近似有：

設

則狀態轉移A矩陣為：

過程噪聲矩陣為：

分別表示陀螺角速度過程噪聲，角加速度過程噪聲，加速度過程噪聲，磁力計過程噪聲。

協方差陣為：

測量噪聲矩陣：

觀測矩陣H為：

測量方程為：

引入測量噪聲：

EKF的卡爾曼增益矩陣：

狀態估計：

更新協方差矩陣：

提取狀態資訊：

最後得到參考系和機體系的變換矩陣：

轉換成尤拉角：

EKF方法計算量大，在計算狀態轉換陣和基礎矩陣時採用了一階近似，這種近似誤差較大，如果計算量允許，可以用2-3階。

原生韌體有關EKF解算的，嵌入式程式碼是matlab自動生成的，千萬不要直接看C程式碼。。。。

大熊2017-04-19 22:32:32

這個程式碼，現在更新到什麼版本了，我記得看的EKF那部分程式碼，總狀態變數一共34個，參與EKF濾波的一共24個吧。至於樓主的問題，尤拉角的獲取，不是都一樣嗎，反解姿態矩陣中的三角函式就可以了。

王敬敏2018-06-30 17:20:50

姿勢形式也會用到！GPS資訊指南針資料沒錯~除非您把GPS插頭拔了！或許手動形式GPS才不會參加作業！~那我就不理解了，GPS和指南針是規劃在一起的，拿NAZA套裝來說：一、假如沒插GPS，則飛控只作業在姿勢或手動兩個形式，切當地說是作業在沒有指南針的姿勢形式，這一點說明書裡寫的很清楚；二、假如插了GPS，可是切換在姿勢形式，指南針卻參加作業了，則作業在有指南針的姿勢形式。這兩個姿勢形式有什麼區別？假如飛場有攪擾，可能導致GPS訊號不良或指南針被攪擾，使用者飛翔前也是知道的，在姿勢形式下飛翔相對算是安全的，但蛋疼的是指南針卻參加了，能不能設定為姿勢形式下指南針不參加？也就是沒插GPS的姿勢形式？

瑤瑤qwq2018-06-30 23:38:19

姿勢形式也會用到！GPS資訊指南針資料沒錯~除非您把GPS插頭拔了！或許手動形式GPS才不會參加作業！~那我就不理解了，GPS和指南針是規劃在一起的，拿NAZA套裝來說：一、假如沒插GPS，則飛控只作業在姿勢或手動兩個形式，切當地說是作業在沒有指南針的姿勢形式，這一點說明書裡寫的很清楚；二、假如插了GPS，可是切換在姿勢形式，指南針卻參加作業了，則作業在有指南針的姿勢形式。這兩個姿勢形式有什麼區別？假如飛場有攪擾，可能導致GPS訊號不良或指南針被攪擾，使用者飛翔前也是知道的，在姿勢形式下飛翔相對算是安全的，但蛋疼的是指南針卻參加了，能不能設定為姿勢形式下指南針不參加？也就是沒插GPS的姿勢形式？

中箭的冒險家2018-07-01 18:07:45

那我就不理解了，GPS和指南針是規劃在一起的，拿NAZA套裝來說：一、假如沒插GPS，則飛控只作業在姿勢或手動兩個形式，切當地說是作業在沒有指南針的姿勢形式，這一點闡明書裡寫的很清楚；二、假如插了GPS，可是切換在姿勢形式，指南針卻參加作業了，則作業在有指南針的姿勢形式。這兩個姿勢形式有什麼區別？假如飛場有攪擾，可能導致GPS訊號不良或指南針被攪擾，使用者飛翔前也是知道的，在姿勢形式下飛翔相對算是安全的，但蛋疼的是指南針卻參加了，能不能設定為姿勢形式下指南針不參加？也就是沒插GPS的姿勢形式？那我就不理解了，GPS和指南針是規劃在一起的，拿NAZA套裝來說：一、假如沒插GPS，則飛控只作業在姿勢或手動兩個形式，切當地說是作業在沒有指南針的姿勢形式，這一點闡明書裡寫的很清楚；二、假如插了GPS，可是切換在姿勢形式，指南針卻參加作業了，則作業在有指南針的姿勢形式。這兩個姿勢形式有什麼區別？假如飛場有攪擾，可能導致GPS訊號不良或指南針被攪擾，使用者飛翔前也是知道的，在姿勢形式下飛翔相對算是安全的，但蛋疼的是指南針卻參加了，能不能設定為姿勢形式下指南針不參加？也就是沒插GPS的姿勢形式？

檢查原帖>>