diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index 85ce9b9c17..f33f9d8d04 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -286,10 +286,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 1a8466c96d..5c778ac613 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -259,7 +259,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 5820494f8e..01772f3de5 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -1026,7 +1026,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. @@ -1080,7 +1081,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);