TECS: Combine both airspeed and airspeed derivative filters in TECS into one MIMO filter using a steady state Kalman filter.

msg/TecsStatus.msg

@@ -18,21 +18,11 @@ float32 true_airspeed_filtered
 float32 true_airspeed_derivative_sp
 float32 true_airspeed_derivative
 float32 true_airspeed_derivative_raw
-float32 true_airspeed_innovation
 
-float32 total_energy_error
-float32 energy_distribution_error
-float32 total_energy_rate_error
-float32 energy_distribution_rate_error
-
-float32 total_energy
 float32 total_energy_rate
-float32 total_energy_balance
 float32 total_energy_balance_rate
 
-float32 total_energy_sp
 float32 total_energy_rate_sp
-float32 total_energy_balance_sp
 float32 total_energy_balance_rate_sp
 
 float32 throttle_integ

src/lib/tecs/TECS.cpp

@@ -42,9 +42,13 @@
 
 #include <px4_platform_common/defines.h>
 
+#include "matrix/Matrix.hpp"
+#include "matrix/Vector2.hpp"
+
 using math::constrain;
 using math::max;
 using math::min;
+using namespace time_literals;
 
 static inline constexpr bool TIMESTAMP_VALID(float dt) { return (PX4_ISFINITE(dt) && dt > FLT_EPSILON);}
 
@@ -54,7 +58,6 @@ void TECSAirspeedFilter::initialize(const float equivalent_airspeed)
 
 	_airspeed_state.speed = equivalent_airspeed;
 	_airspeed_state.speed_rate = 0.0f;
-	_airspeed_rate_filter.reset(0.0f);
 }
 
 void TECSAirspeedFilter::update(const float dt, const Input &input, const Param &param,
@@ -85,49 +88,51 @@ void TECSAirspeedFilter::update(const float dt, const Input &input, const Param
 		airspeed_derivative = 0.0f;
 	}
 
-	// Alpha filtering done in the TECS module. TODO merge with the second order complementary filter.
+	/* Filter airspeed and rate using a constant airspeed rate model in a steady state Kalman Filter.
-	_airspeed_rate_filter.setParameters(dt, param.speed_derivative_time_const);
+	 We use the gains of the continuous Kalman filter Kc and approximate the discrete version Kalman gain Kd =dt*Kc,
+	 since the continuous algebraic Riccatti equation is easier to solve.
+	*/
 
-	if (PX4_ISFINITE(input.equivalent_airspeed_rate) && airspeed_sensor_available) {
+	matrix::Vector2f new_state_predicted;
-		_airspeed_rate_filter.update(airspeed_derivative);
 
-	} else {
+	new_state_predicted(0) = _airspeed_state.speed + dt * _airspeed_state.speed_rate;
-		_airspeed_rate_filter.reset(0.0f);
+	new_state_predicted(1) = _airspeed_state.speed_rate;
-	}
 
-	AirspeedFilterState new_airspeed_state;
+	const float airspeed_noise_inv{1.0f / param.airspeed_measurement_std_dev};
-	// Update TAS rate state
+	const float airspeed_rate_noise_inv{1.0f / param.airspeed_rate_measurement_std_dev};
-	const float airspeed_innovation = airspeed - _airspeed_state.speed;
+	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 airspeed_rate_state_input = airspeed_innovation * param.airspeed_estimate_freq * param.airspeed_estimate_freq;
+	const float denom{airspeed_noise_inv + airspeed_rate_noise_inv_squared_process_noise};
-	new_airspeed_state.speed_rate = _airspeed_state.speed_rate + airspeed_rate_state_input * dt;
+	const float common_nom{std::sqrt(param.airspeed_rate_noise_std_dev * (2.0f * airspeed_noise_inv + airspeed_rate_noise_inv_squared_process_noise))};
 
-	// Update TAS state
+	matrix::Matrix<float, 2, 2> kalman_gain;
-	const float airspeed_state_input = _airspeed_state.speed_rate + airspeed_innovation * param.airspeed_estimate_freq * 1.4142f;
+	kalman_gain(0, 0) = airspeed_noise_inv * common_nom / denom;
-	new_airspeed_state.speed = _airspeed_state.speed + airspeed_state_input * dt;
+	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;
 
-	// Clip tas at zero
+	const matrix::Vector2f innovation{(airspeed - new_state_predicted(0)), (airspeed_derivative - new_state_predicted(1))};
-	if (new_airspeed_state.speed < 0.0f) {
+	matrix::Vector2f new_state;
-		// clip TAS at zero, calculate input that would result in zero speed.
+	new_state = new_state_predicted + dt * (kalman_gain * (innovation));
-		float desired_airspeed_state_input = -_airspeed_state.speed / dt;
+
-		float desired_airspeed_innovation = (desired_airspeed_state_input - _airspeed_state.speed_rate) /
+	// Clip airspeed at zero
-						    (param.airspeed_estimate_freq * 1.4142f);
+	if (new_state(0) < FLT_EPSILON) {
-		new_airspeed_state.speed_rate = _airspeed_state.speed_rate + (desired_airspeed_innovation *
+		new_state(0) = 0.0f;
-						param.airspeed_estimate_freq * param.airspeed_estimate_freq * dt);
+		// calculate input that would result in zero speed.
-		new_airspeed_state.speed = 0.0f;
+		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));
 	}
 
 	// Update states
