Added math library.

apps/examples/math_demo/Makefile

@@ -0,0 +1,42 @@
+############################################################################
+#
+#   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Basic example application
+#
+
+APPNAME		 = math_demo
+PRIORITY	 = SCHED_PRIORITY_DEFAULT
+STACKSIZE	 = 8192
+
+include $(APPDIR)/mk/app.mk

apps/examples/math_demo/math_demo.cpp

@@ -0,0 +1,110 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Example User <mail@example.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file math_demo.cpp
+ * Demonstration of math library
+ */
+
+#include <nuttx/config.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <systemlib/systemlib.h>
+#include <systemlib/math/Vector.hpp>
+#include <systemlib/math/Matrix.hpp>
+#include <systemlib/math/Quaternion.hpp>
+#include <systemlib/math/Vector3.hpp>
+#include <systemlib/math/Dcm.hpp>
+#include <systemlib/math/EulerAngles.hpp>
+
+/**
+ * Management function.
+ */
+extern "C" __EXPORT int math_demo_main(int argc, char *argv[]);
+
+/**
+ * Test function
+ */
+void test();
+
+/**
+ * Print the correct usage.
+ */
+static void usage(const char *reason);
+
+static void
+usage(const char *reason)
+{
+    if (reason)
+        fprintf(stderr, "%s\n", reason);
+    fprintf(stderr, "usage: math_demo {test}\n\n");
+    exit(1);
+}
+
+/**
+ * The deamon app only briefly exists to start
+ * the background job. The stack size assigned in the
+ * Makefile does only apply to this management task.
+ * 
+ * The actual stack size should be set in the call
+ * to task_create().
+ */
+int math_demo_main(int argc, char *argv[])
+{
+
+    if (argc < 1)
+        usage("missing command");
+
+    if (!strcmp(argv[1], "test")) {
+        test();
+        exit(0);
+    }
+
+    usage("unrecognized command");
+    exit(1);
+}
+
+void test()
+{
+    printf("beginning math lib test\n");
+    using namespace math;
+    vectorTest();
+    matrixTest();
+    vector3Test();
+    eulerAnglesTest();
+    quaternionTest();
+    dcmTest();
+}

apps/systemlib/math/Dcm.cpp

@@ -0,0 +1,136 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Dcm.cpp
+ *
+ * math direction cosine matrix
+ */
+
+#include <systemlib/test/test.hpp>
+
+#include "Dcm.hpp"
+#include "Quaternion.hpp"
+#include "EulerAngles.hpp"
+#include "Vector3.hpp"
+
+namespace math
+{
+
+Dcm::Dcm() :
+    Matrix(Matrix::identity(3))
+{
+}
+
+Dcm::Dcm(const float * data) :
+    Matrix(3,3,data)
+{
+}
+
+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;
+    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(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(2,2) = aSq - bSq - cSq + dSq;
+}
+
+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());
+
+    dcm(0,0) = cosThe*cosPsi;
+    dcm(0,1) = -cosPhi*sinPsi + sinPhi*sinThe*cosPsi;
+    dcm(0,2) = sinPhi*sinPsi + cosPhi*sinThe*cosPsi;
+
+    dcm(1,0) = cosThe*sinPsi;
+    dcm(1,1) = cosPhi*cosPsi + sinPhi*sinThe*sinPsi;
+    dcm(1,2) = -sinPhi*cosPsi + cosPhi*sinThe*sinPsi;
+
+    dcm(2,0) = -sinThe;
+    dcm(2,1) = sinPhi*cosThe;
+    dcm(2,2) = cosPhi*cosThe;
+}
+
+Dcm::Dcm(const Dcm & right) :
+    Matrix(right)
+{
+}
+
+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)),
+                    Matrix::identity(3)));
+    ASSERT(vectorEqual(Vector3(-2,1,3),
+        Dcm(EulerAngles(0,0,M_PI_2_F))*vB));
+    ASSERT(vectorEqual(Vector3(3,2,-1),
+        Dcm(EulerAngles(0,M_PI_2_F,0))*vB));
+    ASSERT(vectorEqual(Vector3(1,-3,2),
+        Dcm(EulerAngles(M_PI_2_F,0,0))*vB));
+    ASSERT(vectorEqual(Vector3(3,2,-1),
+        Dcm(EulerAngles(
+                M_PI_2_F,M_PI_2_F,M_PI_2_F))*vB));
+    printf("PASS\n");
+    return 0;
+}
+} // namespace math

apps/systemlib/math/Dcm.hpp

@@ -0,0 +1,95 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Dcm.hpp
+ *
+ * math direction cosine matrix
+ */
+
+//#pragma once
+
+#include "Vector.hpp"
+#include "Matrix.hpp"
+
+namespace math {
+
+class Quaternion;
+class EulerAngles;
+
+/**
+ * This is a Tait Bryan, Body 3-2-1 sequence.
+ * (yaw)-(pitch)-(roll)
+ * The Dcm transforms a vector in the body frame
+ * to the navigation frame, typically represented
+ * as C_nb. C_bn can be obtained through use 
+ * of the transpose() method.
+ */
+class __EXPORT Dcm : public Matrix
+{
+public:
+    /**
+     * default ctor
+     */
+    Dcm();
+
+    /**
+     * data ctor
+     */
+    Dcm(const float * data);
+
+    /**
+     * quaternion ctor
+     */
+    Dcm(const Quaternion & q);
+
+    /**
+     * euler angles ctor
+     */
+    Dcm(const EulerAngles & euler);
+
+    /**
+     * copy ctor (deep)
+     */
+    Dcm(const Dcm & right);
+
+    /**
+     * dtor
+     */
+    virtual ~Dcm();
+};
+
+int __EXPORT dcmTest();
+
+} // math
+

apps/systemlib/math/EulerAngles.cpp

