dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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-08 12:34:38 +02:00 committed by Paul Riseborough
parent 377338109c
commit e105869986
6 changed files with 25 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/lib/parameters/px4params/srcparser.py
View File

@ -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',

8
src/lib/wind_estimator/WindEstimator.cpp
View File

@ -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;
}

10
src/lib/wind_estimator/WindEstimator.hpp
View File

@ -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

4
src/modules/airspeed_selector/AirspeedValidator.hpp
View File

@ -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;

12
src/modules/airspeed_selector/airspeed_selector_main.cpp
View File

@ -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());

14
src/modules/airspeed_selector/airspeed_selector_params.c
View File

@ -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