mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-08-21 03:10:34 +08:00
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.
This commit is contained in:
@@ -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
|
||||
|
||||
@@ -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<float> _thrust_mean{}; ///< low-pass mean thrust
|
||||
AlphaFilter<float> _thrust_var{}; ///< low-pass thrust variance
|
||||
|
||||
Reference in New Issue
Block a user