CleanUp GPS drift checks

CMakeLists.txt

@@ -132,7 +132,7 @@ if(CMAKE_SOURCE_DIR STREQUAL PROJECT_SOURCE_DIR)
 	include(ExternalProject)
 	ExternalProject_Add(matrix
 			GIT_REPOSITORY "https://github.com/PX4/Matrix.git"
-			GIT_TAG 2f6398168d04451998cbb60b44d7f4898279caf0
+			GIT_TAG 4f3565da94d00c4cd1feb560f1f72a81296522f8
 			UPDATE_COMMAND ""
 			PATCH_COMMAND ""
 			CONFIGURE_COMMAND ""

EKF/common.h

@@ -66,7 +66,7 @@ struct gps_message {
 	float epv;		///< GPS vertical position accuracy in m
 	float sacc;		///< GPS speed accuracy in m/s
 	float vel_m_s;		///< GPS ground speed (m/sec)
-	float vel_ned[3];	///< GPS ground speed NED
+	float vel_ned[3];	///< GPS ground speed NED - TODO: make Vector3f
 	bool vel_ned_valid;	///< GPS ground speed is valid
 	uint8_t nsats;		///< number of satellites used
 	float pdop;		///< position dilution of precision

EKF/ekf.cpp

@@ -192,7 +192,7 @@ bool Ekf::initialiseFilter()
 		setControlBaroHeight();
 
 		// reset variables that are shared with post alignment GPS checks
-		_gps_drift_velD = 0.0f;
+		_gps_pos_derived_filt(2) = 0.0f;
 		_gps_alt_ref = 0.0f;
 
 		if(!initialiseTilt()){

EKF/ekf.h

@@ -444,12 +444,9 @@ private:
 	float _output_tracking_error[3] {}; ///< contains the magnitude of the angle, velocity and position track errors (rad, m/s, m)
 
 	// variables used for the GPS quality checks
-	float _gpsDriftVelN{0.0f};		///< GPS north position derivative (m/sec)
-	float _gpsDriftVelE{0.0f};		///< GPS east position derivative (m/sec)
-	float _gps_drift_velD{0.0f};		///< GPS down position derivative (m/sec)
+	Vector3f _gps_pos_derived_filt;	///< GPS NED position derivative (m/sec)
+	Vector2f _gps_velNE_filt;	///< filtered GPS North and East velocity (m/sec)
 	float _gps_velD_diff_filt{0.0f};	///< GPS filtered Down velocity (m/sec)
-	float _gps_velN_filt{0.0f};		///< GPS filtered North velocity (m/sec)
-	float _gps_velE_filt{0.0f};		///< GPS filtered East velocity (m/sec)
 	uint64_t _last_gps_fail_us{0};		///< last system time in usec that the GPS failed it's checks
 	uint64_t _last_gps_pass_us{0};		///< last system time in usec that the GPS passed it's checks
 	float _gps_error_norm{1.0f};		///< normalised gps error

EKF/gps_checks.cpp

@@ -94,7 +94,7 @@ bool Ekf::collect_gps(const gps_message &gps)
 
 		if (_params.vdist_sensor_type == VDIST_SENSOR_GPS) {
 			ECL_INFO_TIMESTAMPED("GPS checks passed (WGS-84 origin set, using GPS height)");
-			setControlGPSHeight();			
+			setControlGPSHeight();
 			// zero the sensor offset
 			_hgt_sensor_offset = 0.0f;
 		} else {
@@ -146,51 +146,40 @@ bool Ekf::gps_is_good(const gps_message &gps)
 	if (!_control_status.flags.in_air && _vehicle_at_rest) {
 		// Calculate position movement since last measurement
 		float delta_posN = 0.0f;
-		float delta_PosE = 0.0f;
+		float delta_posE = 0.0f;
 
 		// calculate position movement since last GPS fix
 		if (_gps_pos_prev.timestamp > 0) {
-			map_projection_project(&_gps_pos_prev, lat, lon, &delta_posN, &delta_PosE);
+			map_projection_project(&_gps_pos_prev, lat, lon, &delta_posN, &delta_posE);
 
 		} else {
 			// no previous position has been set
 			map_projection_init_timestamped(&_gps_pos_prev, lat, lon, _time_last_imu);
 			_gps_alt_prev = 1e-3f * (float)gps.alt;
-
 		}
 
-		// 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);
-		float velE = fminf(fmaxf(delta_PosE / dt, -vel_limit), vel_limit);
+		// Calculate the horizontal and vertical drift velocity components and limit to 10x the threshold
+		const Vector3f vel_limit{_params.req_hdrift, _params.req_hdrift, _params.req_vdrift};
+		Vector3f pos_derived{delta_posN, delta_posE, (_gps_alt_prev - 1e-3f * (float)gps.alt)};
+		pos_derived = matrix::constrain(pos_derived / dt, -10.0f * vel_limit, 10.0f * vel_limit);
 
 		// Apply a low pass filter
-		_gpsDriftVelN = velN * filter_coef + _gpsDriftVelN * (1.0f - filter_coef);
-		_gpsDriftVelE = velE * filter_coef + _gpsDriftVelE * (1.0f - filter_coef);
+		_gps_pos_derived_filt = pos_derived * filter_coef + _gps_pos_derived_filt * (1.0f-filter_coef);
 
 		// Calculate the horizontal drift speed and fail if too high
-		_gps_drift_metrics[0] = sqrtf(_gpsDriftVelN * _gpsDriftVelN + _gpsDriftVelE * _gpsDriftVelE);
+		_gps_drift_metrics[0] = Vector2f{_gps_pos_derived_filt.xy()}.norm();
 		_gps_check_fail_status.flags.hdrift = (_gps_drift_metrics[0] > _params.req_hdrift);
 
-		// Calculate the vertical drift velocity and limit to 10x the threshold
-		float vz_drift_limit = 10.0f * _params.req_vdrift;
-		float gps_alt_m = 1e-3f * (float)gps.alt;
-		float velD = math::constrain(((_gps_alt_prev - gps_alt_m) / dt), -vz_drift_limit, vz_drift_limit);
-
-		// Apply a low pass filter to the vertical velocity
-		_gps_drift_velD = velD * filter_coef + _gps_drift_velD * (1.0f - filter_coef);
-
 		// Fail if the vertical drift speed is too high
-		_gps_drift_metrics[1] = fabsf(_gps_drift_velD);
+		_gps_drift_metrics[1] = fabsf(_gps_pos_derived_filt(2));
 		_gps_check_fail_status.flags.vdrift = (_gps_drift_metrics[1] > _params.req_vdrift);
 
 		// Check the magnitude of the filtered horizontal GPS velocity
-		float vxy_drift_limit = 10.0f * _params.req_hdrift;
-		float gps_velN = fminf(fmaxf(gps.vel_ned[0], -vxy_drift_limit), vxy_drift_limit);
-		float gps_velE = fminf(fmaxf(gps.vel_ned[1], -vxy_drift_limit), vxy_drift_limit);
-		_gps_velN_filt = gps_velN * filter_coef + _gps_velN_filt * (1.0f - filter_coef);
-		_gps_velE_filt  = gps_velE * filter_coef + _gps_velE_filt  * (1.0f - filter_coef);
-		_gps_drift_metrics[2] = sqrtf(_gps_velN_filt * _gps_velN_filt + _gps_velE_filt * _gps_velE_filt);
+		Vector2f gps_velNE = matrix::constrain(Vector2f{gps.vel_ned[0], gps.vel_ned[1]}, // TODO: when vel_ned vector3f use .xy()
+								-10.0f * _params.req_hdrift,
+								 10.0f * _params.req_hdrift);
+		_gps_velNE_filt = gps_velNE * filter_coef + _gps_velNE_filt * (1.0f - filter_coef);
+		_gps_drift_metrics[2] = _gps_velNE_filt.norm();
 		_gps_check_fail_status.flags.hspeed = (_gps_drift_metrics[2] > _params.req_hdrift);
 
 		_gps_drift_updated = true;