ekf2: utils add getEulerYaw() that uses the best rotation sequence

src/modules/ekf2/EKF/EKFGSF_yaw.cpp

@@ -245,10 +245,7 @@ void EKFGSF_yaw::predictEKF(const uint8_t model_index)
 	}
 
 	// Calculate the yaw state using a projection onto the horizontal that avoids gimbal lock
-	const Dcmf &R = _ahrs_ekf_gsf[model_index].R;
+	_ekf_gsf[model_index].X(2) = getEulerYaw(_ahrs_ekf_gsf[model_index].R);
-	_ekf_gsf[model_index].X(2) = shouldUse321RotationSequence(R) ?
-				     getEuler321Yaw(R) :
-				     getEuler312Yaw(R);
 
 	// calculate delta velocity in a horizontal front-right frame
 	const Vector3f del_vel_NED = _ahrs_ekf_gsf[model_index].R * _delta_vel;

src/modules/ekf2/EKF/airspeed_fusion.cpp

@@ -187,9 +187,7 @@ void Ekf::resetWind()
 */
 void Ekf::resetWindUsingAirspeed()
 {
-	const float euler_yaw = shouldUse321RotationSequence(_R_to_earth)
+	const float euler_yaw = getEulerYaw(_R_to_earth);
-				? getEuler321Yaw(_state.quat_nominal)
-				: getEuler312Yaw(_state.quat_nominal);
 
 	// estimate wind using zero sideslip assumption and airspeed measurement if airspeed available
 	_state.wind_vel(0) = _state.vel(0) - _airspeed_sample_delayed.true_airspeed * cosf(euler_yaw);

src/modules/ekf2/EKF/covariance.cpp

@@ -1137,7 +1137,7 @@ void Ekf::resetWindCovarianceUsingAirspeed()
 {
 	// Derived using EKF/matlab/scripts/Inertial Nav EKF/wind_cov.py
 	// TODO: explicitly include the sideslip angle in the derivation
-	const float euler_yaw = getEuler321Yaw(_state.quat_nominal);
+	const float euler_yaw = getEulerYaw(_R_to_earth);
 	const float R_TAS = sq(math::constrain(_params.eas_noise, 0.5f, 5.0f) * math::constrain(_airspeed_sample_delayed.eas2tas, 0.9f, 10.0f));
 	constexpr float initial_sideslip_uncertainty = math::radians(15.0f);
 	const float initial_wind_var_body_y = sq(_airspeed_sample_delayed.true_airspeed * sinf(initial_sideslip_uncertainty));

src/modules/ekf2/EKF/ekf.cpp

@@ -261,7 +261,6 @@ void Ekf::predictState()
 
 	// rotate the previous quaternion by the delta quaternion using a quaternion multiplication
 	_state.quat_nominal = (_state.quat_nominal * dq).normalized();
-
 	_R_to_earth = Dcmf(_state.quat_nominal);
 
 	// Calculate an earth frame delta velocity

src/modules/ekf2/EKF/ekf_helper.cpp

@@ -428,7 +428,7 @@ bool Ekf::realignYawGPS(const Vector3f &mag)
 		if (!_control_status.flags.mag_aligned_in_flight) {
 			// This is our first flight alignment so we can assume that the recent change in velocity has occurred due to a
 			// forward direction takeoff or launch and therefore the inertial and GPS ground course discrepancy is due to yaw error
-			const float current_yaw = getEuler321Yaw(_state.quat_nominal);
+			const float current_yaw = getEulerYaw(_R_to_earth);
 			yaw_new = current_yaw + courseYawError;
 			_control_status.flags.mag_aligned_in_flight = true;
 
@@ -454,7 +454,6 @@ bool Ekf::realignYawGPS(const Vector3f &mag)
 
 		// 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;
 
 		resetMagCov();
@@ -541,7 +540,7 @@ bool Ekf::resetMagHeading(bool increase_yaw_var, bool update_buffer)
 
 bool Ekf::resetYawToEv()
 {
-	const float yaw_new = getEuler312Yaw(_ev_sample_delayed.quat);
+	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);
@@ -1695,10 +1694,8 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer)
 	const Quatf quat_before_reset = _state.quat_nominal;
 
 	// update transformation matrix from body to world frame using the current estimate
-	_R_to_earth = Dcmf(_state.quat_nominal);
-
 	// update the rotation matrix using the new yaw value
-	_R_to_earth = updateYawInRotMat(yaw, _R_to_earth);
+	_R_to_earth = updateYawInRotMat(yaw, Dcmf(_state.quat_nominal));
 
 	// calculate the amount that the quaternion has changed by
 	const Quatf quat_after_reset(_R_to_earth);

src/modules/ekf2/EKF/gps_control.cpp

@@ -192,9 +192,7 @@ bool Ekf::isYawFailure() const
 		return false;
 	}
 
-	const float euler_yaw = shouldUse321RotationSequence(_R_to_earth)
+	const float euler_yaw = getEulerYaw(_R_to_earth);
-				? getEuler321Yaw(_R_to_earth)
-				: getEuler312Yaw(_R_to_earth);
 	const float yaw_error = wrap_pi(euler_yaw - _yawEstimator.getYaw());
 
 	return fabsf(yaw_error) > math::radians(25.f);

src/modules/ekf2/EKF/mag_fusion.cpp

@@ -720,11 +720,7 @@ void Ekf::fuseHeading(float measured_hdg, float obs_var)
 	float R_YAW = PX4_ISFINITE(obs_var) ? obs_var : 0.01f;
 
 	// update transformation matrix from body to world frame using the current state estimate
-	_R_to_earth = Dcmf(_state.quat_nominal);
+	const float predicted_hdg = getEulerYaw(_R_to_earth);
-
-	const bool use_321_rotation_seq = shouldUse321RotationSequence(_R_to_earth);
-
-	const float predicted_hdg = use_321_rotation_seq ? getEuler321Yaw(_R_to_earth) : getEuler312Yaw(_R_to_earth);
 
 	if (!PX4_ISFINITE(measured_hdg)) {
 		measured_hdg = predicted_hdg;
@@ -765,7 +761,7 @@ void Ekf::fuseHeading(float measured_hdg, float obs_var)
 		_last_static_yaw = predicted_hdg;
 	}
 
-	if (use_321_rotation_seq) {
+	if (shouldUse321RotationSequence(_R_to_earth)) {
 		fuseYaw321(measured_hdg, R_YAW, fuse_zero_innov);
 
 	} else {

src/modules/ekf2/EKF/utils.cpp

@@ -32,7 +32,7 @@ float kahanSummation(float sum_previous, float input, float &accumulator)
 	return t;
 }
 
-matrix::Dcmf quatToInverseRotMat(const  matrix::Quatf &quat)
+matrix::Dcmf quatToInverseRotMat(const matrix::Quatf &quat)
 {
 	const float q00 = quat(0) * quat(0);
 	const float q11 = quat(1) * quat(1);
@@ -59,11 +59,6 @@ matrix::Dcmf quatToInverseRotMat(const  matrix::Quatf &quat)
 	return dcm;
 }
 
-bool shouldUse321RotationSequence(const matrix::Dcmf &R)
-{
-	return fabsf(R(2, 0)) < fabsf(R(2, 1));
-}
-
 float getEuler321Yaw(const matrix::Quatf &q)
 {
 	// Values from yaw_input_321.c file produced by
@@ -73,11 +68,6 @@ float getEuler321Yaw(const matrix::Quatf &q)
 	return atan2f(a, b);
 }
 
-float getEuler321Yaw(const matrix::Dcmf &R)
-{
-	return atan2f(R(1, 0), R(0, 0));
-}
-
 float getEuler312Yaw(const matrix::Quatf &q)
 {
 	// Values from yaw_input_312.c file produced by
@@ -87,11 +77,6 @@ float getEuler312Yaw(const matrix::Quatf &q)
 	return atan2f(a, b);
 }
 
-float getEuler312Yaw(const matrix::Dcmf &R)
-{
-	return atan2f(-R(0, 1), R(1, 1));
-}
-
 matrix::Dcmf updateEuler321YawInRotMat(float yaw, const matrix::Dcmf &rot_in)
 {
 	matrix::Eulerf euler321(rot_in);

src/modules/ekf2/EKF/utils.hpp

@@ -27,13 +27,23 @@ matrix::Dcmf quatToInverseRotMat(const matrix::Quatf &quat);
 // We should use a 3-2-1 Tait-Bryan (yaw-pitch-roll) rotation sequence
 // when there is more roll than pitch tilt and a 3-1-2 rotation sequence
 // when there is more pitch than roll tilt to avoid gimbal lock.
-bool shouldUse321RotationSequence(const matrix::Dcmf &R);
+inline bool shouldUse321RotationSequence(const matrix::Dcmf &R) { return fabsf(R(2, 0)) < fabsf(R(2, 1)); }
 
-float getEuler321Yaw(const matrix::Quatf &q);
+inline float getEuler321Yaw(const matrix::Dcmf &R) { return atan2f(R(1, 0), R(0, 0)); }
-float getEuler321Yaw(const matrix::Dcmf &R);
+inline float getEuler312Yaw(const matrix::Dcmf &R) { return atan2f(-R(0, 1), R(1, 1)); }
 
+float getEuler321Yaw(const matrix::Quatf &q);
 float getEuler312Yaw(const matrix::Quatf &q);
-float getEuler312Yaw(const matrix::Dcmf &R);
+
+inline float getEulerYaw(const matrix::Dcmf &R)
+{
+	if (shouldUse321RotationSequence(R)) {
+		return getEuler321Yaw(R);
+
+	} else {
+		return getEuler312Yaw(R);
+	}
+}
 
 matrix::Dcmf updateEuler321YawInRotMat(float yaw, const matrix::Dcmf &rot_in);
 matrix::Dcmf updateEuler312YawInRotMat(float yaw, const matrix::Dcmf &rot_in);