@@ -0,0 +1,124 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Vector.cpp
+ *
+ * math vector
+ */
+
+#include <systemlib/test/test.hpp>
+
+#include "EulerAngles.hpp"
+#include "Quaternion.hpp"
+#include "Dcm.hpp"
+#include "Vector3.hpp"
+
+namespace math
+{
+
+EulerAngles::EulerAngles() :
+    Vector(3)
+{
+    setPhi(0.0f);
+    setTheta(0.0f);
+    setPsi(0.0f);
+}
+
+EulerAngles::EulerAngles(float phi, float theta, float psi) :
+    Vector(3)
+{
+    setPhi(phi);
+    setTheta(theta);
+    setPsi(psi);
+}
+
+EulerAngles::EulerAngles(const Quaternion & q) :
+    Vector(3)
+{
+    (*this) = EulerAngles(Dcm(q));
+}
+
+EulerAngles::EulerAngles(const Dcm & dcm) :
+    Vector(3)
+{
+    setTheta(asinf(-dcm(2,0))); 
+    if (fabsf(getTheta() - M_PI_2_F) < 1.0e-3f)
+    {
+        setPhi(0.0f);
+        setPsi(atan2f(dcm(1,2) - dcm(0,1),
+            dcm(0,2) + dcm(1,1)) + getPhi());
+    }
+    else if (fabsf(getTheta() + M_PI_2_F) < 1.0e-3f)
+    {
+        setPhi(0.0f);
+        setPsi(atan2f(dcm(1,2) - dcm(0,1),
+            dcm(0,2) + dcm(1,1)) - getPhi());
+    }
+    else
+    {
+        setPhi(atan2f(dcm(2,1),dcm(2,2)));
+        setPsi(atan2f(dcm(1,0),dcm(0,0)));
+    }
+}
+
+EulerAngles::~EulerAngles()
+{
+}
+
+int __EXPORT eulerAnglesTest()
+{
+    printf("Test EulerAngles\t: ");
+    EulerAngles euler(1,2,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));
+
+    // test dcm ctor
+
+    // 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));
+    
+    printf("PASS\n");
+    return 0;
+}
+
+} // namespace math

apps/systemlib/math/EulerAngles.hpp

@@ -0,0 +1,73 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Vector.h
+ *
+ * math vector
+ */
+
+#pragma once
+
+#include "Vector.hpp"
+
+namespace math {
+
+class Quaternion;
+class Dcm;
+
+class __EXPORT EulerAngles : public Vector
+{
+public:
+    EulerAngles();
+    EulerAngles(float phi, float theta, float psi);
+    EulerAngles(const Quaternion & q);
+    EulerAngles(const Dcm & dcm);
+    virtual ~EulerAngles();
+
+    // alias
+    void setPhi(float phi) { (*this)(0) = phi; }
+    void setTheta(float theta) { (*this)(1) = theta; }
+    void setPsi(float psi) { (*this)(2) = psi; }
+
+    // const accessors
+    const float & getPhi() const { return (*this)(0); }
+    const float & getTheta() const { return (*this)(1); }
+    const float & getPsi() const { return (*this)(2); }
+
+};
+
+int __EXPORT eulerAnglesTest();
+
+} // math
+

apps/systemlib/math/Matrix.cpp

@@ -0,0 +1,180 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Matrix.cpp
+ *
+ * matrix code
+ */
+
+#include <systemlib/test/test.hpp>
+#include <math.h>
+
+#include "Matrix.hpp"
+
+namespace math
+{
+
+static const float data_testA[] = 
+    {1,2,3,
+     4,5,6};
+static Matrix testA(2,3,data_testA);
+
+static const float data_testB[] = 
+    {0,1,3,
+     7,-1,2};
+static Matrix testB(2,3,data_testB);
+
+static const float data_testC[] = 
+    {0,1,
+     2,1,
+     3,2};
+static Matrix testC(3,2,data_testC);
+
+static const float data_testD[] = 
+    {0,1,2,
+     2,1,4,
+     5,2,0};
+static Matrix testD(3,3,data_testD);
+
+static const float data_testE[] = 
+    {1,-1,2,
+     0,2,3,
+     2,-1,1};
+static Matrix testE(3,3,data_testE);
+
+static const float data_testF[] = 
+    {3.777e006f, 2.915e007f, 0.000e000f,
+     2.938e007f, 2.267e008f, 0.000e000f,
+     0.000e000f, 0.000e000f, 6.033e008f};
+static Matrix testF(3,3,data_testF);
+
+int __EXPORT matrixTest()
+{
+    matrixAddTest();
+    matrixSubTest();
+    matrixMultTest();
+    matrixInvTest();
+    matrixDivTest();
+    return 0;
+}
+
+int matrixAddTest()
+{
+    printf("Test Matrix Add\t\t: ");
+    Matrix r = testA + testB;
+    float data_test[] =
+        { 1.0f, 3.0f, 6.0f,
+         11.0f, 4.0f, 8.0f};
+    ASSERT(matrixEqual(Matrix(2,3,data_test),r));
+    printf("PASS\n");
+    return 0;
+}
+
+int matrixSubTest()
+{
+    printf("Test Matrix Sub\t\t: ");
+    Matrix r = testA - testB;
+    float data_test[] =
+        { 1.0f, 1.0f, 0.0f,
+         -3.0f, 6.0f, 4.0f};
+    ASSERT(matrixEqual(Matrix(2,3,data_test),r));
+    printf("PASS\n");
+    return 0;
+}
+
+int matrixMultTest()
+{
+    printf("Test Matrix Mult\t: ");
+    Matrix r = testC * testB;
+    float data_test[] =
+        { 7.0f, -1.0f,  2.0f,
+          7.0f,  1.0f,  8.0f,
+         14.0f,  1.0f, 13.0f};
+    ASSERT(matrixEqual(Matrix(3,3,data_test),r));
+    printf("PASS\n");
+    return 0;
+}
+
+int matrixInvTest()
+{
+    printf("Test Matrix Inv\t\t: ");
+    Matrix origF = testF;
+    Matrix r = testF.inverse();
+    float data_test[] =
+        { -0.0012518f,  0.0001610f, 0.0000000f,
+           0.0001622f, -0.0000209f, 0.0000000f,
+           0.0000000f,  0.0000000f, 1.6580e-9f};
+    ASSERT(matrixEqual(Matrix(3,3,data_test),r));
+    // make sure F in unchanged
+    ASSERT(matrixEqual(origF,testF));
+    printf("PASS\n");
+    return 0;
+}
+
+int matrixDivTest()
+{
+    printf("Test Matrix Div\t\t: ");
+    Matrix r = testD / testE;
+    float data_test[] = {
+         0.2222222f, 0.5555556f, -0.1111111f,
+               0.0f,       1.0f,         1.0,
+        -4.1111111f, 1.2222222f,  4.5555556f};
+    ASSERT(matrixEqual(Matrix(3,3,data_test),r));
+    printf("PASS\n");
+    return 0;
+}
+
+bool matrixEqual(const Matrix & a, const Matrix & b, float eps)
+{
+    if (a.getRows() != b.getRows()) {
+        printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows());
+        return false;
+    } else if (a.getCols() != b.getCols()) {
+        printf("column number not equal a: %d, b:%d\n", a.getCols(), b.getCols());
+        return false;
+    }
+    bool ret = true;
+    for (size_t i=0;i<a.getRows();i++)
+        for (size_t j =0;j<a.getCols();j++)
+    {
+        if (!equal(a(i,j),b(i,j),eps))
+        {
+            printf("element mismatch (%d, %d)\n", i, j);
+            ret = false;
+        }
+    }
+    return ret;
+}
+ 
+} // namespace math

apps/systemlib/math/Matrix.hpp

