gps_alt: extract measurement variance computation and saturation

EKF/control.cpp

@@ -1103,11 +1103,7 @@ void Ekf::controlHeightFusion()
 			Vector3f gps_hgt_obs_var;
 			// vertical position innovation - gps measurement has opposite sign to earth z axis
 			_gps_pos_innov(2) = _state.pos(2) + _gps_sample_delayed.hgt - _gps_alt_ref - _hgt_sensor_offset;
-			// observation variance - receiver defined and parameter limited
+			gps_hgt_obs_var(2) = getGpsAltVar();
-			// use scaled horizontal position accuracy assuming typical ratio of VDOP/HDOP
-			const float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
-			const float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit);
-			gps_hgt_obs_var(2) = sq(1.5f * math::constrain(_gps_sample_delayed.vacc, lower_limit, upper_limit));
 			// innovation gate size
 			gps_hgt_innov_gate(1) = fmaxf(_params.baro_innov_gate, 1.0f);
 			// fuse height information

EKF/ekf.h

@@ -851,6 +851,9 @@ private:
 
 	void updateBaroHgtOffset();
 
+	// return an estimation of the GPS altitude variance
+	float getGpsAltVar();
+
 	// calculate the measurement variance for the optical flow sensor
 	float calcOptFlowMeasVar();
 

EKF/ekf_helper.cpp

@@ -1244,6 +1244,16 @@ void Ekf::updateBaroHgtOffset()
 	}
 }
 
+float Ekf::getGpsAltVar()
+{
+	// observation variance - receiver defined and parameter limited
+	// use scaled horizontal position accuracy assuming typical ratio of VDOP/HDOP
+	const float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
+	const float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit);
+	const float gps_alt_var = sq(1.5f * math::constrain(_gps_sample_delayed.vacc, lower_limit, upper_limit));
+	return gps_alt_var;
+}
+
 Vector3f Ekf::getVisionVelocityInEkfFrame() const
 {
 	Vector3f vel;