ekf2: initialiseFilter() simplify mag heading init and resetQuatStateYaw

- most of resetQuatStateYaw doesn't apply to initial heading init, so
removing the special case keeps it simple

src/modules/ekf2/EKF/ekf.cpp

@@ -176,13 +176,6 @@ bool Ekf::initialiseFilter()
 		}
 	}
 
-	if (_params.mag_fusion_type <= MagFuseType::MAG_3D) {
-		if (_mag_counter < _obs_buffer_length) {
-			// not enough mag samples accumulated
-			return false;
-		}
-	}
-
 	if (_baro_counter < _obs_buffer_length) {
 		// not enough baro samples accumulated
 		return false;
@@ -198,18 +191,25 @@ bool Ekf::initialiseFilter()
 	// calculate the initial magnetic field and yaw alignment
 	// but do not mark the yaw alignement complete as it needs to be
 	// reset once the leveling phase is done
-	if ((_params.mag_fusion_type <= MagFuseType::MAG_3D) && (_mag_counter != 0)) {
+	if (_params.mag_fusion_type <= MagFuseType::MAG_3D) {
-		// rotate the magnetometer measurements into earth frame using a zero yaw angle
+		if (_mag_counter > 1) {
-		// the angle of the projection onto the horizontal gives the yaw angle
+			// rotate the magnetometer measurements into earth frame using a zero yaw angle
-		const Vector3f mag_earth_pred = updateYawInRotMat(0.f, _R_to_earth) * _mag_lpf.getState();
+			// the angle of the projection onto the horizontal gives the yaw angle
-		float yaw_new = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
+			const Vector3f mag_earth_pred = updateYawInRotMat(0.f, _R_to_earth) * _mag_lpf.getState();
+			float yaw_new = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
 
-		// update quaternion states and corresponding covarainces
+			// update the rotation matrix using the new yaw value
-		resetQuatStateYaw(yaw_new, 0.f, false);
+			_R_to_earth = updateYawInRotMat(yaw_new, Dcmf(_state.quat_nominal));
+			_state.quat_nominal = _R_to_earth;
 
-		// set the earth magnetic field states using the updated rotation
+			// set the earth magnetic field states using the updated rotation
-		_state.mag_I = _R_to_earth * _mag_lpf.getState();
+			_state.mag_I = _R_to_earth * _mag_lpf.getState();
-		_state.mag_B.zero();
+			_state.mag_B.zero();
+
+		} else {
+			// not enough mag samples accumulated
+			return false;
+		}
 	}
 
 	// initialise the state covariance matrix now we have starting values for all the states

src/modules/ekf2/EKF/ekf.h

@@ -1071,8 +1071,7 @@ private:
 	// reset the quaternion states and covariances to the new yaw value, preserving the roll and pitch
 	// yaw : Euler yaw angle (rad)
 	// yaw_variance : yaw error variance (rad^2)
-	// update_buffer : true if the state change should be also applied to the output observer buffer
+	void resetQuatStateYaw(float yaw, float yaw_variance);
-	void resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer = true);
 
 	// Declarations used to control use of the EKF-GSF yaw estimator
 

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -370,7 +370,7 @@ bool Ekf::realignYawGPS(const Vector3f &mag)
 		const float yaw_variance_new = sq(asinf(sineYawError));
 
 		// Apply updated yaw and yaw variance to states and covariances
-		resetQuatStateYaw(yaw_new, yaw_variance_new, true);
+		resetQuatStateYaw(yaw_new, yaw_variance_new);
 
 		// Use the last magnetometer measurements to reset the field states
 		_state.mag_B.zero();
@@ -458,7 +458,7 @@ bool Ekf::resetYawToEv()
 	const float yaw_new = getEulerYaw(_ev_sample_delayed.quat);
 	const float yaw_new_variance = fmaxf(_ev_sample_delayed.angVar, sq(1.0e-2f));
 
-	resetQuatStateYaw(yaw_new, yaw_new_variance, true);
+	resetQuatStateYaw(yaw_new, yaw_new_variance);
 	_R_ev_to_ekf.setIdentity();
 
 	return true;
@@ -1567,7 +1567,7 @@ void Ekf::stopFlowFusion()
 	}
 }
 
-void Ekf::resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer)
+void Ekf::resetQuatStateYaw(float yaw, float yaw_variance)
 {
 	// save a copy of the quaternion state for later use in calculating the amount of reset change
 	const Quatf quat_before_reset = _state.quat_nominal;
@@ -1593,16 +1593,14 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer)
 	}
 
 	// add the reset amount to the output observer buffered data
-	if (update_buffer) {
+	for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
-		for (uint8_t i = 0; i < _output_buffer.get_length(); i++) {
+		_output_buffer[i].quat_nominal = _state_reset_status.quat_change * _output_buffer[i].quat_nominal;
-			_output_buffer[i].quat_nominal = _state_reset_status.quat_change * _output_buffer[i].quat_nominal;
-		}
-
-		// apply the change in attitude quaternion to our newest quaternion estimate
-		// which was already taken out from the output buffer
-		_output_new.quat_nominal = _state_reset_status.quat_change * _output_new.quat_nominal;
 	}
 
+	// apply the change in attitude quaternion to our newest quaternion estimate
+	// which was already taken out from the output buffer
+	_output_new.quat_nominal = _state_reset_status.quat_change * _output_new.quat_nominal;
+
 	_last_static_yaw = NAN;
 
 	// capture the reset event
@@ -1618,7 +1616,7 @@ bool Ekf::resetYawToEKFGSF()
 		return false;
 	}
 
-	resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar(), true);
+	resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar());
 
 	// record a magnetic field alignment event to prevent possibility of the EKF trying to reset the yaw to the mag later in flight
 	_flt_mag_align_start_time = _imu_sample_delayed.time_us;

src/modules/ekf2/EKF/gps_yaw_fusion.cpp

@@ -216,7 +216,7 @@ bool Ekf::resetYawToGps()
 	const float measured_yaw = _gps_sample_delayed.yaw;
 
 	const float yaw_variance = sq(fmaxf(_params.gps_heading_noise, 1.e-2f));
-	resetQuatStateYaw(measured_yaw, yaw_variance, true);
+	resetQuatStateYaw(measured_yaw, yaw_variance);
 
 	_aid_src_gnss_yaw.time_last_fuse = _imu_sample_delayed.time_us;
 	_gnss_yaw_signed_test_ratio_lpf.reset(0.f);