Yaw drift with vision position estimate

dwiel · March 23, 2020, 4:34pm

It looks like yaw information is not merged here:

ArduPilot/ardupilot/blob/master/libraries/AP_AHRS/AP_AHRS_DCM.cpp#L480


    }


    // use the compass
    return true;
}


// yaw drift correction using the compass or GPS
// this function prodoces the _omega_yaw_P vector, and also
// contributes to the _omega_I.z long term yaw drift estimate
void
AP_AHRS_DCM::drift_correction_yaw(void)
{
    bool new_value = false;
    float yaw_error;
    float yaw_deltat;


    const AP_GPS &_gps = AP::gps();


    if (_compass && _compass->is_calibrating()) {
        // don't do any yaw correction while calibrating
        return;

However, i’m not sure that code path is even used. The yaw information does seem to be used here:

github.com

ArduPilot/ardupilot/blob/master/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp#L344


    // if compass is disabled, also use it for yaw
    if (!use_compass()) {
        extNavUsedForYaw = true;
        if (!yawAlignComplete) {
            extNavYawResetRequest = true;
            magYawResetRequest = false;
            gpsYawResetRequest = false;
            controlMagYawReset();
            finalInflightYawInit = true;
        } else {
            fuseEulerYaw();
        }
    } else {
        extNavUsedForYaw = false;
    }


} else {
    fuseVelData = false;
    fusePosData = false;
}

The thing that is confusing me is that nobody else seems to be having this problem over in this series of blog posts Integration of ArduPilot and VIO tracking camera (Part 1): Getting started with the Intel Realsense T265 on Rasberry Pi 3B

I would think that people flying indoors with a T265 would also tend not to have good compass readings.