diff --git a/matrix/Euler.hpp b/matrix/Euler.hpp
index 98eae6e9e9..d23ba6b153 100644
--- a/matrix/Euler.hpp
+++ b/matrix/Euler.hpp
@@ -48,22 +48,19 @@ public:
 
     Euler(const Dcm<Type> & dcm) : Vector<Type, 3>()
     {
-        Type psi_val = Type(atan(dcm(1, 0)/ dcm(0, 0)));
-        Type theta_val = Type(asin(-dcm(2,0)));
-        Type phi_val = Type(atan(dcm(2, 1)/ dcm(2, 2)));
+        theta() = (Type)asin(-dcm(2,0));
 
-        // protection against NaN if dcm(0,0) or dcm(2,2) == 0
-        psi() = 0;
-        theta() = 0;
-        phi() = 0;
-        if (psi() >= -(Type)M_PI_2 && psi() <= (Type)M_PI_2) {
-            psi() = psi_val;
-        }
-        if (theta() >= -(Type)M_PI_2 && theta() <= (Type)M_PI_2) {
-            theta() = theta_val;
-        }
-        if (phi() >= -(Type)M_PI_2 && phi() <= (Type)M_PI_2) {
-            phi() = phi_val;
+        if (fabs(theta() - (Type)M_PI_2) < 1.0e-3) {
+            phi() = (Type)0.0;
+            psi() = (Type)atan2(dcm(1,2) - dcm(0,1), dcm(0,2) + dcm(1,1)) + theta();
+
+        } else if ((Type)fabs(theta() + (Type)M_PI_2) < (Type)1.0e-3) {
+            phi() = (Type)0.0;
+            psi() = (Type)atan2(dcm(1,2) - dcm(0,1), dcm(0,2) + dcm(1,1)) - theta();
+
+        } else {
+            phi() = (Type)atan2(dcm(2,1), dcm(2,2));
+            psi() = (Type)atan2(dcm(1,0), dcm(0,0));
         }
     }