ArduPilot——AP_NavEKF3针对AP_NavEKF2做了哪些改进/变动？

版权声明：本文为博主原创博文，未经允许不得转载，若要转载，请说明出处并给出博文链接

本篇博文主要针对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的最新动态
                 ⑳其他范围信标更新

 

                 ...........................................等等小细节

 

 

              这些具体的改变可以通过查看代码一点一点去感受，这里就不继续罗嗦了。

 

              如有错误，还望指正！！！