ekf2: on accel clipping immediately reset to GNSS vel/pos (if data is good)

src/modules/ekf2/EKF/ekf.cpp

@@ -250,6 +250,27 @@ void Ekf::predictState()
 	_state.quat_nominal = (_state.quat_nominal * dq).normalized();
 	_R_to_earth = Dcmf(_state.quat_nominal);
 
+	// if there's accel clipping (asymmetric railing) then for convenience register in earth frame
+	if (_imu_sample_delayed.delta_vel_clipping[0] || _imu_sample_delayed.delta_vel_clipping[1] || _imu_sample_delayed.delta_vel_clipping[2]) {
+
+		// delta velocity clipping in body fixed frame
+		const Vector3f clipping_bf {
+			_imu_sample_delayed.delta_vel_clipping[0] ? 1.f : 0.f,
+			_imu_sample_delayed.delta_vel_clipping[1] ? 1.f : 0.f,
+			_imu_sample_delayed.delta_vel_clipping[2] ? 1.f : 0.f
+		};
+
+		// delta velocity clipping in earth frame
+		const Vector3f clipping_ef = _R_to_earth * clipping_bf;
+
+		_imu_accel_clipping_NE = fabsf(clipping_ef(0)) > 0.5f || fabsf(clipping_ef(1)) > 0.5f; // north or east
+		_imu_accel_clipping_D = fabsf(clipping_ef(2)) > 0.5f; // down
+
+	} else {
+		_imu_accel_clipping_NE = false; // north or east
+		_imu_accel_clipping_D = false; // down
+	}
+
 	// Calculate an earth frame delta velocity
 	const Vector3f delta_vel_bias_scaled = getAccelBias() * _imu_sample_delayed.delta_vel_dt;
 	const Vector3f corrected_delta_vel = _imu_sample_delayed.delta_vel - delta_vel_bias_scaled;

src/modules/ekf2/EKF/ekf.h

@@ -593,6 +593,7 @@ private:
 	float _gps_error_norm{1.0f};		///< normalised gps error
 	uint32_t _min_gps_health_time_us{10000000}; ///< GPS is marked as healthy only after this amount of time
 	bool _gps_checks_passed{false};		///> true when all active GPS checks have passed
+	uint64_t _last_gnss_reset_accel_clipping_us{0}; ///< last time GPS control performed an emergency reset due to accel clipping
 
 	// Variables used to publish the WGS-84 location of the EKF local NED origin
 	float _gps_alt_ref{NAN};		///< WGS-84 height (m)

src/modules/ekf2/EKF/estimator_interface.h

@@ -297,6 +297,8 @@ protected:
 	float _dt_ekf_avg{0.010f}; ///< average update rate of the ekf in s
 
 	imuSample _imu_sample_delayed{};	// captures the imu sample on the delayed time horizon
+	bool _imu_accel_clipping_NE{false}; // IMU sample delayed accel clipping in earth NE axis
+	bool _imu_accel_clipping_D{false};  // IMU sample delayed accel clipping in earth D axis
 
 	// measurement samples capturing measurements on the delayed time horizon
 	gpsSample _gps_sample_delayed{};

src/modules/ekf2/EKF/gnss_height_control.cpp

@@ -114,15 +114,6 @@ void Ekf::controlGnssHeightFusion(const gpsSample &gps_sample)
 					resetVerticalPositionTo(-(measurement - bias_est.getBias()), measurement_var);
 					bias_est.setBias(_state.pos(2) + measurement);
 
-					// reset vertical velocity
-					if (PX4_ISFINITE(gps_sample.vel(2)) && (_params.gnss_ctrl & GnssCtrl::VEL)) {
-						// use 1.5 as a typical ratio of vacc/hacc
-						resetVerticalVelocityTo(gps_sample.vel(2), sq(1.5f * gps_sample.sacc));
-
-					} else {
-						resetVerticalVelocityToZero();
-					}
-
 					aid_src.time_last_fuse = _imu_sample_delayed.time_us;
 
 				} else if (is_fusion_failing) {

src/modules/ekf2/EKF/gps_control.cpp

@@ -136,6 +136,39 @@ void Ekf::controlGpsFusion()
 						_information_events.flags.reset_pos_to_gps = true;
 						resetVelocityTo(gps_sample.vel, vel_obs_var);
 						resetHorizontalPositionTo(gps_sample.pos, pos_obs_var);
+
+					} else if ((_imu_accel_clipping_NE || _imu_accel_clipping_D) && gps_checks_passing
+						   && isTimedOut(_last_gnss_reset_accel_clipping_us, (uint64_t)3e6)) {
+						// check for accel clipping and perform an emergency reset if GNSS is good (our last resort)
+						if (_imu_accel_clipping_NE) {
+							// accel clipping on horizontal axis
+
+							// reset velocity (if current sample speed accuracy is good)
+							if (gps_sample.sacc < _params.req_sacc) {
+								_information_events.flags.reset_vel_to_gps = true;
+								resetVelocityTo(gps_sample.vel, vel_obs_var);
+								_aid_src_gnss_vel.time_last_fuse = _imu_sample_delayed.time_us;
+
+								_last_gnss_reset_accel_clipping_us = _imu_sample_delayed.time_us;
+							}
+
+							// reset position (if current sample horizontal accuracy is good)
+							if (gps_sample.hacc < _params.req_hacc) {
+								_information_events.flags.reset_pos_to_gps = true;
+								resetHorizontalPositionTo(gps_sample.pos, pos_obs_var);
+								_aid_src_gnss_pos.time_last_fuse = _imu_sample_delayed.time_us;
+
+								_last_gnss_reset_accel_clipping_us = _imu_sample_delayed.time_us;
+							}
+
+						} else if (_imu_accel_clipping_D || isHeightResetRequired()) {
+							// accel clipping on vertical axis or height failure, reset vertical velocity
+							if (gps_sample.sacc < _params.req_sacc) {
+								resetVerticalVelocityTo(gps_sample.vel(2), vel_obs_var(2));
+
+								_last_gnss_reset_accel_clipping_us = _imu_sample_delayed.time_us;
+							}
+						}
 					}
 
 				} else {