diff --git a/EKF/common.h b/EKF/common.h index 6c6a25633b5..8ff1f19e66c 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -238,6 +238,7 @@ struct parameters { 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) + float wind_vel_p_noise_scaler{0.0f}; ///< 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/EKF/covariance.cpp b/EKF/covariance.cpp index 07d71b35545..63307b95830 100644 --- a/EKF/covariance.cpp +++ b/EKF/covariance.cpp @@ -226,9 +226,13 @@ void Ekf::predictCovariance() float wind_vel_sig; + // Calculate low pass filtered height rate + float alpha_height_rate_lpf = 0.1f * dt; // 10 seconds time constant + _height_rate_lpf = _height_rate_lpf * (1.0f - alpha_height_rate_lpf) + _state.vel(2) * alpha_height_rate_lpf; + // 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); + 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)); } else { wind_vel_sig = 0.0f; diff --git a/EKF/ekf.h b/EKF/ekf.h index 68dccb1291c..16e89563d21 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -474,6 +474,8 @@ private: float _rng_stuck_min_val{0.0f}; ///< minimum value for new rng measurement when being stuck float _rng_stuck_max_val{0.0f}; ///< maximum value for new rng measurement when being stuck + float _height_rate_lpf{0.0f}; + // update the real time complementary filter states. This includes the prediction // and the correction step void calculateOutputStates();