mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-07-24 15:27:41 +08:00
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:
@@ -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;
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user