feat(follow_target): expose velocity filter gain as tunable parameter FLW_TGT_VG

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 <noreply@anthropic.com>
This commit is contained in:
Claude
2026-05-18 04:46:03 +00:00
parent 5eecffe60a
commit bdbbe7fd52
3 changed files with 16 additions and 2 deletions

View File

@@ -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;

View File

@@ -203,7 +203,8 @@ protected:
matrix::Vector3f _vel_measurement_old{};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::FLW_TGT_RS>) _param_flw_tgt_rs
(ParamFloat<px4::params::FLW_TGT_RS>) _param_flw_tgt_rs,
(ParamFloat<px4::params::FLW_TGT_VG>) _param_flw_tgt_vg
)
// Subscriptions

View File

@@ -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