EKF: Don't use EKF origin in GPS drift check calculation

The GPS drift calculations need to be able to run independently of the EKF origin.

EKF/estimator_interface.cpp

@@ -67,6 +67,8 @@ EstimatorInterface::EstimatorInterface():
 	_gps_origin_eph(0.0f),
 	_gps_origin_epv(0.0f),
 	_pos_ref{},
+	_gps_pos_prev{},
+	_gps_alt_prev(0.0f),
 	_yaw_test_ratio(0.0f),
 	_mag_test_ratio{},
 	_vel_pos_test_ratio{},

EKF/estimator_interface.h

@@ -328,7 +328,9 @@ protected:
 	bool _gps_speed_valid;
 	float _gps_origin_eph; // horizontal position uncertainty of the GPS origin
 	float _gps_origin_epv; // vertical position uncertainty of the GPS origin
-	struct map_projection_reference_s _pos_ref;    // Contains WGS-84 position latitude and longitude (radians)
+	struct map_projection_reference_s _pos_ref;    // Contains WGS-84 position latitude and longitude (radians) of the EKF origin
+	struct map_projection_reference_s _gps_pos_prev;    // Contains WGS-84 position latitude and longitude (radians) of the previous GPS message
+	float _gps_alt_prev;	// height from the previous GPS message (m)
 
 	// innovation consistency check monitoring ratios
 	float _yaw_test_ratio;          // yaw innovation consistency check ratio

EKF/gps_checks.cpp

@@ -135,19 +135,23 @@ bool Ekf::gps_is_good(struct gps_message *gps)
 	double lat = gps->lat * 1.0e-7;
 	double lon = gps->lon * 1.0e-7;
 
-	if (_pos_ref.init_done) {
+	// calculate position movement since last GPS fix
-		map_projection_project(&_pos_ref, lat, lon, &delta_posN, &delta_PosE);
+	if (_gps_pos_prev.timestamp > 0) {
-
+		map_projection_project(&_gps_pos_prev, lat, lon, &delta_posN, &delta_PosE);
 	} else {
-		map_projection_init_timestamped(&_pos_ref, lat, lon, _time_last_imu);
+		// no previous position has been set
-		_gps_alt_ref = 1e-3f * (float)gps->alt;
+		map_projection_init_timestamped(&_gps_pos_prev, lat, lon, _time_last_imu);
+		_gps_alt_prev = 1e-3f * (float)gps->alt;
 	}
 
 	// Calculate time lapsed since last update, limit to prevent numerical errors and calculate the lowpass filter coefficient
 	const float filt_time_const = 10.0f;
-	float dt = fminf(fmaxf(float(_time_last_imu - _last_gps_origin_time_us) * 1e-6f, 0.001f), filt_time_const);
+	float dt = fminf(fmaxf(float(_time_last_imu - _gps_pos_prev.timestamp) * 1e-6f, 0.001f), filt_time_const);
 	float filter_coef = dt / filt_time_const;
 
+	// save GPS fix for next time
+	map_projection_init_timestamped(&_gps_pos_prev, lat, lon, _time_last_imu);
+
 	// Calculate the horizontal drift velocity components and limit to 10x the threshold
 	float vel_limit = 10.0f * _params.req_hdrift;
 	float velN = fminf(fmaxf(delta_posN / dt, -vel_limit), vel_limit);
@@ -169,10 +173,9 @@ bool Ekf::gps_is_good(struct gps_message *gps)
 
 	// Calculate the vertical drift velocity and limit to 10x the threshold
 	vel_limit = 10.0f * _params.req_vdrift;
-	float velD = fminf(fmaxf((_gps_alt_ref - 1e-3f * (float)gps->alt) / dt, -vel_limit), vel_limit);
+	float gps_alt_m = 1e-3f * (float)gps->alt;
-
+	float velD = math::constrain(((_gps_alt_prev - gps_alt_m) / dt), -vel_limit, vel_limit);
-	// Save the current height as the reference for next time
+	_gps_alt_prev = gps_alt_m;
-	_gps_alt_ref = 1e-3f * (float)gps->alt;
 
 	// Apply a low pass filter to the vertical velocity
 	_gps_drift_velD = velD * filter_coef + _gps_drift_velD * (1.0f - filter_coef);