diff --git a/matrix/Euler.hpp b/matrix/Euler.hpp
index 3fe125d62c..74e364940f 100644
--- a/matrix/Euler.hpp
+++ b/matrix/Euler.hpp
@@ -10,8 +10,6 @@
 
 #include "math.hpp"
 
-#include <cmath>
-
 namespace matrix
 {
 
@@ -54,17 +52,17 @@ public:
         Type theta_val = Type(asin(-dcm(2,0)));
         Type phi_val = Type(atan(dcm(2, 1)/ dcm(2, 2)));
 
-        // protection against gimbal lock
+        // protection against NaN if dcm(0,0) or dcm(2,2) == 0
         psi() = 0;
         theta() = 0;
         phi() = 0;
-        if (std::isfinite(psi_val)) {
+        if (psi() >= -M_PI_2 && psi() <= M_PI_2) {
             psi() = psi_val;
         }
-        if (std::isfinite(theta_val)) {
+        if (theta() >= -M_PI_2 && theta() <= M_PI_2) {
             theta() = theta_val;
         }
-        if (std::isfinite(phi_val)) {
+        if (phi() >= -M_PI_2 && phi() <= M_PI_2) {
             phi() = phi_val;
         }
     }
diff --git a/matrix/SquareMatrix.hpp b/matrix/SquareMatrix.hpp
index ee06ccdd29..36cc716bec 100644
--- a/matrix/SquareMatrix.hpp
+++ b/matrix/SquareMatrix.hpp
@@ -22,6 +22,9 @@ namespace matrix
 template <typename Type, size_t M, size_t N>
 class Matrix;
 
+template <typename Type, size_t M>
+class Vector;
+
 template<typename Type, size_t  M>
 class SquareMatrix : public Matrix<Type, M, M>
 {