diff --git a/Tools/fix_code_style_ubuntu.sh b/Tools/fix_code_style_ubuntu.sh
new file mode 100755
index 0000000000..90ab57b895
--- /dev/null
+++ b/Tools/fix_code_style_ubuntu.sh
@@ -0,0 +1,19 @@
+#!/bin/sh
+astyle \
+    --style=linux		\
+    --indent=force-tab=8	\
+    --indent-cases		\
+    --indent-preprocessor	\
+    --break-blocks=all		\
+    --pad-oper			\
+    --pad-header		\
+    --unpad-paren		\
+    --keep-one-line-blocks	\
+    --keep-one-line-statements	\
+    --align-pointer=name	\
+    --suffix=none		\
+    --lineend=linux		\
+    $*
+    #--ignore-exclude-errors-x	\
+    #--exclude=EASTL		\
+    #--align-reference=name	\
diff --git a/apps/systemlib/math/Dcm.cpp b/apps/systemlib/math/Dcm.cpp
index 1e9c4ee6d0..b9f4309cc1 100644
--- a/apps/systemlib/math/Dcm.cpp
+++ b/apps/systemlib/math/Dcm.cpp
@@ -48,64 +48,64 @@ namespace math
 {
 
 Dcm::Dcm() :
-    Matrix(Matrix::identity(3))
+	Matrix(Matrix::identity(3))
 {
 }
 
-Dcm::Dcm(const float * data) :
-    Matrix(3,3,data)
+Dcm::Dcm(const float *data) :
+	Matrix(3, 3, data)
 {
 }
 
-Dcm::Dcm(const Quaternion & q) :
-    Matrix(3,3)
+Dcm::Dcm(const Quaternion &q) :
+	Matrix(3, 3)
 {
-    Dcm & dcm = *this;
-    float a = q.getA();
-    float b = q.getB();
-    float c = q.getC();
-    float d = q.getD();
-    float aSq = a*a;
-    float bSq = b*b;
-    float cSq = c*c;
-    float dSq = d*d;
-    dcm(0,0) = aSq + bSq - cSq - dSq;
-    dcm(0,1) = 2*(b*c - a*d);
-    dcm(0,2) = 2*(a*c + b*d);
-    dcm(1,0) = 2*(b*c + a*d);
-    dcm(1,1) = aSq - bSq + cSq - dSq;
-    dcm(1,2) = 2*(c*d - a*b);
-    dcm(2,0) = 2*(b*d - a*c);
-    dcm(2,1) = 2*(a*b + c*d);
-    dcm(2,2) = aSq - bSq - cSq + dSq;
+	Dcm &dcm = *this;
+	float a = q.getA();
+	float b = q.getB();
+	float c = q.getC();
+	float d = q.getD();
+	float aSq = a * a;
+	float bSq = b * b;
+	float cSq = c * c;
+	float dSq = d * d;
+	dcm(0, 0) = aSq + bSq - cSq - dSq;
+	dcm(0, 1) = 2 * (b * c - a * d);
+	dcm(0, 2) = 2 * (a * c + b * d);
+	dcm(1, 0) = 2 * (b * c + a * d);
+	dcm(1, 1) = aSq - bSq + cSq - dSq;
+	dcm(1, 2) = 2 * (c * d - a * b);
+	dcm(2, 0) = 2 * (b * d - a * c);
+	dcm(2, 1) = 2 * (a * b + c * d);
+	dcm(2, 2) = aSq - bSq - cSq + dSq;
 }
 
-Dcm::Dcm(const EulerAngles & euler) :
-    Matrix(3,3)
+Dcm::Dcm(const EulerAngles &euler) :
+	Matrix(3, 3)
 {
-    Dcm & dcm = *this;
-    float cosPhi = cosf(euler.getPhi());
-    float sinPhi = sinf(euler.getPhi());
-    float cosThe = cosf(euler.getTheta());
-    float sinThe = sinf(euler.getTheta());
-    float cosPsi = cosf(euler.getPsi());
-    float sinPsi = sinf(euler.getPsi());
+	Dcm &dcm = *this;
+	float cosPhi = cosf(euler.getPhi());
+	float sinPhi = sinf(euler.getPhi());
+	float cosThe = cosf(euler.getTheta());
+	float sinThe = sinf(euler.getTheta());
+	float cosPsi = cosf(euler.getPsi());
+	float sinPsi = sinf(euler.getPsi());
 
-    dcm(0,0) = cosThe*cosPsi;
-    dcm(0,1) = -cosPhi*sinPsi + sinPhi*sinThe*cosPsi;
-    dcm(0,2) = sinPhi*sinPsi + cosPhi*sinThe*cosPsi;
+	dcm(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(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(2, 0) = -sinThe;
+	dcm(2, 1) = sinPhi * cosThe;
+	dcm(2, 2) = cosPhi * cosThe;
 }
 
-Dcm::Dcm(const Dcm & right) :
-    Matrix(right)
+Dcm::Dcm(const Dcm &right) :
+	Matrix(right)
 {
 }
 
@@ -115,22 +115,22 @@ Dcm::~Dcm()
 
 int __EXPORT dcmTest()
 {
-    printf("Test DCM\t\t: ");
-    Vector3 vB(1,2,3);
-    ASSERT(matrixEqual(Dcm(Quaternion(1,0,0,0)),
-                    Matrix::identity(3)));
-    ASSERT(matrixEqual(Dcm(EulerAngles(0,0,0)),
-                    Matrix::identity(3)));
-    ASSERT(vectorEqual(Vector3(-2,1,3),
-        Dcm(EulerAngles(0,0,M_PI_2_F))*vB));
-    ASSERT(vectorEqual(Vector3(3,2,-1),
-        Dcm(EulerAngles(0,M_PI_2_F,0))*vB));
-    ASSERT(vectorEqual(Vector3(1,-3,2),
-        Dcm(EulerAngles(M_PI_2_F,0,0))*vB));
-    ASSERT(vectorEqual(Vector3(3,2,-1),
-        Dcm(EulerAngles(
-                M_PI_2_F,M_PI_2_F,M_PI_2_F))*vB));
-    printf("PASS\n");
-    return 0;
+	printf("Test DCM\t\t: ");
+	Vector3 vB(1, 2, 3);
+	ASSERT(matrixEqual(Dcm(Quaternion(1, 0, 0, 0)),
+			   Matrix::identity(3)));
+	ASSERT(matrixEqual(Dcm(EulerAngles(0, 0, 0)),
+			   Matrix::identity(3)));
+	ASSERT(vectorEqual(Vector3(-2, 1, 3),
+			   Dcm(EulerAngles(0, 0, M_PI_2_F))*vB));
+	ASSERT(vectorEqual(Vector3(3, 2, -1),
+			   Dcm(EulerAngles(0, M_PI_2_F, 0))*vB));
+	ASSERT(vectorEqual(Vector3(1, -3, 2),
+			   Dcm(EulerAngles(M_PI_2_F, 0, 0))*vB));
+	ASSERT(vectorEqual(Vector3(3, 2, -1),
+			   Dcm(EulerAngles(
+				       M_PI_2_F, M_PI_2_F, M_PI_2_F))*vB));
+	printf("PASS\n");
+	return 0;
 }
 } // namespace math
diff --git a/apps/systemlib/math/Dcm.hpp b/apps/systemlib/math/Dcm.hpp
index 658d902faa..de69a7aa4d 100644
--- a/apps/systemlib/math/Dcm.hpp
+++ b/apps/systemlib/math/Dcm.hpp
@@ -42,7 +42,8 @@
 #include "Vector.hpp"
 #include "Matrix.hpp"
 
-namespace math {
+namespace math
+{
 
 class Quaternion;
 class EulerAngles;
@@ -52,41 +53,41 @@ class EulerAngles;
  * (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 
+ * as C_nb. C_bn can be obtained through use
  * of the transpose() method.
  */
 class __EXPORT Dcm : public Matrix
 {
 public:
-    /**
-     * default ctor
-     */
-    Dcm();
+	/**
+	 * default ctor
+	 */
+	Dcm();
 
-    /**
-     * data ctor
-     */
-    Dcm(const float * data);
+	/**
+	 * data ctor
+	 */
+	Dcm(const float *data);
 
-    /**
-     * quaternion ctor
-     */
-    Dcm(const Quaternion & q);
+	/**
+	 * quaternion ctor
+	 */
+	Dcm(const Quaternion &q);
 
-    /**
-     * euler angles ctor
-     */
-    Dcm(const EulerAngles & euler);
+	/**
+	 * euler angles ctor
+	 */
+	Dcm(const EulerAngles &euler);
 
-    /**
-     * copy ctor (deep)
-     */
-    Dcm(const Dcm & right);
+	/**
+	 * copy ctor (deep)
+	 */
+	Dcm(const Dcm &right);
 
-    /**
-     * dtor
-     */
-    virtual ~Dcm();
+	/**
+	 * dtor
+	 */
+	virtual ~Dcm();
 };
 
 int __EXPORT dcmTest();
diff --git a/apps/systemlib/math/EulerAngles.cpp b/apps/systemlib/math/EulerAngles.cpp
index b530f172d8..259c866c73 100644
--- a/apps/systemlib/math/EulerAngles.cpp
+++ b/apps/systemlib/math/EulerAngles.cpp
@@ -48,48 +48,46 @@ namespace math
 {
 
 EulerAngles::EulerAngles() :
-    Vector(3)
+	Vector(3)
 {
-    setPhi(0.0f);
-    setTheta(0.0f);
-    setPsi(0.0f);
+	setPhi(0.0f);
+	setTheta(0.0f);
+	setPsi(0.0f);
 }
 
 EulerAngles::EulerAngles(float phi, float theta, float psi) :
-    Vector(3)
+	Vector(3)
 {
-    setPhi(phi);
-    setTheta(theta);
-    setPsi(psi);
+	setPhi(phi);
+	setTheta(theta);
+	setPsi(psi);
 }
 
-EulerAngles::EulerAngles(const Quaternion & q) :
-    Vector(3)
+EulerAngles::EulerAngles(const Quaternion &q) :
+	Vector(3)
 {
-    (*this) = EulerAngles(Dcm(q));
+	(*this) = EulerAngles(Dcm(q));
 }
 
-EulerAngles::EulerAngles(const Dcm & dcm) :
-    Vector(3)
+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)));
-    }
+	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()
@@ -98,27 +96,27 @@ EulerAngles::~EulerAngles()
 
 int __EXPORT eulerAnglesTest()
 {
-    printf("Test EulerAngles\t: ");
-    EulerAngles euler(1,2,3);
+	printf("Test EulerAngles\t: ");
+	EulerAngles euler(1, 2, 3);
 
-    // test ctor
-    ASSERT(vectorEqual(Vector3(1,2,3),euler));
-    ASSERT(equal(euler.getPhi(),1));
-    ASSERT(equal(euler.getTheta(),2));
-    ASSERT(equal(euler.getPsi(),3));
+	// test ctor
+	ASSERT(vectorEqual(Vector3(1, 2, 3), euler));
+	ASSERT(equal(euler.getPhi(), 1));
+	ASSERT(equal(euler.getTheta(), 2));
+	ASSERT(equal(euler.getPsi(), 3));
 
-    // test dcm ctor
+	// test dcm ctor
 
-    // test assignment
-    euler.setPhi(4);
-    ASSERT(equal(euler.getPhi(),4));
-    euler.setTheta(5);
-    ASSERT(equal(euler.getTheta(),5));
-    euler.setPsi(6);
-    ASSERT(equal(euler.getPsi(),6));
-    
-    printf("PASS\n");
-    return 0;
+	// test assignment
+	euler.setPhi(4);
+	ASSERT(equal(euler.getPhi(), 4));
+	euler.setTheta(5);
+	ASSERT(equal(euler.getTheta(), 5));
+	euler.setPsi(6);
+	ASSERT(equal(euler.getPsi(), 6));
+
+	printf("PASS\n");
+	return 0;
 }
 
 } // namespace math
diff --git a/apps/systemlib/math/EulerAngles.hpp b/apps/systemlib/math/EulerAngles.hpp
index 59e5500ad0..399eecfa7d 100644
--- a/apps/systemlib/math/EulerAngles.hpp
+++ b/apps/systemlib/math/EulerAngles.hpp
@@ -41,7 +41,8 @@
 
 #include "Vector.hpp"
 
-namespace math {
+namespace math
+{
 
 class Quaternion;
 class Dcm;
@@ -49,21 +50,21 @@ 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();
+	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; }
+	// 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); }
+	// const accessors
+	const float &getPhi() const { return (*this)(0); }
+	const float &getTheta() const { return (*this)(1); }
+	const float &getPsi() const { return (*this)(2); }
 
 };
 
diff --git a/apps/systemlib/math/Matrix.cpp b/apps/systemlib/math/Matrix.cpp
index 08796da89b..5e536c3a8d 100644
--- a/apps/systemlib/math/Matrix.cpp
+++ b/apps/systemlib/math/Matrix.cpp
@@ -45,136 +45,149 @@
 namespace math
 {
 
-static const float data_testA[] = 
-    {1,2,3,
-     4,5,6};
-static Matrix testA(2,3,data_testA);
+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_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_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_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_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);
+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;
+	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;
+	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;
+	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;
+	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;
+	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;
+	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)
+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;
+	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
diff --git a/apps/systemlib/math/Matrix.hpp b/apps/systemlib/math/Matrix.hpp
index efa71265c8..4d77316534 100644
--- a/apps/systemlib/math/Matrix.hpp
+++ b/apps/systemlib/math/Matrix.hpp
@@ -45,7 +45,8 @@
 #include "generic/Matrix.hpp"
 #endif
 
