• Ardupilot — EKF3使用光流室内定位代码梳理

    文章目录

    前言

    1 Copter.cpp

    1.1 void IRAM_ATTR Copter::fast_loop()

    1.2 void Copter::read_AHRS(void)

    1.3 对象ahrs说明

    2 AP_AHRS_NavEKF.cpp

    2.1 void AP_AHRS_NavEKF::update(bool skip_ins_update)

    2.2 void AP_AHRS_NavEKF::update_EKF3(void)

    2.3 对象EKF3说明

    3 AP_NavEKF3.cpp

    3.1 void IRAM_ATTR NavEKF3::UpdateFilter(void)

    3.2 对象core说明

    4 AP_NavEKF3_core.cpp

    4.1 void IRAM_ATTR NavEKF3_core::UpdateFilter(bool predict)

    5 AP_NavEKF3_Control.cpp

    5.1 void NavEKF3_core::controlFilterModes()

    5.2 void NavEKF3_core::setAidingMode()

    6 AP_NavEKF3_OptFlowFusion.cpp

    6.1 void NavEKF3_core::SelectFlowFusion()

    6.2 void NavEKF3_core::FuseOptFlow()

    前言

    故事的开始，要从参数 EK3_FLOW_USE 说起。

    注意：该参数适用于高级用户。

    控制是否将光流数据融合到 24 状态导航估算器或 1 状态地形高度估算器中。

    RebootRequired

    Values

    True

    Value

    Meaning

    0

    None

    1

    Navigation

    2

    Terrain

    本文主要梳理一下，在旋翼中 EKF3 的整个运行流程，以及在哪一步融合光流数据进行室内定位飞行。

    前置参数：

    1、AHRS_EKF_TYPE = 3；

    使用 EKF3 卡尔曼滤波器进行姿态和位置估算。

    2、EK3_GPS_TYPE = 3；

    禁止使用 GPS - 当在 GPS 质量较差、多径误差较大的环境中使用光流量传感器飞行时，这一点非常有用。

    1 Copter.cpp

    1.1 void IRAM_ATTR Copter::fast_loop()

    Ardupilot 代码中，需求资源多，运算频率高的任务，一般在 fast_loop() 函数中。这里我们只展示和 EKF3 运行相关的代码段。

    运行 EKF 状态估算器（耗资巨大）。

    1. // Main loop - 400hz
    2. void IRAM_ATTR Copter::fast_loop()
    3. {
    4.     ...
    5.     // run EKF state estimator (expensive)
    6.     // --------------------
    7.     read_AHRS();
    8.     ...
    9. }

    1.2 void Copter::read_AHRS(void)

    读取姿态航向参考系统信息的入口函数。

    我们告诉 AHRS 跳过 INS 更新，因为我们已经在 fast_loop() 中进行了更新。

    1. void Copter::read_AHRS(void)
    2. {
    3.     // Perform IMU calculations and get attitude info
    4.     //-----------------------------------------------
    5. #if HIL_MODE != HIL_MODE_DISABLED
    6.     // update hil before ahrs update
    7.     gcs().update_receive();
    8.     gcs().update_send();
    9. #endif
    10.  
    11.     // we tell AHRS to skip INS update as we have already done it in fast_loop()
    12.     ahrs.update(true);
    13. }

    1.3 对象ahrs说明

    在 Copter.h 中，我们用 AP_AHRS_NavEKF 类定义了 ahrs 对象。

    AP_AHRS_NavEKF ahrs{EKF2, EKF3, AP_AHRS_NavEKF::FLAG_ALWAYS_USE_EKF};

    2 AP_AHRS_NavEKF.cpp

    2.1 void AP_AHRS_NavEKF::update(bool skip_ins_update)

    所以，我们在跳转 update() 这个成员函数的时候，跳转到 AP_AHRS_NavEKF 类的 update() 函数。

    根据 AHRS_EKF_TYPE = 3，我们运行 update_EKF3()。

    1. void AP_AHRS_NavEKF::update(bool skip_ins_update)
    2. {
    3.     ...
    4.     if (_ekf_type == 2) {
    5.         // if EK2 is primary then run EKF2 first to give it CPU
    6.         // priority
    7.         update_EKF2();
    8.         update_EKF3();
    9.     } else {
    10.         // otherwise run EKF3 first
    11.         update_EKF3();
    12.         update_EKF2();
    13.     }
    14.     ...
    15. }

    2.2 void AP_AHRS_NavEKF::update_EKF3(void)

    更新 EKF3。

    1. void AP_AHRS_NavEKF::update_EKF3(void)
    2. {
    3.     ...
    4.     if (_ekf3_started) {
    5.         EKF3.UpdateFilter();
    6.     ...
    7.     }
    8. }

    2.3 对象EKF3说明

    在 AP_AHRS_NavEKF.h 中，我们用 NavEKF3 类定义了 EKF3 对象。

    NavEKF3 &EKF3;

    3 AP_NavEKF3.cpp

    3.1 void IRAM_ATTR NavEKF3::UpdateFilter(void)

    所以，我们在跳转 UpdateFilter() 这个成员函数的时候，跳转到 NavEKF3 类的 UpdateFilter() 函数。

    更新滤波器状态 - 只要有新的 IMU 数据，就应调用该函数。

    1. // Update Filter States - this should be called whenever new IMU data is available
    2. void IRAM_ATTR NavEKF3::UpdateFilter(void)
    3. {
    4.     if (!core) {
    5.         return;
    6.     }
    7.  
    8.     imuSampleTime_us = AP_HAL::micros64();
    9.  
    10.     const AP_InertialSensor &ins = AP::ins();
    11.  
    12.     bool statePredictEnabled[num_cores];
    13.     for (uint8_t i=0; i
    14.         // if we have not overrun by more than 3 IMU frames, and we
    15.         // have already used more than 1/3 of the CPU budget for this
    16.         // loop then suppress the prediction step. This allows
    17.         // multiple EKF instances to cooperate on scheduling
    18.         if (core[i].getFramesSincePredict() < (_framesPerPrediction+3) &&
    19.             (AP_HAL::micros() - ins.get_last_update_usec()) > _frameTimeUsec/3) {
    20.             statePredictEnabled[i] = false;
    21.         } else {
    22.             statePredictEnabled[i] = true;
    23.         }
    24.         core[i].UpdateFilter(statePredictEnabled[i]);
    25.     }
    26.     ...
    27. }

    3.2 对象core说明

    在 AP_NavEKF3.h 中，我们用 NavEKF3_core 类定义了 core 对象。

    NavEKF3_core *core = nullptr;

    4 AP_NavEKF3_core.cpp

    4.1 void IRAM_ATTR NavEKF3_core::UpdateFilter(bool predict)

    所以，我们在跳转 UpdateFilter() 这个成员函数的时候，跳转到 NavEKF3_core 类的 UpdateFilter() 函数。

    如果缓冲区中有新的 IMU 数据，则运行 EKF 方程，在融合时间跨度上进行估算。

    1. /********************************************************
    2. *                 UPDATE FUNCTIONS                      *
    3. ********************************************************/
    4. // Update Filter States - this should be called whenever new IMU data is available
    5. void IRAM_ATTR NavEKF3_core::UpdateFilter(bool predict)
    6. {
    7.     ...
    8.  
    9.     // Check arm status and perform required checks and mode changes
    10.     controlFilterModes();
    11.     ...
    12.  
    13.     // Run the EKF equations to estimate at the fusion time horizon if new IMU data is available in the buffer
    14.     if (runUpdates) {
    15.         // Predict states using IMU data from the delayed time horizon
    16.         UpdateStrapdownEquationsNED();
    17.  
    18.         // Predict the covariance growth
    19.         CovariancePrediction();
    20.  
    21.         // Update states using  magnetometer or external yaw sensor data
    22.         SelectMagFusion();
    23.  
    24.         // Update states using GPS and altimeter data
    25.         SelectVelPosFusion();
    26.  
    27.         // Update states using range beacon data
    28.         SelectRngBcnFusion();
    29.  
    30.         // Update states using optical flow data
    31.         SelectFlowFusion();
    32.  
    33.         // Update states using body frame odometry data
    34.         SelectBodyOdomFusion();
    35.  
    36.         // Update states using airspeed data
    37.         SelectTasFusion();
    38.  
    39.         // Update states using sideslip constraint assumption for fly-forward vehicles
    40.         SelectBetaFusion();
    41.  
    42.         // Update the filter status
    43.         updateFilterStatus();
    44.     }
    45.     ...
    46. }

    这里有两个函数和 EKF3 使用光流传感器有关：controlFilterModes()，SelectFlowFusion()。

    5 AP_NavEKF3_Control.cpp

    5.1 void NavEKF3_core::controlFilterModes()

    控制滤波器模式转换。

    1. // Control filter mode transitions
    2. void NavEKF3_core::controlFilterModes()
    3. {
    4.     ...
    5.     // Set the type of inertial navigation aiding used
    6.     setAidingMode();
    7.     ...
    8. }

    5.2 void NavEKF3_core::setAidingMode()

    设置所使用的惯性导航辅助类型。

    我们把飞控连接 QGC，小喇叭会不断的弹出“...stopped aiding”和“...started relative aiding”消息。

    根据 AidingMode 的枚举定义，分为三种情况。

    1、AID_ABSOLUTE = 0；正在使用 GPS 或其他形式的绝对位置参考辅助（也可同时使用光流），因此位置估算是绝对的。

    2、AID_NONE = 1；不使用辅助，因此只有姿态和高度估计值。必须使用 constVelMode 或 constPosMode 来限制倾斜漂移。

    3、AID_RELATIVE = 2；只使用光流辅助，因此位置估算值将是相对的。

    这里，如果光流传感器数据良好，我们运行 AID_RELATIVE；如果光流数据较差或没有，我们运行 AID_NONE。

    1. // Set inertial navigation aiding mode
    2. void NavEKF3_core::setAidingMode()
    3. {
    4.     ...
    5.     // 检查我们是否开始或停止援助，并根据需要设置状态和模式
    6.     // check to see if we are starting or stopping aiding and set states and modes as required
    7.     if (PV_AidingMode != PV_AidingModePrev) {
    8.         // set various usage modes based on the condition when we start aiding. These are then held until aiding is stopped.
    9.         switch (PV_AidingMode) {
    10.         case AID_NONE:
    11.             // We have ceased aiding
    12.             gcs().send_text(MAV_SEVERITY_WARNING, "EKF3 IMU%u stopped aiding",(unsigned)imu_index);
    13.             // When not aiding, estimate orientation & height fusing synthetic constant position and zero velocity measurement to constrain tilt errors
    14.             // 无辅助时，利用合成恒定位置和零速度测量来估计方位和高度，以限制倾斜误差
    15.     ...
    16.  
    17.         case AID_RELATIVE:
    18.             // We are doing relative position navigation where velocity errors are constrained, but position drift will occur
    19.             // 我们正在进行相对位置导航，速度误差受到限制，但位置漂移会发生
    20.             gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u started relative aiding",(unsigned)imu_index);
    21.     ...
    22. }

    6 AP_NavEKF3_OptFlowFusion.cpp

    6.1 void NavEKF3_core::SelectFlowFusion()

    选择性融合光学流量传感器的测量。

    1. // select fusion of optical flow measurements
    2. void NavEKF3_core::SelectFlowFusion()
    3. {
    4.     ...
    5.     // 将光流数据融合到主滤波器中
    6.     // Fuse optical flow data into the main filter
    7.     if (flowDataToFuse && tiltOK) {
    8.         if (frontend->_flowUse == FLOW_USE_NAV) {
    9.             // Set the flow noise used by the fusion processes
    10.             R_LOS = sq(MAX(frontend->_flowNoise, 0.05f));
    11.             // Fuse the optical flow X and Y axis data into the main filter sequentially
    12.             FuseOptFlow();
    13.         }
    14.         // reset flag to indicate that no new flow data is available for fusion
    15.         flowDataToFuse = false;
    16.     }
    17.     ...
    18. }

    6.2 void NavEKF3_core::FuseOptFlow()

    依次将光流 X 轴和 Y 轴数据融合到主滤波器中。

    首次融合光流传感器数据，会提示："EKF3 IMU%u fusing optical flow"。

    1. void NavEKF3_core::FuseOptFlow()
    2. {
    3.     ...
    4.             // notify first time only
    5.             if (!flowFusionActive) {
    6.                 flowFusionActive = true;
    7.                 gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u fusing optical flow",(unsigned)imu_index);
    8.             }
    9.     ...
    10. }
  • 相关阅读:
    java毕业生设计中小学图书馆管理计算机源码+系统+mysql+调试部署+lw
    基于YOLOv5、YOLOv8的火灾检测（超实用毕业设计项目）
    Python批处理（一）提取txt中数据存入excel
    异常与错误处理高级用法
    什么是Ajax？全面了解
    水浒传之“领导力的定义：梁山集团三代领导人的特点”
    【博客531】linux kubernetes网络非法报文校验参数以及追踪
    Vue模板语法下集（03）
    Github每日精选（第13期）：实时目标检测网络YOLOv7
    vue中或者react中的excel导入和导出
  • 原文地址：https://blog.csdn.net/qq_20016593/article/details/132900567