diff --git a/src/modules/ekf2/EKF/common.h b/src/modules/ekf2/EKF/common.h index 521f108fe6..4c23a9c51d 100644 --- a/src/modules/ekf2/EKF/common.h +++ b/src/modules/ekf2/EKF/common.h @@ -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) diff --git a/src/modules/ekf2/EKF/covariance.cpp b/src/modules/ekf2/EKF/covariance.cpp index a4631004a4..4af55707a5 100644 --- a/src/modules/ekf2/EKF/covariance.cpp +++ b/src/modules/ekf2/EKF/covariance.cpp @@ -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 diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index 0e43e218a4..82255b40eb 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -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), diff --git a/src/modules/ekf2/EKF2.hpp b/src/modules/ekf2/EKF2.hpp index 714f3a1ff7..9dff6dcf40 100644 --- a/src/modules/ekf2/EKF2.hpp +++ b/src/modules/ekf2/EKF2.hpp @@ -377,8 +377,8 @@ private: _param_ekf2_mag_e_noise, ///< process noise for earth magnetic field prediction (Gauss/sec) (ParamExtFloat) _param_ekf2_mag_b_noise, ///< process noise for body magnetic field prediction (Gauss/sec) - (ParamExtFloat) - _param_ekf2_wind_noise, ///< process noise for wind velocity prediction (m/sec**2) + (ParamExtFloat) + _param_ekf2_wind_nsd, ///< process noise spectral density for wind velocity prediction (m/sec**2/sqrt(Hz)) (ParamExtFloat) _param_ekf2_terr_noise, ///< process noise for terrain offset (m/sec) (ParamExtFloat) _param_ekf2_terr_grad, ///< gradient of terrain used to estimate process noise due to changing position (m/m) diff --git a/src/modules/ekf2/ekf2_params.c b/src/modules/ekf2/ekf2_params.c index 125c61399e..271882a6e9 100644 --- a/src/modules/ekf2/ekf2_params.c +++ b/src/modules/ekf2/ekf2_params.c @@ -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.