-namespace math {
+namespace math
+{
 class Matrix;
 int matrixTest();
 int matrixAddTest();
@@ -54,5 +55,5 @@ int matrixMultTest();
 int matrixInvTest();
 int matrixDivTest();
 int matrixArmTest();
-bool matrixEqual(const Matrix & a, const Matrix & b, float eps=1.0e-5f);
+bool matrixEqual(const Matrix &a, const Matrix &b, float eps = 1.0e-5f);
 } // namespace math
diff --git a/apps/systemlib/math/Quaternion.cpp b/apps/systemlib/math/Quaternion.cpp
index 79a913af65..12ae72c0e5 100644
--- a/apps/systemlib/math/Quaternion.cpp
+++ b/apps/systemlib/math/Quaternion.cpp
@@ -48,68 +48,68 @@ namespace math
 {
 
 Quaternion::Quaternion() :
-    Vector(4)
+	Vector(4)
 {
-    setA(1.0f);
-    setB(0.0f);
-    setC(0.0f);
-    setD(0.0f);
+	setA(1.0f);
+	setB(0.0f);
+	setC(0.0f);
+	setD(0.0f);
 }
 
 Quaternion::Quaternion(float a, float b,
-        float c, float d) :
-    Vector(4)
+		       float c, float d) :
+	Vector(4)
 {
-    setA(a);
-    setB(b);
-    setC(c);
-    setD(d);
+	setA(a);
+	setB(b);
+	setC(c);
+	setD(d);
 }
 
-Quaternion::Quaternion(const float * data) :
-    Vector(4,data)
+Quaternion::Quaternion(const float *data) :
+	Vector(4, data)
 {
 }
 
-Quaternion::Quaternion(const Vector & v) :
-    Vector(v)
+Quaternion::Quaternion(const Vector &v) :
+	Vector(v)
 {
 }
 
-Quaternion::Quaternion(const Dcm & dcm) :
-    Vector(4)
+Quaternion::Quaternion(const Dcm &dcm) :
+	Vector(4)
 {
-    setA(0.5f*sqrtf(1 + dcm(0,0) + 
-            dcm(1,1) + dcm(2,2)));
-    setB((dcm(2,1) - dcm(1,2))/
-            (4*getA()));
-    setC((dcm(0,2) - dcm(2,0))/
-        (4*getA()));
-    setD((dcm(1,0) - dcm(0,1))/
-        (4*getA()));
+	setA(0.5f * sqrtf(1 + dcm(0, 0) +
+			  dcm(1, 1) + dcm(2, 2)));
+	setB((dcm(2, 1) - dcm(1, 2)) /
+	     (4 * getA()));
+	setC((dcm(0, 2) - dcm(2, 0)) /
+	     (4 * getA()));
+	setD((dcm(1, 0) - dcm(0, 1)) /
+	     (4 * getA()));
 }
 
-Quaternion::Quaternion(const EulerAngles & euler) :
-    Vector(4)
+Quaternion::Quaternion(const EulerAngles &euler) :
+	Vector(4)
 {
-    float cosPhi_2 = cosf(euler.getPhi()/2.0f);
-    float cosTheta_2 = cosf(euler.getTheta()/2.0f);
-    float cosPsi_2 = cosf(euler.getPsi()/2.0f);
-    float sinPhi_2 = sinf(euler.getPhi()/2.0f);
-    float sinTheta_2 = sinf(euler.getTheta()/2.0f);
-    float sinPsi_2 = sinf(euler.getPsi()/2.0f);
-    setA(cosPhi_2*cosTheta_2*cosPsi_2 + 
-        sinPhi_2*sinTheta_2*sinPsi_2);
-    setB(sinPhi_2*cosTheta_2*cosPsi_2 -
-        cosPhi_2*sinTheta_2*sinPsi_2);
-    setC(cosPhi_2*sinTheta_2*cosPsi_2 +
-        sinPhi_2*cosTheta_2*sinPsi_2);
-    setD(cosPhi_2*cosTheta_2*sinPsi_2 +
-        sinPhi_2*sinTheta_2*cosPsi_2);
+	float cosPhi_2 = cosf(euler.getPhi() / 2.0f);
+	float cosTheta_2 = cosf(euler.getTheta() / 2.0f);
+	float cosPsi_2 = cosf(euler.getPsi() / 2.0f);
+	float sinPhi_2 = sinf(euler.getPhi() / 2.0f);
+	float sinTheta_2 = sinf(euler.getTheta() / 2.0f);
+	float sinPsi_2 = sinf(euler.getPsi() / 2.0f);
+	setA(cosPhi_2 * cosTheta_2 * cosPsi_2 +
+	     sinPhi_2 * sinTheta_2 * sinPsi_2);
+	setB(sinPhi_2 * cosTheta_2 * cosPsi_2 -
+	     cosPhi_2 * sinTheta_2 * sinPsi_2);
+	setC(cosPhi_2 * sinTheta_2 * cosPsi_2 +
+	     sinPhi_2 * cosTheta_2 * sinPsi_2);
+	setD(cosPhi_2 * cosTheta_2 * sinPsi_2 +
+	     sinPhi_2 * sinTheta_2 * cosPsi_2);
 }
 
-Quaternion::Quaternion(const Quaternion & right) :
-    Vector(right)
+Quaternion::Quaternion(const Quaternion &right) :
+	Vector(right)
 {
 }
 
@@ -117,64 +117,65 @@ Quaternion::~Quaternion()
 {
 }
 
-Vector Quaternion::derivative(const Vector & w)
+Vector Quaternion::derivative(const Vector &w)
 {
 #ifdef QUATERNION_ASSERT
-    ASSERT(w.getRows()==3);
+	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; 
+	float dataQ[] = {
+		getA(), -getB(), -getC(), -getD(),
+		getB(),  getA(), -getD(),  getC(),
+		getC(),  getD(),  getA(), -getB(),
+		getD(), -getC(),  getB(),  getA()
+	};
+	Vector v(4);
+	v(0) = 0.0f;
+	v(1) = w(0);
+	v(2) = w(1);
+	v(3) = w(2);
+	Matrix Q(4, 4, dataQ);
+	return Q * v * 0.5f;
 }
 
 int __EXPORT quaternionTest()
 {
-    printf("Test Quaternion\t\t: ");
-    // test default ctor
-    Quaternion q;
-    ASSERT(equal(q.getA(),1));  
-    ASSERT(equal(q.getB(),0));  
-    ASSERT(equal(q.getC(),0));  
-    ASSERT(equal(q.getD(),0));  
-    // test float ctor
-    q = Quaternion(0,1,0,0);
-    ASSERT(equal(q.getA(),0));  
-    ASSERT(equal(q.getB(),1));  
-    ASSERT(equal(q.getC(),0));  
-    ASSERT(equal(q.getD(),0));  
-    // test euler ctor
-    q = Quaternion(EulerAngles(0,0,0));
-    ASSERT(equal(q.getA(),1));  
-    ASSERT(equal(q.getB(),0));  
-    ASSERT(equal(q.getC(),0));  
-    ASSERT(equal(q.getD(),0));     
-    // test dcm ctor
-    q = Quaternion(Dcm());
-    ASSERT(equal(q.getA(),1));  
-    ASSERT(equal(q.getB(),0));  
-    ASSERT(equal(q.getC(),0));  
-    ASSERT(equal(q.getD(),0));     
-    // TODO test derivative
-    // test accessors
-    q.setA(0.1);
-    q.setB(0.2);
-    q.setC(0.3);
-    q.setD(0.4);
-    ASSERT(equal(q.getA(),0.1));  
-    ASSERT(equal(q.getB(),0.2));  
-    ASSERT(equal(q.getC(),0.3));  
-    ASSERT(equal(q.getD(),0.4));  
-    printf("PASS\n");
-    return 0;
+	printf("Test Quaternion\t\t: ");
+	// test default ctor
+	Quaternion q;
+	ASSERT(equal(q.getA(), 1));
+	ASSERT(equal(q.getB(), 0));
+	ASSERT(equal(q.getC(), 0));
+	ASSERT(equal(q.getD(), 0));
+	// test float ctor
+	q = Quaternion(0, 1, 0, 0);
+	ASSERT(equal(q.getA(), 0));
+	ASSERT(equal(q.getB(), 1));
+	ASSERT(equal(q.getC(), 0));
+	ASSERT(equal(q.getD(), 0));
+	// test euler ctor
+	q = Quaternion(EulerAngles(0, 0, 0));
+	ASSERT(equal(q.getA(), 1));
+	ASSERT(equal(q.getB(), 0));
+	ASSERT(equal(q.getC(), 0));
+	ASSERT(equal(q.getD(), 0));
+	// test dcm ctor
+	q = Quaternion(Dcm());
+	ASSERT(equal(q.getA(), 1));
+	ASSERT(equal(q.getB(), 0));
+	ASSERT(equal(q.getC(), 0));
+	ASSERT(equal(q.getD(), 0));
+	// TODO test derivative
+	// test accessors
+	q.setA(0.1);
+	q.setB(0.2);
+	q.setC(0.3);
+	q.setD(0.4);
+	ASSERT(equal(q.getA(), 0.1));
+	ASSERT(equal(q.getB(), 0.2));
+	ASSERT(equal(q.getC(), 0.3));
+	ASSERT(equal(q.getD(), 0.4));
+	printf("PASS\n");
+	return 0;
 }
 
 } // namespace math
