diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp index d034881fc7..7cdba5a9ad 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp @@ -111,9 +111,19 @@ void NavEKF2_core::setWindMagStateLearningMode() inhibitMagStates = true; } else if (inhibitMagStates && !setMagInhibit) { inhibitMagStates = false; - // when commencing use of magnetic field states, set the variances equal to the observation uncertainty - for (uint8_t index=16; index<=21; index++) { - P[index][index] = sq(frontend->_magNoise); + if (magFieldLearned) { + // if we have already learned the field states, then retain the learned variances + P[16][16] = earthMagFieldVar.x; + P[17][17] = earthMagFieldVar.y; + P[18][18] = earthMagFieldVar.z; + P[19][19] = bodyMagFieldVar.x; + P[20][20] = bodyMagFieldVar.y; + P[21][21] = bodyMagFieldVar.z; + } else { + // set the variances equal to the observation variances + for (uint8_t index=16; index<=21; index++) { + P[index][index] = sq(frontend->_magNoise); + } } // request a reset of the yaw and magnetic field states if not done before if (!magStateInitComplete || (!finalInflightMagInit && inFlight)) { diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp index 72ca44414e..4d8c71cb5a 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp @@ -265,6 +265,26 @@ void NavEKF2_core::SelectMagFusion() } } + // If the final yaw reset has been performed and the state variances are sufficiently low + // record that the earth field has been learned. + bool earthMagFieldConverged = false; + if (!magFieldLearned && finalInflightMagInit) { + earthMagFieldConverged = (P[16][16] < sq(0.01f)) && (P[17][17] < sq(0.01f)) && (P[18][18] < sq(0.01f)); + } + if (magFieldLearned || earthMagFieldConverged) { + magFieldLearned = true; + } + + // record the last learned field variances + if (magFieldLearned && !inhibitMagStates) { + earthMagFieldVar.x = P[16][16]; + earthMagFieldVar.y = P[17][17]; + earthMagFieldVar.z = P[18][18]; + bodyMagFieldVar.x = P[19][19]; + bodyMagFieldVar.y = P[20][20]; + bodyMagFieldVar.z = P[21][21]; + } + // stop performance timer hal.util->perf_end(_perf_FuseMagnetometer); } @@ -1062,6 +1082,11 @@ void NavEKF2_core::FuseDeclination(float declErr) // align the NE earth magnetic field states with the published declination void NavEKF2_core::alignMagStateDeclination() { + // don't do this if we already have a learned magnetic field + if (magFieldLearned) { + return; + } + // get the magnetic declination float magDecAng = use_compass() ? _ahrs->get_compass()->get_declination() : 0; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index c098f51fd2..fe0249a10b 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp @@ -261,6 +261,7 @@ void NavEKF2_core::InitialiseVariables() posDownAtLastMagReset = stateStruct.position.z; yawInnovAtLastMagReset = 0.0f; quatAtLastMagReset = stateStruct.quat; + magFieldLearned = false; // zero data buffers storedIMU.reset(); @@ -1374,55 +1375,51 @@ Quaternion NavEKF2_core::calcQuatAndFieldStates(float roll, float pitch) // calculate yaw angle rel to true north yaw = magDecAng - magHeading; - // calculate initial filter quaternion states using yaw from magnetometer if mag heading healthy - // otherwise use existing heading - if (!badMagYaw) { - // store the yaw change so that it can be retrieved externally for use by the control loops to prevent yaw disturbances following a reset - Vector3f tempEuler; - stateStruct.quat.to_euler(tempEuler.x, tempEuler.y, tempEuler.z); - // this check ensures we accumulate the resets that occur within a single iteration of the EKF - if (imuSampleTime_ms != lastYawReset_ms) { - yawResetAngle = 0.0f; - } - yawResetAngle += wrap_PI(yaw - tempEuler.z); - lastYawReset_ms = imuSampleTime_ms; - // calculate an initial quaternion using the new yaw value - initQuat.from_euler(roll, pitch, yaw); - // zero the attitude covariances becasue the corelations will now be invalid - zeroAttCovOnly(); - } else { - initQuat = stateStruct.quat; + // calculate initial filter quaternion states using yaw from magnetometer + // store the yaw change so that it can be retrieved externally for use by the control loops to prevent yaw disturbances following a reset + Vector3f tempEuler; + stateStruct.quat.to_euler(tempEuler.x, tempEuler.y, tempEuler.z); + // this check ensures we accumulate the resets that occur within a single iteration of the EKF + if (imuSampleTime_ms != lastYawReset_ms) { + yawResetAngle = 0.0f; } + yawResetAngle += wrap_PI(yaw - tempEuler.z); + lastYawReset_ms = imuSampleTime_ms; + // calculate an initial quaternion using the new yaw value + initQuat.from_euler(roll, pitch, yaw); + // zero the attitude covariances becasue the corelations will now be invalid + zeroAttCovOnly(); // calculate initial Tbn matrix and rotate Mag measurements into NED // to set initial NED magnetic field states - initQuat.rotation_matrix(Tbn); - stateStruct.earth_magfield = Tbn * magDataDelayed.mag; + // don't do this if the earth field has already been learned + if (!magFieldLearned) { + initQuat.rotation_matrix(Tbn); + stateStruct.earth_magfield = Tbn * magDataDelayed.mag; - // align the NE earth magnetic field states with the published declination - alignMagStateDeclination(); + // align the NE earth magnetic field states with the published declination + alignMagStateDeclination(); - // zero the magnetic field state associated covariances - zeroRows(P,16,21); - zeroCols(P,16,21); - // set initial earth magnetic field variances - P[16][16] = sq(frontend->_magNoise); - P[17][17] = P[16][16]; - P[18][18] = P[16][16]; - // set initial body magnetic field variances - P[19][19] = sq(frontend->_magNoise); - P[20][20] = P[19][19]; - P[21][21] = P[19][19]; + // zero the magnetic field state associated covariances + zeroRows(P,16,21); + zeroCols(P,16,21); + // set initial earth magnetic field variances + P[16][16] = sq(frontend->_magNoise); + P[17][17] = P[16][16]; + P[18][18] = P[16][16]; + // set initial body magnetic field variances + P[19][19] = sq(frontend->_magNoise); + P[20][20] = P[19][19]; + P[21][21] = P[19][19]; - // clear bad magnetic yaw status - badMagYaw = false; - - // clear mag state reset request - magStateResetRequest = false; + } // record the fact we have initialised the magnetic field states recordMagReset(); + // clear mag state reset request + magStateResetRequest = false; + } else { // this function should not be called if there is no compass data but if is is, return the // current attitude diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index f579f9e501..98502df699 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -795,6 +795,9 @@ private: float posDownObsNoise; // observation noise variance on the vertical position used by the state and covariance update step (m^2) Vector3f delAngCorrected; // corrected IMU delta angle vector at the EKF time horizon (rad) Vector3f delVelCorrected; // corrected IMU delta velocity vector at the EKF time horizon (m/s) + bool magFieldLearned; // true when the magnetic field has been learned + Vector3f earthMagFieldVar; // NED earth mag field variances for last learned field (mGauss^2) + Vector3f bodyMagFieldVar; // XYZ body mag field variances for last learned field (mGauss^2) Vector3f outputTrackError;