EKF always fill position and velocity accuracy (#425)

EKF/ekf.h

@@ -132,13 +132,13 @@ public:
 	bool get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt);
 
 	// get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
-	void get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_reckoning);
+	void get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv);
 
 	// get the 1-sigma horizontal and vertical position uncertainty of the ekf local position
-	void get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_reckoning);
+	void get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv);
 
 	// get the 1-sigma horizontal and vertical velocity uncertainty
-	void get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv, bool *dead_reckoning);
+	void get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv);
 
 	/*
 	Returns the following vehicle control limits required by the estimator.

EKF/ekf_helper.cpp

@@ -981,114 +981,66 @@ void Ekf::get_imu_vibe_metrics(float vibe[3])
 }
 
 // get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
-void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_reckoning)
+void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv)
 {
 	// report absolute accuracy taking into account the uncertainty in location of the origin
 	// If not aiding, return 0 for horizontal position estimate as no estimate is available
 	// TODO - allow for baro drift in vertical position error
-	float hpos_err;
-	float vpos_err;
-
-	if (global_position_is_valid()) {
-		hpos_err = sqrtf(P[7][7] + P[8][8] + sq(_gps_origin_eph));
-		vpos_err = sqrtf(P[9][9] + sq(_gps_origin_epv));
-
-	} else {
-		hpos_err = 0.0f;
-		vpos_err = 0.0f;
-
-	}
+	float hpos_err = sqrtf(P[7][7] + P[8][8] + sq(_gps_origin_eph));
 
 	// If we are dead-reckoning, use the innovations as a conservative alternate measure of the horizontal position error
-	// The reason is that complete rejection of measurements is often casued by heading misalignment or inertial sensing errors
+	// The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors
 	// and using state variances for accuracy reporting is overly optimistic in these situations
 	if (_is_dead_reckoning && (_control_status.flags.gps || _control_status.flags.ev_pos)) {
-		hpos_err = math::max(hpos_err, sqrtf(_vel_pos_innov[3] * _vel_pos_innov[3] + _vel_pos_innov[4] * _vel_pos_innov[4]));
-
+		hpos_err = math::max(hpos_err, sqrtf(sq(_vel_pos_innov[3]) + sq(_vel_pos_innov[4])));
 	}
 
-	memcpy(ekf_eph, &hpos_err, sizeof(float));
-	memcpy(ekf_epv, &vpos_err, sizeof(float));
-	memcpy(dead_reckoning, &_is_dead_reckoning, sizeof(bool));
+	*ekf_eph = hpos_err;
+	*ekf_epv = sqrtf(P[9][9] + sq(_gps_origin_epv));
 }
 
 // get the 1-sigma horizontal and vertical position uncertainty of the ekf local position
-void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_reckoning)
+void Ekf::get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv)
 {
 	// TODO - allow for baro drift in vertical position error
-	float hpos_err;
-	float vpos_err;
-	bool vel_pos_aiding = (_control_status.flags.gps ||
-			       _control_status.flags.opt_flow ||
-			       _control_status.flags.ev_pos ||
-			       (_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd));
-
-	if (vel_pos_aiding && _NED_origin_initialised) {
-		hpos_err = sqrtf(P[7][7] + P[8][8]);
-		vpos_err = sqrtf(P[9][9]);
-
-	} else {
-		hpos_err = 0.0f;
-		vpos_err = 0.0f;
-
-	}
+	float hpos_err = sqrtf(P[7][7] + P[8][8]);
 
 	// If we are dead-reckoning, use the innovations as a conservative alternate measure of the horizontal position error
-	// The reason is that complete rejection of measurements is often casued by heading misalignment or inertial sensing errors
+	// The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors
 	// and using state variances for accuracy reporting is overly optimistic in these situations
 	if (_is_dead_reckoning && (_control_status.flags.gps || _control_status.flags.ev_pos)) {
-		hpos_err = math::max(hpos_err, sqrtf(_vel_pos_innov[3] * _vel_pos_innov[3] + _vel_pos_innov[4] * _vel_pos_innov[4]));
-
+		hpos_err = math::max(hpos_err, sqrtf(sq(_vel_pos_innov[3]) + sq(_vel_pos_innov[4])));
 	}
 
-	memcpy(ekf_eph, &hpos_err, sizeof(float));
-	memcpy(ekf_epv, &vpos_err, sizeof(float));
-	memcpy(dead_reckoning, &_is_dead_reckoning, sizeof(bool));
+	*ekf_eph = hpos_err;
+	*ekf_epv = sqrtf(P[9][9]);
 }
 
 // get the 1-sigma horizontal and vertical velocity uncertainty
-void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv, bool *dead_reckoning)
+void Ekf::get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv)
 {
-	float hvel_err;
-	float vvel_err;
-	bool vel_pos_aiding = (_control_status.flags.gps ||
-			       _control_status.flags.opt_flow ||
-			       _control_status.flags.ev_pos ||
-			       (_control_status.flags.fuse_beta && _control_status.flags.fuse_aspd));
-
-	if (vel_pos_aiding && _NED_origin_initialised) {
-		hvel_err = sqrtf(P[4][4] + P[5][5]);
-		vvel_err = sqrtf(P[6][6]);
-
-	} else {
-		hvel_err = 0.0f;
-		vvel_err = 0.0f;
-
-	}
+	float hvel_err = sqrtf(P[4][4] + P[5][5]);
 
 	// If we are dead-reckoning, use the innovations as a conservative alternate measure of the horizontal velocity error
 	// The reason is that complete rejection of measurements is often caused by heading misalignment or inertial sensing errors
 	// and using state variances for accuracy reporting is overly optimistic in these situations
-	float vel_err_conservative = 0.0f;
-
 	if (_is_dead_reckoning) {
+		float vel_err_conservative = 0.0f;
+
 		if (_control_status.flags.opt_flow) {
 			float gndclearance = math::max(_params.rng_gnd_clearance, 0.1f);
-			vel_err_conservative = math::max((_terrain_vpos - _state.pos(2)),
-							 gndclearance) * sqrtf(_flow_innov[0] * _flow_innov[0] + _flow_innov[1] * _flow_innov[1]);
+			vel_err_conservative = math::max((_terrain_vpos - _state.pos(2)), gndclearance) * sqrtf(sq(_flow_innov[0]) + sq(_flow_innov[1]));
 		}
 
 		if (_control_status.flags.gps || _control_status.flags.ev_pos) {
-			vel_err_conservative = math::max(vel_err_conservative,
-							 sqrtf(_vel_pos_innov[0] * _vel_pos_innov[0] + _vel_pos_innov[1] * _vel_pos_innov[1]));
+			vel_err_conservative = math::max(vel_err_conservative, sqrtf(sq(_vel_pos_innov[0]) + sq(_vel_pos_innov[1])));
 		}
 
 		hvel_err = math::max(hvel_err, vel_err_conservative);
 	}
 
-	memcpy(ekf_evh, &hvel_err, sizeof(float));
-	memcpy(ekf_evv, &vvel_err, sizeof(float));
-	memcpy(dead_reckoning, &_is_dead_reckoning, sizeof(bool));
+	*ekf_evh = hvel_err;
+	*ekf_evv = sqrtf(P[6][6]);
 }
 
 /*

EKF/estimator_interface.h

@@ -144,13 +144,13 @@ public:
 	virtual bool get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *origin_pos, float *origin_alt) = 0;
 
 	// get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
-	virtual void get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_reckoning) = 0;
+	virtual void get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv) = 0;
 
 	// get the 1-sigma horizontal and vertical position uncertainty of the ekf local position
-	virtual void get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_reckoning) = 0;
+	virtual void get_ekf_lpos_accuracy(float *ekf_eph, float *ekf_epv) = 0;
 
 	// get the 1-sigma horizontal and vertical velocity uncertainty
-	virtual void get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv, bool *dead_reckoning) = 0;
+	virtual void get_ekf_vel_accuracy(float *ekf_evh, float *ekf_evv) = 0;
 
 	/*
 	Returns the following vehicle control limits required by the estimator.