diff --git a/apps/systemlib/math/Quaternion.hpp b/apps/systemlib/math/Quaternion.hpp
index 334b0dd036..4b4e959d8b 100644
--- a/apps/systemlib/math/Quaternion.hpp
+++ b/apps/systemlib/math/Quaternion.hpp
@@ -42,7 +42,8 @@
 #include "Vector.hpp"
 #include "Matrix.hpp"
 
-namespace math {
+namespace math
+{
 
 class Dcm;
 class EulerAngles;
@@ -51,62 +52,62 @@ class __EXPORT Quaternion : public Vector
 {
 public:
 
-    /**
-     * default ctor
-     */
-    Quaternion();
+	/**
+	 * default ctor
+	 */
+	Quaternion();
 
-    /**
-     * ctor from floats
-     */
-    Quaternion(float a, float b, float c, float d);
+	/**
+	 * ctor from floats
+	 */
+	Quaternion(float a, float b, float c, float d);
 
-    /**
-     * ctor from data
-     */
-    Quaternion(const float * data);
+	/**
+	 * ctor from data
+	 */
+	Quaternion(const float *data);
 
-    /**
-     * ctor from Vector
-     */
-    Quaternion(const Vector & v);
+	/**
+	 * ctor from Vector
+	 */
+	Quaternion(const Vector &v);
 
-    /**
-     * ctor from EulerAngles
-     */
-    Quaternion(const EulerAngles & euler);
+	/**
+	 * ctor from EulerAngles
+	 */
+	Quaternion(const EulerAngles &euler);
 
-    /**
-     * ctor from Dcm
-     */
-    Quaternion(const Dcm & dcm);
+	/**
+	 * ctor from Dcm
+	 */
+	Quaternion(const Dcm &dcm);
 
-    /**
-     * deep copy ctor
-     */
-    Quaternion(const Quaternion & right);
+	/**
+	 * deep copy ctor
+	 */
+	Quaternion(const Quaternion &right);
 
-    /**
-     * dtor
-     */
-    virtual ~Quaternion();
-    
-    /**
-     * derivative
-     */
-    Vector derivative(const Vector & w);
+	/**
+	 * dtor
+	 */
+	virtual ~Quaternion();
 
-    /**
-     * 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); }
+	/**
+	 * 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();
diff --git a/apps/systemlib/math/Vector.cpp b/apps/systemlib/math/Vector.cpp
index 5bdbc91689..c039a758bd 100644
--- a/apps/systemlib/math/Vector.cpp
+++ b/apps/systemlib/math/Vector.cpp
@@ -44,56 +44,57 @@
 namespace math
 {
 
-static const float data_testA[] = {1,3};
-static const float data_testB[] = {4,1};
+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);
+static Vector testA(2, data_testA);
+static Vector testB(2, data_testB);
 
 int __EXPORT vectorTest()
 {
-    vectorAddTest();
-    vectorSubTest();
-    return 0;
+	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;
+	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;
+	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)
+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;
+	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
diff --git a/apps/systemlib/math/Vector.hpp b/apps/systemlib/math/Vector.hpp
index 0c5ac2b195..773bd50323 100644
--- a/apps/systemlib/math/Vector.hpp
+++ b/apps/systemlib/math/Vector.hpp
@@ -45,10 +45,11 @@
 #include "generic/Vector.hpp"
 #endif
 
-namespace math {
+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);
+bool vectorEqual(const Vector &a, const Vector &b, float eps = 1.0e-5f);
 } // math
diff --git a/apps/systemlib/math/Vector3.cpp b/apps/systemlib/math/Vector3.cpp
index a30b6d1982..edba663cb6 100644
--- a/apps/systemlib/math/Vector3.cpp
+++ b/apps/systemlib/math/Vector3.cpp
@@ -45,28 +45,28 @@ namespace math
 {
 
 Vector3::Vector3() :
-    Vector(3)
+	Vector(3)
 {
 }
 
-Vector3::Vector3(const Vector & right) :
-    Vector(right)
+Vector3::Vector3(const Vector &right) :
+	Vector(right)
 {
 #ifdef VECTOR_ASSERT
-    ASSERT(right.getRows()==3);
+	ASSERT(right.getRows() == 3);
 #endif
 }
 
 Vector3::Vector3(float x, float y, float z) :
-    Vector(3)
+	Vector(3)
 {
-    setX(x);
-    setY(y);
-    setZ(z);
+	setX(x);
+	setY(y);
+	setZ(z);
 }
 
-Vector3::Vector3(const float * data) :
-    Vector(3,data)
+Vector3::Vector3(const float *data) :
+	Vector(3, data)
 {
 }
 
@@ -74,26 +74,26 @@ Vector3::~Vector3()
 {
 }
 
-Vector3 Vector3::cross(const Vector3 & b)
+Vector3 Vector3::cross(const Vector3 &b)
 {
-    Vector3 & a = *this;
-    Vector3 result;
-    result(0) = a(1)*b(2) - a(2)*b(1);
-    result(1) = a(2)*b(0) - a(0)*b(2);
-    result(2) = a(0)*b(1) - a(1)*b(0);
-    return result;
+	Vector3 &a = *this;
+	Vector3 result;
+	result(0) = a(1) * b(2) - a(2) * b(1);
+	result(1) = a(2) * b(0) - a(0) * b(2);
+	result(2) = a(0) * b(1) - a(1) * b(0);
+	return result;
 }
 
 int __EXPORT vector3Test()
 {
-    printf("Test Vector3\t\t: ");
-    // test float ctor
-    Vector3 v(1,2,3);
-    ASSERT(equal(v(0),1)); 
-    ASSERT(equal(v(1),2)); 
-    ASSERT(equal(v(2),3)); 
-    printf("PASS\n");
-    return 0;
+	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
diff --git a/apps/systemlib/math/Vector3.hpp b/apps/systemlib/math/Vector3.hpp
index 48dfd5b22e..8c36ac1347 100644
--- a/apps/systemlib/math/Vector3.hpp
+++ b/apps/systemlib/math/Vector3.hpp
@@ -45,25 +45,25 @@ namespace math
 {
 
 class __EXPORT Vector3 :
-    public Vector
+	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);
+	Vector3();
+	Vector3(const Vector &right);
+	Vector3(float x, float y, float z);
+	Vector3(const float *data);
+	virtual ~Vector3();
+	Vector3 cross(const Vector3 &b);
 
-    /**
-     * accessors
-     */
-    void setX(float x) { (*this)(0) = x; }
-    void setY(float y) { (*this)(1) = y; }
-    void setZ(float z) { (*this)(2) = z; }
-    const float & getX() const { return (*this)(0); }
-    const float & getY() const { return (*this)(1); }
-    const float & getZ() const { return (*this)(2); }
+	/**
+	 * accessors
+	 */
+	void setX(float x) { (*this)(0) = x; }
+	void setY(float y) { (*this)(1) = y; }
+	void setZ(float z) { (*this)(2) = z; }
+	const float &getX() const { return (*this)(0); }
+	const float &getY() const { return (*this)(1); }
+	const float &getZ() const { return (*this)(2); }
 };
 
 int __EXPORT vector3Test();
diff --git a/apps/systemlib/math/arm/Matrix.hpp b/apps/systemlib/math/arm/Matrix.hpp
index a949808723..a95b5a2b0c 100644
--- a/apps/systemlib/math/arm/Matrix.hpp
+++ b/apps/systemlib/math/arm/Matrix.hpp
@@ -56,257 +56,237 @@
 namespace math
 {
 
-class __EXPORT Matrix {
+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)
-    {
+	// 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());
+		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)
-    {
+
+		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());
+		ASSERT(i < getRows());
+		ASSERT(j < getCols());
 #endif
-        return getData()[i*getCols() + j];
-    }
-    inline const float & operator()(size_t i, size_t j) const
-    {
+		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());
+		ASSERT(i < getRows());
+		ASSERT(j < getCols());
 #endif
-        return getData()[i*getCols() + j];
-    }
-    // output
-    inline void print() const
-    {
-        for (size_t i=0; i<getRows(); i++)
-        {
-            for (size_t j=0; j<getCols(); j++)
-            {
-                float sig;
-                int exp;
-                float num = (*this)(i,j);
-                float2SigExp(num,sig,exp);
-                printf ("%6.3fe%03.3d,", (double)sig, exp);
-            }
-            printf("\n");
-        }
-    }
-    // boolean ops
-    inline bool operator==(const Matrix & right) const
-    {
-        for (size_t i=0; i<getRows(); i++)
-        {
-            for (size_t j=0; j<getCols(); j++)
-            {
-                if (fabsf((*this)(i,j)-right(i,j)) > 1e-30f)
-                    return false;
-            }
-        }
-        return true;
-    }
-    // scalar ops
-    inline Matrix operator+(float right) const
-    {
-        Matrix result(getRows(), getCols());
-        arm_offset_f32((float *)getData(),right,
-                (float *)result.getData(),getRows()*getCols());
-        return result;
-    }
-    inline Matrix operator-(float right) const
-    {
-        Matrix result(getRows(), getCols());
-        arm_offset_f32((float *)getData(),-right,
-                (float *)result.getData(),getRows()*getCols());
-        return result;
-    }
-    inline Matrix operator*(float right) const
-    {
-        Matrix result(getRows(), getCols());
-        arm_mat_scale_f32(&_matrix,right,
-                &(result._matrix));
-        return result;
-    }
-    inline Matrix operator/(float right) const
-    {
-        Matrix result(getRows(), getCols());
-        arm_mat_scale_f32(&_matrix,1.0f/right,
-                &(result._matrix));
-        return result;
-    }
-    // vector ops
-    inline Vector operator*(const Vector & right) const
-    {
+		return getData()[i * getCols() + j];
+	}
+	// output
+	inline void print() const {
+		for (size_t i = 0; i < getRows(); i++) {
+			for (size_t j = 0; j < getCols(); j++) {
+				float sig;
+				int exp;
+				float num = (*this)(i, j);
+				float2SigExp(num, sig, exp);
+				printf("%6.3fe%03.3d,", (double)sig, exp);
+			}
+
+			printf("\n");
+		}
+	}
+	// boolean ops
+	inline bool operator==(const Matrix &right) const {
+		for (size_t i = 0; i < getRows(); i++) {
+			for (size_t j = 0; j < getCols(); j++) {
+				if (fabsf((*this)(i, j) - right(i, j)) > 1e-30f)
+					return false;
+			}
+		}
+
+		return true;
+	}
+	// scalar ops
+	inline Matrix operator+(float right) const {
+		Matrix result(getRows(), getCols());
+		arm_offset_f32((float *)getData(), right,
+			       (float *)result.getData(), getRows()*getCols());
+		return result;
+	}
+	inline Matrix operator-(float right) const {
+		Matrix result(getRows(), getCols());
+		arm_offset_f32((float *)getData(), -right,
+			       (float *)result.getData(), getRows()*getCols());
+		return result;
+	}
+	inline Matrix operator*(float right) const {
+		Matrix result(getRows(), getCols());
+		arm_mat_scale_f32(&_matrix, right,
+				  &(result._matrix));
+		return result;
+	}
+	inline Matrix operator/(float right) const {
+		Matrix result(getRows(), getCols());
+		arm_mat_scale_f32(&_matrix, 1.0f / right,
+				  &(result._matrix));
+		return result;
+	}
+	// vector ops
+	inline Vector operator*(const Vector &right) const {
 #ifdef MATRIX_ASSERT
-        ASSERT(getCols()==right.getRows());
+		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
-    {
+		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());
+		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
-    {
+		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());
+		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
-    {
+		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());
+		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
-    {
+		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());
+		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
-    {
+		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());
+		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;
-    }
+		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; }
+	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;
+	arm_matrix_instance_f32 _matrix;
 };
 
 } // namespace math
