Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-09 22:08:56 +08:00
Code Issues Actions 3 Packages Projects Releases Wiki Activity

chore(tecs): remove TECS airspeed filter parameters

Browse Source 
These parameters are never tuned by the end-user, so we can use constants instead and immediately set the kalman gain at compile time
This commit is contained in:
mahima-yoga
2026-04-02 14:51:30 +02:00
committed by Silvan Fuhrer
parent 8980657425
commit c4c68df4e4
5 changed files with 24 additions and 68 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/tecs/TECS.cpp
+5 -18
View File
@@ -103,31 +103,18 @@ void TECSAirspeedFilter::update(const float dt, const Input &input, const Param
new_state_predicted(0) = _airspeed_state.speed + dt * _airspeed_state.speed_rate;
new_state_predicted(1) = _airspeed_state.speed_rate;
const float airspeed_noise_inv{1.0f / param.airspeed_measurement_std_dev};
const float airspeed_rate_noise_inv{1.0f / param.airspeed_rate_measurement_std_dev};
const float airspeed_rate_noise_inv_squared_process_noise{airspeed_rate_noise_inv *airspeed_rate_noise_inv * param.airspeed_rate_noise_std_dev};
const float denom{airspeed_noise_inv + airspeed_rate_noise_inv_squared_process_noise};
const float common_nom{std::sqrt(param.airspeed_rate_noise_std_dev * (2.0f * airspeed_noise_inv + airspeed_rate_noise_inv_squared_process_noise))};
matrix::Matrix<float, 2, 2> kalman_gain;
kalman_gain(0, 0) = airspeed_noise_inv * common_nom / denom;
kalman_gain(0, 1) = airspeed_rate_noise_inv_squared_process_noise / denom;
kalman_gain(1, 0) = airspeed_noise_inv * airspeed_noise_inv * param.airspeed_rate_noise_std_dev / denom;
kalman_gain(1, 1) = airspeed_rate_noise_inv_squared_process_noise * common_nom / denom;
const matrix::Vector2f innovation{(airspeed - new_state_predicted(0)), (airspeed_derivative - new_state_predicted(1))};
matrix::Vector2f new_state;
new_state = new_state_predicted + dt * (kalman_gain * (innovation));
new_state(0) = new_state_predicted(0) + dt * (kKg00 * innovation(0) + kKg01 * innovation(1));
new_state(1) = new_state_predicted(1) + dt * (kKg10 * innovation(0) + kKg11 * innovation(1));
// Clip airspeed at zero
if (new_state(0) < FLT_EPSILON) {
new_state(0) = 0.0f;
// calculate input that would result in zero speed.
const float desired_airspeed_innovation = (-new_state_predicted(0) / dt - kalman_gain(0,
1) * innovation(1)) / kalman_gain(0,
0);
new_state(1) = new_state_predicted(1) + dt * (kalman_gain(1, 0) * desired_airspeed_innovation + kalman_gain(1,
1) * innovation(1));
const float desired_airspeed_innovation = (-new_state_predicted(0) / dt - kKg01 * innovation(1)) / kKg00;
new_state(1) = new_state_predicted(1) + dt * (kKg10 * desired_airspeed_innovation + kKg11 * innovation(1));
}
// Update states
src/lib/tecs/TECS.hpp
+19 -9
View File
@@ -70,9 +70,6 @@ public:
*/
struct Param {
float equivalent_airspeed_trim; ///< the trim value of the equivalent airspeed [m/s].
float airspeed_measurement_std_dev; ///< airspeed measurement standard deviation in [m/s].
float airspeed_rate_measurement_std_dev;///< airspeed rate measurement standard deviation in [m/s²].
float airspeed_rate_noise_std_dev; ///< standard deviation on the airspeed rate deviation in the model in [m/s²].
};
/**
@@ -116,6 +113,25 @@ public:
private:
// States
AirspeedFilterState _airspeed_state{.speed = 0.0f, .speed_rate = 0.0f}; ///< Complimentary filter state
// Steady-state Kalman gains for the 2-state (airspeed, airspeed_rate) complementary filter.
//
// Noise model: R = diag(σ_meas², σ_rate_meas²), Q = diag(0, σ_proc²)
//
// Defining: a = 1/σ_meas, b = σ_proc/σ_rate_meas², d = a + b, n = sqrt(σ_proc·(2a + b))
//
// K = | a·n/d b/d |
// | a²·σ_proc/d b·n/d |
//
// With σ_meas=0.07 m/s, σ_rate_meas=0.2 m/s², σ_proc=0.2 m/s²:
// a=14.2857, b=5, d=19.2857, n=2.5912
//
// To recompute if any noise value changes:
// python3 -c "import math; sm=0.07; sr=0.2; sp=0.2; a=1/sm; b=sp/sr**2; d=a+b; n=math.sqrt(sp*(2*a+b)); print(a*n/d, b/d, a*a*sp/d, b*n/d)"
static constexpr float kKg00{1.91940287f};
static constexpr float kKg01{0.25925926f};
static constexpr float kKg10{2.11640212f};
static constexpr float kKg11{0.67179101f};
};
class TECSAltitudeReferenceModel
@@ -598,9 +614,6 @@ public:
void set_detect_underspeed_enabled(bool enabled) { _control_flag.detect_underspeed_enabled = enabled; };
// setters for parameters
void set_airspeed_measurement_std_dev(float std_dev) {_airspeed_filter_param.airspeed_measurement_std_dev = std_dev;};
void set_airspeed_rate_measurement_std_dev(float std_dev) {_airspeed_filter_param.airspeed_rate_measurement_std_dev = std_dev;};
void set_airspeed_filter_process_std_dev(float std_dev) {_airspeed_filter_param.airspeed_rate_noise_std_dev = std_dev;};
void set_integrator_gain_throttle(float gain) { _control_param.integrator_gain_throttle = gain;};
void set_integrator_gain_pitch(float gain) { _control_param.integrator_gain_pitch = gain; };
@@ -710,9 +723,6 @@ private:
/// Airspeed filter parameters.
TECSAirspeedFilter::Param _airspeed_filter_param{
.equivalent_airspeed_trim = 15.0f,
.airspeed_measurement_std_dev = 0.2f,
.airspeed_rate_measurement_std_dev = 0.05f,
.airspeed_rate_noise_std_dev = 0.02f
};
/// Reference model parameters.
TECSAltitudeReferenceModel::Param _reference_param{
src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp
-3
View File
@@ -110,9 +110,6 @@ FwLateralLongitudinalControl::parameters_update()
_tecs.set_airspeed_error_time_constant(_param_fw_t_tas_error_tc.get());
_tecs.set_ste_rate_time_const(_param_ste_rate_time_const.get());
_tecs.set_seb_rate_ff_gain(_param_seb_rate_ff.get());
_tecs.set_airspeed_measurement_std_dev(_param_speed_standard_dev.get());
_tecs.set_airspeed_rate_measurement_std_dev(_param_speed_rate_standard_dev.get());
_tecs.set_airspeed_filter_process_std_dev(_param_process_noise_standard_dev.get());
_roll_slew_rate.setSlewRate(radians(_param_fw_pn_r_slew_max.get()));
src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp
-3
View File
@@ -160,9 +160,6 @@ private:
(ParamFloat<px4::params::FW_T_STE_R_TC>) _param_ste_rate_time_const,
(ParamFloat<px4::params::FW_T_SEB_R_FF>) _param_seb_rate_ff,
(ParamFloat<px4::params::FW_T_SPDWEIGHT>) _param_t_spdweight,
(ParamFloat<px4::params::FW_T_SPD_STD>) _param_speed_standard_dev,
(ParamFloat<px4::params::FW_T_SPD_DEV_STD>) _param_speed_rate_standard_dev,
(ParamFloat<px4::params::FW_T_SPD_PRC_STD>) _param_process_noise_standard_dev,
(ParamFloat<px4::params::FW_T_CLMB_R_SP>) _param_climbrate_target,
(ParamFloat<px4::params::FW_T_SINK_R_SP>) _param_sinkrate_target,
(ParamFloat<px4::params::FW_THR_MAX>) _param_fw_thr_max,
src/modules/fw_lateral_longitudinal_control/fw_lat_long_params.yaml
-35
View File
@@ -103,41 +103,6 @@ parameters:
max: 10.0
decimal: 1
increment: 0.5
FW_T_SPD_STD:
description:
short: Airspeed measurement standard deviation
long: For the airspeed filter in TECS.
type: float
default: 0.07
unit: m/s
min: 0.01
max: 10.0
decimal: 2
increment: 0.1
FW_T_SPD_DEV_STD:
description:
short: Airspeed rate measurement standard deviation
long: For the airspeed filter in TECS.
type: float
default: 0.2
unit: m/s^2
min: 0.01
max: 10.0
decimal: 2
increment: 0.1
FW_T_SPD_PRC_STD:
description:
short: Process noise standard deviation for the airspeed rate
long: |-
This is defining the noise in the airspeed rate for the constant airspeed rate model
of the TECS airspeed filter.
type: float
default: 0.2
unit: m/s^2
min: 0.01
max: 10.0
decimal: 2
increment: 0.1
FW_T_RLL2THR:
description:
short: Roll -> Throttle feedforward