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

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

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);
}

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

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();
    }
    ...
}

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

NavEKF3 &EKF3;

// 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]);
    }
    ...
}

NavEKF3_core *core = nullptr;

/********************************************************
*                 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();
    }
    ...
}

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

// 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);
    ...
}

// 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;
    }
    ...
}

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);
            }
    ...
}