diff --git a/apps/systemlib/math/arm/Vector.hpp b/apps/systemlib/math/arm/Vector.hpp
index b12f112f68..c1e13f29a3 100644
--- a/apps/systemlib/math/arm/Vector.hpp
+++ b/apps/systemlib/math/arm/Vector.hpp
@@ -55,186 +55,166 @@
 namespace math
 {
 
-class __EXPORT Vector {
+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)
-    {
+	// 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());
+		ASSERT(getRows() == right.getRows());
 #endif
-        if (this != &right)
-        {
-            memcpy(getData(),right.getData(),
-                        right.getSize());
-        }
-        return *this;
-    }
-    // element accessors
-    inline float& operator()(size_t i)
-    {
+
+		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());
+		ASSERT(i < getRows());
 #endif
-        return getData()[i];
-    }
-    inline const float& operator()(size_t i) const
-    {
+		return getData()[i];
+	}
+	inline const float &operator()(size_t i) const {
 #ifdef VECTOR_ASSERT
-        ASSERT(i<getRows());
+		ASSERT(i < getRows());
 #endif
-        return getData()[i];
-    }
-    // output
-    inline void print() const
-    {
-        for (size_t i=0; i<getRows(); i++)
-        {
-            float sig;
-            int exp;
-            float num = (*this)(i);
-            float2SigExp(num,sig,exp);
-            printf ("%6.3fe%03.3d,", (double)sig, exp);
-        }
-        printf("\n");
-    }
-    // boolean ops
-    inline bool operator==(const Vector & right) const
-    {
-        for (size_t i=0; i<getRows(); i++)
-        {
-            if (fabsf(((*this)(i) - right(i))) > 1e-30f)
-                return false;
-        }
-        return true;
-    }
-    // scalar ops
-    inline Vector operator+(float right) const
-    {
-        Vector result(getRows());
-        arm_offset_f32((float*)getData(),
-                right, result.getData(),
-                getRows());
-        return result;
-    }
-    inline Vector operator-(float right) const
-    {
-        Vector result(getRows());
-        arm_offset_f32((float*)getData(),
-                -right, result.getData(),
-                getRows());
-        return result;
-    }
-    inline Vector operator*(float right) const
-    {
-        Vector result(getRows());
-        arm_scale_f32((float*)getData(),
-                right, result.getData(),
-                getRows());
-        return result;
-    }
-    inline Vector operator/(float right) const
-    {
-        Vector result(getRows());
-        arm_scale_f32((float*)getData(),
-                1.0f/right, result.getData(),
-                getRows());
-        return result;
-    }
-    // vector ops
-    inline Vector operator+(const Vector & right) const
-    {
+		return getData()[i];
+	}
+	// output
+	inline void print() const {
+		for (size_t i = 0; i < getRows(); i++) {
+			float sig;
+			int exp;
+			float num = (*this)(i);
+			float2SigExp(num, sig, exp);
+			printf("%6.3fe%03.3d,", (double)sig, exp);
+		}
+
+		printf("\n");
+	}
+	// boolean ops
+	inline bool operator==(const Vector &right) const {
+		for (size_t i = 0; i < getRows(); i++) {
+			if (fabsf(((*this)(i) - right(i))) > 1e-30f)
+				return false;
+		}
+
+		return true;
+	}
+	// scalar ops
+	inline Vector operator+(float right) const {
+		Vector result(getRows());
+		arm_offset_f32((float *)getData(),
+			       right, result.getData(),
+			       getRows());
+		return result;
+	}
+	inline Vector operator-(float right) const {
+		Vector result(getRows());
+		arm_offset_f32((float *)getData(),
+			       -right, result.getData(),
+			       getRows());
+		return result;
+	}
+	inline Vector operator*(float right) const {
+		Vector result(getRows());
+		arm_scale_f32((float *)getData(),
+			      right, result.getData(),
+			      getRows());
+		return result;
+	}
+	inline Vector operator/(float right) const {
+		Vector result(getRows());
+		arm_scale_f32((float *)getData(),
+			      1.0f / right, result.getData(),
+			      getRows());
+		return result;
+	}
+	// vector ops
+	inline Vector operator+(const Vector &right) const {
 #ifdef VECTOR_ASSERT
-        ASSERT(getRows()==right.getRows());
+		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
-    {
+		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());
+		ASSERT(getRows() == right.getRows());
 #endif
-        Vector result(getRows());
-        arm_sub_f32((float*)getData(),
-                (float*)right.getData(),
-                result.getData(),
-                getRows());
-        return result;
-    }
-    // other functions
-    inline float dot(const Vector & right)
-    {
-        float result = 0;
-        arm_dot_prod_f32((float*)getData(),
-                (float*)right.getData(),
-                getRows(),
-                &result);
-        return result;
-    }
-    inline float norm()
-    {
-        return sqrtf(dot(*this));
-    }
-    inline Vector unit()
-    {
-        return (*this)/norm();
-    }
-    inline static Vector zero(size_t rows)
-    {
-        Vector result(rows);
-        // calloc returns zeroed memory
-        return result;
-    }
-    inline void setAll(const float & val)
-    {
-        for (size_t i=0;i<getRows();i++)
-        {
-            (*this)(i) = val;
-        }
-    }
-    inline void set(const float * data)
-    {
-        memcpy(getData(),data,getSize());
-    }
-    inline size_t getRows() const { return _rows; }
-    inline const float * getData() const { return _data; }
+		Vector result(getRows());
+		arm_sub_f32((float *)getData(),
+			    (float *)right.getData(),
+			    result.getData(),
+			    getRows());
+		return result;
+	}
+	// other functions
+	inline float dot(const Vector &right) {
+		float result = 0;
+		arm_dot_prod_f32((float *)getData(),
+				 (float *)right.getData(),
+				 getRows(),
+				 &result);
+		return result;
+	}
+	inline float norm() {
+		return sqrtf(dot(*this));
+	}
+	inline Vector unit() {
+		return (*this) / norm();
+	}
+	inline static Vector zero(size_t rows) {
+		Vector result(rows);
+		// calloc returns zeroed memory
+		return result;
+	}
+	inline void setAll(const float &val) {
+		for (size_t i = 0; i < getRows(); i++) {
+			(*this)(i) = val;
+		}
+	}
+	inline void set(const float *data) {
+		memcpy(getData(), data, getSize());
+	}
+	inline size_t getRows() const { return _rows; }
+	inline const float *getData() const { return _data; }
 protected:
