EKF: Add parameter to set initial and max allowed wind uncertainty

This enables the initial uncertainty to be set based on application and also ensures that the max allowed growth in wind state variance is consistent with the initial uncertainty specified.

EKF/common.h

@@ -219,6 +219,7 @@ struct parameters {
 	float switch_on_gyro_bias{0.1f};	// 1-sigma gyro bias uncertainty at switch on (rad/sec)
 	float switch_on_accel_bias{0.2f};	// 1-sigma accelerometer bias uncertainty at switch on (m/s**2)
 	float initial_tilt_err{0.1f};		// 1-sigma tilt error after initial alignment using gravity vector (rad)
+	float initial_wind_uncertainty{1.0f};	// 1-sigma initial uncertainty in wind velocity (m/s)
 
 	// position and velocity fusion
 	float gps_vel_noise{5.0e-1f};		// observation noise for gps velocity fusion (m/sec)

EKF/covariance.cpp

@@ -100,8 +100,8 @@ void Ekf::initialiseCovariance()
 	}
 
 	// wind
-	P[22][22] = 1.0f;
-	P[23][23] = 1.0f;
+	P[22][22] = sq(_params.initial_wind_uncertainty);
+	P[23][23] = sq(_params.initial_wind_uncertainty);
 
 }
 
@@ -204,7 +204,7 @@ void Ekf::predictCovariance()
 	float wind_vel_sig;
 
 	// 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]) < 2.0f) {
+	if (_control_status.flags.wind && (P[22][22] + P[23][23]) < 2.0f * sq(_params.initial_wind_uncertainty)) {
 		wind_vel_sig = dt * math::constrain(_params.wind_vel_p_noise, 0.0f, 1.0f);
 
 	} else {