Add vision height covariance passthrough (#507)

* Add vision height covariance passthrough

* Fix hgtErr layout

EKF/common.h

@@ -80,7 +80,8 @@ struct flow_message {
 struct ext_vision_message {
 	Vector3f posNED;	///< measured NED position relative to the local origin (m)
 	Quatf quat;		///< measured quaternion orientation defining rotation from NED to body frame
-	float posErr;		///< 1-Sigma spherical position accuracy (m)
+	float posErr;		///< 1-Sigma horizontal position accuracy (m)
+	float hgtErr;		///< 1-Sigma height accuracy (m)
 	float angErr;		///< 1-Sigma angular error (rad)
 };
 
@@ -148,7 +149,8 @@ struct flowSample {
 struct extVisionSample {
 	Vector3f posNED;	///< measured NED position relative to the local origin (m)
 	Quatf quat;		///< measured quaternion orientation defining rotation from NED to body frame
-	float posErr;		///< 1-Sigma spherical position accuracy (m)
+	float posErr;		///< 1-Sigma horizontal position accuracy (m)
+	float hgtErr;		///< 1-Sigma height accuracy (m)
 	float angErr;		///< 1-Sigma angular error (rad)
 	uint64_t time_us;	///< timestamp of the measurement (uSec)
 };

EKF/estimator_interface.cpp

@@ -436,6 +436,7 @@ void EstimatorInterface::setExtVisionData(uint64_t time_usec, ext_vision_message
 		// copy required data
 		ev_sample_new.angErr = evdata->angErr;
 		ev_sample_new.posErr = evdata->posErr;
+		ev_sample_new.hgtErr = evdata->hgtErr;
 		ev_sample_new.quat = evdata->quat;
 		ev_sample_new.posNED = evdata->posNED;
 

EKF/vel_pos_fusion.cpp

@@ -149,7 +149,7 @@ void Ekf::fuseVelPosHeight()
 			// calculate the innovation assuming the external vision observaton is in local NED frame
 			innovation[5] = _state.pos(2) - _ev_sample_delayed.posNED(2);
 			// observation variance - defined externally
-			R[5] = fmaxf(_ev_sample_delayed.posErr, 0.01f);
+			R[5] = fmaxf(_ev_sample_delayed.hgtErr, 0.01f);
 			R[5] = R[5] * R[5];
 			// innovation gate size
 			gate_size[5] = fmaxf(_params.ev_innov_gate, 1.0f);