From 41377709e630943593bca810295df9f488602831 Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 23 Feb 2015 22:15:47 +0100 Subject: [PATCH 1/2] added test for quaternion methods --- src/systemcmds/tests/test_mathlib.cpp | 67 +++++++++++++++++++++++++++ 1 file changed, 67 insertions(+) diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp index 9fa52aaaa0..f56667d673 100644 --- a/src/systemcmds/tests/test_mathlib.cpp +++ b/src/systemcmds/tests/test_mathlib.cpp @@ -205,5 +205,72 @@ int test_mathlib(int argc, char *argv[]) } + { + // test conversion rotation matrix to quaternion and back + math::Matrix<3,3> R_orig; + math::Matrix<3,3> R; + math::Quaternion q; + float diff = 0.1f; + float tol = 0.00001f; + + for(float roll = -M_PI_F;roll <= M_PI_F;roll+=diff) { + for(float pitch = -M_PI_2_F;pitch <= M_PI_2_F;pitch+=diff) { + for(float yaw = -M_PI_F;yaw <= M_PI_F;yaw+=diff) { + R_orig.from_euler(roll,pitch,yaw); + q.from_dcm(R_orig); + R = q.to_dcm(); + for(int i = 0;i< 3;i++) { + for(int j=0;j<3;j++) { + if(fabsf(R_orig.data[i][j] - R.data[i][j]) > 0.00001f) { + warnx("Quaternion method 'from_dcm' or 'to_dcm' outside tolerance!"); + rc = 1; + } + } + } + } + } + } + + // test against some known values + tol = 0.0001f; + math::Quaternion q_true = {1.0f,0.0f,0.0f,0.0f}; + R_orig.identity(); + q.from_dcm(R_orig); + for(unsigned i = 0;i<4;i++) { + if(fabsf(q.data[i] - q_true.data[i]) > tol) { + warnx("Quaternion method 'from_dcm()' outside tolerance!"); + rc = 1; + } + } + + q_true.from_euler(0.3f,0.2f,0.1f); + q = {0.9833f,0.1436f,0.1060f,0.0343f}; + for(unsigned i = 0;i<4;i++) { + if(fabsf(q.data[i] - q_true.data[i]) > tol) { + warnx("Quaternion method 'from_euler()' outside tolerance!"); + rc = 1; + } + } + + q_true.from_euler(-1.5f,-0.2f,0.5f); + q = {0.7222f,-0.6391f,-0.2386f,0.1142f}; + for(unsigned i = 0;i<4;i++) { + if(fabsf(q.data[i] - q_true.data[i]) > tol) { + warnx("Quaternion method 'from_euler()' outside tolerance!"); + rc = 1; + } + } + + q_true.from_euler(M_PI_2_F,-M_PI_2_F,-M_PI_F/3); + q = {0.6830f,0.1830f,-0.6830f,0.1830f}; + for(unsigned i = 0;i<4;i++) { + if(fabsf(q.data[i] - q_true.data[i]) > tol) { + warnx("Quaternion method 'from_euler()' outside tolerance!"); + rc = 1; + } + } + + } + return rc; } From a252f4a99111597babd447c962208287e3f244ba Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 23 Feb 2015 22:26:41 +0100 Subject: [PATCH 2/2] fixed quaternion method from_dcm() --- src/lib/mathlib/math/Quaternion.hpp | 34 ++++++++++++++++++++++++----- 1 file changed, 28 insertions(+), 6 deletions(-) diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp index bf6e3365dd..6ea4cf557a 100644 --- a/src/lib/mathlib/math/Quaternion.hpp +++ b/src/lib/mathlib/math/Quaternion.hpp @@ -135,12 +135,34 @@ public: data[3] = static_cast(cosPhi_2 * cosTheta_2 * sinPsi_2 - sinPhi_2 * sinTheta_2 * cosPsi_2); } - void from_dcm(const Matrix<3, 3> &m) { - // avoiding singularities by not using division equations - data[0] = 0.5f * sqrtf(1.0f + m.data[0][0] + m.data[1][1] + m.data[2][2]); - data[1] = 0.5f * sqrtf(1.0f + m.data[0][0] - m.data[1][1] - m.data[2][2]); - data[2] = 0.5f * sqrtf(1.0f - m.data[0][0] + m.data[1][1] - m.data[2][2]); - data[3] = 0.5f * sqrtf(1.0f - m.data[0][0] - m.data[1][1] + m.data[2][2]); + void from_dcm(const Matrix<3, 3> &dcm) { + float tr = dcm.data[0][0] + dcm.data[1][1] + dcm.data[2][2]; + if (tr > 0.0f) { + float s = sqrtf(tr + 1.0f); + data[0] = s * 0.5f; + s = 0.5f / s; + data[1] = (dcm.data[2][1] - dcm.data[1][2]) * s; + data[2] = (dcm.data[0][2] - dcm.data[2][0]) * s; + data[3] = (dcm.data[1][0] - dcm.data[0][1]) * s; + } else { + /* Find maximum diagonal element in dcm + * store index in dcm_i */ + int dcm_i = 0; + for (int i = 1; i < 3; i++) { + if (dcm.data[i][i] > dcm.data[dcm_i][dcm_i]) { + dcm_i = i; + } + } + int dcm_j = (dcm_i + 1) % 3; + int dcm_k = (dcm_i + 2) % 3; + float s = sqrtf((dcm.data[dcm_i][dcm_i] - dcm.data[dcm_j][dcm_j] - + dcm.data[dcm_k][dcm_k]) + 1.0f); + data[dcm_i + 1] = s * 0.5f; + s = 0.5f / s; + data[dcm_j + 1] = (dcm.data[dcm_i][dcm_j] + dcm.data[dcm_j][dcm_i]) * s; + data[dcm_k + 1] = (dcm.data[dcm_k][dcm_i] + dcm.data[dcm_i][dcm_k]) * s; + data[0] = (dcm.data[dcm_k][dcm_j] - dcm.data[dcm_j][dcm_k]) * s; + } } /**