diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index dd2ce97cf6..5820fc74d7 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -296,10 +296,12 @@ void Ekf::constrainStateVar(const IdxDof &state, float min, float max) } else if (P(i, i) > max) { // Constrain the variance growth by fusing zero innovation as clipping the variance // would artifically increase the correlation between states and destabilize the filter. + // constrain_variances=false prevents unbounded recursion via + // fuseDirectStateMeasurement -> constrainStateVariances -> constrainStateVar -> here. const float innov = 0.f; const float R = 10.f * P(i, i); // This reduces the variance by ~10% as K = P / (P + R) const float innov_var = P(i, i) + R; - fuseDirectStateMeasurement(innov, innov_var, R, i); + fuseDirectStateMeasurement(innov, innov_var, R, i, /*constrain_variances=*/false); } } } diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 5e36b1a1d5..8fa5834702 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -263,7 +263,10 @@ public: } // fuse single direct state measurement (eg NED velocity, NED position, mag earth field, etc) - void fuseDirectStateMeasurement(const float innov, const float innov_var, const float R, const int state_index); + // constrain_variances must be false when called from inside constrainStateVar to prevent + // unbounded recursion (constrainStateVariances can call back into this function). + void fuseDirectStateMeasurement(const float innov, const float innov_var, const float R, const int state_index, + bool constrain_variances = true); bool measurementUpdate(VectorState &K, const VectorState &H, const float R, const float innovation); diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 1ea83ceac8..381c10eca9 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -1027,7 +1027,8 @@ void Ekf::updateIMUBiasInhibit(const imuSample &imu_delayed) } } -void Ekf::fuseDirectStateMeasurement(const float innov, const float innov_var, const float R, const int state_index) +void Ekf::fuseDirectStateMeasurement(const float innov, const float innov_var, const float R, const int state_index, + bool constrain_variances) { VectorState K; // Kalman gain vector for any single observation - sequential fusion is used. @@ -1081,7 +1082,9 @@ void Ekf::fuseDirectStateMeasurement(const float innov, const float innov_var, c #endif - constrainStateVariances(); + if (constrain_variances) { + constrainStateVariances(); + } // apply the state corrections fuse(K, innov);