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 状态地形高度估算器中。

本文主要梳理一下，在旋翼中 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 状态估算器（耗资巨大）。

// Main loop - 400hz
void IRAM_ATTR Copter::fast_loop()
{
    ...
    // run EKF state estimator (expensive)
    // --------------------
    read_AHRS();
    ...
}

1.2 void Copter::read_AHRS(void)

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

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

void Copter::read_AHRS(void)
{
    // Perform IMU calculations and get attitude info
    //-----------------------------------------------
#if HIL_MODE != HIL_MODE_DISABLED
    // update hil before ahrs update
    gcs().update_receive();
    gcs().update_send();
#endif

    // we tell AHRS to skip INS update as we have already done it in fast_loop()
    ahrs.update(true);
}

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()。

void AP_AHRS_NavEKF::update(bool skip_ins_update)
{
    ...
    if (_ekf_type == 2) {
        // if EK2 is primary then run EKF2 first to give it CPU
        // priority
        update_EKF2();
        update_EKF3();
    } else {
        // otherwise run EKF3 first
        update_EKF3();
        update_EKF2();
    }
    ...
}

2.2 void AP_AHRS_NavEKF::update_EKF3(void)

更新 EKF3。

void AP_AHRS_NavEKF::update_EKF3(void)
{
    ...
    if (_ekf3_started) {
        EKF3.UpdateFilter();
    ...
    }
}

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 数据，就应调用该函数。

// Update Filter States - this should be called whenever new IMU data is available
void IRAM_ATTR NavEKF3::UpdateFilter(void)
{
    if (!core) {
        return;
    }

    imuSampleTime_us = AP_HAL::micros64();

    const AP_InertialSensor &ins = AP::ins();

    bool statePredictEnabled[num_cores];
    for (uint8_t i=0; i<num_cores; i++) {
        // if we have not overrun by more than 3 IMU frames, and we
        // have already used more than 1/3 of the CPU budget for this
        // loop then suppress the prediction step. This allows
        // multiple EKF instances to cooperate on scheduling
        if (core[i].getFramesSincePredict() < (_framesPerPrediction+3) &&
            (AP_HAL::micros() - ins.get_last_update_usec()) > _frameTimeUsec/3) {
            statePredictEnabled[i] = false;
        } else {
            statePredictEnabled[i] = true;
        }
        core[i].UpdateFilter(statePredictEnabled[i]);
    }
    ...
}

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 方程，在融合时间跨度上进行估算。

/********************************************************
*                 UPDATE FUNCTIONS                      *
********************************************************/
// Update Filter States - this should be called whenever new IMU data is available
void IRAM_ATTR NavEKF3_core::UpdateFilter(bool predict)
{
    ...

    // Check arm status and perform required checks and mode changes
    controlFilterModes();
    ...

    // Run the EKF equations to estimate at the fusion time horizon if new IMU data is available in the buffer
    if (runUpdates) {
        // Predict states using IMU data from the delayed time horizon
        UpdateStrapdownEquationsNED();

        // Predict the covariance growth
        CovariancePrediction();

        // Update states using  magnetometer or external yaw sensor data
        SelectMagFusion();

        // Update states using GPS and altimeter data
        SelectVelPosFusion();

        // Update states using range beacon data
        SelectRngBcnFusion();

        // Update states using optical flow data
        SelectFlowFusion();

        // Update states using body frame odometry data
        SelectBodyOdomFusion();

        // Update states using airspeed data
        SelectTasFusion();

        // Update states using sideslip constraint assumption for fly-forward vehicles
        SelectBetaFusion();

        // Update the filter status
        updateFilterStatus();
    }
    ...
}

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

5 AP_NavEKF3_Control.cpp

5.1 void NavEKF3_core::controlFilterModes()

控制滤波器模式转换。

// Control filter mode transitions
void NavEKF3_core::controlFilterModes()
{
    ...
    // Set the type of inertial navigation aiding used
    setAidingMode();
    ...
}

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。

// Set inertial navigation aiding mode
void NavEKF3_core::setAidingMode()
{
    ...
    // 检查我们是否开始或停止援助，并根据需要设置状态和模式
    // check to see if we are starting or stopping aiding and set states and modes as required
    if (PV_AidingMode != PV_AidingModePrev) {
        // set various usage modes based on the condition when we start aiding. These are then held until aiding is stopped.
        switch (PV_AidingMode) {
        case AID_NONE:
            // We have ceased aiding
            gcs().send_text(MAV_SEVERITY_WARNING, "EKF3 IMU%u stopped aiding",(unsigned)imu_index);
            // When not aiding, estimate orientation & height fusing synthetic constant position and zero velocity measurement to constrain tilt errors
            // 无辅助时，利用合成恒定位置和零速度测量来估计方位和高度，以限制倾斜误差
    ...

        case AID_RELATIVE:
            // We are doing relative position navigation where velocity errors are constrained, but position drift will occur
            // 我们正在进行相对位置导航，速度误差受到限制，但位置漂移会发生
            gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u started relative aiding",(unsigned)imu_index);
    ...
}

6 AP_NavEKF3_OptFlowFusion.cpp

6.1 void NavEKF3_core::SelectFlowFusion()

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

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

6.2 void NavEKF3_core::FuseOptFlow()

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

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

void NavEKF3_core::FuseOptFlow()
{
    ...
            // notify first time only
            if (!flowFusionActive) {
                flowFusionActive = true;
                gcs().send_text(MAV_SEVERITY_INFO, "EKF3 IMU%u fusing optical flow",(unsigned)imu_index);
            }
    ...
}