EKF: add accessor function for local position accuracy

EKF/ekf.h

@@ -120,6 +120,9 @@ public:
 	// 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);
 
+	// 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_vel_var(Vector3f &vel_var);
 
 	void get_pos_var(Vector3f &pos_var);

EKF/ekf_helper.cpp

@@ -748,6 +748,31 @@ void Ekf::get_ekf_gpos_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_recko
 	memcpy(dead_reckoning, &is_dead_reckoning, sizeof(bool));
 }
 
+// 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)
+{
+	// 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);
+	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;
+
+	}
+
+	// report dead reckoning if it is more than a second since we fused in position measurements
+	bool is_dead_reckoning = (_time_last_imu - _time_last_pos_fuse > 1e6);
+
+	memcpy(ekf_eph, &hpos_err, sizeof(float));
+	memcpy(ekf_epv, &vpos_err, sizeof(float));
+	memcpy(dead_reckoning, &is_dead_reckoning, sizeof(bool));
+}
+
 // get EKF innovation consistency check status information comprising of:
 // status - a bitmask integer containing the pass/fail status for each EKF measurement innovation consistency check
 // Innovation Test Ratios - these are the ratio of the innovation to the acceptance threshold.

EKF/estimator_interface.h

@@ -132,6 +132,9 @@ public:
 	// 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;
 
+	// 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;
+
 	// ask estimator for sensor data collection decision and do any preprocessing if required, returns true if not defined
 	virtual bool collect_gps(uint64_t time_usec, struct gps_message *gps) { return true; }