From bdbbe7fd52a4ffc9180d8ce064bd2622345832c8 Mon Sep 17 00:00:00 2001 From: Claude Date: Mon, 18 May 2026 04:46:03 +0000 Subject: [PATCH] feat(follow_target): expose velocity filter gain as tunable parameter FLW_TGT_VG MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The TargetEstimator's velocity beta coefficient was hardcoded as 0.9 * beta_p. Introduce FLW_TGT_VG (default 0.9, range 0.1–1.0) so operators can tune the velocity estimate's smoothness vs. responsiveness independently of FLW_TGT_RS. Signed-off-by: Claude --- .../follow_target_estimator/TargetEstimator.cpp | 2 +- .../follow_target_estimator/TargetEstimator.hpp | 3 ++- .../AutoFollowTarget/follow_target_params.yaml | 13 +++++++++++++ 3 files changed, 16 insertions(+), 2 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.cpp b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.cpp index f3765a7ca62..6f5dd9e2f91 100644 --- a/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.cpp +++ b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.cpp @@ -194,7 +194,7 @@ void TargetEstimator::update_filter_gains(filter_gains_s &filter_gains) const // From alpha-beta-gamma filter equations: G = 1-beta^3 // Therefore: beta = (1-Gp)^(1/3) = (1-(1-responsiveness))^(1/3) = (r)^(1/3) const float beta_p = powf((filter_gains.responsiveness), 1.0f / 3.0f); - const float beta_v = 0.9f * beta_p; // velocity fusion gain is slightly lower. TODO: individual parameter? + const float beta_v = math::constrain(_param_flw_tgt_vg.get(), 0.1f, 1.0f) * beta_p; // Estimator gains for horizontal position update filter_gains.G_p = 1.0f - beta_p * beta_p * beta_p; diff --git a/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.hpp b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.hpp index 7886c21977c..55749b0a249 100644 --- a/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.hpp +++ b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_estimator/TargetEstimator.hpp @@ -203,7 +203,8 @@ protected: matrix::Vector3f _vel_measurement_old{}; DEFINE_PARAMETERS( - (ParamFloat) _param_flw_tgt_rs + (ParamFloat) _param_flw_tgt_rs, + (ParamFloat) _param_flw_tgt_vg ) // Subscriptions diff --git a/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_params.yaml b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_params.yaml index 76d600be763..dcb3aae7efe 100644 --- a/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_params.yaml +++ b/src/modules/flight_mode_manager/tasks/AutoFollowTarget/follow_target_params.yaml @@ -12,6 +12,19 @@ parameters: min: 0.0 max: 1.0 decimal: 2 + FLW_TGT_VG: + description: + short: Velocity fusion gain factor in Target Estimator + long: |- + Scales the velocity filter's beta coefficient relative to the position + beta derived from FLW_TGT_RS. Lower values make the velocity estimate + smoother but less responsive. At 1.0 position and velocity filters have + equal responsiveness. + type: float + default: 0.9 + min: 0.1 + max: 1.0 + decimal: 2 FLW_TGT_HT: description: short: Follow target height