fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

ekf2 wind: use noise spectral density for process noise tuning

Browse Source 
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 EKF2 is 100Hz, the
same tuning is obtained by dividing the std_dev legacy parameter by 10:
nsd = sqrt(std_dev^2 / 100Hz)
This commit is contained in:
bresch 2022-06-08 15:00:16 +02:00 committed by Paul Riseborough
parent e105869986
commit 0d256b8ff6
5 changed files with 14 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/modules/ekf2/EKF/common.h
View File

@ -263,8 +263,8 @@ struct parameters {
float accel_bias_p_noise{1.0e-2f}; ///< process noise for IMU accelerometer bias prediction (m/sec**3)
float mage_p_noise{1.0e-3f}; ///< process noise for earth magnetic field prediction (Gauss/sec)
float magb_p_noise{1.0e-4f}; ///< process noise for body magnetic field prediction (Gauss/sec)
float wind_vel_p_noise{1.0e-1f}; ///< process noise for wind velocity prediction (m/sec**2)
const float wind_vel_p_noise_scaler{0.5f}; ///< scaling of wind process noise with vertical velocity
float wind_vel_nsd{1.0e-2f}; ///< process noise spectral density for wind velocity prediction (m/sec**2/sqrt(Hz))
const float wind_vel_nsd_scaler{0.5f}; ///< scaling of wind process noise with vertical velocity
float terrain_p_noise{5.0f}; ///< process noise for terrain offset (m/sec)
float terrain_gradient{0.5f}; ///< gradient of terrain used to estimate process noise due to changing position (m/m)

8
src/modules/ekf2/EKF/covariance.cpp
View File

@ -187,7 +187,7 @@ void Ekf::predictCovariance()
mag_B_sig = 0.0f;
}
float wind_vel_sig;
float wind_vel_nsd_scaled;
// Calculate low pass filtered height rate
float alpha_height_rate_lpf = 0.1f * dt; // 10 seconds time constant
@ -195,10 +195,10 @@ void Ekf::predictCovariance()
// Don't continue to grow wind velocity state variances if they are becoming too large or we are not using wind velocity states as this can make the covariance matrix badly conditioned
if (_control_status.flags.wind && (P(22,22) + P(23,23)) < sq(_params.initial_wind_uncertainty)) {
wind_vel_sig = dt * math::constrain(_params.wind_vel_p_noise, 0.0f, 1.0f) * (1.0f + _params.wind_vel_p_noise_scaler * fabsf(_height_rate_lpf));
wind_vel_nsd_scaled = math::constrain(_params.wind_vel_nsd, 0.0f, 1.0f) * (1.0f + _params.wind_vel_nsd_scaler * fabsf(_height_rate_lpf));
} else {
wind_vel_sig = 0.0f;
wind_vel_nsd_scaled = 0.0f;
}
// compute noise variance for stationary processes
@ -216,7 +216,7 @@ void Ekf::predictCovariance()
// body frame magnetic field states
process_noise.slice<3, 1>(19, 0) = sq(mag_B_sig);
// wind velocity states
process_noise.slice<2, 1>(22, 0) = sq(wind_vel_sig);
process_noise.slice<2, 1>(22, 0) = sq(wind_vel_nsd_scaled) * dt;
// assign IMU noise variances
// inputs to the system are 3 delta angles and 3 delta velocities

2
src/modules/ekf2/EKF2.cpp
View File

@ -72,7 +72,7 @@ EKF2::EKF2(bool multi_mode, const px4::wq_config_t &config, bool replay_mode):
_param_ekf2_acc_b_noise(_params->accel_bias_p_noise),
_param_ekf2_mag_e_noise(_params->mage_p_noise),
_param_ekf2_mag_b_noise(_params->magb_p_noise),
_param_ekf2_wind_noise(_params->wind_vel_p_noise),
_param_ekf2_wind_nsd(_params->wind_vel_nsd),
_param_ekf2_terr_noise(_params->terrain_p_noise),
_param_ekf2_terr_grad(_params->terrain_gradient),
_param_ekf2_gps_v_noise(_params->gps_vel_noise),

4
src/modules/ekf2/EKF2.hpp
View File

@ -377,8 +377,8 @@ private:
_param_ekf2_mag_e_noise, ///< process noise for earth magnetic field prediction (Gauss/sec)
(ParamExtFloat<px4::params::EKF2_MAG_B_NOISE>)
_param_ekf2_mag_b_noise, ///< process noise for body magnetic field prediction (Gauss/sec)
(ParamExtFloat<px4::params::EKF2_WIND_NOISE>)
_param_ekf2_wind_noise, ///< process noise for wind velocity prediction (m/sec**2)
(ParamExtFloat<px4::params::EKF2_WIND_NSD>)
_param_ekf2_wind_nsd, ///< process noise spectral density for wind velocity prediction (m/sec**2/sqrt(Hz))
(ParamExtFloat<px4::params::EKF2_TERR_NOISE>) _param_ekf2_terr_noise, ///< process noise for terrain offset (m/sec)
(ParamExtFloat<px4::params::EKF2_TERR_GRAD>)
_param_ekf2_terr_grad, ///< gradient of terrain used to estimate process noise due to changing position (m/m)

8
src/modules/ekf2/ekf2_params.c
View File

@ -320,15 +320,17 @@ PARAM_DEFINE_FLOAT(EKF2_MAG_B_NOISE, 1.0e-4f);
PARAM_DEFINE_FLOAT(EKF2_MAG_E_NOISE, 1.0e-3f);
/**
* Process noise for wind velocity prediction.
* Process noise spectral density for wind velocity prediction.
*
* When unaided, the wind estimate uncertainty (1-sigma, in m/s) increases by this amount every second.
*
* @group EKF2
* @min 0.0
* @max 1.0
* @unit m/s^2
* @unit m/s^2/sqrt(Hz)
* @decimal 3
*/
PARAM_DEFINE_FLOAT(EKF2_WIND_NOISE, 1.0e-1f);
PARAM_DEFINE_FLOAT(EKF2_WIND_NSD, 1.0e-2f);
/**
* Measurement noise for gps horizontal velocity.