EKF: Don't allow tilt alignment if vehicle is moving excessively (#768)

Also remove unnecessary update of the _accel_lpf after alignment.

EKF/ekf.cpp

@@ -105,9 +105,6 @@ bool Ekf::update()
 
 	// Only run the filter if IMU data in the buffer has been updated
 	if (_imu_updated) {
-		const imuSample &imu_init = _imu_buffer.get_newest();
-		_accel_lpf.update(imu_init.delta_vel);
-
 		// perform state and covariance prediction for the main filter
 		predictState();
 		predictCovariance();
@@ -130,18 +127,21 @@ bool Ekf::update()
 
 bool Ekf::initialiseFilter()
 {
-	// Keep accumulating measurements until we have a minimum of 10 samples for the required sensors
-
 	// Filter accel for tilt initialization
 	const imuSample &imu_init = _imu_buffer.get_newest();
 
-	if(_is_first_imu_sample){
-		_accel_lpf.reset(imu_init.delta_vel);
-		_is_first_imu_sample = false;
+	// protect against zero data
+	if (imu_init.delta_vel_dt < 1e-4f || imu_init.delta_ang_dt < 1e-4f) {
+		return false;
 	}
-	else
-	{
-		_accel_lpf.update(imu_init.delta_vel);
+
+	if (_is_first_imu_sample) {
+		_accel_lpf.reset(imu_init.delta_vel / imu_init.delta_vel_dt);
+		_gyro_lpf.reset(imu_init.delta_ang / imu_init.delta_ang_dt);
+		_is_first_imu_sample = false;
+	} else {
+		_accel_lpf.update(imu_init.delta_vel / imu_init.delta_vel_dt);
+		_gyro_lpf.update(imu_init.delta_ang / imu_init.delta_ang_dt);
 	}
 
 	// Sum the magnetometer measurements
@@ -222,7 +222,11 @@ bool Ekf::initialiseFilter()
 
 bool Ekf::initialiseTilt()
 {
-	if (_accel_lpf.getState().norm() < 0.001f) {
+	const float accel_norm = _accel_lpf.getState().norm();
+	const float gyro_norm = _gyro_lpf.getState().norm();
+	if (accel_norm < 0.9f * CONSTANTS_ONE_G ||
+	    accel_norm > 1.1f * CONSTANTS_ONE_G ||
+	    gyro_norm > math::radians(15.0f)) {
 		return false;
 	}
 

EKF/ekf.h

@@ -445,12 +445,15 @@ class Ekf : public EstimatorInterface
 	uint64_t _last_gps_origin_time_us{0};	///< time the origin was last set (uSec)
 	float _gps_alt_ref{0.0f};		///< WGS-84 height (m)
 
-	// Variables used to initialise the filter states
+	// Variables used by the initial filter alignment
 	bool _is_first_imu_sample{true};
 	uint32_t _baro_counter{0};		///< number of baro samples read during initialisation
 	uint32_t _mag_counter{0};		///< number of magnetometer samples read during initialisation
-	AlphaFilterVector3f _mag_lpf;		///< filtered magnetometer measurement for instant reset(Gauss)
-	AlphaFilterVector3f _accel_lpf;		///< filtered accelerometer measurement for instant reset(Gauss)
+	AlphaFilterVector3f _accel_lpf;		///< filtered accelerometer measurement used to align tilt (m/s/s)
+	AlphaFilterVector3f _gyro_lpf;		///< filtered gyro measurement used for alignment excessive movement check (rad/sec)
+
+	// Variables used to perform in flight resets and switch between height sources
+	AlphaFilterVector3f _mag_lpf;		///< filtered magnetometer measurement for instant reset (Gauss)
 	float _hgt_sensor_offset{0.0f};		///< set as necessary if desired to maintain the same height after a height reset (m)
 	float _baro_hgt_offset{0.0f};		///< baro height reading at the local NED origin (m)