EKF: allow initialising without mag depending on configuration

- reduce initial number of required mag and baro samples before init

EKF/ekf.cpp

@@ -158,37 +158,40 @@ bool Ekf::initialiseFilter()
 	// Sum the magnetometer measurements
 	if (_mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) {
 		if (_mag_sample_delayed.time_us != 0) {
-			_mag_counter ++;
-
-			// wait for all bad initial data to be flushed
-			if (_mag_counter <= uint8_t(_obs_buffer_length + 1)) {
+			if (_mag_counter == 0) {
 				_mag_lpf.reset(_mag_sample_delayed.mag);
 
 			} else {
 				_mag_lpf.update(_mag_sample_delayed.mag);
 			}
+
+			_mag_counter++;
 		}
 	}
 
 	// accumulate enough height measurements to be confident in the quality of the data
 	if (_baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed)) {
 		if (_baro_sample_delayed.time_us != 0) {
-			_baro_counter ++;
-
-			// wait for all bad initial data to be flushed
-			if (_baro_counter <= uint8_t(_obs_buffer_length + 1)) {
+			if (_baro_counter == 0) {
 				_baro_hgt_offset = _baro_sample_delayed.hgt;
 
-			} else if (_baro_counter > (uint8_t)(_obs_buffer_length + 1)) {
+			} else {
 				_baro_hgt_offset = 0.9f * _baro_hgt_offset + 0.1f * _baro_sample_delayed.hgt;
 			}
+
+			_baro_counter++;
 		}
 	}
 
-	const bool not_enough_baro_samples_accumulated = _baro_counter <= 2u * _obs_buffer_length;
-	const bool not_enough_mag_samples_accumulated = _mag_counter <= 2u * _obs_buffer_length;
+	if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
+		if (_mag_counter < _obs_buffer_length) {
+			// not enough mag samples accumulated
+			return false;
+		}
+	}
 
-	if (not_enough_baro_samples_accumulated || not_enough_mag_samples_accumulated) {
+	if (_baro_counter < _obs_buffer_length) {
+		// not enough baro samples accumulated
 		return false;
 	}
 
@@ -233,8 +236,8 @@ bool Ekf::initialiseTilt()
 	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 ||
+	if (accel_norm < 0.8f * CONSTANTS_ONE_G ||
+	    accel_norm > 1.2f * CONSTANTS_ONE_G ||
 	    gyro_norm > math::radians(15.0f)) {
 		return false;
 	}
@@ -244,8 +247,7 @@ bool Ekf::initialiseTilt()
 	const float pitch = asinf(gravity_in_body(0));
 	const float roll = atan2f(-gravity_in_body(1), -gravity_in_body(2));
 
-	const Eulerf euler_init(roll, pitch, 0.0f);
-	_state.quat_nominal = Quatf(euler_init);
+	_state.quat_nominal = Quatf{Eulerf{roll, pitch, 0.0f}};
 	_R_to_earth = Dcmf(_state.quat_nominal);
 
 	return true;
@@ -505,7 +507,7 @@ void Ekf::applyCorrectionToVerticalOutputBuffer(float vert_vel_correction)
 * The vel and pos state history are corrected individually so they track the EKF states at
 * the fusion time horizon. This option provides the most accurate tracking of EKF states.
 */
-void Ekf::applyCorrectionToOutputBuffer(const Vector3f& vel_correction, const Vector3f& pos_correction)
+void Ekf::applyCorrectionToOutputBuffer(const Vector3f &vel_correction, const Vector3f &pos_correction)
 {
 	// loop through the output filter state history and apply the corrections to the velocity and position states
 	for (uint8_t index = 0; index < _output_buffer.get_length(); index++) {

EKF/ekf_helper.cpp

@@ -347,9 +347,10 @@ void Ekf::resetHeight()
 // align output filter states to match EKF states at the fusion time horizon
 void Ekf::alignOutputFilter()
 {
-	const outputSample output_delayed = _output_buffer.get_oldest();
+	const outputSample &output_delayed = _output_buffer.get_oldest();
+
 	// calculate the quaternion rotation delta from the EKF to output observer states at the EKF fusion time horizon
-	Quatf q_delta = _state.quat_nominal * output_delayed.quat_nominal.inversed();
+	Quatf q_delta{_state.quat_nominal * output_delayed.quat_nominal.inversed()};
 	q_delta.normalize();
 
 	// calculate the velocity and position deltas between the output and EKF at the EKF fusion time horizon

EKF/estimator_interface.cpp

@@ -50,8 +50,7 @@ void EstimatorInterface::setIMUData(const imuSample &imu_sample)
 {
 	// TODO: resolve misplaced responsibility
 	if (!_initialised) {
-		init(imu_sample.time_us);
-		_initialised = true;
+		_initialised = init(imu_sample.time_us);
 	}
 
 	const float dt = math::constrain((imu_sample.time_us - _time_last_imu) / 1e6f, 1.0e-4f, 0.02f);

test/test_EKF_initialization.cpp

@@ -94,9 +94,9 @@ class EkfInitializationTest : public ::testing::Test {
 		const Vector3f pos = _ekf->getPosition();
 		const Vector3f vel = _ekf->getVelocity();
 
-		EXPECT_TRUE(matrix::isEqual(pos, Vector3f{}, 0.001f))
+		EXPECT_TRUE(matrix::isEqual(pos, Vector3f{}, 0.002f))
 			<< "pos = " << pos(0) << ", " << pos(1) << ", " << pos(2);
-		EXPECT_TRUE(matrix::isEqual(vel, Vector3f{}, 0.002f))
+		EXPECT_TRUE(matrix::isEqual(vel, Vector3f{}, 0.003f))
 			<< "vel = " << vel(0) << ", " << vel(1) << ", " << vel(2);
 	}
 
@@ -105,7 +105,7 @@ class EkfInitializationTest : public ::testing::Test {
 		const Vector3f pos_var = _ekf->getPositionVariance();
 		const Vector3f vel_var = _ekf->getVelocityVariance();
 
-		const float pos_variance_limit = 0.2f;
+		const float pos_variance_limit = 0.1f;
 		EXPECT_TRUE(pos_var(0) > pos_variance_limit) << "pos_var(0)" << pos_var(0);
 		EXPECT_TRUE(pos_var(1) > pos_variance_limit) << "pos_var(1)" << pos_var(1);
 		EXPECT_TRUE(pos_var(2) > pos_variance_limit) << "pos_var(2)" << pos_var(2);