diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index 5190ad1213..13f2399be5 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -163,22 +163,26 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) #if defined(CONFIG_EKF2_MAGNETOMETER) - if (_control_status.flags.mag) { - // mag_I: add process noise - float mag_I_sig = dt * math::constrain(_params.mage_p_noise, 0.f, 1.f); - float mag_I_process_noise = sq(mag_I_sig); + // mag_I: add process noise + float mag_I_sig = dt * math::constrain(_params.mage_p_noise, 0.f, 1.f); + float mag_I_process_noise = sq(mag_I_sig); - for (unsigned index = 0; index < State::mag_I.dof; index++) { - const unsigned i = State::mag_I.idx + index; + for (unsigned index = 0; index < State::mag_I.dof; index++) { + const unsigned i = State::mag_I.idx + index; + + if (P(i, i) < sq(_params.mag_noise)) { P(i, i) += mag_I_process_noise; } + } - // mag_B: add process noise - float mag_B_sig = dt * math::constrain(_params.magb_p_noise, 0.f, 1.f); - float mag_B_process_noise = sq(mag_B_sig); + // mag_B: add process noise + float mag_B_sig = dt * math::constrain(_params.magb_p_noise, 0.f, 1.f); + float mag_B_process_noise = sq(mag_B_sig); - for (unsigned index = 0; index < State::mag_B.dof; index++) { - const unsigned i = State::mag_B.idx + index; + for (unsigned index = 0; index < State::mag_B.dof; index++) { + const unsigned i = State::mag_B.idx + index; + + if (P(i, i) < sq(_params.mag_noise)) { P(i, i) += mag_B_process_noise; } }