ekf2: refactor mag control

- remove class _mag_sample_delayed
 - update mag fusion call graph to use mag sample delayed functionally
 - Ekf::resetMagHeading()
   - use low pass mag directly, but check if valid (magnitude)
   - MAG_FUSE_TYPE_INDOOR treat like auto if heading required

src/modules/ekf2/EKF/control.cpp

@@ -110,31 +110,6 @@ void Ekf::controlFusionModes()
 		_gps_data_ready = _gps_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed);
 	}
 
-
-	if (_mag_buffer) {
-		_mag_data_ready = _mag_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed);
-
-		if (_mag_data_ready) {
-			_mag_lpf.update(_mag_sample_delayed.mag);
-
-			// if enabled, use knowledge of theoretical magnetic field vector to calculate a synthetic magnetomter Z component value.
-			// this is useful if there is a lot of interference on the sensor measurement.
-			if (_params.synthesize_mag_z && (_params.mag_declination_source & MASK_USE_GEO_DECL)
-			    && (_NED_origin_initialised || PX4_ISFINITE(_mag_declination_gps))
-			   ) {
-
-				const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps)) * Vector3f(_mag_strength_gps, 0, 0);
-				_mag_sample_delayed.mag(2) = calculate_synthetic_mag_z_measurement(_mag_sample_delayed.mag, mag_earth_pred);
-				_control_status.flags.synthetic_mag_z = true;
-
-			} else {
-				_control_status.flags.synthetic_mag_z = false;
-			}
-		}
-
-
-	}
-
 	if (_range_buffer) {
 		// Get range data from buffer and check validity
 		bool is_rng_data_ready = _range_buffer->pop_first_older_than(_imu_sample_delayed.time_us, _range_sensor.getSampleAddress());
@@ -371,7 +346,14 @@ void Ekf::controlExternalVisionFusion()
 				resetYawToEv();
 			}
 
-			fuseHeading();
+			if (shouldUse321RotationSequence(_R_to_earth)) {
+				float measured_hdg = getEuler321Yaw(_ev_sample_delayed.quat);
+				fuseYaw321(measured_hdg, _ev_sample_delayed.angVar);
+
+			} else {
+				float measured_hdg = getEuler312Yaw(_ev_sample_delayed.quat);
+				fuseYaw312(measured_hdg, _ev_sample_delayed.angVar);
+			}
 		}
 
 		// record observation and estimate for use next time

src/modules/ekf2/EKF/ekf.cpp

@@ -143,16 +143,20 @@ bool Ekf::initialiseFilter()
 	}
 
 	// Sum the magnetometer measurements
