From 0639f5370c39aeb626c27a0570829668d409a76e Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 7 Mar 2024 15:03:59 +0100 Subject: [PATCH] ekf2: fix mag and wind covariance prediction --- src/modules/ekf2/EKF/covariance.cpp | 21 ++++++++++++++++++--- 1 file changed, 18 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index b1a4fff58d..c6ba03f256 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -166,11 +166,21 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) 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); - P.slice(State::mag_I.idx, 0) += sq(mag_I_sig); + 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; + 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); - P.slice(State::mag_B.idx, 0) += sq(mag_B_sig); + 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; + P(i, i) += mag_B_process_noise; + } } #endif // CONFIG_EKF2_MAGNETOMETER @@ -179,7 +189,12 @@ void Ekf::predictCovariance(const imuSample &imu_delayed) if (_control_status.flags.wind) { // wind vel: add process noise float wind_vel_nsd_scaled = math::constrain(_params.wind_vel_nsd, 0.f, 1.f) * (1.f + _params.wind_vel_nsd_scaler * fabsf(_height_rate_lpf)); - P.slice(State::wind_vel.idx, 0) += sq(wind_vel_nsd_scaled) * dt; + float wind_vel_process_noise = sq(wind_vel_nsd_scaled) * dt; + + for (unsigned index = 0; index < State::wind_vel.dof; index++) { + const unsigned i = State::wind_vel.idx + index; + P(i, i) += wind_vel_process_noise; + } } #endif // CONFIG_EKF2_WIND