Ardupilot — EKF3 uses TOF for high-level source code grooming

The main height sensor used by EKF . If the selected option is not available, use the air pressure sensor. 1 Use rangefinder, only for optical flow navigation ( EK3_GPS_TYPE = 3 ), do not use " 1 " for terrain tracking. NOTE: The EK3_RNG_USE_HGT parameter can be used to switch to rangefinder when close to the ground.

RebootRequired

Values

True

Value	Meaning
0	Use Baro
1	Use Range Finder
2	Use GPS
3	Use Range Beacon

Closer to home, this article mainly summarizes the entire operation process of EKF3 in the rotor , and at which step the rangefinder is selected as the altitude source.

1 Copter.cpp

1.1 void IRAM_ATTR Copter::fast_loop()

In Ardupilot code, tasks that require a lot of resources and have high computing frequency are usually in the fast_loop() function. Here we only show the code snippets related to EKF3 operation.

Run the EKF state estimator (costly).

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

1.2 void Copter::read_AHRS(void)

Read the information entry function of the attitude and heading reference system.

We tell AHRS to skip the INS update since we already did it in 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 Description of object ahrs

In Copter.h , we define the ahrs object using the AP_AHRS_NavEKF class .

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)

Therefore, when we jump to the update() member function, we jump to the update() function of the AP_AHRS_NavEKF class .

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)

Update EKF3 .

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

2.3 Description of object EKF3

In AP_AHRS_NavEKF.h , we define the EKF3 object using the NavEKF3 class .

NavEKF3 &EKF3;

3 AP_NavEKF3.cpp

3.1 void IRAM_ATTR NavEKF3::UpdateFilter(void)

Therefore, when we jump to the UpdateFilter() member function, we jump to the UpdateFilter() function of the NavEKF3 class .

Update filter status - This function should be called whenever new IMU data is available.

// 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 Object core description

In AP_NavEKF3.h , we define the core object using the NavEKF3_core class .

NavEKF3_core *core = nullptr;

4 AP_NavEKF3_core.cpp

4.1 void IRAM_ATTR NavEKF3_core::UpdateFilter(bool predict)

Therefore, when we jump to the UpdateFilter() member function, we jump to the UpdateFilter() function of the NavEKF3_core class .

If there is new IMU data in the buffer, the EKF equation is run to estimate over the fusion time span.

/********************************************************
*                 UPDATE FUNCTIONS                      *
********************************************************/
// Update Filter States - this should be called whenever new IMU data is available
void IRAM_ATTR NavEKF3_core::UpdateFilter(bool predict)
{
    ...
    // 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();
    }
    ...
}

Regarding the position and speed fusion of EKF3 , Ardupilot separates a separate file.

5 AP_NavEKF3_PosVelFusion.cpp

5.1 void NavEKF3_core::SelectVelPosFusion()

Choose to blend speed, position, and height measurements.

/********************************************************
*                   FUSE MEASURED_DATA                  *
********************************************************/
// select fusion of velocity, position and height measurements
void NavEKF3_core::SelectVelPosFusion()
{
    ...
    // Select height data to be fused from the available baro, range finder and GPS sources
    selectHeightForFusion();
    ...
}

5.2 void NavEKF3_core::selectHeightForFusion()

Select altitude measurements to fuse from available barometer, rangefinder and GPS sources.

/********************************************************
*                   MISC FUNCTIONS                      *
********************************************************/

// select the height measurement to be fused from the available baro, range finder and GPS sources
void NavEKF3_core::selectHeightForFusion()
{
    // Read range finder data and check for new data in the buffer
    // This data is used by both height and optical flow fusion processing
    readRangeFinder();
    rangeDataToFuse = storedRange.recall(rangeDataDelayed,imuDataDelayed.time_ms);

    // correct range data for the body frame position offset relative to the IMU
    // the corrected reading is the reading that would have been taken if the sensor was
    // co-located with the IMU
    if (rangeDataToFuse) {
        AP_RangeFinder_Backend *sensor = frontend->_rng.get_backend(rangeDataDelayed.sensor_idx);
        if (sensor != nullptr) {
            Vector3f posOffsetBody = sensor->get_pos_offset() - accelPosOffset;
            if (!posOffsetBody.is_zero()) {
                Vector3f posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
                rangeDataDelayed.rng += posOffsetEarth.z / prevTnb.c.z;
            }
        }
    }

    // read baro height data from the sensor and check for new data in the buffer
    readBaroData();
    baroDataToFuse = storedBaro.recall(baroDataDelayed, imuDataDelayed.time_ms);

    // select height source
    if (((frontend->_useRngSwHgt > 0) && (frontend->_altSource == 1)) && (imuSampleTime_ms - rngValidMeaTime_ms < 500)) {
        if (frontend->_altSource == 1) {
            // always use range finder
            activeHgtSource = HGT_SOURCE_RNG;
        }
        ...
    }
    ...
}

5.3 Parameter _altSource description

In the AP_NavEKF3.cpp file, we define the content ( element ) of the parameter EK3_ALT_SOURCE as _altSource .

    // @Param: ALT_SOURCE
    // @DisplayName: Primary altitude sensor source
    // @Description: Primary height sensor used by the EKF. If the selected option cannot be used, baro is used. 1 uses the range finder and only with optical flow navigation (EK2_GPS_TYPE = 3), Do not use "1" for terrain following. NOTE: the EK3_RNG_USE_HGT parameter can be used to switch to range-finder when close to the ground.
    // @Values: 0:Use Baro, 1:Use Range Finder, 2:Use GPS, 3:Use Range Beacon
    // @User: Advanced
    // @RebootRequired: True
    AP_GROUPINFO("ALT_SOURCE", 9, NavEKF3, _altSource, 0),

At this point, if we set the parameter EK3_ALT_SOURCE to 1 , then at this code ( activeHgtSource = HGT_SOURCE_RNG; ), the height of the rangefinder ( TOF ) will be passed into EKF3 for calculation.