From f7b1c63b8688af14b4c8008a1edfbe68fea6dbb8 Mon Sep 17 00:00:00 2001 From: jgoppert Date: Thu, 5 Nov 2015 10:44:21 -0500 Subject: [PATCH] Work on testing. --- .gitignore | 1 + matrix/Dcm.hpp | 8 ++++ matrix/Euler.hpp | 4 +- matrix/Matrix.hpp | 39 +++++++++++++++++ matrix/Vector.hpp | 19 +++++--- test/CMakeLists.txt | 4 +- test/attitude.cpp | 73 +++++++++++++++++++++++++++++++ test/dcm.cpp | 19 -------- test/euler.cpp | 20 --------- test/quaternion.cpp | 50 --------------------- test/test_data.py | 104 ++++++++++++++++++++++++++++++++++++++++++++ test/test_math.py | 66 ---------------------------- 12 files changed, 242 insertions(+), 165 deletions(-) create mode 100644 test/attitude.cpp delete mode 100644 test/dcm.cpp delete mode 100644 test/euler.cpp delete mode 100644 test/quaternion.cpp create mode 100644 test/test_data.py delete mode 100644 test/test_math.py diff --git a/.gitignore b/.gitignore index f1feb3e425..b486f209c8 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,3 @@ build*/ *.orig +*.swp diff --git a/matrix/Dcm.hpp b/matrix/Dcm.hpp index 40de899688..cbee44f810 100644 --- a/matrix/Dcm.hpp +++ b/matrix/Dcm.hpp @@ -31,6 +31,14 @@ public: Matrix::setIdentity(); } + Dcm(const Type *data) : Matrix(data) + { + } + + Dcm(const Matrix &other) : Matrix(other) + { + } + Dcm(const Quaternion & q) { Dcm &dcm = *this; Type a = q(0); diff --git a/matrix/Euler.hpp b/matrix/Euler.hpp index a6701a9cec..2924994f06 100644 --- a/matrix/Euler.hpp +++ b/matrix/Euler.hpp @@ -42,11 +42,11 @@ public: Type phi = 0; Type psi = 0; - if (abs(theta - M_PI_2) < 1.0e-3) { + if (fabs(theta - M_PI_2) < 1.0e-3) { psi = atan2(dcm(1, 2) - dcm(0, 1), dcm(0, 2) + dcm(1, 1)); - } else if (abs(theta + M_PI_2) < 1.0e-3) { + } else if (fabs(theta + M_PI_2) < 1.0e-3) { psi = atan2(dcm(1, 2) - dcm(0, 1), dcm(0, 2) + dcm(1, 1)); diff --git a/matrix/Matrix.hpp b/matrix/Matrix.hpp index 5103626282..b73dece8b3 100644 --- a/matrix/Matrix.hpp +++ b/matrix/Matrix.hpp @@ -293,6 +293,45 @@ public: } } + Matrix abs() + { + Matrix r; + for (int i=0; i max) { + max = val; + } + } + } + return max; + } + + Type min() + { + Type min = (*this)(0,0); + for (int i=0; i diff --git a/matrix/Vector.hpp b/matrix/Vector.hpp index 6b77cfc7ff..671b26c614 100644 --- a/matrix/Vector.hpp +++ b/matrix/Vector.hpp @@ -47,20 +47,24 @@ public: return v(i, 0); } - Type dot(const Vector & b) { - Vector &a(*this); + Type dot(const Vector & b) const { + const Vector &a(*this); Type r = 0; - for (int i = 0; i<3; i++) { + for (int i = 0; i operator/(Type scalar) const + { + return (*this)*(1.0/scalar); + } + Vector operator+(Type scalar) const { Vector res; diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 53702b78c1..97bf5b5963 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -6,11 +6,9 @@ set(tests matrixAssignment matrixScalarMult transpose - quaternion - dcm vector vector3 - euler + attitude filter ) diff --git a/test/attitude.cpp b/test/attitude.cpp new file mode 100644 index 0000000000..3dc96d329f --- /dev/null +++ b/test/attitude.cpp @@ -0,0 +1,73 @@ +#include +#include + +#include "Quaternion.hpp" +#include "Vector3.hpp" +#include "matrix.hpp" + +using namespace matrix; + +template class Quaternion; +template class Euler; +template class Dcm; + +int main() +{ + double eps = 1e-6; + + // check data + Eulerf euler_check(0.1, 0.2, 0.3); + Quatf q_check(0.98334744, 0.0342708, 0.10602051, .14357218); + float dcm_data[] = { + 0.93629336, -0.27509585, 0.21835066, + 0.28962948, 0.95642509, -0.03695701, + -0.19866933, 0.0978434 , 0.97517033}; + Dcmf dcm_check(dcm_data); + + // euler ctor + euler_check.T().print(); + assert((euler_check - Vector3f(0.1, 0.2, 0.3)).norm() < eps); + + // quaternion ctor + Quatf q(1, 2, 3, 4); + assert(q(0) == 1); + assert(q(1) == 2); + assert(q(2) == 3); + assert(q(3) == 4); + + q.T().print(); + q.normalize(); + q.T().print(); + assert((q - Quatf( + 0.18257419, 0.36514837, 0.54772256, 0.73029674) + ).norm() < eps); + + // euler to quaternion + q = Quatf(euler_check); + q.T().print(); + assert((q - q_check).norm() < eps); + + // euler to dcm + Dcmf dcm(euler_check); + dcm.print(); + assert((dcm - dcm_check).abs().max() < eps); + + // quaternion to euler + Eulerf e1(q_check); + assert((e1 - euler_check).norm() < eps); + + // quaternion to dcm + Dcmf dcm1(q_check); + assert((dcm1 - dcm_check).abs().max() < eps); + + // dcm to euler + Eulerf e2(dcm_check); + assert((e2 - euler_check).norm() < eps); + + // dcm to quaterion + Quatf q2(dcm_check); + assert((q2 - q_check).norm() < eps); + +} + +/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/test/dcm.cpp b/test/dcm.cpp deleted file mode 100644 index a01c8e05db..0000000000 --- a/test/dcm.cpp +++ /dev/null @@ -1,19 +0,0 @@ -#include -#include - -#include "matrix.hpp" - -using namespace matrix; - -int main() -{ - Dcmf dcm; - Quatf q(1,0,0,0); - dcm = Dcmf(q); - Matrix3f I = eye(); - dcm = Dcmf(q); - Eulerf e = Eulerf(dcm); - return 0; -}; - -/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/test/euler.cpp b/test/euler.cpp deleted file mode 100644 index 236f6e0225..0000000000 --- a/test/euler.cpp +++ /dev/null @@ -1,20 +0,0 @@ -#include "Euler.hpp" -#include "Scalar.hpp" -#include -#include - -using namespace matrix; - -int main() -{ - Eulerf e; - float dp = Scalarf(e.T()*e); - (void)dp; - Dcmf dcm = Dcmf(e); - Quatf q = Quatf(e); - float n = e.norm(); - (void)n; - return 0; -} - -/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/test/quaternion.cpp b/test/quaternion.cpp deleted file mode 100644 index 018f03a6ad..0000000000 --- a/test/quaternion.cpp +++ /dev/null @@ -1,50 +0,0 @@ -#include "Quaternion.hpp" -#include -#include - -using namespace matrix; - -// instantiate so coverage works -template class Quaternion; -template class Euler; -template class Dcm; - -int main() -{ - // test default ctor - Quatf q; - assert(q(0) == 1); - assert(q(1) == 0); - assert(q(2) == 0); - assert(q(3) == 0); - - q = Quatf(1,2,3,4); - assert(q(0) == 1); - assert(q(1) == 2); - assert(q(2) == 3); - assert(q(3) == 4); - - q = Quatf(0.1825742f, 0.3651484f, 0.5477226f, 0.7302967f); - assert(q(0) == 0.1825742f); - assert(q(1) == 0.3651484f); - assert(q(2) == 0.5477226f); - assert(q(3) == 0.7302967f); - - // test euler ctor - q = Quatf(Eulerf(0.1f, 0.2f, 0.3f)); - assert((q - Quatf(0.983347f, 0.034271f, 0.106021f, 0.143572f)).norm() < 1e-5); - - // test dcm ctor - q = Quatf(Dcmf()); - assert(q == Quatf(1.0f, 0.0f, 0.0f, 0.0f)); - // TODO test derivative - // test accessors - q(0) = 0.1f; - q(1) = 0.2f; - q(2) = 0.3f; - q(3) = 0.4f; - assert(q == Quatf(0.1f, 0.2f, 0.3f, 0.4f)); - return 0; -} - -/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */ diff --git a/test/test_data.py b/test/test_data.py new file mode 100644 index 0000000000..5f64994b06 --- /dev/null +++ b/test/test_data.py @@ -0,0 +1,104 @@ +from __future__ import print_function +from pylab import * +from pprint import pprint + +# test cases, derived from doc/nasa_rotation_def.pdf + +#pylint: disable=all + +def euler_to_quat(phi, theta, psi): + "Quaternion from (body 3(psi)-2(theta)-1(phi) euler angles" + s1 = sin(psi/2) + c1 = cos(psi/2) + s2 = sin(theta/2) + c2 = cos(theta/2) + s3 = sin(phi/2) + c3 = cos(phi/2) + return array([ + s1*s2*s3 + c1*c2*c3, + -s1*s2*c3 + s3*c1*c2, + s1*s3*c2 + s2*c1*c3, + s1*c2*c3 - s2*s3*c1 + ]) + +def euler_to_dcm(phi, theta, psi): + s1 = sin(psi) + c1 = cos(psi) + s2 = sin(theta) + c2 = cos(theta) + s3 = sin(phi) + c3 = cos(phi) + return array([ + [c1*c2, c1*s2*s3 - s1*c3, c1*s2*c3 + s1*s3], + [s1*c2, s1*s2*s3 + c1*c3, s1*s2*c3 - c1*s3], + [-s2, c2*s3, c2*c3], + ]) + +def quat_prod(q, r): + "Quaternion product" + return array([ + r[0]*q[0] - r[1]*q[1] - r[2]*q[2] - r[3]*q[3], + r[0]*q[1] + r[1]*q[0] - r[2]*q[3] + r[3]*q[2], + r[0]*q[2] + r[1]*q[3] + r[2]*q[0] - r[3]*q[1], + r[0]*q[3] - r[1]*q[2] + r[2]*q[1] + r[3]*q[0] + ]) + +def dcm_to_euler(dcm): + return array([ + arctan(dcm[2,1]/ dcm[2,2]), + arctan(-dcm[2,0]/ sqrt(1 - dcm[2,0]**2)), + arctan(dcm[1,0]/ dcm[0,0]), + ]) + +def dcm_from_quat(q): + q1 = q[0] + q2 = q[1] + q3 = q[2] + q4 = q[3] + return array([ + [q1*q1 + q2*q2 - q3*q3 - q4*q4, 2*(q2*q3 - q1*q4), 2*(q2*q4 + q1*q3)], + [2*(q2*q3 + q1*q4), q1*q1 - q2*q2 + q3*q3 - q4*q4, 2*(q3*q4 - q1*q2)], + [2*(q2*q4 - q1*q3), 2*(q3*q4 + q1*q2), q1*q1 - q2*q2 - q3*q3 + q4*q4] + ]) + +def quat_to_euler(q): + "Quaternion to (body 3(psi)-2(theta)-1(phi) euler angles" + return dcm_to_euler(dcm_from_quat(q)) + +def quat_to_dcm(q): + return euler_to_dcm(quat_to_euler(q)) + +phi = 0.1 +theta = 0.2 +psi = 0.3 +print('euler', phi, theta, psi) + +q = euler_to_quat(phi, theta, psi) +assert(abs(norm(q) - 1) < 1e-10) +assert(abs(norm(q) - 1) < 1e-10) +assert(norm(array(quat_to_euler(q)) - array([phi, theta, psi])) < 1e-10) +print('\nq:') +pprint(q) + +dcm = euler_to_dcm(phi, theta, psi) +assert(norm(dcm[:,0]) == 1) +assert(norm(dcm[:,1]) == 1) +assert(norm(dcm[:,2]) == 1) +assert(abs(dcm[:,0].dot(dcm[:,1])) < 1e-10) +assert(abs(dcm[:,0].dot(dcm[:,2])) < 1e-10) +print('\ndcm:') +pprint(dcm) + + +q2 = quat_prod(q, q) +pprint(q2) +print(norm(q2)) + +print('\nq3:') +q3 = array([1,2,3,4]) +pprint(q3) +print('\nq3_norm:') +q3_norm =q3 / norm(q3) +pprint(q3_norm) + +# vim: set et ft=python fenc=utf-8 ff=unix sts=4 sw=4 ts=8 : diff --git a/test/test_math.py b/test/test_math.py deleted file mode 100644 index d5d24da60c..0000000000 --- a/test/test_math.py +++ /dev/null @@ -1,66 +0,0 @@ -from __future__ import print_function -from pylab import * -from pprint import pprint - -#pylint: disable=all - -phi = 0.1 -theta = 0.2 -psi = 0.3 - -cosPhi = cos(phi) -cosPhi_2 = cos(phi/2) -sinPhi = sin(phi) -sinPhi_2 = sin(phi/2) - -cosTheta = cos(theta) -cosTheta_2 = cos(theta/2) -sinTheta = sin(theta) -sinTheta_2 = sin(theta/2) - -cosPsi = cos(psi) -cosPsi_2 = cos(psi/2) -sinPsi = sin(psi) -sinPsi_2 = sin(psi/2) - -C_nb = array([ - [cosTheta*cosPsi, -cosPhi*sinPsi + sinPhi*sinTheta*cosPsi, sinPhi*sinPsi + cosPhi*sinTheta*cosPsi], - [cosTheta*sinPsi, cosPhi*cosPsi + sinPhi*sinTheta*sinPsi, -sinPhi*cosPsi + cosPhi*sinTheta*sinPsi], - [-sinTheta, sinPhi*cosTheta, cosPhi*cosTheta]]) - -print('\nC_nb') -pprint(C_nb) - -theta = arcsin(-C_nb[2,0]) -phi = arctan(C_nb[2,1]/ C_nb[2,2]) -psi = arctan(C_nb[1,0]/ C_nb[0,0]) -print('\nphi {:f}, theta {:f}, psi {:f}\n'.format(phi, theta, psi)) - -q = array([ - cosPhi_2*cosTheta_2*cosPsi_2 + sinPhi_2*sinTheta_2*sinPsi_2, - sinPhi_2*cosTheta_2*cosPsi_2 - cosPhi_2*sinTheta_2*sinPsi_2, - cosPhi_2*sinTheta_2*cosPsi_2 + sinPhi_2*cosTheta_2*sinPsi_2, - cosPhi_2*cosTheta_2*sinPsi_2 - sinPhi_2*sinTheta_2*cosPsi_2]) - -a = q[0] -b = q[1] -c = q[2] -d = q[3] - -print('\nq') -pprint(q.T) - -a2 = a*a -b2 = b*b -c2 = c*c -d2 = d*d - -C2_nb = array([ - [a2 + b2 - c2 - d2, 2*(b*c - a*d), 2*(b*d + a*c)], - [2*(b*c + a*d), a2 - b2 + c2 - d2, 2*(c*d - a*b)], - [2*(b*d - a*c), 2*(c*d + a*b), a2 - b2 - c2 + d2]]) - -print('\nC2_nb') -pprint(C2_nb) - -# vim: set et ft=python fenc=utf-8 ff=unix sts=4 sw=4 ts=8 :