EKF: Calculate and publish horizontal and vertical position accuracy

This calculation takes into account the uncertainty of the origin.
Dead reckoning status is also published

EKF/ekf.h

@@ -95,6 +95,9 @@ public:
 	// get the ekf WGS-84 origin position and height and the system time it was last set
 	void 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_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_reckoning);
+
 private:
 
 	static const uint8_t _k_num_states = 24;

EKF/ekf_helper.cpp

@@ -309,6 +309,21 @@ void Ekf::get_ekf_origin(uint64_t *origin_time, map_projection_reference_s *orig
 	memcpy(origin_alt, &_gps_alt_ref, sizeof(float));
 }
 
+// get the 1-sigma horizontal and vertical position uncertainty of the ekf WGS-84 position
+void Ekf::get_ekf_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_reckoning)
+{
+	// report absolute accuracy taking into account the uncertainty in location of the origin
+	// TODO we a need a way to allow for baro drift error
+	float temp1 = sqrtf(P[6][6] + P[7][7] + sq(_gps_origin_eph));
+	float temp2 = sqrtf(P[8][8] + sq(_gps_origin_epv));
+	memcpy(ekf_eph, &temp1, sizeof(float));
+	memcpy(ekf_epv, &temp2, sizeof(float));
+
+	// report dead reckoning if it is more than a second since we fused in GPS
+	bool temp3 = (_time_last_imu - _time_last_pos_fuse > 1e6);
+	memcpy(dead_reckoning, &temp3, sizeof(bool));
+}
+
 // fuse measurement
 void Ekf::fuse(float *K, float innovation)
 {

EKF/estimator_interface.h

@@ -83,6 +83,9 @@ public:
 	// get the ekf WGS-84 origin position and height and the system time it was last set
 	virtual void 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_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; }
 
@@ -198,6 +201,8 @@ protected:
 	float _gps_speed_accuracy;                  // GPS receiver reported speed accuracy (m/s)
 	struct map_projection_reference_s _pos_ref; // Contains WGS-84 position latitude and longitude (radians)
 	float _gps_hpos_accuracy = 0.0f; // GPS receiver reported 1-sigma horizontal accuracy (m)
+	float _gps_origin_eph = 0.0f; // horizontal position uncertainty of the GPS origin
+	float _gps_origin_epv = 0.0f; // vertical position uncertainty of the GPS origin
 
 	bool _mag_healthy;              // computed by mag innovation test
 	float _yaw_test_ratio;          // yaw innovation consistency check ratio

EKF/gps_checks.cpp

@@ -70,6 +70,9 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps)
 			_last_gps_origin_time_us = _time_last_imu;
 			// set the magnetic declination returned by the geo library using the current GPS position
 			_mag_declination_gps = math::radians(get_mag_declination(lat, lon));
+			// save the horizontal and vertical position uncertainty of the origin
+			_gps_origin_eph = gps->eph;
+			_gps_origin_epv = gps->epv;
 		}
 	}