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

@@ -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,
+		const float desired_airspeed_innovation = (-new_state_predicted(0) / dt - kKg01 * innovation(1)) / kKg00;
-				1) * innovation(1)) / kalman_gain(0,
+		new_state(1) = new_state_predicted(1) + dt * (kKg10 * desired_airspeed_innovation + kKg11 * innovation(1));
-						0);
-		new_state(1) = new_state_predicted(1) + dt * (kalman_gain(1, 0) * desired_airspeed_innovation + kalman_gain(1,
-				1) * innovation(1));
 	}
 
 	// Update states

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{

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

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