-    inline size_t getSize() const { return sizeof(float)*getRows(); }
-    inline float * getData() { return _data; }
-    inline void setData(float * data) { _data = data; }
+	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;
+	size_t _rows;
+	float *_data;
 };
 
 } // math
diff --git a/apps/systemlib/math/generic/Matrix.hpp b/apps/systemlib/math/generic/Matrix.hpp
index fd6c8ba515..d10208a1ef 100644
--- a/apps/systemlib/math/generic/Matrix.hpp
+++ b/apps/systemlib/math/generic/Matrix.hpp
@@ -53,395 +53,385 @@
 namespace math
 {
 
-class __EXPORT Matrix {
+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)
-    {
+	// 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());
+		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();
+		if (this != &right) {
+			memcpy(getData(), right.getData(),
+			       right.getSize());
+		}
 
-        // 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);
-                    }
-                }
-            }
+		return *this;
+	}
+	// element accessors
+	inline float &operator()(size_t i, size_t j) {
 #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);
+		ASSERT(i < getRows());
+		ASSERT(j < getCols());
 #endif
-            // failsafe, return zero matrix
-            if (fabsf(U(n,n))<1e-8f)
-            {
-                return Matrix::zero(n);
-            }
+		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);
+			}
 
-            // 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("\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;
+			}
+		}
 
