From 3779acb1d9312cbd098b3b2da7ce2ff4dadae547 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. Now the Kalman gain can be computed once at compile time. --- src/lib/tecs/TECS.cpp | 23 +++--------- src/lib/tecs/TECS.hpp | 26 +++++++++----- .../FwLateralLongitudinalControl.cpp | 3 -- .../FwLateralLongitudinalControl.hpp | 3 -- .../fw_lat_long_params.yaml | 35 ------------------- 5 files changed, 22 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..6ad8733118 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,23 @@ public: private: // States AirspeedFilterState _airspeed_state{.speed = 0.0f, .speed_rate = 0.0f}; ///< Complimentary filter state + + static constexpr float kAirspeedMeasurementStdDev{0.07f}; ///< Airspeed measurement standard deviation [m/s]. + static constexpr float kAirspeedRateMeasurementStdDev{0.2f}; ///< Airspeed rate measurement standard deviation [m/s²]. + static constexpr float kAirspeedRateNoiseStdDev{0.2f}; ///< Standard deviation of the airspeed rate process noise [m/s²]. + + // Intermediate values for the steady-state Kalman gain (all compile-time constants) + static constexpr float _kNoiseInv{1.0f / kAirspeedMeasurementStdDev}; + static constexpr float _kRateNoiseInv{1.0f / kAirspeedRateMeasurementStdDev}; + static constexpr float _kRateInvSqProc{_kRateNoiseInv *_kRateNoiseInv * kAirspeedRateNoiseStdDev}; + static constexpr float _kDenom{_kNoiseInv + _kRateInvSqProc}; + static constexpr float _kCommonNom{__builtin_sqrtf(kAirspeedRateNoiseStdDev * (2.0f * _kNoiseInv + _kRateInvSqProc))}; + + /// Steady-state Kalman gain entries, fixed because the noise constants are fixed. + static constexpr float kKg00{_kNoiseInv *_kCommonNom / _kDenom}; + static constexpr float kKg01{_kRateInvSqProc / _kDenom}; + static constexpr float kKg10{_kNoiseInv *_kNoiseInv *kAirspeedRateNoiseStdDev / _kDenom}; + static constexpr float kKg11{_kRateInvSqProc *_kCommonNom / _kDenom}; }; class TECSAltitudeReferenceModel @@ -598,9 +612,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 +721,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 beafcca947..e006305af6 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