diff --git a/src/lib/parameters/px4params/srcparser.py b/src/lib/parameters/px4params/srcparser.py index 922d53e38c..d3fb119757 100644 --- a/src/lib/parameters/px4params/srcparser.py +++ b/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', diff --git a/src/lib/wind_estimator/WindEstimator.cpp b/src/lib/wind_estimator/WindEstimator.cpp index 8d3f405ac7..9891233b77 100644 --- a/src/lib/wind_estimator/WindEstimator.cpp +++ b/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; } diff --git a/src/lib/wind_estimator/WindEstimator.hpp b/src/lib/wind_estimator/WindEstimator.hpp index b89f501dab..434d8404a6 100644 --- a/src/lib/wind_estimator/WindEstimator.hpp +++ b/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 diff --git a/src/modules/airspeed_selector/AirspeedValidator.hpp b/src/modules/airspeed_selector/AirspeedValidator.hpp index 33429eca2b..f0eebd819e 100644 --- a/src/modules/airspeed_selector/AirspeedValidator.hpp +++ b/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; diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp index d641e8011c..8d816f79a7 100644 --- a/src/modules/airspeed_selector/airspeed_selector_main.cpp +++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -165,8 +165,8 @@ private: }; DEFINE_PARAMETERS( - (ParamFloat) _param_west_w_p_noise, - (ParamFloat) _param_west_sc_p_noise, + (ParamFloat) _param_aspd_wind_nsd, + (ParamFloat) _param_aspd_scale_nsd, (ParamFloat) _param_west_tas_noise, (ParamFloat) _param_west_beta_noise, (ParamInt) _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()); diff --git a/src/modules/airspeed_selector/airspeed_selector_params.c b/src/modules/airspeed_selector/airspeed_selector_params.c index 4e00817deb..86fd882ca3 100644 --- a/src/modules/airspeed_selector/airspeed_selector_params.c +++ b/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