-        //printf("L:\n"); L.print();
-        //printf("U:\n"); U.print();
+		return true;
+	}
+	// scalar ops
+	inline Matrix operator+(const float &right) const {
+		Matrix result(getRows(), getCols());
 
-        // 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
-            }
-        }
+		for (size_t i = 0; i < getRows(); i++) {
+			for (size_t j = 0; j < getCols(); j++) {
+				result(i, j) = (*this)(i, j) + right;
+			}
+		}
 
-        //printf("Y:\n"); Y.print();
+		return result;
+	}
+	inline Matrix operator-(const float &right) const {
+		Matrix result(getRows(), getCols());
 
-        // 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;
-    }
+		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; }
+	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;
+	size_t _rows;
+	size_t _cols;
+	float *_data;
 };
 
 } // namespace math
diff --git a/apps/systemlib/math/generic/Vector.hpp b/apps/systemlib/math/generic/Vector.hpp
index 4d867f3603..01cfa8bc08 100644
--- a/apps/systemlib/math/generic/Vector.hpp
+++ b/apps/systemlib/math/generic/Vector.hpp
@@ -51,190 +51,177 @@
 namespace math
 {
 
-class __EXPORT Vector {
+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)
-    {
+	// 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());
+		ASSERT(getRows() == right.getRows());
 #endif
-        if (this != &right)
-        {
-            memcpy(getData(),right.getData(),
-                        right.getSize());
-        }
-        return *this;
-    }
-    // element accessors
-    inline float& operator()(size_t i)
-    {
+
+		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());
+		ASSERT(i < getRows());
 #endif
-        return getData()[i];
-    }
-    inline const float& operator()(size_t i) const
-    {
+		return getData()[i];
+	}
+	inline const float &operator()(size_t i) const {
 #ifdef VECTOR_ASSERT
-        ASSERT(i<getRows());
+		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
-    {
+		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());
+		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
-    {
+		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());
+		ASSERT(getRows() == right.getRows());
 #endif
-        Vector result(getRows());
-        for (size_t i=0; i<getRows(); i++)
-        {
-            result(i) = (*this)(i) - right(i);
-        }
-        return result;
-    }
-    // other functions
-    inline float dot(const Vector & right)
-    {
-        float result = 0;
-        for (size_t i=0; i<getRows(); i++)
-        {
-            result += (*this)(i)*(*this)(i);
-        }
-        return result;
-    }
-    inline float norm()
-    {
-        return sqrtf(dot(*this));
-    }
-    inline Vector unit()
-    {
-        return (*this)/norm();
-    }
-    inline static Vector zero(size_t rows)
-    {
-        Vector result(rows);
-        // calloc returns zeroed memory
-        return result;
-    }
-    inline void setAll(const float & val)
-    {
-        for (size_t i=0;i<getRows();i++)
-        {
-            (*this)(i) = val;
-        }
-    }
-    inline void set(const float * data)
-    {
-        memcpy(getData(),data,getSize());
-    }
-    inline size_t getRows() const { return _rows; }
+		Vector result(getRows());
+
+		for (size_t i = 0; i < getRows(); i++) {
+			result(i) = (*this)(i) - right(i);
+		}
+
+		return result;
+	}
+	// other functions
+	inline float dot(const Vector &right) {
+		float result = 0;
+
+		for (size_t i = 0; i < getRows(); i++) {
+			result += (*this)(i) * (*this)(i);
+		}
+
+		return result;
+	}
+	inline float norm() {
+		return sqrtf(dot(*this));
+	}
+	inline Vector unit() {
+		return (*this) / norm();
+	}
+	inline static Vector zero(size_t rows) {
+		Vector result(rows);
+		// calloc returns zeroed memory
+		return result;
+	}
+	inline void setAll(const float &val) {
+		for (size_t i = 0; i < getRows(); i++) {
+			(*this)(i) = val;
+		}
+	}
+	inline void set(const float *data) {
+		memcpy(getData(), data, getSize());
+	}
+	inline size_t getRows() const { return _rows; }
 protected:
-    inline size_t getSize() const { return sizeof(float)*getRows(); }
-    inline float * getData() { return _data; }
-    inline const float * getData() const { return _data; }
-    inline void setData(float * data) { _data = data; }
+	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;
+	size_t _rows;
+	float *_data;
 };
 
 } // math
diff --git a/apps/systemlib/test/test.cpp b/apps/systemlib/test/test.cpp
index aa00e66fae..f5db76d704 100644
--- a/apps/systemlib/test/test.cpp
+++ b/apps/systemlib/test/test.cpp
@@ -44,45 +44,50 @@
 
 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;
+	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)
+	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;
-    }
+	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;
+	}
 }
 
 
diff --git a/apps/systemlib/test/test.hpp b/apps/systemlib/test/test.hpp
index 860758e7d1..841c421445 100644
--- a/apps/systemlib/test/test.hpp
+++ b/apps/systemlib/test/test.hpp
@@ -45,6 +45,6 @@
 
 bool equal(float a, float b, float eps = 1e-5);
 void float2SigExp(
-        const float & num,
-        float & sig,
-        int & exp);
+	const float &num,
+	float &sig,
+	int &exp);