gps sacc: apply same minimum for EKF2 and yaw estimator

EKF/EKFGSF_yaw.cpp

@@ -293,7 +293,7 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index)
 bool EKFGSF_yaw::updateEKF(const uint8_t model_index)
 {
 	// set observation variance from accuracy estimate supplied by GPS and apply a sanity check minimum
-	const float velObsVar = sq(fmaxf(_vel_accuracy, 0.5f));
+	const float velObsVar = sq(fmaxf(_vel_accuracy, 0.01f));
 
 	// calculate velocity observation innovations
 	_ekf_gsf[model_index].innov(0) = _ekf_gsf[model_index].X(0) - _vel_NE(0);

EKF/control.cpp

@@ -702,7 +702,9 @@ void Ekf::controlGpsFusion()
 				gps_pos_obs_var(0) = gps_pos_obs_var(1) = sq(math::constrain(_gps_sample_delayed.hacc, lower_limit, upper_limit));
 			}
 
-			_last_vel_obs_var.setAll(sq(fmaxf(_gps_sample_delayed.sacc, _params.gps_vel_noise)));
+			_gps_sample_delayed.sacc = fmaxf(_gps_sample_delayed.sacc, _params.gps_vel_noise);
+
+			_last_vel_obs_var.setAll(sq(_gps_sample_delayed.sacc));
 			_last_vel_obs_var(2) *= sq(1.5f);
 
 			// calculate innovations