ekf: scale wind process noise with low pass filtered height rate

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)
 

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;

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