Merge pull request #61 from PX4/pr-ImprovedAccuracyReporting

EKF: Improve Position Accuracy Reporting

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.cpp

@@ -160,6 +160,7 @@ void EstimatorInterface::setGpsData(uint64_t time_usec, struct gps_message *gps)
 
 		_gps_speed_valid = gps->vel_ned_valid;
 		_gps_speed_accuracy = gps->sacc;
+		_gps_hpos_accuracy = gps->eph;
 
 		float lpos_x = 0.0f;
 		float lpos_y = 0.0f;

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; }
 
@@ -197,6 +200,9 @@ protected:
 	bool _gps_speed_valid;
 	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;
 		}
 	}
 

EKF/vel_pos_fusion.cpp

@@ -40,6 +40,7 @@
  */
 
 #include "ekf.h"
+#include "mathlib.h"
 
 void Ekf::fuseVelPosHeight()
 {
@@ -90,7 +91,10 @@ void Ekf::fuseVelPosHeight()
 			R[3] = fmaxf(_params.pos_noaid_noise, 0.5f);
 
 		} else {
-			R[3] = fmaxf(_params.gps_pos_noise, 0.01f);
+			float lower_limit = fmaxf(_params.gps_pos_noise, 0.01f);
+			float upper_limit = fmaxf(_params.pos_noaid_noise, lower_limit);
+			R[3] = math::constrain(_gps_hpos_accuracy, lower_limit, upper_limit);
+
 		}
 
 		R[3] = R[3] * R[3];