Merge branch 'master' into vector_control

2026-06-26 02:10:34 +08:00 · 2013-11-08 21:56:11 +04:00
parent 28bf8e238e c63995e91c
commit e8224376ca
636 changed files with 40457 additions and 10512 deletions
@@ -0,0 +1,179 @@
+/****************************************************************************
+ *
+ *   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 <mathlib/math/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(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[3][3]) :
+	Matrix(3, 3)
+{
+	Dcm &dcm = *this;
+	/* set rotation matrix */
+	for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
+		dcm(i, j) = data[i][j];
+}
+
+Dcm::Dcm(const float *data) :
+	Matrix(3, 3, data)
+{
+}
+
+Dcm::Dcm(const Quaternion &q) :
+	Matrix(3, 3)
+{
+	Dcm &dcm = *this;
+	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.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.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;
+}
+
+Dcm::Dcm(const EulerAngles &euler) :
+	Matrix(3, 3)
+{
+	Dcm &dcm = *this;
+	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;
+	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(const Matrix &right) :
+	Matrix(right)
+{
+}
+
+Dcm::~Dcm()
+{
+}
+
+int __EXPORT dcmTest()
+{
+	printf("Test DCM\t\t: ");
+	// default ctor
+	ASSERT(matrixEqual(Dcm(),
+			   Matrix::identity(3)));
+	// quaternion ctor
+	ASSERT(matrixEqual(
+		       Dcm(Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f)),
+		       Dcm(0.9362934f, -0.2750958f,  0.2183507f,
+			   0.2896295f,  0.9564251f, -0.0369570f,
+			   -0.1986693f,  0.0978434f,  0.9751703f)));
+	// euler angle ctor
+	ASSERT(matrixEqual(
+		       Dcm(EulerAngles(0.1f, 0.2f, 0.3f)),
+		       Dcm(0.9362934f, -0.2750958f,  0.2183507f,
+			   0.2896295f,  0.9564251f, -0.0369570f,
+			   -0.1986693f,  0.0978434f,  0.9751703f)));
+	// rotations
+	Vector3 vB(1, 2, 3);
+	ASSERT(vectorEqual(Vector3(-2.0f, 1.0f, 3.0f),
+			   Dcm(EulerAngles(0.0f, 0.0f, M_PI_2_F))*vB));
+	ASSERT(vectorEqual(Vector3(3.0f, 2.0f, -1.0f),
+			   Dcm(EulerAngles(0.0f, M_PI_2_F, 0.0f))*vB));
+	ASSERT(vectorEqual(Vector3(1.0f, -3.0f, 2.0f),
+			   Dcm(EulerAngles(M_PI_2_F, 0.0f, 0.0f))*vB));
+	ASSERT(vectorEqual(Vector3(3.0f, 2.0f, -1.0f),
+			   Dcm(EulerAngles(
+				       M_PI_2_F, M_PI_2_F, M_PI_2_F))*vB));
+	printf("PASS\n");
+	return 0;
+}
+} // namespace math
@@ -0,0 +1,113 @@
+/****************************************************************************
+ *
+ *   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();
+
+	/**
+	 * scalar ctor
+	 */
+	Dcm(float c00, float c01, float c02,
+	    float c10, float c11, float c12,
+	    float c20, float c21, float c22);
+
+	/**
+	 * data ctor
+	 */
+	Dcm(const float *data);
+
+	/**
+	 * array ctor
+	 */
+	Dcm(const float data[3][3]);
+
+	/**
+	 * quaternion ctor
+	 */
+	Dcm(const Quaternion &q);
+
+	/**
+	 * euler angles ctor
+	 */
+	Dcm(const EulerAngles &euler);
+
+	/**
+	 * copy ctor (deep)
+	 */
+	Dcm(const Dcm &right);
+
+	/**
+	 * copy ctor (deep)
+	 */
+	Dcm(const Matrix &right);
+
+	/**
+	 * dtor
+	 */
+	virtual ~Dcm();
+};
+
+int __EXPORT dcmTest();
+
+} // math
+
@@ -0,0 +1,126 @@
+/****************************************************************************
+ *
+ *   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 "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(0.1f, 0.2f, 0.3f);
+
+	// test ctor
+	ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler));
+	ASSERT(equal(euler.getPhi(), 0.1f));
+	ASSERT(equal(euler.getTheta(), 0.2f));
+	ASSERT(equal(euler.getPsi(), 0.3f));
+
+	// test dcm ctor
+	euler = Dcm(EulerAngles(0.1f, 0.2f, 0.3f));
+	ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler));
+
+	// test quat ctor
+	euler = Quaternion(EulerAngles(0.1f, 0.2f, 0.3f));
+	ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler));
+
+	// test assignment
+	euler.setPhi(0.4f);
+	euler.setTheta(0.5f);
+	euler.setPsi(0.6f);
+	ASSERT(vectorEqual(Vector3(0.4f, 0.5f, 0.6f), euler));
+
+	printf("PASS\n");
+	return 0;
+}
+
+} // namespace math
@@ -0,0 +1,74 @@
+/****************************************************************************
+ *
+ *   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
+
@@ -0,0 +1,146 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 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 Limits.cpp
+ *
+ * Limiting / constrain helper functions
+ */
+
+
+#include <math.h>
+#include <stdint.h>
+
+#include "Limits.hpp"
+
+
+namespace math {
+
+
+float __EXPORT min(float val1, float val2)
+{
+	return (val1 < val2) ? val1 : val2;
+}
+
+int __EXPORT min(int val1, int val2)
+{
+	return (val1 < val2) ? val1 : val2;
+}
+
+unsigned __EXPORT min(unsigned val1, unsigned val2)
+{
+	return (val1 < val2) ? val1 : val2;
+}
+
+uint64_t __EXPORT min(uint64_t val1, uint64_t val2)
+{
+	return (val1 < val2) ? val1 : val2;
+}
+
+double __EXPORT min(double val1, double val2)
+{
+	return (val1 < val2) ? val1 : val2;
+}
+
+float __EXPORT max(float val1, float val2)
+{
+	return (val1 > val2) ? val1 : val2;
+}
+
+int __EXPORT max(int val1, int val2)
+{
+	return (val1 > val2) ? val1 : val2;
+}
+
+unsigned __EXPORT max(unsigned val1, unsigned val2)
+{
+	return (val1 > val2) ? val1 : val2;
+}
+
+uint64_t __EXPORT max(uint64_t val1, uint64_t val2)
+{
+	return (val1 > val2) ? val1 : val2;
+}
+
+double __EXPORT max(double val1, double val2)
+{
+	return (val1 > val2) ? val1 : val2;
+}
+
+
+float __EXPORT constrain(float val, float min, float max)
+{
+	return (val < min) ? min : ((val > max) ? max : val);
+}
+
+int __EXPORT constrain(int val, int min, int max)
+{
+	return (val < min) ? min : ((val > max) ? max : val);
+}
+
+unsigned __EXPORT constrain(unsigned val, unsigned min, unsigned max)
+{
+	return (val < min) ? min : ((val > max) ? max : val);
+}
+
+uint64_t __EXPORT constrain(uint64_t val, uint64_t min, uint64_t max)
+{
+	return (val < min) ? min : ((val > max) ? max : val);
+}
+
+double __EXPORT constrain(double val, double min, double max)
+{
+	return (val < min) ? min : ((val > max) ? max : val);
+}
+
+float __EXPORT radians(float degrees)
+{
+	return (degrees / 180.0f) * M_PI_F;
+}
+
+double __EXPORT radians(double degrees)
+{
+	return (degrees / 180.0) * M_PI;
+}
+
+float __EXPORT degrees(float radians)
+{
+	return (radians / M_PI_F) * 180.0f;
+}
+
+double __EXPORT degrees(double radians)
+{
+	return (radians / M_PI) * 180.0;
+}
+
+}
@@ -0,0 +1,87 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 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 Limits.hpp
+ *
+ * Limiting / constrain helper functions
+ */
+
+#pragma once
+
+#include <nuttx/config.h>
+#include <stdint.h>
+
+namespace math {
+
+
+float __EXPORT min(float val1, float val2);
+
+int __EXPORT min(int val1, int val2);
+
+unsigned __EXPORT min(unsigned val1, unsigned val2);
+
+uint64_t __EXPORT min(uint64_t val1, uint64_t val2);
+
+double __EXPORT min(double val1, double val2);
+
+float __EXPORT max(float val1, float val2);
+
+int __EXPORT max(int val1, int val2);
+
+unsigned __EXPORT max(unsigned val1, unsigned val2);
+
+uint64_t __EXPORT max(uint64_t val1, uint64_t val2);
+
+double __EXPORT max(double val1, double val2);
+
+
+float __EXPORT constrain(float val, float min, float max);
+
+int __EXPORT constrain(int val, int min, int max);
+
+unsigned __EXPORT constrain(unsigned val, unsigned min, unsigned max);
+
+uint64_t __EXPORT constrain(uint64_t val, uint64_t min, uint64_t max);
+
+double __EXPORT constrain(double val, double min, double max);
+
+float __EXPORT radians(float degrees);
+
+double __EXPORT radians(double degrees);
+
+float __EXPORT degrees(float radians);
+
+double __EXPORT degrees(double radians);
+
+}
@@ -0,0 +1,193 @@
+/****************************************************************************
+ *
+ *   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 "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
@@ -0,0 +1,61 @@
+/****************************************************************************
+ *
+ *   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 <nuttx/config.h>
+
+#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU)
+#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
@@ -0,0 +1,174 @@
+/****************************************************************************
+ *
+ *   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 "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)
+{
+	// 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)
+{
+	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 -
+	     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.0f));
+	ASSERT(equal(q.getB(), 0.0f));
+	ASSERT(equal(q.getC(), 0.0f));
+	ASSERT(equal(q.getD(), 0.0f));
+	// test float ctor
+	q = Quaternion(0.1825742f, 0.3651484f, 0.5477226f, 0.7302967f);
+	ASSERT(equal(q.getA(), 0.1825742f));
+	ASSERT(equal(q.getB(), 0.3651484f));
+	ASSERT(equal(q.getC(), 0.5477226f));
+	ASSERT(equal(q.getD(), 0.7302967f));
+	// test euler ctor
+	q = Quaternion(EulerAngles(0.1f, 0.2f, 0.3f));
+	ASSERT(vectorEqual(q, Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f)));
+	// test dcm ctor
+	q = Quaternion(Dcm());
+	ASSERT(vectorEqual(q, Quaternion(1.0f, 0.0f, 0.0f, 0.0f)));
+	// TODO test derivative
+	// test accessors
+	q.setA(0.1f);
+	q.setB(0.2f);
+	q.setC(0.3f);
+	q.setD(0.4f);
+	ASSERT(vectorEqual(q, Quaternion(0.1f, 0.2f, 0.3f, 0.4f)));
+	printf("PASS\n");
+	return 0;
+}
+
+} // namespace math
@@ -0,0 +1,115 @@
+/****************************************************************************
+ *
+ *   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
+
@@ -0,0 +1,100 @@
+/****************************************************************************
+ *
+ *   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 "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
@@ -0,0 +1,57 @@
+/****************************************************************************
+ *
+ *   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 <nuttx/config.h>
+
+#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU)
+#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
@@ -0,0 +1,103 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 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 Vector2f.cpp
+ *
+ * math vector
+ */
+
+#include "test/test.hpp"
+
+#include "Vector2f.hpp"
+
+namespace math
+{
+
+Vector2f::Vector2f() :
+	Vector(2)
+{
+}
+
+Vector2f::Vector2f(const Vector &right) :
+	Vector(right)
+{
+#ifdef VECTOR_ASSERT
+	ASSERT(right.getRows() == 2);
+#endif
+}
+
+Vector2f::Vector2f(float x, float y) :
+	Vector(2)
+{
+	setX(x);
+	setY(y);
+}
+
+Vector2f::Vector2f(const float *data) :
+	Vector(2, data)
+{
+}
+
+Vector2f::~Vector2f()
+{
+}
+
+float Vector2f::cross(const Vector2f &b) const
+{
+	const Vector2f &a = *this;
+	return a(0)*b(1) - a(1)*b(0);
+}
+
+float Vector2f::operator %(const Vector2f &v) const
+{
+	return cross(v);
+}
+    
+float Vector2f::operator *(const Vector2f &v) const
+{
+    return dot(v);
+}
+
+int __EXPORT vector2fTest()
+{
+	printf("Test Vector2f\t\t: ");
+	// test float ctor
+	Vector2f v(1, 2);
+	ASSERT(equal(v(0), 1));
+	ASSERT(equal(v(1), 2));
+	printf("PASS\n");
+	return 0;
+}
+
+} // namespace math
@@ -0,0 +1,79 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 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 Vector2f.hpp
+ *
+ * math 3 vector
+ */
+
+#pragma once
+
+#include "Vector.hpp"
+
+namespace math
+{
+
+class __EXPORT Vector2f :
+	public Vector
+{
+public:
+	Vector2f();
+	Vector2f(const Vector &right);
+	Vector2f(float x, float y);
+	Vector2f(const float *data);
+	virtual ~Vector2f();
+	float cross(const Vector2f &b) const;
+	float operator %(const Vector2f &v) const;
+    float operator *(const Vector2f &v) const;
+    inline Vector2f operator*(const float &right) const {
+		return Vector::operator*(right);
+	}
+
+	/**
+	 * accessors
+	 */
+	void setX(float x) { (*this)(0) = x; }
+	void setY(float y) { (*this)(1) = y; }
+	const float &getX() const { return (*this)(0); }
+	const float &getY() const { return (*this)(1); }
+};
+    
+class __EXPORT Vector2 :
+	public Vector2f
+{
+};
+
+int __EXPORT vector2fTest();
+} // math
+
@@ -0,0 +1,109 @@
+/****************************************************************************
+ *
+ *   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 "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) const
+{
+	const 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;
+}
+
+Vector3 Vector3::operator %(const Vector3 &v) const
+{
+	return cross(v);
+}
+    
+float Vector3::operator *(const Vector3 &v) const
+{
+    return dot(v);
+}
+
+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
@@ -0,0 +1,78 @@
+/****************************************************************************
+ *
+ *   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) const;
+	Vector3 operator %(const Vector3 &v) const;
+	float operator *(const Vector3 &v) const;
+
+	/**
+	 * 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); }
+};
+    
+class __EXPORT Vector3f :
+	public Vector3
+{
+};
+
+int __EXPORT vector3Test();
+} // math
+
@@ -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"
@@ -0,0 +1,292 @@
+/****************************************************************************
+ *
+ *   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 "../Vector.hpp"
+#include "../Matrix.hpp"
+
+// arm specific
+#include "../../CMSIS/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 exponent;
+				float num = (*this)(i, j);
+				float2SigExp(num, sig, exponent);
+				printf("%6.3fe%03d ", (double)sig, exponent);
+			}
+
+			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
@@ -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"
@@ -0,0 +1,236 @@
+/****************************************************************************
+ *
+ *   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 "../Vector.hpp"
+#include "../test/test.hpp"
+
+// arm specific
+#include "../../CMSIS/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 exponent;
+			float num = (*this)(i);
+			float2SigExp(num, sig, exponent);
+			printf("%6.3fe%03d ", (double)sig, exponent);
+		}
+
+		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*(const 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;
+	}
+	inline Vector operator-(void) const {
+		Vector result(getRows());
+		arm_negate_f32((float *)getData(),
+			    result.getData(),
+			    getRows());
+		return result;
+	}
+	// other functions
+	inline float dot(const Vector &right) const {
+		float result = 0;
+		arm_dot_prod_f32((float *)getData(),
+				 (float *)right.getData(),
+				 getRows(),
+				 &result);
+		return result;
+	}
+	inline float norm() const {
+		return sqrtf(dot(*this));
+	}
+	inline float length() const {
+		return norm();
+	}
+	inline Vector unit() const {
+		return (*this) / norm();
+	}
+	inline Vector normalized() const {
+		return unit();
+	}
+	inline void normalize() {
+		(*this) = (*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
@@ -0,0 +1,77 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/****************************************************************************
+ *
+ *   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	LowPassFilter.cpp
+/// @brief	A class to implement a second order low pass filter 
+/// Author: Leonard Hall <LeonardTHall@gmail.com>
+
+#include "LowPassFilter2p.hpp"
+#include "math.h"
+
+namespace math
+{
+
+void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq)
+{
+    _cutoff_freq = cutoff_freq;
+    float fr = sample_freq/_cutoff_freq;
+    float ohm = tanf(M_PI_F/fr);
+    float c = 1.0f+2.0f*cosf(M_PI_F/4.0f)*ohm + ohm*ohm;
+    _b0 = ohm*ohm/c;
+    _b1 = 2.0f*_b0;
+    _b2 = _b0;
+    _a1 = 2.0f*(ohm*ohm-1.0f)/c;
+    _a2 = (1.0f-2.0f*cosf(M_PI_F/4.0f)*ohm+ohm*ohm)/c;
+}
+
+float LowPassFilter2p::apply(float sample)
+{
+    // do the filtering
+    float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2;
+    if (isnan(delay_element_0) || isinf(delay_element_0)) {
+        // don't allow bad values to propogate via the filter
+        delay_element_0 = sample;
+    }
+    float output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2;
+    
+    _delay_element_2 = _delay_element_1;
+    _delay_element_1 = delay_element_0;
+
+    // return the value.  Should be no need to check limits
+    return output;
+}
+
+} // namespace math
+
@@ -0,0 +1,78 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/****************************************************************************
+ *
+ *   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	LowPassFilter.h
+/// @brief	A class to implement a second order low pass filter 
+/// Author: Leonard Hall <LeonardTHall@gmail.com>
+/// Adapted for PX4 by Andrew Tridgell
+
+#pragma once
+
+namespace math
+{
+class __EXPORT LowPassFilter2p
+{
+public:
+    // constructor
+    LowPassFilter2p(float sample_freq, float cutoff_freq) {
+        // set initial parameters
+        set_cutoff_frequency(sample_freq, cutoff_freq);
+        _delay_element_1 = _delay_element_2 = 0;
+    }
+
+    // change parameters
+    void set_cutoff_frequency(float sample_freq, float cutoff_freq);
+
+    // apply - Add a new raw value to the filter 
+    // and retrieve the filtered result
+    float apply(float sample);
+
+    // return the cutoff frequency
+    float get_cutoff_freq(void) const {
+        return _cutoff_freq;
+    }
+
+private:
+    float           _cutoff_freq; 
+    float           _a1;
+    float           _a2;
+    float           _b0;
+    float           _b1;
+    float           _b2;
+    float           _delay_element_1;        // buffered sample -1
+    float           _delay_element_2;        // buffered sample -2
+};
+
+} // namespace math
@@ -0,0 +1,43 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 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.
+#
+############################################################################
+
+#
+# filter library
+#
+SRCS		 = LowPassFilter2p.cpp
+
+#
+# In order to include .config we first have to save off the
+# current makefile name, since app.mk needs it.
+#
+APP_MAKEFILE	:= $(lastword $(MAKEFILE_LIST))
@@ -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"
@@ -0,0 +1,437 @@
+/****************************************************************************
+ *
+ *   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 "../Vector.hpp"
+#include "../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
@@ -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"
@@ -0,0 +1,245 @@
+/****************************************************************************
+ *
+ *   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 "../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;
+	}
+	inline Vector operator-(void) const {
+		Vector result(getRows());
+
+		for (size_t i = 0; i < getRows(); i++) {
+			result(i) = -((*this)(i));
+		}
+
+		return result;
+	}
+	// other functions
+	inline float dot(const Vector &right) const {
+		float result = 0;
+
+		for (size_t i = 0; i < getRows(); i++) {
+			result += (*this)(i) * (*this)(i);
+		}
+
+		return result;
+	}
+	inline float norm() const {
+		return sqrtf(dot(*this));
+	}
+	inline float length() const {
+		return norm();
+	}
+	inline Vector unit() const {
+		return (*this) / norm();
+	}
+	inline Vector normalized() const {
+		return unit();
+	}
+	inline void normalize() {
+		(*this) = (*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
@@ -0,0 +1,94 @@
+/****************************************************************************
+ *
+ *   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 test.cpp
+ *
+ * Test library code
+ */
+
+#include <stdio.h>
+#include <math.h>
+#include <stdlib.h>
+
+#include "test.hpp"
+
+bool __EXPORT equal(float a, float b, float epsilon)
+{
+	float diff = fabsf(a - b);
+
+	if (diff > epsilon) {
+		printf("not equal ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b));
+		return false;
+
+	} else return true;
+}
+
+void __EXPORT float2SigExp(
+	const float &num,
+	float &sig,
+	int &exp)
+{
+	if (isnan(num) || isinf(num)) {
+		sig = 0.0f;
+		exp = -99;
+		return;
+	}
+
+	if (fabsf(num) < 1.0e-38f) {
+		sig = 0;
+		exp = 0;
+		return;
+	}
+
+	exp = log10f(fabsf(num));
+
+	if (exp > 0) {
+		exp = ceil(exp);
+
+	} else {
+		exp = floor(exp);
+	}
+
+	sig = num;
+
+	// cheap power since it is integer
+	if (exp > 0) {
+		for (int i = 0; i < abs(exp); i++) sig /= 10;
+
+	} else {
+		for (int i = 0; i < abs(exp); i++) sig *= 10;
+	}
+}
+
+
@@ -0,0 +1,50 @@
+/****************************************************************************
+ *
+ *   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 test.hpp
+ *
+ * Controller library code
+ */
+
+#pragma once
+
+//#include <assert.h>
+//#include <time.h>
+//#include <stdlib.h>
+
+bool equal(float a, float b, float eps = 1e-5);
+void float2SigExp(
+	const float &num,
+	float &sig,
+	int &exp);
@@ -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)