@@ -0,0 +1,58 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Matrix.h
+ *
+ * matrix code
+ */
+
+#pragma once
+
+#ifdef ARM_MATH_CM4
+#include "arm/Matrix.hpp"
+#else
+#include "generic/Matrix.hpp"
+#endif
+
+namespace math {
+class Matrix;
+int matrixTest();
+int matrixAddTest();
+int matrixSubTest();
+int matrixMultTest();
+int matrixInvTest();
+int matrixDivTest();
+int matrixArmTest();
+bool matrixEqual(const Matrix & a, const Matrix & b, float eps=1.0e-5f);
+} // namespace math

apps/systemlib/math/Quaternion.cpp

@@ -0,0 +1,180 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Quaternion.cpp
+ *
+ * math vector
+ */
+
+#include <systemlib/test/test.hpp>
+
+
+#include "Quaternion.hpp"
+#include "Dcm.hpp"
+#include "EulerAngles.hpp"
+
+namespace math
+{
+
+Quaternion::Quaternion() :
+    Vector(4)
+{
+    setA(1.0f);
+    setB(0.0f);
+    setC(0.0f);
+    setD(0.0f);
+}
+
+Quaternion::Quaternion(float a, float b,
+        float c, float d) :
+    Vector(4)
+{
+    setA(a);
+    setB(b);
+    setC(c);
+    setD(d);
+}
+
+Quaternion::Quaternion(const float * data) :
+    Vector(4,data)
+{
+}
+
+Quaternion::Quaternion(const Vector & v) :
+    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()));
+}
+
+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);
+    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 +
+        sinPhi_2*sinTheta_2*cosPsi_2);
+}
+
+Quaternion::Quaternion(const Quaternion & right) :
+    Vector(right)
+{
+}
+
+Quaternion::~Quaternion()
+{
+}
+
+Vector Quaternion::derivative(const Vector & w)
+{
+#ifdef QUATERNION_ASSERT
+    ASSERT(w.getRows()==3);
+#endif
+    float dataQ[] = 
+    {getA(), -getB(), -getC(), -getD(),
+     getB(),  getA(), -getD(),  getC(),
+     getC(),  getD(),  getA(), -getB(),
+     getD(), -getC(),  getB(),  getA()};
+    Vector v(4);
+    v(0) = 0.0f;
+    v(1) = w(0);
+    v(2) = w(1);
+    v(3) = w(2);
+    Matrix Q(4,4,dataQ);
+    return Q*v*0.5f; 
+}
+
+int __EXPORT quaternionTest()
+{
+    printf("Test Quaternion\t\t: ");
+    // test default ctor
+    Quaternion q;
+    ASSERT(equal(q.getA(),1));  
+    ASSERT(equal(q.getB(),0));  
+    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));  
+    // 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));     
+    // 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));     
+    // 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));  
+    printf("PASS\n");
+    return 0;
+}
+
+} // namespace math

apps/systemlib/math/Quaternion.hpp

@@ -0,0 +1,114 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Quaternion.hpp
+ *
+ * math quaternion lib
+ */
+
+//#pragma once
+
+#include "Vector.hpp"
+#include "Matrix.hpp"
+
+namespace math {
+
+class Dcm;
+class EulerAngles;
+
+class __EXPORT Quaternion : public Vector
+{
+public:
+
+    /**
+     * default ctor
+     */
+    Quaternion();
+
+    /**
+     * ctor from floats
+     */
+    Quaternion(float a, float b, float c, float d);
+
+    /**
+     * ctor from data
+     */
+    Quaternion(const float * data);
+
+    /**
+     * ctor from Vector
+     */
+    Quaternion(const Vector & v);
+
+    /**
+     * ctor from EulerAngles
+     */
+    Quaternion(const EulerAngles & euler);
+
+    /**
+     * ctor from Dcm
+     */
+    Quaternion(const Dcm & dcm);
+
+    /**
+     * deep copy ctor
+     */
+    Quaternion(const Quaternion & right);
+
+    /**
+     * dtor
+     */
+    virtual ~Quaternion();
+    
+    /**
+     * derivative
+     */
+    Vector derivative(const Vector & w);
+
+    /**
+     * accessors
+     */
+    void setA(float a) { (*this)(0) = a; }
+    void setB(float b) { (*this)(1) = b; }
+    void setC(float c) { (*this)(2) = c; }
+    void setD(float d) { (*this)(3) = d; }
+    const float & getA() const { return (*this)(0); }
+    const float & getB() const { return (*this)(1); }
+    const float & getC() const { return (*this)(2); }
+    const float & getD() const { return (*this)(3); }
+};
+
+int __EXPORT quaternionTest();
+} // math
+

apps/systemlib/math/Vector.cpp

@@ -0,0 +1,99 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Vector.cpp
+ *
+ * math vector
+ */
+
+#include <systemlib/test/test.hpp>
+
+#include "Vector.hpp"
+
+namespace math
+{
+
+static const float data_testA[] = {1,3};
+static const float data_testB[] = {4,1};
+
+static Vector testA(2,data_testA);
+static Vector testB(2,data_testB);
+
+int __EXPORT vectorTest()
+{
+    vectorAddTest();
+    vectorSubTest();
+    return 0;
+}
+
+int vectorAddTest()
+{
+    printf("Test Vector Add\t\t: ");
+    Vector r = testA + testB;
+    float data_test[] = {5.0f, 4.0f};
+    ASSERT(vectorEqual(Vector(2,data_test),r));
+    printf("PASS\n");
+    return 0;
+}
+
+int vectorSubTest()
+{
+    printf("Test Vector Sub\t\t: ");
+    Vector r(2);
+    r = testA - testB;
+    float data_test[] = {-3.0f, 2.0f};
+    ASSERT(vectorEqual(Vector(2,data_test),r));
+    printf("PASS\n");
+    return 0;
+}
+
+bool vectorEqual(const Vector & a, const Vector & b, float eps)
+{
+    if (a.getRows() != b.getRows()) {
+        printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows());
+        return false;
+    }
+    bool ret = true;
+    for (size_t i=0;i<a.getRows();i++)
+    {
+        if (!equal(a(i),b(i),eps))
+        {
+            printf("element mismatch (%d)\n", i);
+            ret = false;
+        }
+    }
+    return ret;
+}
+
+} // namespace math

apps/systemlib/math/Vector.hpp

@@ -0,0 +1,54 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Vector.h
+ *
+ * math vector
+ */
+
+#pragma once
+
+#ifdef ARM_MATH_CM4
+#include "arm/Vector.hpp"
+#else
+#include "generic/Vector.hpp"
+#endif
+
+namespace math {
+class Vector;
+int __EXPORT vectorTest();
+int __EXPORT vectorAddTest();
+int __EXPORT vectorSubTest();
+bool vectorEqual(const Vector & a, const Vector & b, float eps=1.0e-5f);
+} // math

apps/systemlib/math/Vector3.cpp

