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