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.

Author: mahima-yoga

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

@@ -70,9 +70,6 @@ class TECSAirspeedFilter
 	 */
 	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 @@ class TECSAirspeedFilter
 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 @@ class TECS
 	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 @@ class TECS
 	/// 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

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

@@ -160,9 +160,6 @@ class FwLateralLongitudinalControl final : public ModuleBase, public ModuleParam
 		(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

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