AP_NavEKF3: Ensure consistent use of 312 or 321 rotation order

Prevents possibility of a differnt sequence being used to calculate the yaw and application of the yaw to the quaternions

libraries/AP_NavEKF3/AP_NavEKF3_MagFusion.cpp

@@ -157,7 +157,9 @@ void NavEKF3_core::realignYawGPS()
             }
 
             // keep roll and pitch and reset yaw
-            resetQuatStateYawOnly(gpsYaw, sq(radians(45.0f)));
+            rotationOrder order;
+            bestRotationOrder(order);
+            resetQuatStateYawOnly(gpsYaw, sq(radians(45.0f)), order);
 
             // reset the velocity and position states as they will be inaccurate due to bad yaw
             ResetVelocity(resetDataSource::GPS);
@@ -1449,7 +1451,9 @@ bool NavEKF3_core::EKFGSF_resetMainFilterYaw()
     if (yawEstimator->getYawData(yawEKFGSF, yawVarianceEKFGSF) && is_positive(yawVarianceEKFGSF) && yawVarianceEKFGSF < sq(radians(GSF_YAW_ACCURACY_THRESHOLD_DEG))) {
 
         // keep roll and pitch and reset yaw
-        resetQuatStateYawOnly(yawEKFGSF, yawVarianceEKFGSF);
+        rotationOrder order;
+        bestRotationOrder(order);
+        resetQuatStateYawOnly(yawEKFGSF, yawVarianceEKFGSF, order);
 
         // record the emergency reset event
         EKFGSF_yaw_reset_request_ms = 0;
@@ -1484,22 +1488,25 @@ bool NavEKF3_core::EKFGSF_resetMainFilterYaw()
 
 }
 
-void NavEKF3_core::resetQuatStateYawOnly(float yaw, float yawVariance)
+void NavEKF3_core::resetQuatStateYawOnly(float yaw, float yawVariance, rotationOrder order)
 {
     Quaternion quatBeforeReset = stateStruct.quat;
 
-    // check if we should use a 321 or 312 Rotation sequence and update the quaternion
+    // check if we should use a 321 or 312 Rotation order and update the quaternion
     // states using the preferred yaw definition
     stateStruct.quat.inverse().rotation_matrix(prevTnb);
     Vector3f eulerAngles;
-    if (fabsf(prevTnb[2][0]) < fabsf(prevTnb[2][1])) {
+    if (order == rotationOrder::TAIT_BRYAN_321) {
         // rolled more than pitched so use 321 rotation order
         stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);
         stateStruct.quat.from_euler(eulerAngles.x, eulerAngles.y, yaw);
-    } else {
+    } else if (order == rotationOrder::TAIT_BRYAN_312) {
         // pitched more than rolled so use 312 rotation order
         eulerAngles = stateStruct.quat.to_vector312();
         stateStruct.quat.from_vector312(eulerAngles.x, eulerAngles.y, yaw);
+    } else {
+        // rotation order not supported
+        return;
     }
 
     // Update the rotation matrix

libraries/AP_NavEKF3/AP_NavEKF3_core.cpp

@@ -1868,20 +1868,30 @@ void NavEKF3_core::setYawFromMag()
     // read the magnetometer data
     readMagData();
 
-    // rotate the magnetic field into NED axes
-    Vector3f euler321;
-    stateStruct.quat.to_euler(euler321.x, euler321.y, euler321.z);
-
-    // set the yaw to zero and calculate the zero yaw rotation from body to earth frame
+    // use best of either 312 or 321 rotation sequence when calculating yaw
+    rotationOrder order;
+    bestRotationOrder(order);
+    Vector3f eulerAngles;
     Matrix3f Tbn_zeroYaw;
-    Tbn_zeroYaw.from_euler(euler321.x, euler321.y, 0.0f);
+    if (order == rotationOrder::TAIT_BRYAN_321) {
+        // rolled more than pitched so use 321 rotation order
+        stateStruct.quat.to_euler(eulerAngles.x, eulerAngles.y, eulerAngles.z);
+        Tbn_zeroYaw.from_euler(eulerAngles.x, eulerAngles.y, 0.0f);
+    } else if (order == rotationOrder::TAIT_BRYAN_312) {
+        // pitched more than rolled so use 312 rotation order
+        eulerAngles = stateStruct.quat.to_vector312();
+        Tbn_zeroYaw.from_euler312(eulerAngles.x, eulerAngles.y, 0.0f);
+    } else {
+        // rotation order not supported
+        return;
+    }
 
     //Vector3f magMeasNED = Tbn_zeroYaw * magDataDelayed.mag;
     Vector3f magMeasNED = Tbn_zeroYaw * magDataDelayed.mag;
     float yawAngMeasured = wrap_PI(-atan2f(magMeasNED.y, magMeasNED.x) + MagDeclination());
 
     // update quaternion states and covariances
-    resetQuatStateYawOnly(yawAngMeasured, sq(MAX(frontend->_yawNoise, 1.0e-2f)));
+    resetQuatStateYawOnly(yawAngMeasured, sq(MAX(frontend->_yawNoise, 1.0e-2f)), order);
 }
 
 // update mag field states and associated variances using magnetomer and declination data
@@ -1970,3 +1980,14 @@ Vector3f NavEKF3_core::calcRotVecVariances()
 
     return rotVarVec;
 }
+
+void NavEKF3_core::bestRotationOrder(rotationOrder &order)
+{
+    if (fabsf(prevTnb[2][0]) < fabsf(prevTnb[2][1])) {
+        // rolled more than pitched so use 321 sequence
+        order = rotationOrder::TAIT_BRYAN_321;
+    } else {
+        // pitched more than rolled so use 312 sequence
+        order = rotationOrder::TAIT_BRYAN_312;
+    }
+}

libraries/AP_NavEKF3/AP_NavEKF3_core.h

@@ -658,6 +658,12 @@ private:
         EXTNAV=7        // Use external nav data
     };
 
+    // Specifies the rotation order used for the Tait-Bryan or Euler angles where alternative rotation orders are available
+    enum class rotationOrder {
+        TAIT_BRYAN_321=0,
+        TAIT_BRYAN_312=1
+    };
+
     // update the navigation filter status
     void updateFilterStatus(void);
 
@@ -901,6 +907,9 @@ private:
     // Fuse compass measurements using a simple declination observation (doesn't require magnetic field states)
     void fuseEulerYaw(bool usePredictedYaw, bool useExternalYawSensor);
 
+    // return the best Tait-Bryan rotation order to use
+    void bestRotationOrder(rotationOrder &order);
+
     // Fuse declination angle to keep earth field declination from changing when we don't have earth relative observations.
     // Input is 1-sigma uncertainty in published declination
     void FuseDeclination(float declErr);
@@ -964,7 +973,8 @@ private:
     // reset the quaternion state covariances using the supplied yaw variance
     // yaw          : new yaw angle (rad)
     // yaw_variance : variance of new yaw angle (rad^2)
-    void resetQuatStateYawOnly(float yaw, float yawVariance);
+    // order : enum defining Tait-Bryan rotation order used in calculation of the yaw angle
+    void resetQuatStateYawOnly(float yaw, float yawVariance, rotationOrder order);
 
     // attempt to reset the yaw to the EKF-GSF value
     // returns false if unsuccessful