@@ -0,0 +1,99 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Vector3.cpp
+ *
+ * math vector
+ */
+
+#include <systemlib/test/test.hpp>
+
+#include "Vector3.hpp"
+
+namespace math
+{
+
+Vector3::Vector3() :
+    Vector(3)
+{
+}
+
+Vector3::Vector3(const Vector & right) :
+    Vector(right)
+{
+#ifdef VECTOR_ASSERT
+    ASSERT(right.getRows()==3);
+#endif
+}
+
+Vector3::Vector3(float x, float y, float z) :
+    Vector(3)
+{
+    setX(x);
+    setY(y);
+    setZ(z);
+}
+
+Vector3::Vector3(const float * data) :
+    Vector(3,data)
+{
+}
+
+Vector3::~Vector3()
+{
+}
+
+Vector3 Vector3::cross(const Vector3 & b)
+{
+    Vector3 & a = *this;
+    Vector3 result;
+    result(0) = a(1)*b(2) - a(2)*b(1);
+    result(1) = a(2)*b(0) - a(0)*b(2);
+    result(2) = a(0)*b(1) - a(1)*b(0);
+    return result;
+}
+
+int __EXPORT vector3Test()
+{
+    printf("Test Vector3\t\t: ");
+    // test float ctor
+    Vector3 v(1,2,3);
+    ASSERT(equal(v(0),1)); 
+    ASSERT(equal(v(1),2)); 
+    ASSERT(equal(v(2),3)); 
+    printf("PASS\n");
+    return 0;
+}
+
+} // namespace math

apps/systemlib/math/Vector3.hpp

@@ -0,0 +1,71 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Vector3.hpp
+ *
+ * math 3 vector
+ */
+
+#pragma once
+
+#include "Vector.hpp"
+
+namespace math
+{
+
+class __EXPORT Vector3 :
+    public Vector
+{
+public:
+    Vector3();
+    Vector3(const Vector & right);
+    Vector3(float x, float y, float z);
+    Vector3(const float * data);
+    virtual ~Vector3();
+    Vector3 cross(const Vector3 & b);
+
+    /**
+     * accessors
+     */
+    void setX(float x) { (*this)(0) = x; }
+    void setY(float y) { (*this)(1) = y; }
+    void setZ(float z) { (*this)(2) = z; }
+    const float & getX() const { return (*this)(0); }
+    const float & getY() const { return (*this)(1); }
+    const float & getZ() const { return (*this)(2); }
+};
+
+int __EXPORT vector3Test();
+} // math
+

apps/systemlib/math/arm/Matrix.cpp

@@ -0,0 +1,40 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Matrix.cpp
+ *
+ * matrix code
+ */
+
+#include "Matrix.hpp"

apps/systemlib/math/arm/Matrix.hpp

