Yaw drift with vision position estimate

dwiel · March 19, 2020, 12:48am

I am sending vision position estimates to the arducopter over mavlink. The yaw in these messages is constant, yet AHRS.yaw drifts. More importantly, the yaw used by the attitude controller is also drifting during flight.

COMPASS_USE is false
COMPASS_USE2 is false
COMPASS_USE3 is false
COMPASS_ENABLE is false
AHRS_GPS_USE is disabled
GPS_TYPE is none
EK2_GPS_TYPE is 3 (no gps)

the below is while the drone is stationary.

recalibrating the compass seems to help temporarily. I recalibrated it yesterday and again today in the exact same location it is already drifting again. These tests are occurring inside of a metal building where i don’t expect the compass to perform super well. That is why i’m supplying attitude and position via vision_position_estimate messages. Is there some other setting I need to change so that the attitude used by the attitude controller is the attitude sent by these vision_position_estimate messages?

amilcarlucas · March 19, 2020, 9:12am

AFAIK the arducopter code does not thake the YAW information in the vision_position_estimate messages. So you need to change that.

dwiel · March 23, 2020, 4:34pm

It looks like yaw information is not merged here:

github.com

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.

tuuzdu · April 25, 2020, 8:07pm

I have same thing with Optitrack system.
I use @chobitsfan tutorial based on ATT_POS_MOCAP.
EK2 and VISO yaw are ok, but AHRS yaw is drifting. It is observed more in CIRCLE mode for instance. AHRS yaw continuously drifts and adds error to navigation.

amilcarlucas · April 26, 2020, 11:30pm

Are you using latest master with YAW GSF ?

tuuzdu · April 27, 2020, 6:16am

I’m using 4.0.3 version. Sorry, I haven’t noticed that.