EKF: Scale position observation noise with GPS quality

This allows the filter to adapt to variations in GPs quality.
The range of adjustment in observation noise is limited to the range between a lower limit set by the GPS observation noise parameter and an upper limit set by the no-aiding observation noise.

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

@@ -197,6 +197,7 @@ 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)
 
 	bool _mag_healthy;              // computed by mag innovation test
 	float _yaw_test_ratio;          // yaw innovation consistency check ratio

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];