From 464c5245f264b974646b146b5cc1ddea44fbcbf6 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sun, 13 Jan 2013 17:09:22 -0500 Subject: Rebase of changes to sensor_hil_fixedwing branch. --- apps/mathlib/math/Dcm.cpp | 77 ++++++++++++++++++++++---------- apps/mathlib/math/Dcm.hpp | 7 +++ apps/mathlib/math/EulerAngles.cpp | 26 ++++++----- apps/mathlib/math/Quaternion.cpp | 59 +++++++++++------------- apps/mathlib/math/nasa_rotation_def.pdf | Bin 0 -> 709235 bytes apps/mathlib/math/test_math.sce | 63 ++++++++++++++++++++++++++ 6 files changed, 164 insertions(+), 68 deletions(-) create mode 100644 apps/mathlib/math/nasa_rotation_def.pdf create mode 100644 apps/mathlib/math/test_math.sce (limited to 'apps/mathlib') diff --git a/apps/mathlib/math/Dcm.cpp b/apps/mathlib/math/Dcm.cpp index 59f3d88ab..467c6f34e 100644 --- a/apps/mathlib/math/Dcm.cpp +++ b/apps/mathlib/math/Dcm.cpp @@ -52,6 +52,23 @@ Dcm::Dcm() : { } +Dcm::Dcm(float c00, float c01, float c02, + float c10, float c11, float c12, + float c20, float c21, float c22) : + Matrix(3, 3) +{ + Dcm &dcm = *this; + dcm(0,0) = c00; + dcm(0,1) = c01; + dcm(0,2) = c02; + dcm(1,0) = c10; + dcm(1,1) = c11; + dcm(1,2) = c12; + dcm(2,0) = c20; + dcm(2,1) = c21; + dcm(2,2) = c22; +} + Dcm::Dcm(const float *data) : Matrix(3, 3, data) { @@ -61,22 +78,22 @@ Dcm::Dcm(const Quaternion &q) : Matrix(3, 3) { Dcm &dcm = *this; - float a = q.getA(); - float b = q.getB(); - float c = q.getC(); - float d = q.getD(); - float aSq = a * a; - float bSq = b * b; - float cSq = c * c; - float dSq = d * d; + double a = q.getA(); + double b = q.getB(); + double c = q.getC(); + double d = q.getD(); + double aSq = a * a; + double bSq = b * b; + double cSq = c * c; + double dSq = d * d; dcm(0, 0) = aSq + bSq - cSq - dSq; - dcm(0, 1) = 2 * (b * c - a * d); - dcm(0, 2) = 2 * (a * c + b * d); - dcm(1, 0) = 2 * (b * c + a * d); + dcm(0, 1) = 2.0 * (b * c - a * d); + dcm(0, 2) = 2.0 * (a * c + b * d); + dcm(1, 0) = 2.0 * (b * c + a * d); dcm(1, 1) = aSq - bSq + cSq - dSq; - dcm(1, 2) = 2 * (c * d - a * b); - dcm(2, 0) = 2 * (b * d - a * c); - dcm(2, 1) = 2 * (a * b + c * d); + dcm(1, 2) = 2.0 * (c * d - a * b); + dcm(2, 0) = 2.0 * (b * d - a * c); + dcm(2, 1) = 2.0 * (a * b + c * d); dcm(2, 2) = aSq - bSq - cSq + dSq; } @@ -84,12 +101,12 @@ Dcm::Dcm(const EulerAngles &euler) : Matrix(3, 3) { Dcm &dcm = *this; - float cosPhi = cosf(euler.getPhi()); - float sinPhi = sinf(euler.getPhi()); - float cosThe = cosf(euler.getTheta()); - float sinThe = sinf(euler.getTheta()); - float cosPsi = cosf(euler.getPsi()); - float sinPsi = sinf(euler.getPsi()); + double cosPhi = cos(euler.getPhi()); + double sinPhi = sin(euler.getPhi()); + double cosThe = cos(euler.getTheta()); + double sinThe = sin(euler.getTheta()); + double cosPsi = cos(euler.getPsi()); + double sinPsi = sin(euler.getPsi()); dcm(0, 0) = cosThe * cosPsi; dcm(0, 1) = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi; @@ -116,11 +133,23 @@ Dcm::~Dcm() int __EXPORT dcmTest() { printf("Test DCM\t\t: "); - Vector3 vB(1, 2, 3); - ASSERT(matrixEqual(Dcm(Quaternion(1, 0, 0, 0)), - Matrix::identity(3))); - ASSERT(matrixEqual(Dcm(EulerAngles(0, 0, 0)), + // default ctor + ASSERT(matrixEqual(Dcm(), Matrix::identity(3))); + // quaternion ctor + ASSERT(matrixEqual( + Dcm(Quaternion(0.983347, 0.034271, 0.106021, 0.143572)), + Dcm( 0.9362934, -0.2750958, 0.2183507, + 0.2896295, 0.9564251, -0.0369570, + -0.1986693, 0.0978434, 0.9751703))); + // euler angle ctor + ASSERT(matrixEqual( + Dcm(EulerAngles(0.1, 0.2, 0.3)), + Dcm( 0.9362934, -0.2750958, 0.2183507, + 0.2896295, 0.9564251, -0.0369570, + -0.1986693, 0.0978434, 0.9751703))); + // rotations + Vector3 vB(1, 2, 3); ASSERT(vectorEqual(Vector3(-2, 1, 3), Dcm(EulerAngles(0, 0, M_PI_2_F))*vB)); ASSERT(vectorEqual(Vector3(3, 2, -1), diff --git a/apps/mathlib/math/Dcm.hpp b/apps/mathlib/math/Dcm.hpp index de69a7aa4..781933e9e 100644 --- a/apps/mathlib/math/Dcm.hpp +++ b/apps/mathlib/math/Dcm.hpp @@ -64,6 +64,13 @@ public: */ Dcm(); + /** + * scalar ctor + */ + Dcm(float c00, float c01, float c02, + float c10, float c11, float c12, + float c20, float c21, float c22); + /** * data ctor */ diff --git a/apps/mathlib/math/EulerAngles.cpp b/apps/mathlib/math/EulerAngles.cpp index 8991d5623..27ebbf8b3 100644 --- a/apps/mathlib/math/EulerAngles.cpp +++ b/apps/mathlib/math/EulerAngles.cpp @@ -97,23 +97,27 @@ EulerAngles::~EulerAngles() int __EXPORT eulerAnglesTest() { printf("Test EulerAngles\t: "); - EulerAngles euler(1, 2, 3); + EulerAngles euler(0.1, 0.2, 0.3); // test ctor - ASSERT(vectorEqual(Vector3(1, 2, 3), euler)); - ASSERT(equal(euler.getPhi(), 1)); - ASSERT(equal(euler.getTheta(), 2)); - ASSERT(equal(euler.getPsi(), 3)); + ASSERT(vectorEqual(Vector3(0.1, 0.2, 0.3), euler)); + ASSERT(equal(euler.getPhi(), 0.1)); + ASSERT(equal(euler.getTheta(), 0.2)); + ASSERT(equal(euler.getPsi(), 0.3)); // test dcm ctor + euler = Dcm(EulerAngles(0.1,0.2,0.3)); + ASSERT(vectorEqual(Vector3(0.1,0.2,0.3),euler)); + + // test quat ctor + euler = Quaternion(EulerAngles(0.1,0.2,0.3)); + ASSERT(vectorEqual(Vector3(0.1,0.2,0.3),euler)); // test assignment - euler.setPhi(4); - ASSERT(equal(euler.getPhi(), 4)); - euler.setTheta(5); - ASSERT(equal(euler.getTheta(), 5)); - euler.setPsi(6); - ASSERT(equal(euler.getPsi(), 6)); + euler.setPhi(0.4); + euler.setTheta(0.5); + euler.setPsi(0.6); + ASSERT(vectorEqual(Vector3(0.4,0.5,0.6),euler)); printf("PASS\n"); return 0; diff --git a/apps/mathlib/math/Quaternion.cpp b/apps/mathlib/math/Quaternion.cpp index 68fe85300..4f07840c3 100644 --- a/apps/mathlib/math/Quaternion.cpp +++ b/apps/mathlib/math/Quaternion.cpp @@ -79,32 +79,34 @@ Quaternion::Quaternion(const Vector &v) : Quaternion::Quaternion(const Dcm &dcm) : Vector(4) { - setA(0.5f * sqrtf(1 + dcm(0, 0) + - dcm(1, 1) + dcm(2, 2))); - setB((dcm(2, 1) - dcm(1, 2)) / - (4 * getA())); - setC((dcm(0, 2) - dcm(2, 0)) / - (4 * getA())); - setD((dcm(1, 0) - dcm(0, 1)) / - (4 * getA())); + // avoiding singularities by not using + // division equations + setA(0.5 * sqrt(1.0 + + double( dcm(0, 0) + dcm(1, 1) + dcm(2, 2)))); + setB(0.5 * sqrt(1.0 + + double( dcm(0, 0) - dcm(1, 1) - dcm(2, 2)))); + setC(0.5 * sqrt(1.0 + + double(-dcm(0, 0) + dcm(1, 1) - dcm(2, 2)))); + setD(0.5 * sqrt(1.0 + + double(-dcm(0, 0) - dcm(1, 1) + dcm(2, 2)))); } Quaternion::Quaternion(const EulerAngles &euler) : Vector(4) { - float cosPhi_2 = cosf(euler.getPhi() / 2.0f); - float cosTheta_2 = cosf(euler.getTheta() / 2.0f); - float cosPsi_2 = cosf(euler.getPsi() / 2.0f); - float sinPhi_2 = sinf(euler.getPhi() / 2.0f); - float sinTheta_2 = sinf(euler.getTheta() / 2.0f); - float sinPsi_2 = sinf(euler.getPsi() / 2.0f); + double cosPhi_2 = cos(double(euler.getPhi()) / 2.0); + double sinPhi_2 = sin(double(euler.getPhi()) / 2.0); + double cosTheta_2 = cos(double(euler.getTheta()) / 2.0); + double sinTheta_2 = sin(double(euler.getTheta()) / 2.0); + double cosPsi_2 = cos(double(euler.getPsi()) / 2.0); + double sinPsi_2 = sin(double(euler.getPsi()) / 2.0); setA(cosPhi_2 * cosTheta_2 * cosPsi_2 + sinPhi_2 * sinTheta_2 * sinPsi_2); setB(sinPhi_2 * cosTheta_2 * cosPsi_2 - cosPhi_2 * sinTheta_2 * sinPsi_2); setC(cosPhi_2 * sinTheta_2 * cosPsi_2 + sinPhi_2 * cosTheta_2 * sinPsi_2); - setD(cosPhi_2 * cosTheta_2 * sinPsi_2 + + setD(cosPhi_2 * cosTheta_2 * sinPsi_2 - sinPhi_2 * sinTheta_2 * cosPsi_2); } @@ -147,33 +149,24 @@ int __EXPORT quaternionTest() ASSERT(equal(q.getC(), 0)); ASSERT(equal(q.getD(), 0)); // test float ctor - q = Quaternion(0, 1, 0, 0); - ASSERT(equal(q.getA(), 0)); - ASSERT(equal(q.getB(), 1)); - ASSERT(equal(q.getC(), 0)); - ASSERT(equal(q.getD(), 0)); + q = Quaternion(0.1825742, 0.3651484, 0.5477226, 0.7302967); + ASSERT(equal(q.getA(), 0.1825742)); + ASSERT(equal(q.getB(), 0.3651484)); + ASSERT(equal(q.getC(), 0.5477226)); + ASSERT(equal(q.getD(), 0.7302967)); // test euler ctor - q = Quaternion(EulerAngles(0, 0, 0)); - ASSERT(equal(q.getA(), 1)); - ASSERT(equal(q.getB(), 0)); - ASSERT(equal(q.getC(), 0)); - ASSERT(equal(q.getD(), 0)); + q = Quaternion(EulerAngles(0.1, 0.2, 0.3)); + ASSERT(vectorEqual(q, Quaternion(0.983347, 0.034271, 0.106021, 0.143572))); // test dcm ctor q = Quaternion(Dcm()); - ASSERT(equal(q.getA(), 1)); - ASSERT(equal(q.getB(), 0)); - ASSERT(equal(q.getC(), 0)); - ASSERT(equal(q.getD(), 0)); + ASSERT(vectorEqual(q, Quaternion(1, 0, 0, 0))); // TODO test derivative // test accessors q.setA(0.1); q.setB(0.2); q.setC(0.3); q.setD(0.4); - ASSERT(equal(q.getA(), 0.1)); - ASSERT(equal(q.getB(), 0.2)); - ASSERT(equal(q.getC(), 0.3)); - ASSERT(equal(q.getD(), 0.4)); + ASSERT(vectorEqual(q, Quaternion(0.1, 0.2, 0.3, 0.4))); printf("PASS\n"); return 0; } diff --git a/apps/mathlib/math/nasa_rotation_def.pdf b/apps/mathlib/math/nasa_rotation_def.pdf new file mode 100644 index 000000000..eb67a4bfc Binary files /dev/null and b/apps/mathlib/math/nasa_rotation_def.pdf differ diff --git a/apps/mathlib/math/test_math.sce b/apps/mathlib/math/test_math.sce new file mode 100644 index 000000000..c3fba4729 --- /dev/null +++ b/apps/mathlib/math/test_math.sce @@ -0,0 +1,63 @@ +clc +clear +function out = float_truncate(in, digits) + out = round(in*10^digits) + out = out/10^digits +endfunction + +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 = [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] + +disp(C_nb) +//C_nb = float_truncate(C_nb,3) +//disp(C_nb) + +theta = asin(-C_nb(3,1)) +phi = atan(C_nb(3,2), C_nb(3,3)) +psi = atan(C_nb(2,1), C_nb(1,1)) +printf('phi %f\n', phi) +printf('theta %f\n', theta) +printf('psi %f\n', psi) + +q = [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] + +//q = float_truncate(q,3) + +a = q(1) +b = q(2) +c = q(3) +d = q(4) +printf('q: %f %f %f %f\n', a, b, c, d) +a2 = a*a +b2 = b*b +c2 = c*c +d2 = d*d + +C2_nb = [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] + +disp(C2_nb) -- cgit v1.2.3