From c11c75d32e7cb2ec3e7e536a6b0671b1cff484ee Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 22 May 2024 10:11:33 +0200 Subject: [PATCH] ekf2-mag: always add process noise until initial value --- src/modules/ekf2/EKF/covariance.cpp | 26 +++++++++++++++----------- 1 file changed, 15 insertions(+), 11 deletions(-) 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; } }