@@ -0,0 +1,312 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Matrix.h
+ *
+ * matrix code
+ */
+
+#pragma once
+
+
+#include <inttypes.h>
+#include <assert.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdio.h>
+#include <math.h>
+
+#include <systemlib/math/Vector.hpp>
+#include <systemlib/math/Matrix.hpp>
+
+// arm specific
+#include "arm_math.h"
+
+namespace math
+{
+
+class __EXPORT Matrix {
+public:
+    // constructor
+    Matrix(size_t rows, size_t cols) :
+        _matrix()
+    {
+        arm_mat_init_f32(&_matrix,
+            rows, cols,
+            (float*)calloc(rows*cols,sizeof(float)));
+    }
+    Matrix(size_t rows, size_t cols, const float * data) :
+        _matrix()
+    {
+        arm_mat_init_f32(&_matrix,
+            rows, cols,
+            (float*)malloc(rows*cols*sizeof(float)));
+        memcpy(getData(),data,getSize());
+    }
+    // deconstructor
+    virtual ~Matrix()
+    {
+        delete [] _matrix.pData;
+    }
+    // copy constructor (deep)
+    Matrix(const Matrix & right) :
+        _matrix()
+    {
+        arm_mat_init_f32(&_matrix,
+            right.getRows(), right.getCols(),
+            (float*)malloc(right.getRows()*
+                right.getCols()*sizeof(float)));
+        memcpy(getData(),right.getData(),
+                getSize());
+    }
+    // assignment
+    inline Matrix & operator=(const Matrix & right)
+    {
+#ifdef MATRIX_ASSERT
+        ASSERT(getRows()==right.getRows());
+        ASSERT(getCols()==right.getCols());
+#endif
+        if (this != &right)
+        {
+            memcpy(getData(),right.getData(),
+                        right.getSize());
+        }
+        return *this;
+    }
+    // element accessors
+    inline float & operator()(size_t i, size_t j)
+    {
+#ifdef MATRIX_ASSERT
+        ASSERT(i<getRows());
+        ASSERT(j<getCols());
+#endif
+        return getData()[i*getCols() + j];
+    }
+    inline const float & operator()(size_t i, size_t j) const
+    {
+#ifdef MATRIX_ASSERT
+        ASSERT(i<getRows());
+        ASSERT(j<getCols());
+#endif
+        return getData()[i*getCols() + j];
+    }
+    // output
+    inline void print() const
+    {
+        for (size_t i=0; i<getRows(); i++)
+        {
+            for (size_t j=0; j<getCols(); j++)
+            {
+                float sig;
+                int exp;
+                float num = (*this)(i,j);
+                float2SigExp(num,sig,exp);
+                printf ("%6.3fe%03.3d,", (double)sig, exp);
+            }
+            printf("\n");
+        }
+    }
+    // boolean ops
+    inline bool operator==(const Matrix & right) const
+    {
+        for (size_t i=0; i<getRows(); i++)
+        {
+            for (size_t j=0; j<getCols(); j++)
+            {
+                if (fabsf((*this)(i,j)-right(i,j)) > 1e-30f)
+                    return false;
+            }
+        }
+        return true;
+    }
+    // scalar ops
+    inline Matrix operator+(float right) const
+    {
+        Matrix result(getRows(), getCols());
+        arm_offset_f32((float *)getData(),right,
+                (float *)result.getData(),getRows()*getCols());
+        return result;
+    }
+    inline Matrix operator-(float right) const
+    {
+        Matrix result(getRows(), getCols());
+        arm_offset_f32((float *)getData(),-right,
+                (float *)result.getData(),getRows()*getCols());
+        return result;
+    }
+    inline Matrix operator*(float right) const
+    {
+        Matrix result(getRows(), getCols());
+        arm_mat_scale_f32(&_matrix,right,
+                &(result._matrix));
+        return result;
+    }
+    inline Matrix operator/(float right) const
+    {
+        Matrix result(getRows(), getCols());
+        arm_mat_scale_f32(&_matrix,1.0f/right,
+                &(result._matrix));
+        return result;
+    }
+    // vector ops
+    inline Vector operator*(const Vector & right) const
+    {
+#ifdef MATRIX_ASSERT
+        ASSERT(getCols()==right.getRows());
+#endif
+        Matrix resultMat = (*this)*
+            Matrix(right.getRows(),1,right.getData());
+        return Vector(getRows(),resultMat.getData());
+    }
+    // matrix ops
+    inline Matrix operator+(const Matrix & right) const
+    {
+#ifdef MATRIX_ASSERT
+        ASSERT(getRows()==right.getRows());
+        ASSERT(getCols()==right.getCols());
+#endif
+        Matrix result(getRows(), getCols());
+        arm_mat_add_f32(&_matrix, &(right._matrix),
+                &(result._matrix));
+        return result;
+    }
+    inline Matrix operator-(const Matrix & right) const
+    {
+#ifdef MATRIX_ASSERT
+        ASSERT(getRows()==right.getRows());
+        ASSERT(getCols()==right.getCols());
+#endif
+        Matrix result(getRows(), getCols());
+        arm_mat_sub_f32(&_matrix, &(right._matrix),
+                &(result._matrix));
+        return result;
+    }
+    inline Matrix operator*(const Matrix & right) const
+    {
+#ifdef MATRIX_ASSERT
+        ASSERT(getCols()==right.getRows());
+#endif
+        Matrix result(getRows(), right.getCols());
+        arm_mat_mult_f32(&_matrix, &(right._matrix),
+                &(result._matrix));
+        return result;
+    }
+    inline Matrix operator/(const Matrix & right) const
+    {
+#ifdef MATRIX_ASSERT
+        ASSERT(right.getRows()==right.getCols());
+        ASSERT(getCols()==right.getCols());
+#endif
+        return (*this)*right.inverse();
+    }
+    // other functions
+    inline Matrix transpose() const
+    {
+        Matrix result(getCols(),getRows());
+        arm_mat_trans_f32(&_matrix, &(result._matrix));
+        return result;
+    }
+    inline void swapRows(size_t a, size_t b)
+    {
+        if (a==b) return;
+        for(size_t j=0;j<getCols();j++) {
+            float tmp = (*this)(a,j);
+            (*this)(a,j) = (*this)(b,j);
+            (*this)(b,j) = tmp;
+        }
+    }
+    inline void swapCols(size_t a, size_t b)
+    {
+        if (a==b) return;
+        for(size_t i=0;i<getRows();i++) {
+            float tmp = (*this)(i,a);
+            (*this)(i,a) = (*this)(i,b);
+            (*this)(i,b) = tmp;
+        }
+    }
+    /**
+     * inverse based on LU factorization with partial pivotting
+     */
+    Matrix inverse() const
+    {
+#ifdef MATRIX_ASSERT
+        ASSERT(getRows()==getCols());
+#endif
+        Matrix result(getRows(), getCols());
+        Matrix work = (*this);
+        arm_mat_inverse_f32(&(work._matrix),
+                &(result._matrix));
+        return result;
+    }
+    inline void setAll(const float & val)
+    {
+        for (size_t i=0;i<getRows();i++) {
+            for (size_t j=0;j<getCols();j++) {
+                (*this)(i,j) = val;
+            }
+        }
+    }
+    inline void set(const float * data)
+    {
+        memcpy(getData(),data,getSize());
+    }
+    inline size_t getRows() const { return _matrix.numRows; }
+    inline size_t getCols() const { return _matrix.numCols; }
+    inline static Matrix identity(size_t size) {
+        Matrix result(size,size);
+        for (size_t i=0; i<size; i++) {
+            result(i,i) = 1.0f; 
+        }
+        return result;
+    }
+    inline static Matrix zero(size_t size) {
+        Matrix result(size,size);
+        result.setAll(0.0f);
+        return result;
+    }
+    inline static Matrix zero(size_t m, size_t n) {
+        Matrix result(m,n);
+        result.setAll(0.0f);
+        return result;
+    }
+protected:
+    inline size_t getSize() const { return sizeof(float)*getRows()*getCols(); }
+    inline float * getData() { return _matrix.pData; }
+    inline const float * getData() const { return _matrix.pData; }
+    inline void setData(float * data) { _matrix.pData = data; }
+private:
+    arm_matrix_instance_f32 _matrix;
+};
+
+} // namespace math

apps/systemlib/math/arm/Vector.cpp

@@ -0,0 +1,40 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Vector.cpp
+ *
+ * math vector
+ */
+
+#include "Vector.hpp"

apps/systemlib/math/arm/Vector.hpp

@@ -0,0 +1,240 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Vector.h
+ *
+ * math vector
+ */
+
+#pragma once
+
+#include <inttypes.h>
+#include <assert.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdio.h>
+#include <math.h>
+
+#include <systemlib/math/Vector.hpp>
+#include <systemlib/test/test.hpp>
+
+// arm specific
+#include "arm_math.h"
+
+namespace math
+{
+
+class __EXPORT Vector {
+public:
+    // constructor
+    Vector(size_t rows) :
+        _rows(rows),
+        _data((float*)calloc(rows,sizeof(float)))
+    {
+    }
+    Vector(size_t rows, const float * data) :
+        _rows(rows),
+        _data((float*)malloc(getSize()))
+    {
+        memcpy(getData(),data,getSize());
+    }
+    // deconstructor
+    virtual ~Vector()
+    {
+        delete [] getData();
+    }
+    // copy constructor (deep)
+    Vector(const Vector & right) :
+        _rows(right.getRows()),
+        _data((float*)malloc(getSize()))
+    {
+        memcpy(getData(),right.getData(),
+                    right.getSize());
+    }
+    // assignment
+    inline Vector & operator=(const Vector & right)
+    {
+#ifdef VECTOR_ASSERT
+        ASSERT(getRows()==right.getRows());
+#endif
+        if (this != &right)
+        {
+            memcpy(getData(),right.getData(),
+                        right.getSize());
+        }
+        return *this;
+    }
+    // element accessors
+    inline float& operator()(size_t i)
+    {
+#ifdef VECTOR_ASSERT
+        ASSERT(i<getRows());
+#endif
+        return getData()[i];
+    }
+    inline const float& operator()(size_t i) const
+    {
+#ifdef VECTOR_ASSERT
+        ASSERT(i<getRows());
+#endif
+        return getData()[i];
+    }
+    // output
+    inline void print() const
+    {
+        for (size_t i=0; i<getRows(); i++)
+        {
+            float sig;
+            int exp;
+            float num = (*this)(i);
+            float2SigExp(num,sig,exp);
+            printf ("%6.3fe%03.3d,", (double)sig, exp);
+        }
+        printf("\n");
+    }
+    // boolean ops
+    inline bool operator==(const Vector & right) const
+    {
+        for (size_t i=0; i<getRows(); i++)
+        {
+            if (fabsf(((*this)(i) - right(i))) > 1e-30f)
+                return false;
+        }
+        return true;
+    }
+    // scalar ops
+    inline Vector operator+(float right) const
+    {
+        Vector result(getRows());
+        arm_offset_f32((float*)getData(),
+                right, result.getData(),
+                getRows());
+        return result;
+    }
+    inline Vector operator-(float right) const
+    {
+        Vector result(getRows());
+        arm_offset_f32((float*)getData(),
+                -right, result.getData(),
+                getRows());
+        return result;
+    }
+    inline Vector operator*(float right) const
+    {
+        Vector result(getRows());
+        arm_scale_f32((float*)getData(),
+                right, result.getData(),
+                getRows());
+        return result;
+    }
+    inline Vector operator/(float right) const
+    {
+        Vector result(getRows());
+        arm_scale_f32((float*)getData(),
+                1.0f/right, result.getData(),
+                getRows());
+        return result;
+    }
+    // vector ops
+    inline Vector operator+(const Vector & right) const
+    {
+#ifdef VECTOR_ASSERT
+        ASSERT(getRows()==right.getRows());
+#endif
+        Vector result(getRows());
+        arm_add_f32((float*)getData(),
+                (float*)right.getData(),
+                result.getData(),
+                getRows());
+        return result;
+    }
+    inline Vector operator-(const Vector & right) const
+    {
+#ifdef VECTOR_ASSERT
+        ASSERT(getRows()==right.getRows());
+#endif
+        Vector result(getRows());
+        arm_sub_f32((float*)getData(),
+                (float*)right.getData(),
+                result.getData(),
+                getRows());
+        return result;
+    }
+    // other functions
+    inline float dot(const Vector & right)
+    {
+        float result = 0;
+        arm_dot_prod_f32((float*)getData(),
+                (float*)right.getData(),
+                getRows(),
+                &result);
+        return result;
+    }
+    inline float norm()
+    {
+        return sqrtf(dot(*this));
+    }
+    inline Vector unit()
+    {
+        return (*this)/norm();
+    }
+    inline static Vector zero(size_t rows)
+    {
+        Vector result(rows);
+        // calloc returns zeroed memory
+        return result;
+    }
+    inline void setAll(const float & val)
+    {
+        for (size_t i=0;i<getRows();i++)
+        {
+            (*this)(i) = val;
+        }
+    }
+    inline void set(const float * data)
+    {
+        memcpy(getData(),data,getSize());
+    }
+    inline size_t getRows() const { return _rows; }
+    inline const float * getData() const { return _data; }
+protected:
+    inline size_t getSize() const { return sizeof(float)*getRows(); }
+    inline float * getData() { return _data; }
+    inline void setData(float * data) { _data = data; }
+private:
+    size_t _rows;
+    float * _data;
+};
+
+} // math

apps/systemlib/math/generic/Matrix.cpp

@@ -0,0 +1,40 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Matrix.cpp
+ *
+ * matrix code
+ */
+
+#include "Matrix.hpp"

apps/systemlib/math/generic/Matrix.hpp

@@ -0,0 +1,447 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Matrix.h
+ *
+ * matrix code
+ */
+
+#pragma once
+
+
+#include <inttypes.h>
+#include <assert.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdio.h>
+#include <math.h>
+
+#include <systemlib/math/Vector.hpp>
+#include <systemlib/math/Matrix.hpp>
+
+namespace math
+{
+
+class __EXPORT Matrix {
+public:
+    // constructor
+    Matrix(size_t rows, size_t cols) :
+        _rows(rows),
+        _cols(cols),
+        _data((float*)calloc(rows*cols,sizeof(float)))
+    {
+    }
+    Matrix(size_t rows, size_t cols, const float * data) :
+        _rows(rows),
+        _cols(cols),
+        _data((float*)malloc(getSize()))
+    {
+        memcpy(getData(),data,getSize());
+    }
+    // deconstructor
+    virtual ~Matrix()
+    {
+        delete [] getData();
+    }
+    // copy constructor (deep)
+    Matrix(const Matrix & right) :
+        _rows(right.getRows()),
+        _cols(right.getCols()),
+        _data((float*)malloc(getSize()))
+    {
+        memcpy(getData(),right.getData(),
+                    right.getSize());
+    }
+    // assignment
+    inline Matrix & operator=(const Matrix & right)
+    {
+#ifdef MATRIX_ASSERT
+        ASSERT(getRows()==right.getRows());
+        ASSERT(getCols()==right.getCols());
+#endif
+        if (this != &right)
+        {
+            memcpy(getData(),right.getData(),
+                        right.getSize());
+        }
+        return *this;
+    }
+    // element accessors
+    inline float & operator()(size_t i, size_t j)
+    {
+#ifdef MATRIX_ASSERT
+        ASSERT(i<getRows());
+        ASSERT(j<getCols());
+#endif
+        return getData()[i*getCols() + j];
+    }
+    inline const float & operator()(size_t i, size_t j) const
+    {
+#ifdef MATRIX_ASSERT
+        ASSERT(i<getRows());
+        ASSERT(j<getCols());
+#endif
+        return getData()[i*getCols() + j];
+    }
+    // output
+    inline void print() const
+    {
+        for (size_t i=0; i<getRows(); i++)
+        {
+            for (size_t j=0; j<getCols(); j++)
+            {
+                float sig;
+                int exp;
+                float num = (*this)(i,j);
+                float2SigExp(num,sig,exp);
+                printf ("%6.3fe%03.3d,", (double)sig, exp);
+            }
+            printf("\n");
+        }
+    }
+    // boolean ops
+    inline bool operator==(const Matrix & right) const
+    {
+        for (size_t i=0; i<getRows(); i++)
+        {
+            for (size_t j=0; j<getCols(); j++)
+            {
+                if (fabsf((*this)(i,j)-right(i,j)) > 1e-30f)
+                    return false;
+            }
+        }
+        return true;
+    }
+    // scalar ops
+    inline Matrix operator+(const float & right) const
+    {
+        Matrix result(getRows(), getCols());
+        for (size_t i=0; i<getRows(); i++)
+        {
+            for (size_t j=0; j<getCols(); j++)
+            {
+                result(i,j) = (*this)(i,j) + right;
+            }
+        }
+        return result;
+    }
+    inline Matrix operator-(const float & right) const
+    {
+        Matrix result(getRows(), getCols());
+        for (size_t i=0; i<getRows(); i++)
+        {
+            for (size_t j=0; j<getCols(); j++)
+            {
+                result(i,j) = (*this)(i,j) - right;
+            }
+        }
+        return result;
+    }
+    inline Matrix operator*(const float & right) const
+    {
+        Matrix result(getRows(), getCols());
+        for (size_t i=0; i<getRows(); i++)
+        {
+            for (size_t j=0; j<getCols(); j++)
+            {
+                result(i,j) = (*this)(i,j) * right;
+            }
+        }
+        return result;
+    }
+    inline Matrix operator/(const float & right) const
+    {
+        Matrix result(getRows(), getCols());
+        for (size_t i=0; i<getRows(); i++)
+        {
+            for (size_t j=0; j<getCols(); j++)
+            {
+                result(i,j) = (*this)(i,j) / right;
+            }
+        }
+        return result;
+    }
+    // vector ops
+    inline Vector operator*(const Vector & right) const
+    {
+#ifdef MATRIX_ASSERT
+        ASSERT(getCols()==right.getRows());
+#endif
+        Vector result(getRows());
+        for (size_t i=0; i<getRows(); i++)
+        {
+            for (size_t j=0; j<getCols(); j++)
+            {
+                result(i) += (*this)(i,j) * right(j);
+            }
+        }
+        return result;
+    }
+    // matrix ops
+    inline Matrix operator+(const Matrix & right) const
+    {
+#ifdef MATRIX_ASSERT
+        ASSERT(getRows()==right.getRows());
+        ASSERT(getCols()==right.getCols());
+#endif
+        Matrix result(getRows(), getCols());
+        for (size_t i=0; i<getRows(); i++)
+        {
+            for (size_t j=0; j<getCols(); j++)
+            {
+                result(i,j) = (*this)(i,j) + right(i,j);
+            }
+        }
+        return result;
+    }
+    inline Matrix operator-(const Matrix & right) const
+    {
+#ifdef MATRIX_ASSERT
+        ASSERT(getRows()==right.getRows());
+        ASSERT(getCols()==right.getCols());
+#endif
+        Matrix result(getRows(), getCols());
+        for (size_t i=0; i<getRows(); i++)
+        {
+            for (size_t j=0; j<getCols(); j++)
+            {
+                result(i,j) = (*this)(i,j) - right(i,j);
+            }
+        }
+        return result;
+    }
+    inline Matrix operator*(const Matrix & right) const
+    {
+#ifdef MATRIX_ASSERT
+        ASSERT(getCols()==right.getRows());
+#endif
+        Matrix result(getRows(), right.getCols());
+        for (size_t i=0; i<getRows(); i++)
+        {
+            for (size_t j=0; j<right.getCols(); j++)
+            {
+                for (size_t k=0; k<right.getRows(); k++)
+                {
+                    result(i,j) += (*this)(i,k) * right(k,j);
+                }
+            }
+        }
+        return result;
+    }
+    inline Matrix operator/(const Matrix & right) const
+    {
+#ifdef MATRIX_ASSERT
+        ASSERT(right.getRows()==right.getCols());
+        ASSERT(getCols()==right.getCols());
+#endif
+        return (*this)*right.inverse();
+    }
+    // other functions
+    inline Matrix transpose() const
+    {
+        Matrix result(getCols(),getRows());
+        for(size_t i=0;i<getRows();i++) {
+            for(size_t j=0;j<getCols();j++) {
+                result(j,i) = (*this)(i,j);
+            }
+        }
+        return result;
+    }
+    inline void swapRows(size_t a, size_t b)
+    {
+        if (a==b) return;
+        for(size_t j=0;j<getCols();j++) {
+            float tmp = (*this)(a,j);
+            (*this)(a,j) = (*this)(b,j);
+            (*this)(b,j) = tmp;
+        }
+    }
+    inline void swapCols(size_t a, size_t b)
+    {
+        if (a==b) return;
+        for(size_t i=0;i<getRows();i++) {
+            float tmp = (*this)(i,a);
+            (*this)(i,a) = (*this)(i,b);
+            (*this)(i,b) = tmp;
+        }
+    }
+    /**
+     * inverse based on LU factorization with partial pivotting
+     */
+    Matrix inverse() const
+    {
+#ifdef MATRIX_ASSERT
+        ASSERT(getRows()==getCols());
+#endif
+        size_t N = getRows();
+        Matrix L = identity(N);
+        const Matrix & A = (*this);
+        Matrix U = A;
+        Matrix P = identity(N);
+
+        //printf("A:\n"); A.print();
+
+        // for all diagonal elements
+        for (size_t n=0; n<N; n++) {
+
+            // if diagonal is zero, swap with row below
+            if (fabsf(U(n,n))<1e-8f) {
+                //printf("trying pivot for row %d\n",n);
+                for (size_t i=0; i<N; i++) {
+                    if (i==n) continue;
+                    //printf("\ttrying row %d\n",i);
+                    if (fabsf(U(i,n))>1e-8f) {
+                        //printf("swapped %d\n",i);
+                        U.swapRows(i,n);
+                        P.swapRows(i,n);
+                    }
+                }
+            }
+#ifdef MATRIX_ASSERT
+            //printf("A:\n"); A.print();
+            //printf("U:\n"); U.print();
+            //printf("P:\n"); P.print();
+            //fflush(stdout);
+            ASSERT(fabsf(U(n,n))>1e-8f);
+#endif
+            // failsafe, return zero matrix
+            if (fabsf(U(n,n))<1e-8f)
+            {
+                return Matrix::zero(n);
+            }
+
+            // for all rows below diagonal
+            for (size_t i=(n+1); i<N; i++) {
+                L(i,n) = U(i,n)/U(n,n);
+                // add i-th row and n-th row
+                // multiplied by: -a(i,n)/a(n,n)
+                for (size_t k=n; k<N; k++) {
+                    U(i,k) -= L(i,n) * U(n,k);       
+                }
+            }
+        }
+
+        //printf("L:\n"); L.print();
+        //printf("U:\n"); U.print();
+
+        // solve LY=P*I for Y by forward subst
+        Matrix Y = P;
+        // for all columns of Y
+        for (size_t c=0; c<N; c++) {
+            // for all rows of L
+            for (size_t i=0; i<N; i++) {
+                // for all columns of L
+                for (size_t j=0; j<i; j++) {
+                    // for all existing y
+                    // subtract the component they 
+                    // contribute to the solution
+                    Y(i,c) -= L(i,j)*Y(j,c); 
+                }
+                // divide by the factor 
+                // on current
+                // term to be solved
+                // Y(i,c) /= L(i,i);
+                // but L(i,i) = 1.0
+            }
+        }
+
+        //printf("Y:\n"); Y.print();
+
+        // solve Ux=y for x by back subst
+        Matrix X = Y;
+        // for all columns of X
+        for (size_t c=0; c<N; c++) {
+            // for all rows of U
+            for (size_t k=0; k<N; k++) {
+                // have to go in reverse order
+                size_t i = N-1-k; 
+                // for all columns of U
+                for (size_t j=i+1; j<N; j++) {
+                    // for all existing x
+                    // subtract the component they 
+                    // contribute to the solution
+                    X(i,c) -= U(i,j)*X(j,c); 
+                }
+                // divide by the factor 
+                // on current
+                // term to be solved
+                X(i,c) /= U(i,i);
+            }
+        }
+        //printf("X:\n"); X.print();
+        return X;
+    }
+    inline void setAll(const float & val)
+    {
+        for (size_t i=0;i<getRows();i++) {
+            for (size_t j=0;j<getCols();j++) {
+                (*this)(i,j) = val;
+            }
+        }
+    }
+    inline void set(const float * data)
+    {
+        memcpy(getData(),data,getSize());
+    }
+    inline size_t getRows() const { return _rows; }
+    inline size_t getCols() const { return _cols; }
+    inline static Matrix identity(size_t size) {
+        Matrix result(size,size);
+        for (size_t i=0; i<size; i++) {
+            result(i,i) = 1.0f; 
+        }
+        return result;
+    }
+    inline static Matrix zero(size_t size) {
+        Matrix result(size,size);
+        result.setAll(0.0f);
+        return result;
+    }
+    inline static Matrix zero(size_t m, size_t n) {
+        Matrix result(m,n);
+        result.setAll(0.0f);
+        return result;
+    }
+protected:
+    inline size_t getSize() const { return sizeof(float)*getRows()*getCols(); }
+    inline float * getData() { return _data; }
+    inline const float * getData() const { return _data; }
+    inline void setData(float * data) { _data = data; }
+private:
+    size_t _rows;
+    size_t _cols;
+    float * _data;
+};
+
+} // namespace math