-	if (_mag_buffer && _mag_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed)) {
+	if (_mag_buffer) {
-		if (_mag_sample_delayed.time_us != 0) {
+		magSample mag_sample;
-			if (_mag_counter == 0) {
-				_mag_lpf.reset(_mag_sample_delayed.mag);
 
-			} else {
+		if (_mag_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &mag_sample)) {
-				_mag_lpf.update(_mag_sample_delayed.mag);
+			if (mag_sample.time_us != 0) {
-			}
+				if (_mag_counter == 0) {
+					_mag_lpf.reset(mag_sample.mag);
 
-			_mag_counter++;
+				} else {
+					_mag_lpf.update(mag_sample.mag);
+				}
+
+				_mag_counter++;
+			}
 		}
 	}
 
@@ -192,7 +196,7 @@ 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
-	resetMagHeading(_mag_lpf.getState(), false, false);
+	resetMagHeading(false, false);
 
 	// initialise the state covariance matrix now we have starting values for all the states
 	initialiseCovariance();

src/modules/ekf2/EKF/ekf.h

@@ -364,7 +364,6 @@ private:
 
 	// booleans true when fresh sensor data is available at the fusion time horizon
 	bool _gps_data_ready{false};	///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused
-	bool _mag_data_ready{false};	///< true when new magnetometer data has fallen behind the fusion time horizon and is available to be fused
 	bool _baro_data_ready{false};	///< true when new baro height data has fallen behind the fusion time horizon and is available to be fused
 	bool _flow_data_ready{false};	///< true when the leading edge of the optical flow integration period has fallen behind the fusion time horizon
 	bool _ev_data_ready{false};	///< true when new external vision system data has fallen behind the fusion time horizon and is available to be fused
@@ -577,22 +576,22 @@ private:
 	void predictCovariance();
 
 	// ekf sequential fusion of magnetometer measurements
-	void fuseMag();
+	void fuseMag(const Vector3f &mag);
 
 	// fuse the first euler angle from either a 321 or 312 rotation sequence as the observation (currently measures yaw using the magnetometer)
-	void fuseHeading();
+	void fuseHeading(float measured_hdg = NAN, float obs_var = NAN);
 
 	// fuse the yaw angle defined as the first rotation in a 321 Tait-Bryan rotation sequence
 	// yaw : angle observation defined as the first rotation in a 321 Tait-Bryan rotation sequence (rad)
 	// yaw_variance : variance of the yaw angle observation (rad^2)
 	// zero_innovation : Fuse data with innovation set to zero
-	void fuseYaw321(const float yaw, const float yaw_variance, bool zero_innovation);
+	void fuseYaw321(const float yaw, const float yaw_variance, bool zero_innovation = false);
 
 	// fuse the yaw angle defined as the first rotation in a 312 Tait-Bryan rotation sequence
 	// yaw : angle observation defined as the first rotation in a 312 Tait-Bryan rotation sequence (rad)
 	// yaw_variance : variance of the yaw angle observation (rad^2)
 	// zero_innovation : Fuse data with innovation set to zero
-	void fuseYaw312(const float yaw, const float yaw_variance, bool zero_innovation);
+	void fuseYaw312(const float yaw, const float yaw_variance, bool zero_innovation = false);
 
 	// update quaternion states and covariances using an innovation, observation variance and Jacobian vector
 	// innovation : prediction - measurement
@@ -700,7 +699,7 @@ private:
 
 	// reset the heading and magnetic field states using the declination and magnetometer measurements
 	// return true if successful
-	bool resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var = true, bool update_buffer = true);
+	bool resetMagHeading(bool increase_yaw_var = true, bool update_buffer = true);
 
 	// reset the heading using the external vision measurements
 	// return true if successful
@@ -708,7 +707,7 @@ private:
 
 	// Do a forced re-alignment of the yaw angle to align with the horizontal velocity vector from the GPS.
 	// It is used to align the yaw angle after launch or takeoff for fixed wing vehicle.
-	bool realignYawGPS();
+	bool realignYawGPS(const Vector3f &mag);
 
 	// Return the magnetic declination in radians to be used by the alignment and fusion processing
 	float getMagDeclination();
@@ -839,7 +838,7 @@ private:
 	void runOnGroundYawReset();
 	bool isYawResetAuthorized() const { return !_is_yaw_fusion_inhibited; }
 	bool canResetMagHeading() const;
-	void runInAirYawReset();
+	void runInAirYawReset(const Vector3f &mag);
 	bool canRealignYawUsingGps() const { return _control_status.flags.fixed_wing; }
 	void runVelPosReset();
 
@@ -854,11 +853,11 @@ private:
 	void checkMagDeclRequired();
 	void checkMagInhibition();
 	bool shouldInhibitMag() const;
-	void checkMagFieldStrength();
+	void checkMagFieldStrength(const Vector3f &mag);
 	bool isStrongMagneticDisturbance() const { return _control_status.flags.mag_field_disturbed; }
 	static bool isMeasuredMatchingExpected(float measured, float expected, float gate);
-	void runMagAndMagDeclFusions();
+	void runMagAndMagDeclFusions(const Vector3f &mag);
-	void run3DMagAndDeclFusions();
+	void run3DMagAndDeclFusions(const Vector3f &mag);
 
 	// control fusion of range finder observations
 	void controlRangeFinderFusion();

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -376,7 +376,7 @@ void Ekf::alignOutputFilter()
 
 // Do a forced re-alignment of the yaw angle to align with the horizontal velocity vector from the GPS.
 // It is used to align the yaw angle after launch or takeoff for fixed wing vehicle only.
