diff --git a/matrix/Euler.hpp b/matrix/Euler.hpp
index 3321906ded..472ee3da8d 100644
--- a/matrix/Euler.hpp
+++ b/matrix/Euler.hpp
@@ -49,19 +49,22 @@ public:
 
     Euler(const Dcm<Type> & dcm) : Vector<Type, 3>()
     {
-        theta() = Type(asin(-dcm(2, 0)));
+        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)));
 
-        if (fabs(theta() - M_PI_2) < 1.0e-3) {
-            psi() = Type(atan2(dcm(1, 2) - dcm(0, 1),
-                               dcm(0, 2) + dcm(1, 1)));
-
-        } else if (fabs(theta() + M_PI_2) < 1.0e-3) {
-            psi() = Type(atan2(dcm(1, 2) - dcm(0, 1),
-                               dcm(0, 2) + dcm(1, 1)));
-
-        } else {
-            phi() = Type(atan2f(dcm(2, 1), dcm(2, 2)));
-            psi() = Type(atan2f(dcm(1, 0), dcm(0, 0)));
+        // protection against gimbal lock
+        psi() = 0;
+        theta() = 0;
+        phi() = 0;
+        if (isfinite(psi_val)) {
+            psi() = psi_val;
+        }
+        if (isfinite(theta_val)) {
+            theta() = theta_val;
+        }
+        if (isfinite(phi_val)) {
+            phi() = phi_val;
         }
     }
 
diff --git a/matrix/Matrix.hpp b/matrix/Matrix.hpp
index ba16ef801f..d260256e7b 100644
--- a/matrix/Matrix.hpp
+++ b/matrix/Matrix.hpp
@@ -215,9 +215,9 @@ public:
      * Misc. Functions
      */
 
-    void print()
+    void print() const
     {
-        Matrix<Type, M, N> &self = *this;
+        const Matrix<Type, M, N> &self = *this;
         printf("\n");
 
         for (size_t i = 0; i < M; i++) {
diff --git a/test/attitude.cpp b/test/attitude.cpp
index cd4e3c15af..32020bccd6 100644
--- a/test/attitude.cpp
+++ b/test/attitude.cpp
@@ -33,6 +33,14 @@ int main()
     assert(e == e_zero);
     assert(e == e);
 
+    // euler vector ctor
+    Vector<float, 3> v;
+    v(0) = 0.1f;
+    v(1) = 0.2f;
+    v(2) = 0.3f;
+    Eulerf euler_copy(v);
+    assert(euler_copy == euler_check);
+
     // quaternion ctor
     Quatf q(1, 2, 3, 4);
     assert(fabs(q(0) - 1) < eps);
@@ -84,6 +92,26 @@ int main()
     Quatf q2(dcm_check);
     assert(q2 == q_check);
 
+
+    // euler gimbal lock check
+    // note if theta = pi/2, then roll is set to zero
+    float pi_2 = float(M_PI_2);
+    Eulerf euler_gimbal_lock(0.0f, pi_2, 0.0f);
+    Dcmf dcm_lock(euler_gimbal_lock);
+    Eulerf euler_gimbal_lock_out(dcm_lock);
+    euler_gimbal_lock_out.print();
+    euler_gimbal_lock.print();
+    assert(euler_gimbal_lock == euler_gimbal_lock_out);
+
+    // note if theta = pi/2, then roll is set to zero
+    Eulerf euler_gimbal_lock2(0.0f, -pi_2, 0.0f);
+    Dcmf dcm_lock2(euler_gimbal_lock2);
+    Eulerf euler_gimbal_lock_out2(dcm_lock2);
+    euler_gimbal_lock_out2.print();
+    euler_gimbal_lock2.print();
+    assert(euler_gimbal_lock2 == euler_gimbal_lock_out2);
+
+
 }
 
 /* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */