diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index ebf7fbec23..7bed9f7c4b 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -389,7 +389,7 @@ NavEKF2::NavEKF2(const AP_AHRS *ahrs, AP_Baro &baro, const RangeFinder &rng) : hgtRetryTimeMode12_ms(5000), // Height retry time without vertical velocity measurement (msec) tasRetryTime_ms(5000), // True airspeed timeout and retry interval (msec) magFailTimeLimit_ms(10000), // number of msec before a magnetometer failing innovation consistency checks is declared failed (msec) - magVarRateScale(0.05f), // scale factor applied to magnetometer variance due to angular rate + magVarRateScale(0.005f), // scale factor applied to magnetometer variance due to angular rate and measurement timing jitter. Assume timing jitter of 10msec gyroBiasNoiseScaler(2.0f), // scale factor applied to imu gyro bias learning before the vehicle is armed hgtAvg_ms(100), // average number of msec between height measurements betaAvg_ms(100), // average number of msec between synthetic sideslip measurements diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp index 1d94d7218a..89a093c47e 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp @@ -236,7 +236,7 @@ void NavEKF2_core::FuseMagnetometer() MagPred[1] = DCM[1][0]*magN + DCM[1][1]*magE + DCM[1][2]*magD + magYbias; MagPred[2] = DCM[2][0]*magN + DCM[2][1]*magE + DCM[2][2]*magD + magZbias; - // scale magnetometer observation error with total angular rate + // scale magnetometer observation error with total angular rate to allow for timing errors R_MAG = sq(constrain_float(frontend._magNoise, 0.01f, 0.5f)) + sq(frontend.magVarRateScale*imuDataDelayed.delAng.length() / dtIMUavg); // calculate observation jacobians