diff --git a/matrix/Euler.hpp b/matrix/Euler.hpp
index 41d34d0a0e..2928baa6ad 100644
--- a/matrix/Euler.hpp
+++ b/matrix/Euler.hpp
@@ -61,8 +61,6 @@ public:
             psi_val = Type(atan2(-dcm(1,2), -dcm(0,2)));
         }
 
-        if (psi_val < 0) psi_val += 2*pi;
-
         phi() = phi_val;
         theta() = theta_val;
         psi() = psi_val;
diff --git a/test/attitude.cpp b/test/attitude.cpp
index 6206eb1713..784ffb29f3 100644
--- a/test/attitude.cpp
+++ b/test/attitude.cpp
@@ -104,8 +104,8 @@ int main()
                     roll_expected = 0;
                     yaw_expected = yaw + roll;
                 }
-                if (yaw_expected < 0) yaw_expected += 360;
-                if (yaw_expected > 360) yaw_expected -= 360;
+                if (yaw_expected < -180) yaw_expected += 360;
+                if (yaw_expected > 180) yaw_expected -= 360;
 
                 printf("roll:%d pitch:%d yaw:%d\n", roll, pitch, yaw);
                 Euler<double> euler_expected(