From 0d91b428d6d7d6ff0059548db2429d314a2a66a1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 22 Apr 2026 11:17:39 +1200 Subject: [PATCH] fix(ekf2): break unbounded recursion fuseDirectStateMeasurement() ends with a call to constrainStateVariances(), which iterates every state and, for any whose variance has grown past its limit, calls fuseDirectStateMeasurement() again to apply a corrective fuse. That inner call then runs its own constrainStateVariances(), repeating the loop. Call chain: fuseDirectStateMeasurement ekf_helper.cpp:1030 `- constrainStateVariances ekf_helper.cpp:1085 `- constrainStateVar covariance.cpp:290 `- fuseDirectStateMeasurement covariance.cpp:302 `- constrainStateVariances (recursion) `- ... Under normal operation the P(i,i) > max branch is rarely taken and the recursion stays shallow, however, while doing unrelated overnight testing we saw a hard-fault with TCB corruption, presumably due to a stack overflow. The trace pointed to fuseDirectStateMeasurement where Claude detected the recursive call. This fix breaks the recursion by gating the constrainStateVariances() call on a new parameter, defaulting to true for existing callers. constrainStateVar() passes false when it re-enters the fuse from the constraint path, capping the worst-case recursion depth at 2. --- src/modules/ekf2/EKF/covariance.cpp | 4 +++- src/modules/ekf2/EKF/ekf.h | 5 ++++- src/modules/ekf2/EKF/ekf_helper.cpp | 7 +++++-- 3 files changed, 12 insertions(+), 4 deletions(-) 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);