mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-22 17:33:34 +08:00
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:
@@ -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
|
||||
|
||||
Reference in New Issue
Block a user