-bool Ekf::realignYawGPS()
+bool Ekf::realignYawGPS(const Vector3f &mag)
 {
 	const float gpsSpeed = sqrtf(sq(_gps_sample_delayed.vel(0)) + sq(_gps_sample_delayed.vel(1)));
 
@@ -386,7 +386,7 @@ bool Ekf::realignYawGPS()
 
 	if (!gps_yaw_alignment_possible) {
 		// attempt a normal alignment using the magnetometer
-		return resetMagHeading(_mag_lpf.getState());
+		return resetMagHeading();
 	}
 
 	// check for excessive horizontal GPS velocity innovations
@@ -455,7 +455,7 @@ bool Ekf::realignYawGPS()
 		// Use the last magnetometer measurements to reset the field states
 		_state.mag_B.zero();
 		_R_to_earth = Dcmf(_state.quat_nominal);
-		_state.mag_I = _R_to_earth * _mag_sample_delayed.mag;
+		_state.mag_I = _R_to_earth * mag;
 
 		resetMagCov();
 
@@ -471,7 +471,7 @@ bool Ekf::realignYawGPS()
 		// align mag states only
 
 		// calculate initial earth magnetic field states
-		_state.mag_I = _R_to_earth * _mag_sample_delayed.mag;
+		_state.mag_I = _R_to_earth * mag;
 
 		resetMagCov();
 
@@ -483,18 +483,28 @@ bool Ekf::realignYawGPS()
 }
 
 // Reset heading and magnetic field states
-bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool update_buffer)
+bool Ekf::resetMagHeading(bool increase_yaw_var, bool update_buffer)
 {
 	// prevent a reset being performed more than once on the same frame
 	if (_imu_sample_delayed.time_us == _flt_mag_align_start_time) {
 		return true;
 	}
 
+	// low pass filtered mag required
+	if (_mag_counter == 0) {
+		return false;
+	}
+
+	const Vector3f mag_init = _mag_lpf.getState();
+
 	// calculate the observed yaw angle and yaw variance
 	float yaw_new;
 	float yaw_new_variance = 0.0f;
 
-	if (_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) {
+	const bool heading_required_for_navigation = _control_status.flags.gps || _control_status.flags.ev_pos;
+
+	if ((_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) || ((_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR) && heading_required_for_navigation)) {
+
 		// rotate the magnetometer measurements into earth frame using a zero yaw angle
 		const Dcmf R_to_earth = updateYawInRotMat(0.f, _R_to_earth);
 

src/modules/ekf2/EKF/estimator_interface.h

@@ -299,7 +299,6 @@ protected:
 	imuSample _imu_sample_delayed{};	// captures the imu sample on the delayed time horizon
 
 	// measurement samples capturing measurements on the delayed time horizon
-	magSample _mag_sample_delayed{};
 	baroSample _baro_sample_delayed{};
 	gpsSample _gps_sample_delayed{};
 	sensor::SensorRangeFinder _range_sensor{};

src/modules/ekf2/EKF/gps_control.cpp

@@ -70,7 +70,7 @@ void Ekf::controlGpsFusion()
 
 					fuseGpsVelPos();
 
-					if (shouldResetGpsFusion()){
+					if (shouldResetGpsFusion()) {
 						const bool was_gps_signal_lost = isTimedOut(_time_prev_gps_us, 1000000);
 
 						/* A reset is not performed when getting GPS back after a significant period of no data
@@ -88,7 +88,7 @@ void Ekf::controlGpsFusion()
 							// use GPS velocity data to check and correct yaw angle if a FW vehicle
 							if (_control_status.flags.fixed_wing && _control_status.flags.in_air) {
 								// if flying a fixed wing aircraft, do a complete reset that includes yaw
-								_control_status.flags.mag_aligned_in_flight = realignYawGPS();
+								_mag_yaw_reset_req = true;
 							}
 
 							_warning_events.flags.gps_fusion_timout = true;
@@ -131,8 +131,7 @@ void Ekf::controlGpsFusion()
 					startGpsFusion();
 				}
 
-			} else if(!_control_status.flags.yaw_align
+			} else if (!_control_status.flags.yaw_align && (_params.mag_fusion_type == MAG_FUSE_TYPE_NONE)) {
-		                  && (_params.mag_fusion_type == MAG_FUSE_TYPE_NONE)) {
 				// If no mag is used, align using the yaw estimator
 				_do_ekfgsf_yaw_reset = true;
 			}
@@ -208,7 +207,7 @@ void Ekf::processYawEstimatorResetRequest()
 	 * to improve its estimate if the previous reset was not successful.
 	 */
 	if (_do_ekfgsf_yaw_reset
-	    && isTimedOut(_ekfgsf_yaw_reset_time, 5000000)){
+	    && isTimedOut(_ekfgsf_yaw_reset_time, 5000000)) {
 		if (resetYawToEKFGSF()) {
 			_ekfgsf_yaw_reset_time = _time_last_imu;
 			_time_last_hor_pos_fuse = _time_last_imu;

src/modules/ekf2/EKF/mag_control.cpp

@@ -41,7 +41,35 @@
 
 void Ekf::controlMagFusion()
 {
-	checkMagFieldStrength();
+	bool mag_data_ready = false;
+
+	magSample mag_sample;
+
+	if (_mag_buffer) {
+		mag_data_ready = _mag_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &mag_sample);
+
+		if (mag_data_ready) {
+			_mag_lpf.update(mag_sample.mag);
+			_mag_counter++;
+
+			// if enabled, use knowledge of theoretical magnetic field vector to calculate a synthetic magnetomter Z component value.
+			// this is useful if there is a lot of interference on the sensor measurement.
+			if (_params.synthesize_mag_z && (_params.mag_declination_source & MASK_USE_GEO_DECL)
+			    && (_NED_origin_initialised || PX4_ISFINITE(_mag_declination_gps))
+			   ) {
+				const Vector3f mag_earth_pred = Dcmf(Eulerf(0, -_mag_inclination_gps, _mag_declination_gps)) * Vector3f(_mag_strength_gps, 0, 0);
+				mag_sample.mag(2) = calculate_synthetic_mag_z_measurement(mag_sample.mag, mag_earth_pred);
+				_control_status.flags.synthetic_mag_z = true;
+
+			} else {
+				_control_status.flags.synthetic_mag_z = false;
+			}
+		}
+	}
+
+	if (mag_data_ready) {
+		checkMagFieldStrength(mag_sample.mag);
+	}
 
 	// If we are on ground, reset the flight alignment flag so that the mag fields will be
 	// re-initialised next time we achieve flight altitude
@@ -76,7 +104,7 @@ void Ekf::controlMagFusion()
 	_mag_yaw_reset_req |= !_control_status.flags.yaw_align;
 	_mag_yaw_reset_req |= _mag_inhibit_yaw_reset_req;
 
-	if (noOtherYawAidingThanMag() && _mag_data_ready) {
+	if (noOtherYawAidingThanMag() && mag_data_ready) {
 		// Determine if we should use simple magnetic heading fusion which works better when
 		// there are large external disturbances or the more accurate 3-axis fusion
 		switch (_params.mag_fusion_type) {
@@ -101,7 +129,7 @@ void Ekf::controlMagFusion()
 
 		if (_control_status.flags.in_air) {
 			checkHaglYawResetReq();
-			runInAirYawReset();
+			runInAirYawReset(mag_sample.mag);
 			runVelPosReset(); // TODO: review this; a vel/pos reset can be requested from COG reset (for fixedwing) only
 
 		} else {
@@ -116,7 +144,7 @@ void Ekf::controlMagFusion()
 		checkMagDeclRequired();
 		checkMagInhibition();
 
-		runMagAndMagDeclFusions();
+		runMagAndMagDeclFusions(mag_sample.mag);
 	}
 }
 
@@ -142,9 +170,7 @@ void Ekf::checkHaglYawResetReq()
 void Ekf::runOnGroundYawReset()
 {
 	if (_mag_yaw_reset_req && isYawResetAuthorized()) {
-		const bool has_realigned_yaw = canResetMagHeading()
+		const bool has_realigned_yaw = canResetMagHeading() ? resetMagHeading() : false;
-					       ? resetMagHeading(_mag_lpf.getState())
-					       : false;
 
 		if (has_realigned_yaw) {
 			_mag_yaw_reset_req = false;
@@ -166,16 +192,16 @@ bool Ekf::canResetMagHeading() const
 	return !isStrongMagneticDisturbance() && (_params.mag_fusion_type != MAG_FUSE_TYPE_NONE);
 }
 
-void Ekf::runInAirYawReset()
+void Ekf::runInAirYawReset(const Vector3f &mag_sample)
 {
 	if (_mag_yaw_reset_req && isYawResetAuthorized()) {
 		bool has_realigned_yaw = false;
 
 		if (canRealignYawUsingGps()) {
-			has_realigned_yaw = realignYawGPS();
+			has_realigned_yaw = realignYawGPS(mag_sample);
 
 		} else if (canResetMagHeading()) {
-			has_realigned_yaw = resetMagHeading(_mag_lpf.getState());
+			has_realigned_yaw = resetMagHeading();
 		}
 
 		if (has_realigned_yaw) {
@@ -300,25 +326,23 @@ bool Ekf::shouldInhibitMag() const
 	       || isStrongMagneticDisturbance();
 }
 
-void Ekf::checkMagFieldStrength()
+void Ekf::checkMagFieldStrength(const Vector3f &mag_sample)
 {
-	if (_mag_data_ready) {
+	if (_params.check_mag_strength
-		if (_params.check_mag_strength
+	    && ((_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) || (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR && _control_status.flags.gps))) {
-		    && ((_params.mag_fusion_type <= MAG_FUSE_TYPE_3D) || (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR && _control_status.flags.gps))) {
 
-			if (PX4_ISFINITE(_mag_strength_gps)) {
+		if (PX4_ISFINITE(_mag_strength_gps)) {
-				constexpr float wmm_gate_size = 0.2f; // +/- Gauss
+			constexpr float wmm_gate_size = 0.2f; // +/- Gauss
-				_control_status.flags.mag_field_disturbed = !isMeasuredMatchingExpected(_mag_sample_delayed.mag.length(), _mag_strength_gps, wmm_gate_size);
+			_control_status.flags.mag_field_disturbed = !isMeasuredMatchingExpected(mag_sample.length(), _mag_strength_gps, wmm_gate_size);
-
-			} else {
-				constexpr float average_earth_mag_field_strength = 0.45f; // Gauss
-				constexpr float average_earth_mag_gate_size = 0.40f; // +/- Gauss
-				_control_status.flags.mag_field_disturbed = !isMeasuredMatchingExpected(_mag_sample_delayed.mag.length(), average_earth_mag_field_strength, average_earth_mag_gate_size);
-			}
 
 		} else {
-			_control_status.flags.mag_field_disturbed = false;
+			constexpr float average_earth_mag_field_strength = 0.45f; // Gauss
+			constexpr float average_earth_mag_gate_size = 0.40f; // +/- Gauss
+			_control_status.flags.mag_field_disturbed = !isMeasuredMatchingExpected(mag_sample.length(), average_earth_mag_field_strength, average_earth_mag_gate_size);
 		}
+
+	} else {
+		_control_status.flags.mag_field_disturbed = false;
 	}
 }
 
@@ -328,17 +352,25 @@ bool Ekf::isMeasuredMatchingExpected(const float measured, const float expected,
 	       && (measured <= expected + gate);
 }
 
-void Ekf::runMagAndMagDeclFusions()
+void Ekf::runMagAndMagDeclFusions(const Vector3f &mag)
 {
 	if (_control_status.flags.mag_3D) {
-		run3DMagAndDeclFusions();
+		run3DMagAndDeclFusions(mag);
 
 	} else if (_control_status.flags.mag_hdg) {
-		fuseHeading();
+		// Rotate the measurements into earth frame using the zero yaw angle
+		Dcmf R_to_earth = shouldUse321RotationSequence(_R_to_earth) ? updateEuler321YawInRotMat(0.f, _R_to_earth) : updateEuler312YawInRotMat(0.f, _R_to_earth);
+
+		Vector3f mag_earth_pred = R_to_earth * (mag - _state.mag_B);
+
+		// the angle of the projection onto the horizontal gives the yaw angle
+		float measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
+
+		fuseHeading(measured_hdg, sq(_params.mag_heading_noise));
 	}
 }
 
-void Ekf::run3DMagAndDeclFusions()
+void Ekf::run3DMagAndDeclFusions(const Vector3f &mag)
 {
 	if (!_mag_decl_cov_reset) {
 		// After any magnetic field covariance reset event the earth field state
@@ -347,13 +379,13 @@ void Ekf::run3DMagAndDeclFusions()
 		// states for the first few observations.
 		fuseDeclination(0.02f);
 		_mag_decl_cov_reset = true;
-		fuseMag();
+		fuseMag(mag);
 
 	} else {
 		// The normal sequence is to fuse the magnetometer data first before fusing
 		// declination angle at a higher uncertainty to allow some learning of
 		// declination angle over time.
-		fuseMag();
+		fuseMag(mag);
 
 		if (_control_status.flags.mag_dec) {
 			fuseDeclination(0.5f);

src/modules/ekf2/EKF/mag_fusion.cpp

@@ -45,7 +45,7 @@
 
 #include <mathlib/mathlib.h>
 
-void Ekf::fuseMag()
+void Ekf::fuseMag(const Vector3f &mag)
 {
 	// assign intermediate variables
 	const float &q0 = _state.quat_nominal(0);
@@ -161,7 +161,7 @@ void Ekf::fuseMag()
 	const Vector3f mag_I_rot = R_to_body * _state.mag_I;
 
 	// compute magnetometer innovations
-	_mag_innov = mag_I_rot + _state.mag_B - _mag_sample_delayed.mag;
+	_mag_innov = mag_I_rot + _state.mag_B - mag;
 
 	// do not use the synthesized measurement for the magnetomter Z component for 3D fusion
 	if (_control_status.flags.synthetic_mag_z) {
@@ -714,149 +714,62 @@ void Ekf::updateQuaternion(const float innovation, const float variance, const f
 	}
 }
 
-void Ekf::fuseHeading()
+void Ekf::fuseHeading(float measured_hdg, float obs_var)
 {
-	Vector3f mag_earth_pred;
+	// observation variance
-	float measured_hdg;
+	float R_YAW = PX4_ISFINITE(obs_var) ? obs_var : 0.01f;
-
-	// Calculate the observation variance
-	float R_YAW;
-
-	if (_control_status.flags.mag_hdg) {
-		// using magnetic heading tuning parameter
-		R_YAW = sq(_params.mag_heading_noise);
-
-	} else if (_control_status.flags.ev_yaw) {
-		// using error estimate from external vision data
-		R_YAW = _ev_sample_delayed.angVar;
-
-	} else {
-		// default value
-		R_YAW = 0.01f;
-	}
 
 	// update transformation matrix from body to world frame using the current state estimate
 	_R_to_earth = Dcmf(_state.quat_nominal);
 
-	if (shouldUse321RotationSequence(_R_to_earth)) {
+	const bool use_321_rotation_seq = shouldUse321RotationSequence(_R_to_earth);
-		const float predicted_hdg = getEuler321Yaw(_R_to_earth);
-
-		if (_control_status.flags.mag_hdg) {
-			// Rotate the measurements into earth frame using the zero yaw angle
-			const Dcmf R_to_earth = updateEuler321YawInRotMat(0.f, _R_to_earth);
-			mag_earth_pred = R_to_earth * (_mag_sample_delayed.mag - _state.mag_B);
-
-			// the angle of the projection onto the horizontal gives the yaw angle
-			measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
-
-		} else if (_control_status.flags.ev_yaw) {
-			measured_hdg = getEuler321Yaw(_ev_sample_delayed.quat);
-
-		} else {
-			measured_hdg = predicted_hdg;
-		}
-
-		// handle special case where yaw measurement is unavailable
-		bool fuse_zero_innov = false;
-
-		if (_is_yaw_fusion_inhibited) {
-			// The yaw measurement cannot be trusted but we need to fuse something to prevent a badly
-			// conditioned covariance matrix developing over time.
-			if (!_control_status.flags.vehicle_at_rest) {
-				// Vehicle is not at rest so fuse a zero innovation if necessary to prevent
-				// unconstrained quaternion variance growth and record the predicted heading
-				// to use as an observation when movement ceases.
-				// TODO a better way of determining when this is necessary
-				const float sumQuatVar = P(0, 0) + P(1, 1) + P(2, 2) + P(3, 3);
 
-				if (sumQuatVar > _params.quat_max_variance) {
+	const float predicted_hdg = use_321_rotation_seq ? getEuler321Yaw(_R_to_earth) : getEuler312Yaw(_R_to_earth);
-					fuse_zero_innov = true;
-					R_YAW = 0.25f;
 
-				}
+	if (!PX4_ISFINITE(measured_hdg)) {
-
+		measured_hdg = predicted_hdg;
-				_last_static_yaw = predicted_hdg;
+	}
-
-			} else {
-				// Vehicle is at rest so use the last moving prediction as an observation
-				// to prevent the heading from drifting and to enable yaw gyro bias learning
-				// before takeoff.
 
-				if (!PX4_ISFINITE(_last_static_yaw)) {
+	// handle special case where yaw measurement is unavailable
-					_last_static_yaw = predicted_hdg;
+	bool fuse_zero_innov = false;
-				}
 
-				measured_hdg = _last_static_yaw;
+	if (_is_yaw_fusion_inhibited) {
+		// The yaw measurement cannot be trusted but we need to fuse something to prevent a badly
+		// conditioned covariance matrix developing over time.
+		if (!_control_status.flags.vehicle_at_rest) {
+			// Vehicle is not at rest so fuse a zero innovation if necessary to prevent
+			// unconstrained quaternion variance growth and record the predicted heading
+			// to use as an observation when movement ceases.
+			// TODO a better way of determining when this is necessary
+			const float sumQuatVar = P(0, 0) + P(1, 1) + P(2, 2) + P(3, 3);
 
+			if (sumQuatVar > _params.quat_max_variance) {
+				fuse_zero_innov = true;
+				R_YAW = 0.25f;
 			}
 
-		} else {
 			_last_static_yaw = predicted_hdg;
 
-		}
-
-		fuseYaw321(measured_hdg, R_YAW, fuse_zero_innov);
-
-	} else {
-		const float predicted_hdg = getEuler312Yaw(_R_to_earth);
-
-		if (_control_status.flags.mag_hdg) {
-
-			// rotate the magnetometer measurements into earth frame using a zero yaw angle
-			const Dcmf R_to_earth = updateEuler312YawInRotMat(0.f, _R_to_earth);
-			mag_earth_pred = R_to_earth * (_mag_sample_delayed.mag - _state.mag_B);
-
-			// the angle of the projection onto the horizontal gives the yaw angle
-			measured_hdg = -atan2f(mag_earth_pred(1), mag_earth_pred(0)) + getMagDeclination();
-
-		} else if (_control_status.flags.ev_yaw) {
-			measured_hdg = getEuler312Yaw(_ev_sample_delayed.quat);
-
 		} else {
-			measured_hdg = predicted_hdg;
+			// Vehicle is at rest so use the last moving prediction as an observation
-		}
+			// to prevent the heading from drifting and to enable yaw gyro bias learning
-
+			// before takeoff.
-		// handle special case where yaw measurement is unavailable
+			if (!PX4_ISFINITE(_last_static_yaw)) {
-		bool fuse_zero_innov = false;
-
-		if (_is_yaw_fusion_inhibited) {
-			// The yaw measurement cannot be trusted but we need to fuse something to prevent a badly
-			// conditioned covariance matrix developing over time.
-			if (!_control_status.flags.vehicle_at_rest) {
-				// Vehicle is not at rest so fuse a zero innovation if necessary to prevent
-				// unconstrained quaterniion variance growth and record the predicted heading
-				// to use as an observation when movement ceases.
-				// TODO a better way of determining when this is necessary
-				const float sumQuatVar = P(0, 0) + P(1, 1) + P(2, 2) + P(3, 3);
-
-				if (sumQuatVar > _params.quat_max_variance) {
-					fuse_zero_innov = true;
-					R_YAW = 0.25f;
-
-				}
-
 				_last_static_yaw = predicted_hdg;
-
-			} else {
-				// Vehicle is at rest so use the last moving prediction as an observation
-				// to prevent the heading from drifting and to enable yaw gyro bias learning
-				// before takeoff.
-
-				if (!PX4_ISFINITE(_last_static_yaw)) {
-					_last_static_yaw = predicted_hdg;
-				}
-
-				measured_hdg = _last_static_yaw;
-
 			}
 
-		} else {
+			measured_hdg = _last_static_yaw;
-			_last_static_yaw = predicted_hdg;
-
 		}
 
-		fuseYaw312(measured_hdg, R_YAW, fuse_zero_innov);
+	} else {
+		_last_static_yaw = predicted_hdg;
+	}
+
+	if (use_321_rotation_seq) {
+		fuseYaw321(measured_hdg, R_YAW, fuse_zero_innov);
 
+	} else {
+		fuseYaw312(measured_hdg, R_YAW, fuse_zero_innov);
 	}
 }