From c4c68df4e485064956dab5dd50e7b2e034d5329d Mon Sep 17 00:00:00 2001 From: mahima-yoga Date: Thu, 2 Apr 2026 14:51:30 +0200 Subject: [PATCH] chore(tecs): remove TECS airspeed filter parameters These parameters are never tuned by the end-user, so we can use constants instead and immediately set the kalman gain at compile time --- src/lib/tecs/TECS.cpp | 23 +++--------- src/lib/tecs/TECS.hpp | 28 ++++++++++----- .../FwLateralLongitudinalControl.cpp | 3 -- .../FwLateralLongitudinalControl.hpp | 3 -- .../fw_lat_long_params.yaml | 35 ------------------- 5 files changed, 24 insertions(+), 68 deletions(-) diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index 69d124239d..1ed625e36e 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -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 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 diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index a45faeb2e3..8cf1f71819 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -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{ diff --git a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp index 78d8288ab8..9a53ddafc8 100644 --- a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp +++ b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp @@ -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())); diff --git a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp index a77af7d2f6..9aae9387fd 100644 --- a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp +++ b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp @@ -160,9 +160,6 @@ private: (ParamFloat) _param_ste_rate_time_const, (ParamFloat) _param_seb_rate_ff, (ParamFloat) _param_t_spdweight, - (ParamFloat) _param_speed_standard_dev, - (ParamFloat) _param_speed_rate_standard_dev, - (ParamFloat) _param_process_noise_standard_dev, (ParamFloat) _param_climbrate_target, (ParamFloat) _param_sinkrate_target, (ParamFloat) _param_fw_thr_max, diff --git a/src/modules/fw_lateral_longitudinal_control/fw_lat_long_params.yaml b/src/modules/fw_lateral_longitudinal_control/fw_lat_long_params.yaml index cde118cb3e..42c9dfb0e5 100644 --- a/src/modules/fw_lateral_longitudinal_control/fw_lat_long_params.yaml +++ b/src/modules/fw_lateral_longitudinal_control/fw_lat_long_params.yaml @@ -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