Add helper function to alter heading in rotation matrices

EKF/EKFGSF_yaw.cpp

@@ -225,24 +225,14 @@ void EKFGSF_yaw::ahrsAlignYaw()
 	// Align yaw angle for each model
 	for (uint8_t model_index = 0; model_index < N_MODELS_EKFGSF; model_index++) {
 		if (shouldUse321RotationSequence(_ahrs_ekf_gsf[model_index].R)) {
-			// get the roll, pitch, yaw estimates from the rotation matrix using a  321 Tait-Bryan rotation sequence
+			// update the rotation matrix with 321 rotation sequence
-			Eulerf euler_init(_ahrs_ekf_gsf[model_index].R);
+			_ahrs_ekf_gsf[model_index].R = updateEuler321YawInRotMat(wrap_pi(_ekf_gsf[model_index].X(2)),
-
+										_ahrs_ekf_gsf[model_index].R);
-			// set the yaw angle
-			euler_init(2) = wrap_pi(_ekf_gsf[model_index].X(2));
-
-			// update the rotation matrix
-			_ahrs_ekf_gsf[model_index].R = Dcmf(euler_init);
 
 		} else {
-			// Calculate the 312 Tait-Bryan rotation sequence that rotates from earth to body frame
+			// update the rotation matrix with 312 rotation sequence
-			const Vector3f rot312(wrap_pi(_ekf_gsf[model_index].X(2)),  // yaw
+			_ahrs_ekf_gsf[model_index].R = updateEuler312YawInRotMat(wrap_pi(_ekf_gsf[model_index].X(2)),
-					      asinf(_ahrs_ekf_gsf[model_index].R(2, 1)),  // roll
+										_ahrs_ekf_gsf[model_index].R);
-					      atan2f(-_ahrs_ekf_gsf[model_index].R(2, 0),
-						      _ahrs_ekf_gsf[model_index].R(2, 2)));  // pitch
-
-			// Calculate the body to earth frame rotation matrix
-			_ahrs_ekf_gsf[model_index].R = taitBryan312ToRotMat(rot312);
 
 		}
 		_ahrs_ekf_gsf[model_index].aligned = true;

EKF/ekf_helper.cpp

@@ -484,16 +484,11 @@ bool Ekf::resetMagHeading(const Vector3f &mag_init, bool increase_yaw_var, bool
 		Dcmf R_to_earth;
 		if (shouldUse321RotationSequence(_R_to_earth)) {
 			// rolled more than pitched so use 321 rotation order
-			Eulerf euler321(_state.quat_nominal);
+			R_to_earth = updateEuler321YawInRotMat(0.f, _R_to_earth);
-			euler321(2) = 0.0f;
-			R_to_earth = Dcmf(euler321);
 
 		} else {
 			// pitched more than rolled so use 312 rotation order
-			const Vector3f rotVec312(0.0f,  // yaw
+			R_to_earth = updateEuler312YawInRotMat(0.f, _R_to_earth);
-						 asinf(_R_to_earth(2, 1)),  // roll
-						 atan2f(-_R_to_earth(2, 0), _R_to_earth(2, 2)));  // pitch
-			R_to_earth = taitBryan312ToRotMat(rotVec312);
 
 		}
 
@@ -1654,19 +1649,12 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance, bool update_buffer)
 	// update the rotation matrix using the new yaw value
 	// determine if a 321 or 312 Euler sequence is best
 	if (shouldUse321RotationSequence(_R_to_earth)) {
-		// use a 321 sequence
+		_R_to_earth = updateEuler321YawInRotMat(yaw, _R_to_earth);
-		Eulerf euler321(_R_to_earth);
-		euler321(2) = yaw;
-		_R_to_earth = Dcmf(euler321);
 
 	} else {
-		// Calculate the 312 Tait-Bryan rotation sequence that rotates from earth to body frame
 		// We use a 312 sequence as an alternate when there is more pitch tilt than roll tilt
 		// to avoid gimbal lock
-		const Vector3f rot312(yaw,
+		_R_to_earth = updateEuler312YawInRotMat(yaw, _R_to_earth);
-				      asinf(_R_to_earth(2, 1)),
-				      atan2f(-_R_to_earth(2, 0), _R_to_earth(2, 2)));
-		_R_to_earth = taitBryan312ToRotMat(rot312);
 
 	}
 

EKF/mag_fusion.cpp

@@ -702,7 +702,6 @@ void Ekf::fuseHeading()
 {
 	Vector3f mag_earth_pred;
 	float measured_hdg;
-	float predicted_hdg;
 
 	// Calculate the observation variance
 	float R_YAW;
@@ -725,12 +724,11 @@ void Ekf::fuseHeading()
 	// determine if a 321 or 312 Euler sequence is best
 	if (shouldUse321RotationSequence(_R_to_earth)) {
 		// rolled more than pitched so use 321 rotation order to calculate the observed yaw angle
-		Eulerf euler321(_state.quat_nominal);
+		const float predicted_hdg = getEuler321Yaw(_R_to_earth);
-		predicted_hdg = euler321(2);
+
 		if (_control_status.flags.mag_hdg) {
-			// Set the yaw angle to zero and rotate the measurements into earth frame using the zero yaw angle
+			// Rotate the measurements into earth frame using the zero yaw angle
-			euler321(2) = 0.0f;
+			const Dcmf R_to_earth = updateEuler321YawInRotMat(0.f, _R_to_earth);
-			const Dcmf R_to_earth(euler321);
 			if (_control_status.flags.mag_3D) {
 				// don't apply bias corrections if we are learning them
 				mag_earth_pred = R_to_earth * _mag_sample_delayed.mag;
@@ -788,12 +786,9 @@ void Ekf::fuseHeading()
 
 		if (_control_status.flags.mag_hdg) {
 
-			// Calculate the body to earth frame rotation matrix from the euler angles using a 312 rotation sequence
+			// Calculate the body to earth frame rotation matrix from the euler angles
-			// with yaw angle set to to zero
+			// using a 312 rotation sequence with yaw angle set to to zero
-			const Vector3f rotVec312(0.0f,  // yaw
+			const Dcmf R_to_earth = updateEuler312YawInRotMat(0.f, _R_to_earth);
-						 asinf(_R_to_earth(2, 1)),  // roll
-						 atan2f(-_R_to_earth(2, 0), _R_to_earth(2, 2)));  // pitch
-			const Dcmf R_to_earth = taitBryan312ToRotMat(rotVec312);
 
 			// rotate the magnetometer measurements into earth frame using a zero yaw angle
 			if (_control_status.flags.mag_3D) {

EKF/utils.cpp

@@ -86,3 +86,16 @@ float getEuler312Yaw(const matrix::Quatf& q) {
 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);
+	euler321(2) = yaw;
+	return matrix::Dcmf(euler321);
+}
+
+matrix::Dcmf updateEuler312YawInRotMat(float yaw, const matrix::Dcmf& rot_in) {
+	const matrix::Vector3f rotVec312(yaw,  // yaw
+				asinf(rot_in(2, 1)),  // roll
+				atan2f(-rot_in(2, 0), rot_in(2, 2)));  // pitch
+	return taitBryan312ToRotMat(rotVec312);
+}

EKF/utils.hpp

@@ -31,6 +31,9 @@ float getEuler321Yaw(const matrix::Dcmf& R);
 float getEuler312Yaw(const matrix::Quatf& q);
 float getEuler312Yaw(const matrix::Dcmf& R);
 
+matrix::Dcmf updateEuler321YawInRotMat(float yaw, const matrix::Dcmf& rot_in);
+matrix::Dcmf updateEuler312YawInRotMat(float yaw, const matrix::Dcmf& rot_in);
+
 namespace ecl{
 	inline float powf(float x, int exp)
 	{