wind-estimator: learn airspeed scale faster at beginning of flight

src/lib/wind_estimator/WindEstimator.cpp

@@ -68,6 +68,7 @@ WindEstimator::initialise(const matrix::Vector3f &velI, const float hor_vel_vari
 	}
 
 	_wind_estimator_reset = true;
+	_time_initialised = hrt_absolute_time();
 
 	return true;
 }
@@ -94,10 +95,14 @@ WindEstimator::update(uint64_t time_now)
 	const float dt = (float)(time_now - _time_last_update) * 1e-6f;
 	_time_last_update = time_now;
 
+	// if initial fast learning is enabled and we are in the first 5 minutes of flight time, multiply _tas_scale_psd by 100
+	const float tas_psd_multiplier = (_tas_scale_fast_initial_learning && (time_now - _time_initialised < kTASScaleFastLearnTime)) ?
+					 kTASScalePSDMultiplier : 1.f;
+
 	matrix::Matrix3f Qk;
 	Qk(INDEX_W_N, INDEX_W_N) = _wind_psd * dt;
 	Qk(INDEX_W_E, INDEX_W_E) = Qk(INDEX_W_N, INDEX_W_N);
-	Qk(INDEX_TAS_SCALE, INDEX_TAS_SCALE) = _tas_scale_psd * dt;
+	Qk(INDEX_TAS_SCALE, INDEX_TAS_SCALE) = _tas_scale_psd * tas_psd_multiplier * dt;
 	_P += Qk;
 }
 

src/lib/wind_estimator/WindEstimator.hpp

@@ -87,6 +87,7 @@ class WindEstimator
 	// unaided, the state uncertainty (diagonal of sqrt(P)) grows by the process noise spectral density every second
 	void set_wind_process_noise_spectral_density(float wind_nsd) { _wind_psd = wind_nsd * wind_nsd; }
 	void set_tas_scale_process_noise_spectral_density(float tas_scale_nsd) { _tas_scale_psd = tas_scale_nsd * tas_scale_nsd; }
+	void set_tas_scale_fast_initial_learning(bool enable) { _tas_scale_fast_initial_learning = enable; }
 
 	void set_tas_noise(float tas_sigma) { _tas_var = tas_sigma * tas_sigma; }
 	void set_beta_noise(float beta_var) { _beta_var = beta_var * beta_var; }
@@ -129,6 +130,7 @@ class WindEstimator
 
 	float _wind_psd{0.1f};	///< wind process noise power spectral density (m^2/s^4/Hz)
 	float _tas_scale_psd{0.0001f};	///< true airspeed process noise power spectral density (1/s^2/Hz)
+	bool  _tas_scale_fast_initial_learning{false}; ///< enable faster scale learning for the first 300s of flight time
 	float _tas_var{1.4f};		///< true airspeed measurement noise variance
 	float _beta_var{0.5f};	///< sideslip measurement noise variance
 	uint8_t _tas_gate{3};	///< airspeed fusion gate size
@@ -139,6 +141,10 @@ class WindEstimator
 	uint64_t _time_last_airspeed_fuse = 0;	///< timestamp of last airspeed fusion
 	uint64_t _time_last_beta_fuse = 0;	///< timestamp of last sideslip fusion
 	uint64_t _time_last_update = 0;		///< timestamp of last covariance prediction
+	uint64_t _time_initialised = 0;         ///< timestamp when estimator is initialised
+
+	static constexpr float kTASScalePSDMultiplier = 100;
+	static constexpr hrt_abstime kTASScaleFastLearnTime = 300_s;
 
 	bool _wind_estimator_reset = false; ///< wind estimator was reset in this cycle
 

src/modules/airspeed_selector/AirspeedValidator.hpp

@@ -96,6 +96,7 @@ class AirspeedValidator
 	// setters wind estimator parameters
 	void set_wind_estimator_wind_process_noise_spectral_density(float wind_nsd) { _wind_estimator.set_wind_process_noise_spectral_density(wind_nsd); }
 	void set_wind_estimator_tas_scale_process_noise_spectral_density(float tas_scale_nsd) { _wind_estimator.set_tas_scale_process_noise_spectral_density(tas_scale_nsd); }
+	void set_wind_estimator_tas_scale_fast_initial_learning(bool enable) {_wind_estimator.set_tas_scale_fast_initial_learning(enable); }
 	void set_wind_estimator_tas_scale_init(float tas_scale_init)
 	{
 		_tas_scale_init = tas_scale_init;

src/modules/airspeed_selector/airspeed_selector_main.cpp

@@ -187,6 +187,7 @@ class AirspeedModule : public ModuleBase<AirspeedModule>, public ModuleParams,
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::ASPD_WIND_NSD>) _param_aspd_wind_nsd,
 		(ParamFloat<px4::params::ASPD_SCALE_NSD>) _param_aspd_scale_nsd,
+		(ParamBool<px4::params::ASPD_SCALE_FAST>) _param_aspd_scale_fast,
 		(ParamFloat<px4::params::ASPD_TAS_NOISE>) _param_west_tas_noise,
 		(ParamFloat<px4::params::ASPD_BETA_NOISE>) _param_west_beta_noise,
 		(ParamInt<px4::params::ASPD_TAS_GATE>) _param_west_tas_gate,
@@ -522,6 +523,7 @@ void AirspeedModule::update_params()
 	for (int i = 0; i < MAX_NUM_AIRSPEED_SENSORS; i++) {
 		_airspeed_validator[i].set_wind_estimator_wind_process_noise_spectral_density(_param_aspd_wind_nsd.get());
 		_airspeed_validator[i].set_wind_estimator_tas_scale_process_noise_spectral_density(_param_aspd_scale_nsd.get());
+		_airspeed_validator[i].set_wind_estimator_tas_scale_fast_initial_learning(_param_aspd_scale_fast.get());
 		_airspeed_validator[i].set_wind_estimator_tas_noise(_param_west_tas_noise.get());
 		_airspeed_validator[i].set_wind_estimator_beta_noise(_param_west_beta_noise.get());
 		_airspeed_validator[i].set_wind_estimator_tas_gate(_param_west_tas_gate.get());

src/modules/airspeed_selector/airspeed_selector_params.c

@@ -27,6 +27,18 @@ PARAM_DEFINE_FLOAT(ASPD_WIND_NSD, 1.e-1f);
  */
 PARAM_DEFINE_FLOAT(ASPD_SCALE_NSD, 1.e-4f);
 
+/**
+ * Enables faster airspeed scale learning at the beginning of a flight
+ *
+ * When enabled, the wind estimator uses 10x higher process noise spectral density for the airspeed
+ * scale during the first 5 minutes of flight, allowing the scale estimate to converge
+ * much faster.
+ *
+ * @boolean
+ * @group Airspeed Validator
+ */
+PARAM_DEFINE_INT32(ASPD_SCALE_FAST, 0);
+
 /**
  * Wind estimator true airspeed measurement noise
  *