版權聲明：本文爲博主原創博文，未經允許不得轉載，若要轉載，請說明出處並給出博文鏈接
本篇博文主要針對Paul Riseborough大神爲ArduPilot做的導航EKF3算法，相比較EKF2算法有哪些改進/變動？
關於Paul大神的慣性導航相關代碼可以見鏈接：https://github.com/priseborough/InertialNav （包括Matlab/C++源程序，以及如何用matlab派生c++程序，最重要的是有EKF2/EKF3的數學模型，對於理解ardupilot的導航算法很有幫助）
上圖爲ArduPilot導航算法的迭代更新，目前主流採用EKF2 or EKF3.
EKF3的改進：
一、最爲關鍵的數學模型的改變：
①用四元數（4個狀態）取代姿態等效旋轉矢量（3個狀態）
②刪除了陀螺儀比例因子這個狀態
③添加了X、Y兩個方向上的加速度增量速度的偏差估計
④做了相關初始校正
⑤添加了GPS機體框架的偏移補償
大神自己也承認上述①②③的調整是爲了獲取更精確的姿態，但是可能會出現難以修復由於錯誤的IMU數據導致的極端的姿態誤差。
二、一些控制邏輯等其他的改變
①修復錯誤併合並輔助開關邏輯
切入和切出輔助模式在多個地方進行，並使用兩個變量。
由於長時間丟失GPS而導致的GPS模式不能正常工作。
這鞏固了邏輯並確保只通過setAidingMode函數更改PV_AidingMode。
②防止每個濾波器更新發生多個融合模式的更改
③更新調整默認值
④修復導致切入和切出輔助的錯誤
如果GPS接收機斷開連接並且沒有收到任何數據，那麼gpsGoodToAlign檢查沒有機會運行，並且因爲之前的EKF會切換回輔助狀態。通過確保在未執行檢查時gpsGoodToAlign默認爲false，可以防止出現這種情況。在我們宣佈準備使用GPS之前，還會額外檢查確保有GPS數據融合。
⑤修復防止飛機從壞磁力儀恢復的錯誤
這個錯誤創造了一個競賽條件，如果EKF必須重置偏航到GPS地面路線以從壞磁力儀中恢復，那麼當飛機達到預定重置的高度時，新的航向可能被不良磁標記覆蓋。
⑥改進切換到備用磁強計
切換到備用磁力儀時，確保地球磁場估算值已重置。否則由於前面的失效磁鐵而導致的磁場估計可能會導致新磁鐵的數據被拒絕。
⑦啓用自動使用測距儀高度
⑧修復處理無效範圍數據的錯誤
⑨使用無GPSAP_NavEKF3的測距儀修正地面高度漂移：
⑩處理由核心交換機導致的偏航跳躍
⑪啓用同時使用的GPS和光流
⑫修復控制檯狀態報告
⑬將消息發送到mavlink而不是控制檯
這使得GCS能夠更好地處理向用戶顯示的消息。
⑭替換棄用的函數調用
⑮補償傳感器機身框架偏移
⑯修復中值過濾器代碼中的錯誤
⑰在EKF3的位置偏移量中保存一些內存
我們不需要爲每個樣本複製vector3f。 uint8_t完成這項工作
⑱添加範圍信標數據的融合
⑲帶來EKF2的最新動態
⑳其他範圍信標更新
...........................................等等小細節
這些具體的改變可以通過查看代碼一點一點去感受，這裏就不繼續羅嗦了。
如有錯誤，還望指正！！！