From 0c03e714c97a5d087df81747f561b455bbb4e7fb Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Wed, 1 Apr 2026 00:02:49 -0800 Subject: [PATCH] fix(sensors): use cumulative excitation for baro thrust convergence The instantaneous thrust_std check prevented convergence during gentle hovers where excitation was intermittent but sufficient for RLS to identify K. Track cumulative time above the excitation threshold (5s) instead, so bursts of thrust variation accumulate rather than requiring continuous excitation at the moment of convergence. Validated against flight logs where K estimate was accurate but convergence never triggered due to flickering excitation gate. --- .../baro_thrust_estimator/baro_thrust_cf_rls.cpp | 10 +++++++++- .../baro_thrust_estimator/baro_thrust_cf_rls.hpp | 3 +++ 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/src/modules/baro_thrust_estimator/baro_thrust_cf_rls.cpp b/src/modules/baro_thrust_estimator/baro_thrust_cf_rls.cpp index a0d652b7f1..c9e23339a1 100644 --- a/src/modules/baro_thrust_estimator/baro_thrust_cf_rls.cpp +++ b/src/modules/baro_thrust_estimator/baro_thrust_cf_rls.cpp @@ -45,6 +45,7 @@ void BaroThrustCfRls::reset() _converged_locked = false; _converged_elapsed_s = 0.f; _k_stable_elapsed_s = 0.f; + _excitation_elapsed_s = 0.f; _k_est_smoothed.reset(0.f); _thrust_mean.reset(0.f); _thrust_var.reset(0.f); @@ -120,7 +121,14 @@ void BaroThrustCfRls::checkConvergence(float elapsed_since_start_s, float dt) } const bool variance_ok = _rls.P[0][0] < CONVERGENCE_VAR_THR; - const bool excitation_ok = fmaxf(_thrust_var.getState(), 0.f) > (MIN_THRUST_EXCITATION * MIN_THRUST_EXCITATION); + + // Accumulate time with sufficient thrust excitation. RLS doesn't need + // continuous excitation — intermittent bursts are enough to identify K. + if (fmaxf(_thrust_var.getState(), 0.f) > (MIN_THRUST_EXCITATION * MIN_THRUST_EXCITATION)) { + _excitation_elapsed_s += dt; + } + + const bool excitation_ok = _excitation_elapsed_s > MIN_EXCITATION_TIME_S; // Dual-path error check: absolute threshold works for refinement flights // (PCOEF already set), relative threshold allows first calibration where diff --git a/src/modules/baro_thrust_estimator/baro_thrust_cf_rls.hpp b/src/modules/baro_thrust_estimator/baro_thrust_cf_rls.hpp index ed4447bf7b..208bc4761a 100644 --- a/src/modules/baro_thrust_estimator/baro_thrust_cf_rls.hpp +++ b/src/modules/baro_thrust_estimator/baro_thrust_cf_rls.hpp @@ -95,6 +95,7 @@ public: static constexpr float CONVERGENCE_ERR_REL_THR = 0.4f; ///< max error/total variance ratio (model explains >60%) static constexpr float CONVERGENCE_ERR_MAX_THR = 4.0f; ///< absolute error cap for relative path [m^2] static constexpr float MIN_THRUST_EXCITATION = 0.05f; ///< min thrust std dev to trust the estimate + static constexpr float MIN_EXCITATION_TIME_S = 5.f; ///< cumulative seconds above excitation threshold static constexpr float MIN_ESTIMATION_TIME_S = 30.f; ///< min flight time before convergence allowed static constexpr float K_STABILITY_TIME_S = 10.f; ///< K must be stable within threshold for this long static constexpr float CONVERGENCE_HOLD_TIME_S = 10.f; ///< must stay converged this long before locking @@ -166,6 +167,8 @@ private: bool _converged_locked{false}; ///< converged and held long enough — ready to save float _converged_elapsed_s{0.f}; ///< accumulated hold time while converged + float _excitation_elapsed_s{0.f}; ///< cumulative time with sufficient thrust excitation + // Thrust excitation tracking (need variation in thrust to observe K) AlphaFilter _thrust_mean{}; ///< low-pass mean thrust AlphaFilter _thrust_var{}; ///< low-pass thrust variance