diff --git a/matrix/Euler.hpp b/matrix/Euler.hpp index a309f1f3c9..fb339cdab5 100644 --- a/matrix/Euler.hpp +++ b/matrix/Euler.hpp @@ -42,11 +42,11 @@ public: if (fabs(theta() - M_PI_2) < 1.0e-3) { psi() = Type(atan2(dcm(1, 2) - dcm(0, 1), - dcm(0, 2) + dcm(1, 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))); + dcm(0, 2) + dcm(1, 1))); } else { phi() = Type(atan2f(dcm(2, 1), dcm(2, 2))); diff --git a/matrix/Quaternion.hpp b/matrix/Quaternion.hpp index 68162118d4..06c8f9e11f 100644 --- a/matrix/Quaternion.hpp +++ b/matrix/Quaternion.hpp @@ -41,13 +41,13 @@ public: { Quaternion &q = *this; q(0) = Type(0.5 * sqrt(1 + dcm(0, 0) + - dcm(1, 1) + dcm(2, 2))); + dcm(1, 1) + dcm(2, 2))); q(1) = Type((dcm(2, 1) - dcm(1, 2)) / - (4 * q(0))); + (4 * q(0))); q(2) = Type((dcm(0, 2) - dcm(2, 0)) / - (4 * q(0))); + (4 * q(0))); q(3) = Type((dcm(1, 0) - dcm(0, 1)) / - (4 * q(0))); + (4 * q(0))); } Quaternion(const Euler & euler) {