apps/systemlib/math/generic/Vector.cpp

@@ -0,0 +1,40 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Vector.cpp
+ *
+ * math vector
+ */
+
+#include "Vector.hpp"

apps/systemlib/math/generic/Vector.hpp

@@ -0,0 +1,240 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Vector.h
+ *
+ * math vector
+ */
+
+#pragma once
+
+#include <inttypes.h>
+#include <assert.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdio.h>
+#include <math.h>
+
+#include <systemlib/math/Vector.hpp>
+
+namespace math
+{
+
+class __EXPORT Vector {
+public:
+    // constructor
+    Vector(size_t rows) :
+        _rows(rows),
+        _data((float*)calloc(rows,sizeof(float)))
+    {
+    }
+    Vector(size_t rows, const float * data) :
+        _rows(rows),
+        _data((float*)malloc(getSize()))
+    {
+        memcpy(getData(),data,getSize());
+    }
+    // deconstructor
+    virtual ~Vector()
+    {
+        delete [] getData();
+    }
+    // copy constructor (deep)
+    Vector(const Vector & right) :
+        _rows(right.getRows()),
+        _data((float*)malloc(getSize()))
+    {
+        memcpy(getData(),right.getData(),
+                    right.getSize());
+    }
+    // assignment
+    inline Vector & operator=(const Vector & right)
+    {
+#ifdef VECTOR_ASSERT
+        ASSERT(getRows()==right.getRows());
+#endif
+        if (this != &right)
+        {
+            memcpy(getData(),right.getData(),
+                        right.getSize());
+        }
+        return *this;
+    }
+    // element accessors
+    inline float& operator()(size_t i)
+    {
+#ifdef VECTOR_ASSERT
+        ASSERT(i<getRows());
+#endif
+        return getData()[i];
+    }
+    inline const float& operator()(size_t i) const
+    {
+#ifdef VECTOR_ASSERT
+        ASSERT(i<getRows());
+#endif
+        return getData()[i];
+    }
+    // output
+    inline void print() const
+    {
+        for (size_t i=0; i<getRows(); i++)
+        {
+            float sig;
+            int exp;
+            float num = (*this)(i);
+            float2SigExp(num,sig,exp);
+            printf ("%6.3fe%03.3d,", (double)sig, exp);
+        }
+        printf("\n");
+    }
+    // boolean ops
+    inline bool operator==(const Vector & right) const
+    {
+        for (size_t i=0; i<getRows(); i++)
+        {
+            if (fabsf(((*this)(i) - right(i))) > 1e-30f)
+                return false;
+        }
+        return true;
+    }
+    // scalar ops
+    inline Vector operator+(const float & right) const
+    {
+        Vector result(getRows());
+        for (size_t i=0; i<getRows(); i++)
+        {
+            result(i) = (*this)(i) + right;
+        }
+        return result;
+    }
+    inline Vector operator-(const float & right) const
+    {
+        Vector result(getRows());
+        for (size_t i=0; i<getRows(); i++)
+        {
+            result(i) = (*this)(i) - right;
+        }
+        return result;
+    }
+    inline Vector operator*(const float & right) const
+    {
+        Vector result(getRows());
+        for (size_t i=0; i<getRows(); i++)
+        {
+            result(i) = (*this)(i) * right;
+        }
+        return result;
+    }
+    inline Vector operator/(const float & right) const
+    {
+        Vector result(getRows());
+        for (size_t i=0; i<getRows(); i++)
+        {
+            result(i) = (*this)(i) / right;
+        }
+        return result;
+    }
+    // vector ops
+    inline Vector operator+(const Vector & right) const
+    {
+#ifdef VECTOR_ASSERT
+        ASSERT(getRows()==right.getRows());
+#endif
+        Vector result(getRows());
+        for (size_t i=0; i<getRows(); i++)
+        {
+            result(i) = (*this)(i) + right(i);
+        }
+        return result;
+    }
+    inline Vector operator-(const Vector & right) const
+    {
+#ifdef VECTOR_ASSERT
+        ASSERT(getRows()==right.getRows());
+#endif
+        Vector result(getRows());
+        for (size_t i=0; i<getRows(); i++)
+        {
+            result(i) = (*this)(i) - right(i);
+        }
+        return result;
+    }
+    // other functions
+    inline float dot(const Vector & right)
+    {
+        float result = 0;
+        for (size_t i=0; i<getRows(); i++)
+        {
+            result += (*this)(i)*(*this)(i);
+        }
+        return result;
+    }
+    inline float norm()
+    {
+        return sqrtf(dot(*this));
+    }
+    inline Vector unit()
+    {
+        return (*this)/norm();
+    }
+    inline static Vector zero(size_t rows)
+    {
+        Vector result(rows);
+        // calloc returns zeroed memory
+        return result;
+    }
+    inline void setAll(const float & val)
+    {
+        for (size_t i=0;i<getRows();i++)
+        {
+            (*this)(i) = val;
+        }
+    }
+    inline void set(const float * data)
+    {
+        memcpy(getData(),data,getSize());
+    }
+    inline size_t getRows() const { return _rows; }
+protected:
+    inline size_t getSize() const { return sizeof(float)*getRows(); }
+    inline float * getData() { return _data; }
+    inline const float * getData() const { return _data; }
+    inline void setData(float * data) { _data = data; }
+private:
+    size_t _rows;
+    float * _data;
+};
+
+} // math