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.
This commit is contained in:
bresch
2022-06-14 18:39:10 +10:00
committed by Paul Riseborough
parent 377338109c
commit e105869986
6 changed files with 25 additions and 25 deletions
@@ -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;
@@ -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());
@@ -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