Work on testing.

9 years ago · f7b1c63b86
12 changed files with 242 additions and 165 deletions
--- a/.gitignore
+++ b/.gitignore
@ -1,2 +1,3 @@
				@@ -1,2 +1,3 @@
 build*/
 *.orig
+*.swp
--- a/matrix/Dcm.hpp
+++ b/matrix/Dcm.hpp
@ -31,6 +31,14 @@ public:
				@@ -31,6 +31,14 @@ public:
        Matrix<Type, 3, 3>::setIdentity();
    }

+    Dcm(const Type *data) : Matrix<Type, 3, 3>(data)
+    {
+    }
+
+    Dcm(const Matrix<Type, 3, 3> &other) : Matrix<Type, 3, 3>(other)
+    {
+    }
+
    Dcm(const Quaternion<Type> & q) {
        Dcm &dcm = *this;
        Type a = q(0);
--- a/matrix/Euler.hpp
+++ b/matrix/Euler.hpp
@ -42,11 +42,11 @@ public:
				@@ -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));

--- a/matrix/Matrix.hpp
+++ b/matrix/Matrix.hpp
@ -293,6 +293,45 @@ public:
				@@ -293,6 +293,45 @@ public:
        }
    }

+    Matrix<Type, M, N> abs()
+    {
+        Matrix<Type, M, N> r;
+        for (int i=0; i<M; i++) {
+            for (int j=0; j<M; j++) {
+                r(i,j) = fabs((*this)(i,j));
+            }
+        }
+        return r;
+    }
+
+    Type max()
+    {
+        Type max = (*this)(0,0);
+        for (int i=0; i<M; i++) {
+            for (int j=0; j<M; j++) {
+                Type val = (*this)(i,j);
+                if (val > max) {
+                    max = val;
+                }
+            }
+        }
+        return max;
+    }
+
+    Type min()
+    {
+        Type min = (*this)(0,0);
+        for (int i=0; i<M; i++) {
+            for (int j=0; j<M; j++) {
+                Type val = (*this)(i,j);
+                if (val < min) {
+                    min = val;
+                }
+            }
+        }
+        return min;
+    }
+
 };

 template<typename Type, size_t M, size_t N>
--- a/matrix/Vector.hpp
+++ b/matrix/Vector.hpp
@ -47,20 +47,24 @@ public:
				@@ -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<M; i++) {
            r += a(i)*b(i);
        }
        return r;
    }

-    Type norm() {
-        Vector &a(*this);
+    Type norm() const {
+        const Vector &a(*this);
        return sqrt(a.dot(a));
    }

+    inline void normalize() {
+        (*this) /= norm();
+    }
+
    /**
     * Vector Operations
     */
@ -131,6 +135,11 @@ public:
				@@ -131,6 +135,11 @@ public:
        return res;
    }

+    Vector<Type, M> operator/(Type scalar) const
+    {
+        return (*this)*(1.0/scalar);
+    }
+
    Vector<Type, M> operator+(Type scalar) const
    {
        Vector<Type, M> res;
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@ -6,11 +6,9 @@ set(tests
				@@ -6,11 +6,9 @@ set(tests
 	matrixAssignment
 	matrixScalarMult
 	transpose
-	quaternion
-	dcm
 	vector
 	vector3
-	euler
+	attitude
 	filter
 	)

--- a/test/attitude.cpp
+++ b/test/attitude.cpp
@ -0,0 +1,73 @@
				@@ -0,0 +1,73 @@
+#include <cassert>
+#include <cstdio>
+
+#include "Quaternion.hpp"
+#include "Vector3.hpp"
+#include "matrix.hpp"
+
+using namespace matrix;
+
+template class Quaternion<float>;
+template class Euler<float>;
+template class Dcm<float>;
+
+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 : */
--- a/test/dcm.cpp
+++ b/test/dcm.cpp
@ -1,19 +0,0 @@
				@@ -1,19 +0,0 @@
-#include <assert.h>
-#include <stdio.h>
-
-#include "matrix.hpp"
-
-using namespace matrix;
-
-int main()
-{
-    Dcmf dcm;
-    Quatf q(1,0,0,0);
-    dcm = Dcmf(q);
-    Matrix3f I = eye<float, 3>();
-    dcm = Dcmf(q);
-    Eulerf e = Eulerf(dcm);
-    return 0;
-};
-
-/* vim: set et fenc=utf-8 ff=unix sts=0 sw=4 ts=4 : */
--- a/test/euler.cpp
+++ b/test/euler.cpp
@ -1,20 +0,0 @@
				@@ -1,20 +0,0 @@
-#include "Euler.hpp"
-#include "Scalar.hpp"
-#include <assert.h>
-#include <stdio.h>
-
-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 : */
--- a/test/quaternion.cpp
+++ b/test/quaternion.cpp
@ -1,50 +0,0 @@
				@@ -1,50 +0,0 @@
-#include "Quaternion.hpp"
-#include <cassert>
-#include <cstdio>
-
-using namespace matrix;
-
-// instantiate so coverage works
-template class Quaternion<float>;
-template class Euler<float>;
-template class Dcm<float>;
-
-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 : */
--- a/test/test_data.py
+++ b/test/test_data.py
@ -0,0 +1,104 @@
				@@ -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 : 
--- a/test/test_math.py
+++ b/test/test_math.py
@ -1,66 +0,0 @@
				@@ -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 :