-	_airspeed_state = new_airspeed_state;
+	_airspeed_state.speed = new_state(0);
+	_airspeed_state.speed_rate = new_state(1);
 }
 
 TECSAirspeedFilter::AirspeedFilterState TECSAirspeedFilter::getState() const
 {
-	AirspeedFilterState filter_state{
+	return _airspeed_state;
-		.speed = _airspeed_state.speed,
-		.speed_rate = _airspeed_rate_filter.getState()
-	};
-
-	return filter_state;
 }
 
 void TECSReferenceModel::update(const float dt, const AltitudeReferenceState &setpoint, float altitude,
@@ -144,6 +149,7 @@ void TECSReferenceModel::update(const float dt, const AltitudeReferenceState &se
 		altitude = 0.0f;
 	}
 
+	// Consider the altitude rate setpoint already smooth. No need to filter further, simply hold the value for the altitude rate reference.
 	if (PX4_ISFINITE(setpoint.alt_rate)) {
 		_alt_rate_ref = setpoint.alt_rate;
 
@@ -195,23 +201,59 @@ float TECSReferenceModel::getAltitudeRateReference() const
 
 void TECSReferenceModel::initialize(const AltitudeReferenceState &state)
 {
-	AltitudeReferenceState init_state{state};
+	float init_state_alt{state.alt};
+	_alt_rate_ref = state.alt_rate;
 
 	if (!PX4_ISFINITE(state.alt)) {
-		init_state.alt = 0.0f;
+		init_state_alt = 0.0f;
 	}
 
 	if (!PX4_ISFINITE(state.alt_rate)) {
-		init_state.alt_rate = 0.0f;
+		_alt_rate_ref = 0.0f;
 	}
 
-	_alt_control_traj_generator.reset(0.0f, init_state.alt_rate, init_state.alt);
+	_alt_control_traj_generator.reset(0.0f, _alt_rate_ref, init_state_alt);
 }
 
-void TECSControl::initialize()
+void TECSControl::initialize(const Setpoint &setpoint, const Input &input, Param &param, const Flag &flag)
 {
-	_ste_rate_error_filter.reset(0.0f);
 	resetIntegrals();
+
+	AltitudePitchControl control_setpoint;
+
+	control_setpoint.tas_rate_setpoint = _calcAirspeedControlOutput(setpoint, input, param, flag);
+
+	control_setpoint.altitude_rate_setpoint = _calcAltitudeControlOutput(setpoint, input, param);
+
+	SpecificEnergyRates specific_energy_rate{_calcSpecificEnergyRates(control_setpoint, input)};
+
+	_detectUnderspeed(input, param, flag);
+
+	const SpecificEnergyWeighting weight{_updateSpeedAltitudeWeights(param, flag)};
+	ControlValues seb_rate{_calcPitchControlSebRate(weight, specific_energy_rate)};
+
+	_pitch_setpoint = _calcPitchControlOutput(input, seb_rate, param, flag);
+
+	const STERateLimit limit{_calculateTotalEnergyRateLimit(param)};
+
+	_ste_rate_estimate_filter.reset(specific_energy_rate.spe_rate.estimate + specific_energy_rate.ske_rate.estimate);
+
+	ControlValues ste_rate{_calcThrottleControlSteRate(limit, specific_energy_rate, param)};
+
+	_throttle_setpoint = _calcThrottleControlOutput(limit, ste_rate, param, flag);
+
+	_ste_rate = ste_rate.estimate;
+
+	// Debug output
+	_debug_output.total_energy_rate_estimate = ste_rate.estimate;
+	_debug_output.total_energy_rate_sp = ste_rate.setpoint;
+	_debug_output.throttle_integrator = _throttle_integ_state;
+	_debug_output.energy_balance_rate_estimate = seb_rate.estimate;
+	_debug_output.energy_balance_rate_sp = seb_rate.setpoint;
+	_debug_output.pitch_integrator = _pitch_integ_state;
+
+	_debug_output.altitude_rate_control = control_setpoint.altitude_rate_setpoint;
+	_debug_output.true_airspeed_derivative_control = control_setpoint.tas_rate_setpoint;
 }
 
 void TECSControl::update(const float dt, const Setpoint &setpoint, const Input &input, Param &param, const Flag &flag)
@@ -225,25 +267,27 @@ void TECSControl::update(const float dt, const Setpoint &setpoint, const Input &
 
 	AltitudePitchControl control_setpoint;
 
-	control_setpoint.tas_rate_setpoint = _airspeedControl(setpoint, input, param, flag);
+	control_setpoint.tas_rate_setpoint = _calcAirspeedControlOutput(setpoint, input, param, flag);
 
-	control_setpoint.altitude_rate_setpoint = _altitudeControl(setpoint, input, param);
+	control_setpoint.altitude_rate_setpoint = _calcAltitudeControlOutput(setpoint, input, param);
 
-	SpecificEnergy se{_updateEnergyBalance(control_setpoint, input)};
+	SpecificEnergyRates specific_energy_rate{_calcSpecificEnergyRates(control_setpoint, input)};
 
 	_detectUnderspeed(input, param, flag);
 
-	_updatePitchSetpoint(dt, input, se, param, flag);
+	_calcPitchControl(dt, input, specific_energy_rate, param, flag);
 
-	_updateThrottleSetpoint(dt, se, param, flag);
+	_calcThrottleControl(dt, specific_energy_rate, param, flag);
 
 	_debug_output.altitude_rate_control = control_setpoint.altitude_rate_setpoint;
 	_debug_output.true_airspeed_derivative_control = control_setpoint.tas_rate_setpoint;
+	_debug_output.pitch_integrator = _pitch_integ_state;
+	_debug_output.throttle_integrator = _throttle_integ_state;
 }
 
-TECSControl::STELimit TECSControl::_calculateTotalEnergyRateLimit(const Param &param) const
+TECSControl::STERateLimit TECSControl::_calculateTotalEnergyRateLimit(const Param &param) const
 {
-	TECSControl::STELimit limit;
+	TECSControl::STERateLimit limit;
 	// Calculate the specific total energy rate limits from the max throttle limits
 	limit.STE_rate_max = math::max(param.max_climb_rate, FLT_EPSILON) * CONSTANTS_ONE_G;
 	limit.STE_rate_min = - math::max(param.max_sink_rate, FLT_EPSILON) * CONSTANTS_ONE_G;
@@ -251,12 +295,12 @@ TECSControl::STELimit TECSControl::_calculateTotalEnergyRateLimit(const Param &p
 	return limit;
 }
 
-float TECSControl::_airspeedControl(const Setpoint &setpoint, const Input &input, const Param &param,
+float TECSControl::_calcAirspeedControlOutput(const Setpoint &setpoint, const Input &input, const Param &param,
-				    const Flag &flag) const
+		const Flag &flag) const
 {
 	float airspeed_rate_output{0.0f};
 
-	const STELimit limit{_calculateTotalEnergyRateLimit(param)};
+	const STERateLimit limit{_calculateTotalEnergyRateLimit(param)};
 
 	// calculate the demanded true airspeed rate of change based on first order response of true airspeed error
 	// if airspeed measurement is not enabled then always set the rate setpoint to zero in order to avoid constant rate setpoints
@@ -272,7 +316,7 @@ float TECSControl::_airspeedControl(const Setpoint &setpoint, const Input &input
 	return airspeed_rate_output;
 }
 
-float TECSControl::_altitudeControl(const Setpoint &setpoint, const Input &input, const Param &param) const
+float TECSControl::_calcAltitudeControlOutput(const Setpoint &setpoint, const Input &input, const Param &param) const
 {
 	float altitude_rate_output;
 	altitude_rate_output = (setpoint.altitude_reference.alt - input.altitude) * param.altitude_error_gain +
@@ -282,29 +326,27 @@ float TECSControl::_altitudeControl(const Setpoint &setpoint, const Input &input
 	return altitude_rate_output;
 }
 
-TECSControl::SpecificEnergy TECSControl::_updateEnergyBalance(const AltitudePitchControl &control_setpoint,
+TECSControl::SpecificEnergyRates TECSControl::_calcSpecificEnergyRates(const AltitudePitchControl &control_setpoint,
 		const Input &input) const
 {
-	SpecificEnergy se;
+	SpecificEnergyRates specific_energy_rates;
 	// Calculate specific energy rate demands in units of (m**2/sec**3)
-	se.spe.rate_setpoint = control_setpoint.altitude_rate_setpoint * CONSTANTS_ONE_G; // potential energy rate of change
+	specific_energy_rates.spe_rate.setpoint = control_setpoint.altitude_rate_setpoint *
-	se.ske.rate_setpoint = input.tas * control_setpoint.tas_rate_setpoint; // kinetic energy rate of change
+			CONSTANTS_ONE_G; // potential energy rate of change
+	specific_energy_rates.ske_rate.setpoint = input.tas *
+			control_setpoint.tas_rate_setpoint; // kinetic energy rate of change
 
 	// Calculate specific energy rates in units of (m**2/sec**3)
-	se.spe.rate = input.altitude_rate * CONSTANTS_ONE_G; // potential energy rate of change
+	specific_energy_rates.spe_rate.estimate = input.altitude_rate * CONSTANTS_ONE_G; // potential energy rate of change
-	se.ske.rate = input.tas * input.tas_rate;// kinetic energy rate of change
+	specific_energy_rates.ske_rate.estimate = input.tas * input.tas_rate;// kinetic energy rate of change
 
-	// Calculate energy rate error
+	return specific_energy_rates;
-	se.spe.rate_error = se.spe.rate_setpoint - se.spe.rate;
-	se.ske.rate_error = se.ske.rate_setpoint - se.ske.rate;
-
-	return se;
 }
 
 void TECSControl::_detectUnderspeed(const Input &input, const Param &param, const Flag &flag)
 {
 	if (!flag.detect_underspeed_enabled) {
-		_percent_undersped = 0.0f;
+		_ratio_undersped = 0.0f;
 		return;
 	}
 
@@ -320,8 +362,8 @@ void TECSControl::_detectUnderspeed(const Input &input, const Param &param, cons
 	const float tas_fully_undersped = math::max(param.tas_min - tas_error_bound - tas_underspeed_soft_bound, 0.0f);
 	const float tas_starting_to_underspeed = math::max(param.tas_min - tas_error_bound, tas_fully_undersped);
 
-	_percent_undersped = 1.0f - math::constrain((input.tas - tas_fully_undersped) /
+	_ratio_undersped = 1.0f - math::constrain((input.tas - tas_fully_undersped) /
-			     math::max(tas_starting_to_underspeed - tas_fully_undersped, FLT_EPSILON), 0.0f, 1.0f);
+			   math::max(tas_starting_to_underspeed - tas_fully_undersped, FLT_EPSILON), 0.0f, 1.0f);
 }
 
 TECSControl::SpecificEnergyWeighting TECSControl::_updateSpeedAltitudeWeights(const Param &param, const Flag &flag)
@@ -334,8 +376,8 @@ TECSControl::SpecificEnergyWeighting TECSControl::_updateSpeedAltitudeWeights(co
 	if (flag.climbout_mode_active && flag.airspeed_enabled) {
 		pitch_speed_weight = 2.0f;
 
-	} else if (_percent_undersped > FLT_EPSILON && flag.airspeed_enabled) {
+	} else if (_ratio_undersped > FLT_EPSILON && flag.airspeed_enabled) {
-		pitch_speed_weight = 2.0f * _percent_undersped + (1.0f - _percent_undersped) * pitch_speed_weight;
+		pitch_speed_weight = 2.0f * _ratio_undersped + (1.0f - _ratio_undersped) * pitch_speed_weight;
 
 	} else if (!flag.airspeed_enabled) {
 		pitch_speed_weight = 0.0f;
@@ -350,13 +392,35 @@ TECSControl::SpecificEnergyWeighting TECSControl::_updateSpeedAltitudeWeights(co
 	return weight;
 }
 
-void TECSControl::_updatePitchSetpoint(float dt, const Input &input, const SpecificEnergy &se, Param &param,
+void TECSControl::_calcPitchControl(float dt, const Input &input, const SpecificEnergyRates &specific_energy_rates,
-				       const Flag &flag)
+				    const Param &param,
+				    const Flag &flag)
 {
 	const SpecificEnergyWeighting weight{_updateSpeedAltitudeWeights(param, flag)};
+	ControlValues seb_rate{_calcPitchControlSebRate(weight, specific_energy_rates)};
+
+	_calcPitchControlUpdate(dt, seb_rate, param);
+	const float pitch_setpoint{_calcPitchControlOutput(input, seb_rate, param, flag)};
+
+	// Comply with the specified vertical acceleration limit by applying a pitch rate limit
+	// NOTE: at zero airspeed, the pitch increment is unbounded
+	const float pitch_increment = dt * param.vert_accel_limit / math::max(input.tas, FLT_EPSILON);
+	_pitch_setpoint = constrain(pitch_setpoint, _pitch_setpoint - pitch_increment,
+				    _pitch_setpoint + pitch_increment);
+
+	//Debug Output
+	_debug_output.energy_balance_rate_estimate = seb_rate.estimate;
+	_debug_output.energy_balance_rate_sp = seb_rate.setpoint;
+	_debug_output.pitch_integrator = _pitch_integ_state;
+}
+
+TECSControl::ControlValues TECSControl::_calcPitchControlSebRate(const SpecificEnergyWeighting &weight,
+		const SpecificEnergyRates &specific_energy_rates) const
+{
+	ControlValues seb_rate;
 	/*
-	 * The SKE_weighting variable controls how speed and altitude control are prioritised by the pitch demand calculation.
+	 * The SKE_weighting variable controls how speed and altitude control are prioritized by the pitch demand calculation.
-	 * A weighting of 1 givea equal speed and altitude priority
+	 * A weighting of 1 gives equal speed and altitude priority
 	 * A weighting of 0 gives 100% priority to altitude control and must be used when no airspeed measurement is available.
 	 * A weighting of 2 provides 100% priority to speed control and is used when:
 	 * a) an underspeed condition is detected.
@@ -364,18 +428,21 @@ void TECSControl::_updatePitchSetpoint(float dt, const Input &input, const Speci
 	 * rises above the demanded value, the pitch angle demand is increased by the TECS controller to prevent the vehicle overspeeding.
 	 * The weighting can be adjusted between 0 and 2 depending on speed and altitude accuracy requirements.
 	*/
-	// Calculate the specific energy balance rate demand
+	seb_rate.setpoint = specific_energy_rates.spe_rate.setpoint * weight.spe_weighting -
-	const float seb_rate_setpoint = se.spe.rate_setpoint * weight.spe_weighting - se.ske.rate_setpoint * weight.ske_weighting;
+			    specific_energy_rates.ske_rate.setpoint *
+			    weight.ske_weighting;
 
-	// Calculate the specific energy balance rate error
+	seb_rate.estimate = (specific_energy_rates.spe_rate.estimate * weight.spe_weighting) -
-	const float seb_rate_error = (se.spe.rate_error * weight.spe_weighting) - (se.ske.rate_error * weight.ske_weighting);
+			    (specific_energy_rates.ske_rate.estimate * weight.ske_weighting);
 
-	_debug_output.energy_balance_rate_error = seb_rate_error;
+	return seb_rate;
-	_debug_output.energy_balance_rate_sp = seb_rate_setpoint;
+}
 
-	if (param.integrator_gain_pitch > 0.0f) {
+void TECSControl::_calcPitchControlUpdate(float dt, const ControlValues &seb_rate, const Param &param)
+{
+	if (param.integrator_gain_pitch > FLT_EPSILON) {
 		// Calculate pitch integrator input term
-		float pitch_integ_input = seb_rate_error * param.integrator_gain_pitch;
+		float pitch_integ_input = _getControlError(seb_rate) * param.integrator_gain_pitch;
 
 		// Prevent the integrator changing in a direction that will increase pitch demand saturation
 		if (_pitch_setpoint >= param.pitch_max) {
@@ -391,10 +458,16 @@ void TECSControl::_updatePitchSetpoint(float dt, const Input &input, const Speci
 	} else {
 		_pitch_integ_state = 0.0f;
 	}
+}
 
+float TECSControl::_calcPitchControlOutput(const Input &input, const ControlValues &seb_rate, const Param &param,
+		const Flag &flag) const
+{
+	// Pitch setpoint
 	// Calculate a specific energy correction that doesn't include the integrator contribution
-	float SEB_rate_correction = seb_rate_error * param.pitch_damping_gain + _pitch_integ_state + param.seb_rate_ff *
+	float SEB_rate_correction = _getControlError(seb_rate) * param.pitch_damping_gain + _pitch_integ_state +
-				    seb_rate_setpoint;
+				    param.seb_rate_ff *
+				    seb_rate.setpoint;
 
 	// Calculate derivative from change in climb angle to rate of change of specific energy balance
 	const float climb_angle_to_SEB_rate = input.tas * CONSTANTS_ONE_G;
@@ -412,47 +485,70 @@ void TECSControl::_updatePitchSetpoint(float dt, const Input &input, const Speci
 	// pitch transients due to control action or turbulence.
 	const float pitch_setpoint_unc = SEB_rate_correction / climb_angle_to_SEB_rate;
 
-	const float pitch_setpoint = constrain(pitch_setpoint_unc, param.pitch_min, param.pitch_max);
+	return constrain(pitch_setpoint_unc, param.pitch_min, param.pitch_max);
-
-	// Comply with the specified vertical acceleration limit by applying a pitch rate limit
-	// NOTE: at zero airspeed, the pitch increment is unbounded
-	const float pitch_increment = dt * param.vert_accel_limit / input.tas;
-	_pitch_setpoint = constrain(pitch_setpoint, _pitch_setpoint - pitch_increment,
-				    _pitch_setpoint + pitch_increment);
 }
 
-void TECSControl::_updateThrottleSetpoint(float dt, const SpecificEnergy &se, const Param &param, const Flag &flag)
+void TECSControl::_calcThrottleControl(float dt, const SpecificEnergyRates &specific_energy_rates, const Param &param,
+				       const Flag &flag)
 {
-	const STELimit limit{_calculateTotalEnergyRateLimit(param)};
+	const STERateLimit limit{_calculateTotalEnergyRateLimit(param)};
 
-	float STE_rate_setpoint = se.spe.rate_setpoint + se.ske.rate_setpoint;
+	// Update STE rate estimate LP filter
+	const float STE_rate_estimate_raw = specific_energy_rates.spe_rate.estimate + specific_energy_rates.ske_rate.estimate;
+	_ste_rate_estimate_filter.setParameters(dt, param.ste_rate_time_const);
+	_ste_rate_estimate_filter.update(STE_rate_estimate_raw);
+
+	ControlValues ste_rate{_calcThrottleControlSteRate(limit, specific_energy_rates, param)};
+	_calcThrottleControlUpdate(dt, limit, ste_rate, param, flag);
+	float throttle_setpoint{_calcThrottleControlOutput(limit, ste_rate, param, flag)};
+
+	// Rate limit the throttle demand
+	if (fabsf(param.throttle_slewrate) > FLT_EPSILON) {
+		const float throttle_increment_limit = dt * (param.throttle_max - param.throttle_min) * param.throttle_slewrate;
+		throttle_setpoint = constrain(throttle_setpoint, _throttle_setpoint - throttle_increment_limit,
+					      _throttle_setpoint + throttle_increment_limit);
+	}
+
+	_throttle_setpoint = constrain(throttle_setpoint, param.throttle_min, param.throttle_max);
+	_ste_rate = ste_rate.estimate;
+
+	// Debug output
+	_debug_output.total_energy_rate_estimate = ste_rate.estimate;
+	_debug_output.total_energy_rate_sp = ste_rate.setpoint;
+	_debug_output.throttle_integrator = _throttle_integ_state;
+}
+
+TECSControl::ControlValues TECSControl::_calcThrottleControlSteRate(const STERateLimit &limit,
+		const SpecificEnergyRates &specific_energy_rates,
+		const Param &param) const
+{
+	// Output ste rate values
+	ControlValues ste_rate;
+	ste_rate.setpoint = specific_energy_rates.spe_rate.setpoint + specific_energy_rates.ske_rate.setpoint;
 
 	// Adjust the demanded total energy rate to compensate for induced drag rise in turns.
 	// Assume induced drag scales linearly with normal load factor.
 	// The additional normal load factor is given by (1/cos(bank angle) - 1)
-	STE_rate_setpoint += param.load_factor_correction * (param.load_factor - 1.f);
+	ste_rate.setpoint += param.load_factor_correction * (param.load_factor - 1.f);
 
-	STE_rate_setpoint = constrain(STE_rate_setpoint, limit.STE_rate_min, limit.STE_rate_max);
+	ste_rate.setpoint = constrain(ste_rate.setpoint, limit.STE_rate_min, limit.STE_rate_max);
+	ste_rate.estimate = _ste_rate_estimate_filter.getState();
 
-	_ste_rate = se.spe.rate + se.ske.rate;
+	return ste_rate;
-
+}
-	const float STE_rate_error_raw = se.spe.rate_error + se.ske.rate_error;
-	_ste_rate_error_filter.setParameters(dt, param.ste_rate_time_const);
-	_ste_rate_error_filter.update(STE_rate_error_raw);
-	const float STE_rate_error{_ste_rate_error_filter.getState()};
-
-	_debug_output.total_energy_rate_error = STE_rate_error;
-	_debug_output.total_energy_rate_sp = STE_rate_setpoint;
 
+void TECSControl::_calcThrottleControlUpdate(float dt, const STERateLimit &limit, const ControlValues &ste_rate,
+		const Param &param, const Flag &flag)
+{
 	// Calculate gain scaler from specific energy rate error to throttle
 	const float STE_rate_to_throttle = 1.0f / (limit.STE_rate_max - limit.STE_rate_min);
 
 	// Integral handling
 	if (flag.airspeed_enabled) {
-		if (param.integrator_gain_throttle > 0.0f) {
+		if (param.integrator_gain_throttle > FLT_EPSILON) {
 			// underspeed conditions zero out integration
-			float throttle_integ_input = (STE_rate_error * param.integrator_gain_throttle) * dt *
+			float throttle_integ_input = (_getControlError(ste_rate) * param.integrator_gain_throttle) * dt *
-						     STE_rate_to_throttle * (1.0f - _percent_undersped);
+						     STE_rate_to_throttle * (1.0f - _ratio_undersped);
 
 			// only allow integrator propagation into direction which unsaturates throttle
 			if (_throttle_setpoint >= param.throttle_max) {
@@ -477,6 +573,14 @@ void TECSControl::_updateThrottleSetpoint(float dt, const SpecificEnergy &se, co
 		}
 
 	}
+}
+
+float TECSControl::_calcThrottleControlOutput(const STERateLimit &limit, const ControlValues &ste_rate,
+		const Param &param,
+		const Flag &flag) const
+{
+	// Calculate gain scaler from specific energy rate error to throttle
+	const float STE_rate_to_throttle = 1.0f / (limit.STE_rate_max - limit.STE_rate_min);
 
 	// Calculate a predicted throttle from the demanded rate of change of energy, using the cruise throttle
 	// as the starting point. Assume:
@@ -485,20 +589,21 @@ void TECSControl::_updateThrottleSetpoint(float dt, const SpecificEnergy &se, co
 	// Specific total energy rate = _STE_rate_min is achieved when throttle is set to _throttle_setpoint_min
 	float throttle_predicted = 0.0f;
 
-	if (STE_rate_setpoint >= 0) {
+	if (ste_rate.setpoint >= FLT_EPSILON) {
 		// throttle is between trim and maximum
-		throttle_predicted = param.throttle_trim + STE_rate_setpoint / limit.STE_rate_max *
+		throttle_predicted = param.throttle_trim + ste_rate.setpoint / limit.STE_rate_max *
 				     (param.throttle_max - param.throttle_trim);
 
 	} else {
 		// throttle is between trim and minimum
-		throttle_predicted = param.throttle_trim + STE_rate_setpoint / limit.STE_rate_min *
+		throttle_predicted = param.throttle_trim + ste_rate.setpoint / limit.STE_rate_min *
 				     (param.throttle_min - param.throttle_trim);
 
 	}
 
 	// Add proportional and derivative control feedback to the predicted throttle and constrain to throttle limits
-	float throttle_setpoint = (STE_rate_error * param.throttle_damping_gain) * STE_rate_to_throttle + throttle_predicted;
+	float throttle_setpoint = (_getControlError(ste_rate) * param.throttle_damping_gain) * STE_rate_to_throttle +
+				  throttle_predicted;
 
 	if (flag.airspeed_enabled) {
 		// Add the integrator feedback during closed loop operation with an airspeed sensor
@@ -507,16 +612,9 @@ void TECSControl::_updateThrottleSetpoint(float dt, const SpecificEnergy &se, co
 	}
 
 	// ramp in max throttle setting with underspeediness value
-	throttle_setpoint = _percent_undersped * param.throttle_max + (1.0f - _percent_undersped) * throttle_setpoint;
+	throttle_setpoint = _ratio_undersped * param.throttle_max + (1.0f - _ratio_undersped) * throttle_setpoint;
 
-	// Rate limit the throttle demand
+	return constrain(throttle_setpoint, param.throttle_min, param.throttle_max);
-	if (fabsf(param.throttle_slewrate) > 0.01f) {
-		const float throttle_increment_limit = dt * (param.throttle_max - param.throttle_min) * param.throttle_slewrate;
-		throttle_setpoint = constrain(throttle_setpoint, _throttle_setpoint - throttle_increment_limit,
-					      _throttle_setpoint + throttle_increment_limit);
-	}
-
-	_throttle_setpoint = constrain(throttle_setpoint, param.throttle_min, param.throttle_max);
 }
 
 void TECSControl::resetIntegrals()
@@ -528,7 +626,7 @@ void TECSControl::resetIntegrals()
 float TECS::_update_speed_setpoint(const float tas_min, const float tas_max, const float tas_setpoint, const float tas)
 {
 	float new_setpoint{tas_setpoint};
-	const float percent_undersped = _control.getPercentUndersped();
+	const float percent_undersped = _control.getRatioUndersped();
 
 	// Set the TAS demand to the minimum value if an underspeed or
 	// or a uncontrolled descent condition exists to maximise climb rate
@@ -571,7 +669,7 @@ void TECS::_detect_uncommanded_descent(float throttle_setpoint_max, float altitu
 	const float SKE_error = SKE_setpoint - SKE_estimate;
 	const float STE_error = SPE_error + SKE_error;
 
-	const bool underspeed_detected = _control.getPercentUndersped() > FLT_EPSILON;
+	const bool underspeed_detected = _control.getRatioUndersped() > FLT_EPSILON;
 
 	// If total energy is very low and reducing, throttle is high, and we are not in an underspeed condition, then enter uncommanded descent recovery mode
 	const bool enter_mode = !_uncommanded_descent_recovery && !underspeed_detected && (STE_error > 200.0f)
@@ -590,14 +688,38 @@ void TECS::_detect_uncommanded_descent(float throttle_setpoint_max, float altitu
 	}
 }
 
-void TECS::initialize(const float altitude, const float altitude_rate, const float equivalent_airspeed)
+void TECS::initialize(const float altitude, const float altitude_rate, const float equivalent_airspeed,
+		      const float eas_to_tas)
 {
 	// Init subclasses
 	TECSReferenceModel::AltitudeReferenceState current_state{	.alt = altitude,
 			.alt_rate = altitude_rate};
 	_reference_model.initialize(current_state);
 	_airspeed_filter.initialize(equivalent_airspeed);
-	_control.initialize();
+
+	TECSControl::Setpoint control_setpoint;
+	control_setpoint.altitude_reference = _reference_model.getAltitudeReference();
+	control_setpoint.altitude_rate_setpoint = _reference_model.getAltitudeRateReference();
+	control_setpoint.tas_setpoint = equivalent_airspeed * eas_to_tas;
+
+	const TECSControl::Input control_input{ .altitude = altitude,
+						.altitude_rate = altitude_rate,
+						.tas = eas_to_tas * equivalent_airspeed,
+						.tas_rate = 0.0f};
+
+	_control.initialize(control_setpoint, control_input, _control_param, _control_flag);
+
+	_debug_status.tecs_mode = _tecs_mode;
+	_debug_status.control = _control.getDebugOutput();
+	const TECSAirspeedFilter::AirspeedFilterState eas = _airspeed_filter.getState();
+	_debug_status.true_airspeed_filtered = eas_to_tas * eas.speed;
+	_debug_status.true_airspeed_derivative = eas_to_tas * eas.speed_rate;
+	const TECSReferenceModel::AltitudeReferenceState ref_alt{_reference_model.getAltitudeReference()};
+	_debug_status.altitude_sp = ref_alt.alt;
+	_debug_status.altitude_rate_alt_ref = ref_alt.alt_rate;
+	_debug_status.altitude_rate_feedforward = _reference_model.getAltitudeRateReference();
+
+	_update_timestamp = hrt_absolute_time();
 }
 
 void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_setpoint, float equivalent_airspeed,
@@ -605,9 +727,23 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
 		  float throttle_trim, float pitch_limit_min, float pitch_limit_max, float target_climbrate, float target_sinkrate,
 		  const float speed_deriv_forward, float hgt_rate, float hgt_rate_sp)
 {
+
 	// Calculate the time since last update (seconds)
-	const uint64_t now(hrt_absolute_time());
+	const hrt_abstime now(hrt_absolute_time());
-	const float dt = (now - _update_timestamp) * 1e-6f;
+	const float dt = static_cast<float>((now - _update_timestamp)) / 1_s;
+
+	// Update parameters from input
+	// Reference model
+	_reference_param.target_climbrate = target_climbrate;
+	_reference_param.target_sinkrate = target_sinkrate;
+	// Control
+	_control_param.tas_min = eas_to_tas * _equivalent_airspeed_min;
+	_control_param.pitch_max = pitch_limit_max;
+	_control_param.pitch_min = pitch_limit_min;
+	_control_param.throttle_trim = throttle_trim;
+	_control_param.throttle_max = throttle_setpoint_max;
+	_control_param.throttle_min = throttle_min;
+	_control_flag.climbout_mode_active = climb_out_setpoint;
 
 	if (dt < DT_MIN) {
 		// Update intervall too small, do not update. Assume constant states/output in this case.
@@ -616,23 +752,22 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
 
 	if (dt > DT_MAX || _update_timestamp == 0UL) {
 		// Update time intervall too large, can't guarantee sanity of state updates anymore. reset the control loop.
-		initialize(altitude, hgt_rate, equivalent_airspeed);
+		initialize(altitude, hgt_rate, equivalent_airspeed, eas_to_tas);
 
 	} else {
 		// Update airspeedfilter submodule
 		const TECSAirspeedFilter::Input airspeed_input{ .equivalent_airspeed = equivalent_airspeed,
 				.equivalent_airspeed_rate = speed_deriv_forward / eas_to_tas};
-		_airspeed_param.equivalent_airspeed_trim = _equivalent_airspeed_trim;
+
-		_airspeed_filter.update(dt, airspeed_input, _airspeed_param, _airspeed_enabled);
+		_airspeed_filter.update(dt, airspeed_input, _airspeed_filter_param, _control_flag.airspeed_enabled);
 		const TECSAirspeedFilter::AirspeedFilterState eas = _airspeed_filter.getState();
 
 		// Update Reference model submodule
 		const TECSReferenceModel::AltitudeReferenceState setpoint{ .alt = hgt_setpoint,
-				.alt_rate = hgt_rate_sp
+				.alt_rate = hgt_rate_sp};
-								   };
+
-		_reference_param.target_climbrate = target_climbrate;
-		_reference_param.target_sinkrate = target_sinkrate;
 		_reference_model.update(dt, setpoint, altitude, _reference_param);
+
 		TECSControl::Setpoint control_setpoint;
 		control_setpoint.altitude_reference = _reference_model.getAltitudeReference();
 		control_setpoint.altitude_rate_setpoint = _reference_model.getAltitudeRateReference();
@@ -643,59 +778,42 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set
 						eas_to_tas * _equivalent_airspeed_max, EAS_setpoint * eas_to_tas, eas_to_tas * eas.speed);
 
 		const TECSControl::Input control_input{ .altitude = altitude,
-						  .altitude_rate = hgt_rate,
+							.altitude_rate = hgt_rate,
-						  .tas = eas_to_tas * eas.speed,
+							.tas = eas_to_tas * eas.speed,
-						  .tas_rate = eas_to_tas * eas.speed_rate
+							.tas_rate = eas_to_tas * eas.speed_rate};
-						};
+
-		_control_param.equivalent_airspeed_trim = _equivalent_airspeed_trim;
+		_control.update(dt, control_setpoint, control_input, _control_param, _control_flag);
-		_control_param.tas_min = eas_to_tas * _equivalent_airspeed_min;
-		_control_param.pitch_max = pitch_limit_max;
-		_control_param.pitch_min = pitch_limit_min;
-		_control_param.throttle_trim = throttle_trim;
-		_control_param.throttle_max = throttle_setpoint_max;
-		_control_param.throttle_min = throttle_min;
-		const TECSControl::Flag control_flag{ .airspeed_enabled = _airspeed_enabled,
-						.climbout_mode_active = climb_out_setpoint,
-						.detect_underspeed_enabled = _detect_underspeed_enabled
-					      };
-		_control.update(dt, control_setpoint, control_input, _control_param, control_flag);
 
 		// Detect an uncommanded descent caused by an unachievable airspeed demand
 		_detect_uncommanded_descent(throttle_setpoint_max, altitude, hgt_setpoint, equivalent_airspeed * eas_to_tas,
 					    control_setpoint.tas_setpoint);
 
-		TECSControl::DebugOutput control_status = _control.getDebugOutput();
+		// Update time stamps
-		_debug_status.altitude_rate_control = control_status.altitude_rate_control;
+		_update_timestamp = now;
-		_debug_status.energy_balance_rate_error = control_status.energy_balance_rate_error;
+
-		_debug_status.energy_balance_rate_sp = control_status.energy_balance_rate_sp;
+
-		_debug_status.total_energy_rate_error = control_status.total_energy_rate_error;
+		// Set TECS mode for next frame
-		_debug_status.total_energy_rate_sp = control_status.total_energy_rate_sp;
+		if (_control.getRatioUndersped() > FLT_EPSILON) {
-		_debug_status.true_airspeed_derivative_control = control_status.true_airspeed_derivative_control;
+			_tecs_mode = ECL_TECS_MODE_UNDERSPEED;
+
+		} else if (_uncommanded_descent_recovery) {
+			_tecs_mode = ECL_TECS_MODE_BAD_DESCENT;
+
+		} else if (climb_out_setpoint) {
+			_tecs_mode = ECL_TECS_MODE_CLIMBOUT;
+
+		} else {
+			// This is the default operation mode
+			_tecs_mode = ECL_TECS_MODE_NORMAL;
+		}
+
+		_debug_status.tecs_mode = _tecs_mode;
+		_debug_status.control = _control.getDebugOutput();
 		_debug_status.true_airspeed_filtered = eas_to_tas * eas.speed;
 		_debug_status.true_airspeed_derivative = eas_to_tas * eas.speed_rate;
 		_debug_status.altitude_sp = control_setpoint.altitude_reference.alt;
-		_debug_status.altitude_rate = control_setpoint.altitude_reference.alt_rate;
+		_debug_status.altitude_rate_alt_ref = control_setpoint.altitude_reference.alt_rate;
-		_debug_status.altitude_rate_setpoint = control_setpoint.altitude_rate_setpoint;
+		_debug_status.altitude_rate_feedforward = control_setpoint.altitude_rate_setpoint;
 	}
-
-	// Update time stamps
-	_update_timestamp = now;
-
-	// Set TECS mode for next frame
-	if (_control.getPercentUndersped() > FLT_EPSILON) {
-		_tecs_mode = ECL_TECS_MODE_UNDERSPEED;
-
-	} else if (_uncommanded_descent_recovery) {
-		_tecs_mode = ECL_TECS_MODE_BAD_DESCENT;
-
-	} else if (climb_out_setpoint) {
-		_tecs_mode = ECL_TECS_MODE_CLIMBOUT;
-
-	} else {
-		// This is the default operation mode
-		_tecs_mode = ECL_TECS_MODE_NORMAL;
-	}
-
-	_debug_status.tecs_mode = _tecs_mode;
 }
 

src/lib/tecs/TECS.hpp

@@ -39,6 +39,7 @@
 
 #pragma once
 
+#include <drivers/drv_hrt.h>
 #include <mathlib/mathlib.h>
 #include <matrix/math.hpp>
 #include <lib/mathlib/math/filter/AlphaFilter.hpp>
@@ -66,9 +67,10 @@ public:
 	 *
 	 */
 	struct Param {
-		float equivalent_airspeed_trim; 	///< the trim value of the equivalent airspeed om [m/s].
+		float equivalent_airspeed_trim; 	///< the trim value of the equivalent airspeed [m/s].
-		float airspeed_estimate_freq; 		///< cross-over frequency of the equivalent airspeed complementary filter [rad/sec].
+		float airspeed_measurement_std_dev; 	///< airspeed measurement standard deviation in [m/s].
-		float speed_derivative_time_const;	///< speed derivative filter time constant [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²].
 	};
 
 	/**
@@ -108,7 +110,6 @@ public:
 
 private:
 	// States
-	AlphaFilter<float> _airspeed_rate_filter;				///< Alpha filter for airspeed rate
 	AirspeedFilterState _airspeed_state{.speed = 0.0f, .speed_rate = 0.0f};	///< Complimentary filter state
 };
 
@@ -176,8 +177,6 @@ private:
 	// State
 	VelocitySmoothing
 	_alt_control_traj_generator;		///< Generates altitude rate and altitude setpoint trajectory when altitude is commanded.
-
-	// Output
 	float _alt_rate_ref; 			///< Altitude rate reference in [m/s].
 };
 
@@ -235,10 +234,12 @@ public:
 	struct DebugOutput {
 		float altitude_rate_control;			///< Altitude rate setpoint from altitude control loop [m/s].
 		float true_airspeed_derivative_control;		///< Airspeed rate setpoint from airspeed control loop [m/s²].
-		float total_energy_rate_error;			///< Total energy rate error [m²/s³].
+		float total_energy_rate_estimate;		///< Total energy rate estimate [m²/s³].
 		float total_energy_rate_sp;			///< Total energy rate setpoint [m²/s³].
-		float energy_balance_rate_error;		///< Energy balance rate error [m²/s³].
+		float energy_balance_rate_estimate;		///< Energy balance rate estimate [m²/s³].
 		float energy_balance_rate_sp;			///< Energy balance rate setpoint [m²/s³].
+		float pitch_integrator;				///< Pitch control integrator state [-].
+		float throttle_integrator;			///< Throttle control integrator state [-].
 	};
 
 	/**
@@ -278,7 +279,7 @@ public:
 	 * @brief Initialization of the state.
 	 *
 	 */
-	void initialize();
+	void initialize(const Setpoint &setpoint, const Input &input, Param &param, const Flag &flag);
 	/**
 	 * @brief Update state and output.
 	 *
@@ -297,9 +298,9 @@ public:
 	/**
 	 * @brief Get the percent of the undersped.
 	 *
-	 * @return Percentage of detected undersped.
+	 * @return Ratio of detected undersped [0,1].
 	 */
-	float getPercentUndersped() const {return _percent_undersped;};
+	float getRatioUndersped() const {return _ratio_undersped;};
 	/**
 	 * @brief Get the throttle setpoint.
 	 *
@@ -323,33 +324,35 @@ public:
 	 *
 	 * @return the debug outpus struct.
 	 */
-	DebugOutput getDebugOutput() const {return _debug_output;};
+	const DebugOutput &getDebugOutput() const { return _debug_output; }
 
 private:
 	/**
-	 * @brief Specific total energy limit.
+	 * @brief Specific total energy rate limit.
 	 *
 	 */
-	struct STELimit {
+	struct STERateLimit {
 		float STE_rate_max;	///< Maximum specific total energy rate limit [m²/s³].
 		float STE_rate_min;	///< Minimal specific total energy rate limit [m²/s³].
 	};
 
 	/**
-	 * @brief Calculated specific energy.
+	 * @brief Control values.
+	 * setpoint and current state estimate value as input to a controller.
 	 *
 	 */
-	struct SpecificEnergy {
+	struct ControlValues {
-		struct {
+		float setpoint;		///< Control setpoint.
-			float rate;		///< Specific kinetic energy rate in [m²/s³].
+		float estimate;		///< Control estimate of current state value.
-			float rate_setpoint;	///< Specific kinetic energy setpoint rate in [m²/s³].
+	};
-			float rate_error;	///< Specific kinetic energy rate error in [m²/s³].
+
-		} ske;				///< Specific kinetic energy.
+	/**
-		struct {
+	 * @brief Calculated specific energy rates.
-			float rate;		///< Specific potential energy rate in [m²/s³].
+	 *
-			float rate_setpoint;	///< Specific potential energy setpoint rate in [m²/s³].
+	 */
-			float rate_error;	///< Specific potential energy rate error in [m²/s³].
+	struct SpecificEnergyRates {
-		} spe;				///< Specific potential energy rate.
+		ControlValues ske_rate;	///< Specific kinetic energy rate [m²/s³].
+		ControlValues spe_rate;	///< Specific potential energy rate [m²/s³].
 	};
 
 	/**
@@ -371,15 +374,22 @@ private:
 	};
 
 private:
+	/**
+	 * @brief Get control error from etpoint and estimate
+	 *
+	 * @param val is the current control setpoint and estimate.
+	 * @return error value
+	 */
+	static inline constexpr float _getControlError(TECSControl::ControlValues val) {return (val.setpoint - val.estimate);};
 	/**
 	 * @brief Calculate specific total energy rate limits.
 	 *
 	 * @param[in] param are the control parametes.
-	 * @return Specific total energy rate limits.
+	 * @return Specific total energy rate limits in [m²/s³].
 	 */
-	STELimit _calculateTotalEnergyRateLimit(const Param &param) const;
+	STERateLimit _calculateTotalEnergyRateLimit(const Param &param) const;
 	/**
-	 * @brief Airspeed control loop.
+	 * @brief calculate airspeed control proportional output.
 	 *
 	 * @param setpoint is the control setpoints.
 	 * @param input	is the current input measurment of the UAS.
@@ -387,24 +397,25 @@ private:
 	 * @param flag	is the control flags.
 	 * @return controlled airspeed rate setpoint in [m/s²].
 	 */
-	float _airspeedControl(const Setpoint &setpoint, const Input &input, const Param &param, const Flag &flag) const;
+	float _calcAirspeedControlOutput(const Setpoint &setpoint, const Input &input, const Param &param,
+					 const Flag &flag) const;
 	/**
-	 * @brief Altitude control loop.
+	 * @brief calculate altitude control proportional output.
 	 *
 	 * @param setpoint is the control setpoints.
 	 * @param input is the current input measurment of the UAS.
 	 * @param param is the control parameters.
 	 * @return controlled altitude rate setpoint in [m/s].
 	 */
-	float _altitudeControl(const Setpoint &setpoint, const Input &input, const Param &param) const;
+	float _calcAltitudeControlOutput(const Setpoint &setpoint, const Input &input, const Param &param) const;
 	/**
-	 * @brief Update energy balance.
+	 * @brief Calculate specific energy rates.
 	 *
 	 * @param control_setpoint is the controlles altitude and airspeed rate setpoints.
 	 * @param input is the current input measurment of the UAS.
-	 * @return Specific energy rates.
+	 * @return Specific energy rates in [m²/s³].
 	 */
-	SpecificEnergy _updateEnergyBalance(const AltitudePitchControl &control_setpoint, const Input &input) const;
+	SpecificEnergyRates _calcSpecificEnergyRates(const AltitudePitchControl &control_setpoint, const Input &input) const;
 	/**
 	 * @brief Detect underspeed.
 	 *
@@ -422,37 +433,108 @@ private:
 	 */
 	SpecificEnergyWeighting _updateSpeedAltitudeWeights(const Param &param, const Flag &flag);
 	/**
-	 * @brief Update controlled pitch setpoint.
+	 * @brief Calculate pitch control.
 	 *
 	 * @param dt is the update time intervall in [s].
-	 * @param input is the current input measurment of the UAS.
+	 * @param input is the current input measurement of the UAS.
-	 * @param se is the calculated specific energy.
+	 * @param specific_energy_rate is the calculated specific energy.
 	 * @param param is the control parameters.
 	 * @param flag is the control flags.
 	 */
-	void _updatePitchSetpoint(float dt, const Input &input, const SpecificEnergy &se, Param &param, const Flag &flag);
+	void _calcPitchControl(float dt, const Input &input, const SpecificEnergyRates &specific_energy_rate,
+			       const Param &param,
+			       const Flag &flag);
+
+	/**
+	 * @brief Calculate pitch control specific energy balance rates.
+	 *
+	 * @param weight is the weighting use of the potential and kinetic energy.
+	 * @param specific_energy_rate is the specific energy rates in [m²/s³].
+	 * @return specific energy balance rate values in [m²/s³].
+	 */
+	ControlValues _calcPitchControlSebRate(const SpecificEnergyWeighting &weight,
+					       const SpecificEnergyRates &specific_energy_rate) const;
+
+	/**
+	 * @brief Calculate the pitch control update function.
+	 * Update the states of the pitch control
+	 *
+	 * @param dt is the update time intervall in [s].
+	 * @param seb_rate is the specific energy balance rate in [m²/s³].
+	 * @param param is the control parameters.
+	 */
+	void _calcPitchControlUpdate(float dt, const ControlValues &seb_rate, const Param &param);
+
+	/**
+	 * @brief Calculate the pitch control output function.
+	 *
+	 * @param input is the current input measurement of the UAS.
+	 * @param seb_rate is the specific energy balance rate in [m²/s³].
+	 * @param param is the control parameters.
+	 * @param flag is the control flags.
+	 * @return pitch setpoint angle [rad].
+	 */
+	float _calcPitchControlOutput(const Input &input, const ControlValues &seb_rate, const Param &param,
+				      const Flag &flag) const;
+
 	/**
 	 * @brief Update controlled throttle setpoint.
 	 *
 	 * @param dt is the update time intervall in [s].
-	 * @param se is the calculated specific energy.
+	 * @param specific_energy_rate is the calculated specific energy.
+	 * @param flag is the control flags.
+	 */
+	void _calcThrottleControl(float dt, const SpecificEnergyRates &specific_energy_rate, const Param &param,
+				  const Flag &flag);
+
+	/**
+	 * @brief Calculate throttle control specific total energy
+	 *
+	 * @param limit is the specific total energy rate limits in [m²/s³].
+	 * @param specific_energy_rate is the specific energy rates in [m²/s³].
+	 * @param param is the control parameters.
+	 * @return specific total energy rate values in [m²/s³]
+	 */
+	ControlValues _calcThrottleControlSteRate(const STERateLimit &limit, const SpecificEnergyRates &specific_energy_rate,
+			const Param &param) const;
+
+	/**
+	 * @brief Calculate the throttle control update function.
+	 * Update the throttle control states.
+	 *
+	 * @param dt is the update time intervall in [s].
+	 * @param limit is the specific total energy rate limits in [m²/s³].
+	 * @param ste_rate is the specific total energy rates in [m²/s³].
 	 * @param param is the control parameters.
 	 * @param flag is the control flags.
 	 */
-	void _updateThrottleSetpoint(float dt, const SpecificEnergy &se, const Param &param, const Flag &flag);
+	void _calcThrottleControlUpdate(float dt, const STERateLimit &limit, const ControlValues &ste_rate, const Param &param,
+					const Flag &flag);
+
+	/**
+	 * @brief Calculate the throttle control output function.
+	 *
+	 * @param limit is the specific total energy rate limits in [m²/s³].
+	 * @param ste_rate is the specific total energy rates in [m²/s³].
+	 * @param param is the control parameters.
+	 * @param flag is the control flags.
+	 * @return throttle setpoin in [0,1].
+	 */
+	float _calcThrottleControlOutput(const STERateLimit &limit, const ControlValues &ste_rate, const Param &param,
+					 const Flag &flag) const;
 
 private:
 	// State
-	AlphaFilter<float> _ste_rate_error_filter;		///< Low pass filter for the specific total energy rate.
+	AlphaFilter<float> _ste_rate_estimate_filter;		///< Low pass filter for the specific total energy rate.
 	float _pitch_integ_state{0.0f};				///< Pitch integrator state [rad].
 	float _throttle_integ_state{0.0f};			///< Throttle integrator state.
 
 
 	// Output
-	DebugOutput _debug_output;
+	DebugOutput _debug_output;				///< Debug output.
 	float _pitch_setpoint{0.0f};				///< Controlled pitch setpoint [rad].
-	float _throttle_setpoint{0.0f};				///< Controlled throttle setpoint.
+	float _throttle_setpoint{0.0f};				///< Controlled throttle setpoint [0,1].
-	float _percent_undersped{0.0f};				///< A continuous representation of how "undersped" the TAS is [0,1]
+	float _ratio_undersped{0.0f};				///< A continuous representation of how "undersped" the TAS is [0,1]
 	float _ste_rate{0.0f};					///< Specific total energy rate [m²/s³].
 };
 
@@ -466,12 +548,13 @@ public:
 		ECL_TECS_MODE_CLIMBOUT
 	};
 
-	struct DebugOutput : TECSControl::DebugOutput {
+	struct DebugOutput {
+		TECSControl::DebugOutput control;
 		float true_airspeed_filtered;
 		float true_airspeed_derivative;
 		float altitude_sp;
-		float altitude_rate;
+		float altitude_rate_alt_ref;
-		float altitude_rate_setpoint;
+		float altitude_rate_feedforward;
 		enum ECL_TECS_MODE tecs_mode;
 	};
 public:
@@ -484,19 +567,19 @@ public:
 	TECS(TECS &&) = delete;
 	TECS &operator=(TECS &&) = delete;
 
-	DebugOutput getStatus() const {return _debug_status;};
+	const DebugOutput &getStatus() const { return _debug_status; }
 
 	/**
 	 * Get the current airspeed status
 	 *
 	 * @return true if airspeed is enabled for control
 	 */
-	bool airspeed_sensor_enabled() { return _airspeed_enabled; }
+	bool airspeed_sensor_enabled() { return _control_flag.airspeed_enabled; }
 
 	/**
 	 * Set the airspeed enable state
 	 */
-	void enable_airspeed(bool enabled) { _airspeed_enabled = enabled; }
+	void enable_airspeed(bool enabled) { _control_flag.airspeed_enabled = enabled; }
 
 	/**
 	 * @brief Update the control loop calculations
@@ -505,22 +588,26 @@ public:
 	void update(float pitch, float altitude, float hgt_setpoint, float EAS_setpoint, float equivalent_airspeed,
 		    float eas_to_tas, bool climb_out_setpoint, float pitch_min_climbout, float throttle_min, float throttle_setpoint_max,
 		    float throttle_trim, float pitch_limit_min, float pitch_limit_max, float target_climbrate, float target_sinkrate,
-		    const float speed_deriv_forward, float hgt_rate, float hgt_rate_sp = NAN);
+		    float speed_deriv_forward, float hgt_rate, float hgt_rate_sp = NAN);
 
 	/**
 	 * @brief Initialize the control loop
 	 *
 	 */
-	void initialize(const float altitude, const float altitude_rate, const float equivalent_airspeed);
+	void initialize(float altitude, float altitude_rate, float equivalent_airspeed, float eas_to_tas);
 
 	void resetIntegrals()
 	{
 		_control.resetIntegrals();
 	}
 
-	void set_detect_underspeed_enabled(bool enabled) { _detect_underspeed_enabled = enabled; };
+	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; };
 
@@ -532,12 +619,11 @@ public:
 
 	void set_equivalent_airspeed_max(float airspeed) { _equivalent_airspeed_max = airspeed; }
 	void set_equivalent_airspeed_min(float airspeed) { _equivalent_airspeed_min = airspeed; }
-	void set_equivalent_airspeed_trim(float airspeed) { _equivalent_airspeed_trim = airspeed; }
+	void set_equivalent_airspeed_trim(float airspeed) { _control_param.equivalent_airspeed_trim = airspeed; _airspeed_filter_param.equivalent_airspeed_trim = airspeed; }
 
 	void set_pitch_damping(float damping) { _control_param.pitch_damping_gain = damping; }
 	void set_vertical_accel_limit(float limit) { _reference_param.vert_accel_limit = limit; _control_param.vert_accel_limit = limit; };
 
-	void set_speed_comp_filter_omega(float omega) { _airspeed_param.airspeed_estimate_freq = omega; };
 	void set_speed_weight(float weight) { _control_param.pitch_speed_weight = weight; };
 	void set_airspeed_error_time_constant(float time_const) { _control_param.airspeed_error_gain = 1.0f / math::max(time_const, 0.1f); };
 
@@ -547,7 +633,6 @@ public:
 	void set_roll_throttle_compensation(float compensation) { _control_param.load_factor_correction = compensation; };
 	void set_load_factor(float load_factor) { _control_param.load_factor = load_factor; };
 
-	void set_speed_derivative_time_constant(float time_const) { _airspeed_param.speed_derivative_time_const = time_const; };
 	void set_ste_rate_time_const(float time_const) { _control_param.ste_rate_time_const = time_const; };
 
 	void set_seb_rate_ff_gain(float ff_gain) { _control_param.seb_rate_ff = ff_gain; };
@@ -574,8 +659,6 @@ public:
 	uint64_t timestamp() { return _update_timestamp; }
 	ECL_TECS_MODE tecs_mode() { return _tecs_mode; }
 
-	static constexpr float DT_DEFAULT = 0.02f;
-
 private:
 	TECSControl 		_control;				///< Control submodule.
 	TECSAirspeedFilter 	_airspeed_filter;			///< Airspeed filter submodule.
@@ -583,38 +666,32 @@ private:
 
 	enum ECL_TECS_MODE _tecs_mode {ECL_TECS_MODE_NORMAL};		///< Current activated mode.
 
-	uint64_t _update_timestamp{0};					///< last timestamp of the update function call.
+	hrt_abstime _update_timestamp{0};				///< last timestamp of the update function call.
 
 	float _equivalent_airspeed_min{3.0f};				///< equivalent airspeed demand lower limit (m/sec)
 	float _equivalent_airspeed_max{30.0f};				///< equivalent airspeed demand upper limit (m/sec)
-	float _equivalent_airspeed_trim{15.0f};				///< equivalent cruise airspeed for airspeed less mode (m/sec)
 
 	// controller mode logic
 	bool _uncommanded_descent_recovery{false};			///< true when a continuous descent caused by an unachievable airspeed demand has been detected
-	bool _airspeed_enabled{false};					///< true when airspeed use has been enabled
-	bool _detect_underspeed_enabled{false};				///< true when underspeed detection is enabled
 
 	static constexpr float DT_MIN = 0.001f;				///< minimum allowed value of _dt (sec)
 	static constexpr float DT_MAX = 1.0f;				///< max value of _dt allowed before a filter state reset is performed (sec)
 
-	static constexpr float _jerk_max = 1000.0f;			///< Magnitude of the maximum jerk allowed [m/s³].
-	static constexpr float _tas_error_percentage =
-		0.15f;		///< [0,1] percentage of true airspeed trim corresponding to expected (safe) true airspeed tracking errors
-
 	DebugOutput _debug_status{};
 
 	// Params
 	/// Airspeed filter parameters.
-	TECSAirspeedFilter::Param _airspeed_param{
+	TECSAirspeedFilter::Param _airspeed_filter_param{
-		.equivalent_airspeed_trim = 0.0f,
+		.equivalent_airspeed_trim = 15.0f,
-		.airspeed_estimate_freq = 0.0f,
+		.airspeed_measurement_std_dev = 0.2f,
-		.speed_derivative_time_const = 0.01f,
+		.airspeed_rate_measurement_std_dev = 0.05f,
+		.airspeed_rate_noise_std_dev = 0.02f
 	};
 	/// Reference model parameters.
 	TECSReferenceModel::Param _reference_param{
 		.target_climbrate = 2.0f,
 		.target_sinkrate = 2.0f,
-		.jerk_max = _jerk_max,
+		.jerk_max = 1000.0f,
 		.vert_accel_limit = 0.0f,
 		.max_climb_rate = 2.0f,
 		.max_sink_rate = 2.0f,
@@ -633,7 +710,7 @@ private:
 		.throttle_min = 0.1f,
 		.altitude_error_gain = 0.2f,
 		.altitude_setpoint_gain_ff = 0.0f,
-		.tas_error_percentage = _tas_error_percentage,
+		.tas_error_percentage = 0.15f,
 		.airspeed_error_gain = 0.1f,
 		.ste_rate_time_const = 0.1f,
 		.seb_rate_ff = 1.0f,
@@ -647,6 +724,12 @@ private:
 		.load_factor = 1.0f,
 	};
 
+	TECSControl::Flag _control_flag{
+		.airspeed_enabled = false,
+		.climbout_mode_active = false,
+		.detect_underspeed_enabled = false,
+	};
+
 	/**
 	 * Update the desired airspeed
 	 */

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -134,15 +134,16 @@ FixedwingPositionControl::parameters_update()
 	_tecs.set_integrator_gain_pitch(_param_fw_t_I_gain_pit.get());
 	_tecs.set_throttle_slewrate(_param_fw_thr_slew_max.get());
 	_tecs.set_vertical_accel_limit(_param_fw_t_vert_acc.get());
-	_tecs.set_speed_comp_filter_omega(_param_fw_t_spd_omega.get());
 	_tecs.set_roll_throttle_compensation(_param_fw_t_rll2thr.get());
 	_tecs.set_pitch_damping(_param_fw_t_ptch_damp.get());
 	_tecs.set_altitude_error_time_constant(_param_fw_t_h_error_tc.get());
 	_tecs.set_altitude_rate_ff(_param_fw_t_hrate_ff.get());
 	_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_speed_derivative_time_constant(_param_tas_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());
 
 	int check_ret = PX4_OK;
 
@@ -457,11 +458,12 @@ FixedwingPositionControl::adapt_airspeed_setpoint(const float control_interval,
 }
 
 void
-FixedwingPositionControl::tecs_status_publish()
+FixedwingPositionControl::tecs_status_publish(float alt_sp, float equivalent_airspeed_sp,
+		float true_airspeed_derivative_raw, float throttle_trim)
 {
 	tecs_status_s t{};
 
-	TECS::DebugOutput debug_output{_tecs.getStatus()};
+	const TECS::DebugOutput &debug_output{_tecs.getStatus()};
 
 	switch (_tecs.tecs_mode()) {
 	case TECS::ECL_TECS_MODE_NORMAL:
@@ -481,25 +483,25 @@ FixedwingPositionControl::tecs_status_publish()
 		break;
 	}
 
+	t.altitude_sp = alt_sp;
 	t.altitude_filtered = debug_output.altitude_sp;
-
+	t.height_rate_setpoint = debug_output.control.altitude_rate_control;
+	t.height_rate = -_local_pos.vz;
+	t.equivalent_airspeed_sp = equivalent_airspeed_sp;
+	t.true_airspeed_sp = _eas2tas * equivalent_airspeed_sp;
 	t.true_airspeed_filtered = debug_output.true_airspeed_filtered;
-
+	t.true_airspeed_derivative_sp = debug_output.control.true_airspeed_derivative_control;
-	t.height_rate_setpoint = debug_output.altitude_rate_setpoint;
-	t.height_rate = debug_output.altitude_rate;
-
-	t.true_airspeed_derivative_sp = debug_output.true_airspeed_derivative_control;
 	t.true_airspeed_derivative = debug_output.true_airspeed_derivative;
-
+	t.true_airspeed_derivative_raw = true_airspeed_derivative_raw;
-	t.total_energy_rate_error = debug_output.total_energy_rate_error;
+	t.total_energy_rate = debug_output.control.total_energy_rate_estimate;
-
+	t.total_energy_balance_rate = debug_output.control.energy_balance_rate_estimate;
-	t.energy_distribution_rate_error = debug_output.energy_balance_rate_error;
+	t.total_energy_rate_sp = debug_output.control.total_energy_rate_sp;
-
+	t.total_energy_balance_rate_sp = debug_output.control.energy_balance_rate_sp;
-	t.total_energy_rate_sp = debug_output.total_energy_rate_sp;
+	t.throttle_integ = debug_output.control.throttle_integrator;
-	t.total_energy_balance_rate_sp = debug_output.energy_balance_rate_sp;
+	t.pitch_integ = debug_output.control.pitch_integrator;
-
 	t.throttle_sp = _tecs.get_throttle_setpoint();
 	t.pitch_sp_rad = _tecs.get_pitch_setpoint();
+	t.throttle_trim = throttle_trim;
 
 	t.timestamp = hrt_absolute_time();
 
@@ -826,7 +828,7 @@ FixedwingPositionControl::update_in_air_states(const hrt_abstime now)
 		_was_in_air = true;
 		_time_went_in_air = now;
 
-		_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed);
+		_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed, _eas2tas);
 	}
 
 	/* reset flag when airplane landed */
@@ -834,7 +836,7 @@ FixedwingPositionControl::update_in_air_states(const hrt_abstime now)
 		_was_in_air = false;
 		_completed_manual_takeoff = false;
 
-		_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed);
+		_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed, _eas2tas);
 	}
 }
 
@@ -2314,7 +2316,7 @@ FixedwingPositionControl::Run()
 				_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
 				_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
 
-				_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed);
+				_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed, _eas2tas);
 
 				break;
 			}
@@ -2491,12 +2493,12 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
 	}
 
 	// We need an altitude lock to calculate the TECS control
-	if (!(_local_pos.timestamp > 0)) {
+	if (_local_pos.timestamp == 0) {
 		_reinitialize_tecs = true;
 	}
 
 	if (_reinitialize_tecs) {
-		_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed);
+		_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed, _eas2tas);
 		_reinitialize_tecs = false;
 	}
 
@@ -2504,7 +2506,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
 	_tecs.set_detect_underspeed_enabled(!disable_underspeed_detection);
 
 	if (_landed) {
-		_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed);
+		_tecs.initialize(_current_altitude, -_local_pos.vz, _airspeed, _eas2tas);
 	}
 
 	/* update TECS vehicle state estimates */
@@ -2530,7 +2532,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interva
 		     -_local_pos.vz,
 		     hgt_rate_sp);
 
-	tecs_status_publish();
+	tecs_status_publish(alt_sp, airspeed_sp, -_local_pos.vz, throttle_trim_comp);
 }
 
 float

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -437,7 +437,8 @@ private:
 
 	void status_publish();
 	void landing_status_publish();
-	void tecs_status_publish();
+	void tecs_status_publish(float alt_sp, float equivalent_airspeed_sp, float true_airspeed_derivative_raw,
+				 float throttle_trim);
 	void publishLocalPositionSetpoint(const position_setpoint_s &current_waypoint);
 
 	/**
@@ -822,16 +823,17 @@ private:
 		(ParamFloat<px4::params::FW_T_RLL2THR>) _param_fw_t_rll2thr,
 		(ParamFloat<px4::params::FW_T_SINK_MAX>) _param_fw_t_sink_max,
 		(ParamFloat<px4::params::FW_T_SINK_MIN>) _param_fw_t_sink_min,
-		(ParamFloat<px4::params::FW_T_SPD_OMEGA>) _param_fw_t_spd_omega,
 		(ParamFloat<px4::params::FW_T_SPDWEIGHT>) _param_fw_t_spdweight,
 		(ParamFloat<px4::params::FW_T_TAS_TC>) _param_fw_t_tas_error_tc,
 		(ParamFloat<px4::params::FW_T_THR_DAMP>) _param_fw_t_thr_damp,
 		(ParamFloat<px4::params::FW_T_VERT_ACC>) _param_fw_t_vert_acc,
 		(ParamFloat<px4::params::FW_T_STE_R_TC>) _param_ste_rate_time_const,
-		(ParamFloat<px4::params::FW_T_TAS_R_TC>) _param_tas_rate_time_const,
 		(ParamFloat<px4::params::FW_T_SEB_R_FF>) _param_seb_rate_ff,
 		(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_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_THR_TRIM>) _param_fw_thr_trim,
 		(ParamFloat<px4::params::FW_THR_IDLE>) _param_fw_thr_idle,

src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c

@@ -709,22 +709,50 @@ PARAM_DEFINE_FLOAT(FW_T_I_GAIN_PIT, 0.1f);
 PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f);
 
 /**
- * Complementary filter "omega" parameter for speed
+ * Airspeed measurement standard deviation for airspeed filter.
  *
- * This is the cross-over frequency (in radians/second) of the complementary
+ * This is the measurement standard deviation for the airspeed used in the airspeed filter in TECS.
- * filter used to fuse longitudinal acceleration and airspeed to obtain an
- * improved airspeed estimate. Increasing this frequency weights the solution
- * more towards use of the airspeed sensor, whilst reducing it weights the
- * solution more towards use of the accelerometer data.
  *
- * @unit rad/s
+ * @unit m/s
- * @min 1.0
+ * @min 0.01
  * @max 10.0
- * @decimal 1
+ * @decimal 2
- * @increment 0.5
+ * @increment 0.1
  * @group FW TECS
  */
-PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f);
+PARAM_DEFINE_FLOAT(FW_T_SPD_STD, 0.2f);
+
+/**
+ * Airspeed rate measurement standard deviation for airspeed filter.
+ *
+ * This is the measurement standard deviation for the airspeed rate used in the airspeed filter in TECS.
+ *
+ * @unit m/s^2
+ * @min 0.01
+ * @max 10.0
+ * @decimal 2
+ * @increment 0.1
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_SPD_DEV_STD, 0.05f);
+
+/**
+ * Process noise standard deviation for the airspeed rate in the airspeed filter.
+ *
+ * This is the process noise standard deviation in the airspeed filter filter defining the noise in the
+ * airspeed rate for the constant airspeed rate model. This is used to define how much the airspeed and
+ * the airspeed rate are filtered. The smaller the value the more the measurements are smoothed with the
+ * drawback for delays.
+ *
+ * @unit m/s^2
+ * @min 0.01
+ * @max 10.0
+ * @decimal 2
+ * @increment 0.1
+ * @group FW TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_SPD_PRC_STD, 0.2f);
+
 
 /**
  * Roll -> Throttle feedforward
@@ -856,21 +884,6 @@ PARAM_DEFINE_INT32(FW_POS_STK_CONF, 2);
  */
 PARAM_DEFINE_FLOAT(FW_T_STE_R_TC, 0.4f);
 
-
-/**
- * True airspeed rate first order filter time constant.
- *
- * This filter is applied to the true airspeed rate.
- *
- * @min 0.0
- * @max 2
- * @decimal 2
- * @increment 0.01
- * @group FW TECS
- */
-PARAM_DEFINE_FLOAT(FW_T_TAS_R_TC, 0.2f);
-
-
 /**
  * Specific total energy balance rate feedforward gain.
  *