wind_estimator: use noise spectral density for process noise tuning

The noise spectral density, NSD, (square root of power spectral density) is a
continuous-time parameter that makes the tuning independent from the EKF
prediction rate.
NSD corresponds to the rate at which the state uncertainty increases
when no measurements are fused into the filter.
Given that the current prediction rate of the wind estimator is 1Hz, the
same tuning is obtained with the same values as before.

src/lib/parameters/px4params/srcparser.py

@@ -358,7 +358,7 @@ class SourceParser(object):
                                 'deg', 'deg*1e7', 'deg/s',
                                 'celcius', 'gauss', 'gauss/s', 'gauss^2',
                                 'hPa', 'kg', 'kg/m^2', 'kg m^2',
-                                'mm', 'm', 'm/s', 'm^2', 'm/s^2', 'm/s^3', 'm/s^2/sqrt(Hz)', 'm/s/rad',
+                                'mm', 'm', 'm/s', 'm^2', 'm/s^2', 'm/s^3', 'm/s^2/sqrt(Hz)', '1/s/sqrt(Hz)', 'm/s/rad',
                                 'Ohm', 'V', 'A',
                                 'us', 'ms', 's',
                                 'S', 'A/%', '(m/s^2)^2', 'm/m',  'tan(rad)^2', '(m/s)^2', 'm/rad',

src/lib/wind_estimator/WindEstimator.cpp

@@ -113,16 +113,12 @@ WindEstimator::update(uint64_t time_now)
 	}
 
 	float dt = (float)(time_now - _time_last_update) * 1e-6f;
-	float dt2 = dt * dt;
 	_time_last_update = time_now;
 
-	float q_w = _wind_p_var;
-	float q_k_tas = _tas_scale_p_var;
-
 	matrix::Matrix3f Qk;
-	Qk(0, 0) = q_w * dt2;
+	Qk(0, 0) = _wind_psd * dt;
 	Qk(1, 1) = Qk(0, 0);
-	Qk(2, 2) = q_k_tas * dt2;
+	Qk(2, 2) = _tas_scale_psd * dt;
 	_P += Qk;
 }
 

src/lib/wind_estimator/WindEstimator.hpp

@@ -78,8 +78,10 @@ public:
 	matrix::Vector2f  get_wind_var() { return matrix::Vector2f{_P(0, 0), _P(1, 1)}; }
 	bool get_wind_estimator_reset() { return _wind_estimator_reset; }
 
-	void set_wind_p_noise(float wind_sigma) { _wind_p_var = wind_sigma * wind_sigma; }
-	void set_tas_scale_p_noise(float tas_scale_sigma) { _tas_scale_p_var = tas_scale_sigma * tas_scale_sigma; }
+	// 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_noise(float tas_sigma) { _tas_var = tas_sigma * tas_sigma; }
 	void set_beta_noise(float beta_var) { _beta_var = beta_var * beta_var; }
 	void set_tas_gate(uint8_t gate_size) {_tas_gate = gate_size; }
@@ -104,8 +106,8 @@ private:
 
 	bool _initialised{false};	///< True: filter has been initialised
 
-	float _wind_p_var{0.1f};	///< wind process noise variance
-	float _tas_scale_p_var{0.0001f};	///< true airspeed scale process noise variance
+	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)
 	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

src/modules/airspeed_selector/AirspeedValidator.hpp

@@ -87,8 +87,8 @@ public:
 	airspeed_wind_s get_wind_estimator_states(uint64_t timestamp);
 
 	// setters wind estimator parameters
-	void set_wind_estimator_wind_p_noise(float wind_sigma) { _wind_estimator.set_wind_p_noise(wind_sigma); }
-	void set_wind_estimator_tas_scale_p_noise(float tas_scale_sigma) { _wind_estimator.set_tas_scale_p_noise(tas_scale_sigma); }
+	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_init(float tas_scale_init)
 	{
 		_tas_scale_init = tas_scale_init;

src/modules/airspeed_selector/airspeed_selector_main.cpp

@@ -165,8 +165,8 @@ private:
 	};
 
 	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::ASPD_W_P_NOISE>) _param_west_w_p_noise,
-		(ParamFloat<px4::params::ASPD_SC_P_NOISE>) _param_west_sc_p_noise,
+		(ParamFloat<px4::params::ASPD_WIND_NSD>) _param_aspd_wind_nsd,
+		(ParamFloat<px4::params::ASPD_SCALE_NSD>) _param_aspd_scale_nsd,
 		(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,
@@ -447,16 +447,16 @@ void AirspeedModule::update_params()
 	_param_airspeed_scale[1] = _param_airspeed_scale_2.get();
 	_param_airspeed_scale[2] = _param_airspeed_scale_3.get();
 
-	_wind_estimator_sideslip.set_wind_p_noise(_param_west_w_p_noise.get());
-	_wind_estimator_sideslip.set_tas_scale_p_noise(_param_west_sc_p_noise.get());
+	_wind_estimator_sideslip.set_wind_process_noise_spectral_density(_param_aspd_wind_nsd.get());
+	_wind_estimator_sideslip.set_tas_scale_process_noise_spectral_density(_param_aspd_scale_nsd.get());
 	_wind_estimator_sideslip.set_tas_noise(_param_west_tas_noise.get());
 	_wind_estimator_sideslip.set_beta_noise(_param_west_beta_noise.get());
 	_wind_estimator_sideslip.set_tas_gate(_param_west_tas_gate.get());
 	_wind_estimator_sideslip.set_beta_gate(_param_west_beta_gate.get());
 
 	for (int i = 0; i < MAX_NUM_AIRSPEED_SENSORS; i++) {
-		_airspeed_validator[i].set_wind_estimator_wind_p_noise(_param_west_w_p_noise.get());
-		_airspeed_validator[i].set_wind_estimator_tas_scale_p_noise(_param_west_sc_p_noise.get());
+		_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_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

@@ -1,29 +1,31 @@
 
 /**
- * Airspeed Selector: Wind estimator wind process noise
+ * Airspeed Selector: Wind estimator wind process noise noise spectral density
  *
  * Wind process noise of the internal wind estimator(s) of the airspeed selector.
+ * When unaided, the wind estimate uncertainty (1-sigma, in m/s) increases by this amount every second.
  *
  * @min 0
  * @max 1
- * @unit m/s^2
+ * @unit m/s^2/sqrt(Hz)
  * @decimal 2
  * @group Airspeed Validator
  */
-PARAM_DEFINE_FLOAT(ASPD_W_P_NOISE, 0.1f);
+PARAM_DEFINE_FLOAT(ASPD_WIND_NSD, 0.1f);
 
 /**
- * Airspeed Selector: Wind estimator true airspeed scale process noise
+ * Airspeed Selector: Wind estimator true airspeed scale process noise spectral density
  *
  * Airspeed scale process noise of the internal wind estimator(s) of the airspeed selector.
+ * When unaided, the scale uncertainty (1-sigma, unitless) increases by this amount every second.
  *
  * @min 0
  * @max 0.1
- * @unit Hz
+ * @unit 1/s/sqrt(Hz)
  * @decimal 5
  * @group Airspeed Validator
  */
-PARAM_DEFINE_FLOAT(ASPD_SC_P_NOISE, 0.0001f);
+PARAM_DEFINE_FLOAT(ASPD_SCALE_NSD, 0.0001f);
 
 /**
  * Airspeed Selector: Wind estimator true airspeed measurement noise