PX4 代碼中 position_estimator_inav（互補濾波）理解

Local_position_estimator 爲卡爾曼濾波估計
position_estimator_inav 需要解決三個問題：

1. 如何由加速度進行速度和位置估計；

2. 加速度偏差的計算；

3. 如何使用融合過程。
   借用網上一個圖詮釋下
   

   這裏px4_poll()函數設定最小50Hz頻率，用來保證姿態數據能夠實時獲取到，根據返回值判斷是否符合要求，不符合要求則mavlink_log_info()函數來打印"[inav] poll error on init"信息。
   對於滿足條件的情況，開始實際操作：
   1.相關數據更新
   2.檢查相關主題的超時
   3.檢查位置估計工具的有效性
   4.使用位置估計工具更新傳感器補償
   5.對高度和位置進行濾波處理
   6.地形估計和發佈數據

先得到每個Sensor的Corr，然後進行判斷是否有效–>是否採用進行彌補–> 給予多大的權重 --> 累加到一起得到–>bias
然後 加速度計值減去bias得到準確值。最後一次積分得到速度，二次積分得到位置信息。