Merge branch 'mathlib_new' into vector_control2

44 files changed, 1298 insertions, 3036 deletions

diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp
index b078562c2..614877b18 100644
--- a/src/lib/conversion/rotation.cpp
+++ b/src/lib/conversion/rotation.cpp
@@ -41,22 +41,11 @@
 #include "rotation.h"
 
 __EXPORT void
-get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix)
+get_rot_matrix(enum Rotation rot, math::Matrix<3,3> *rot_matrix)
 {
-	/* first set to zero */
-	rot_matrix->Matrix::zero(3, 3);
-
 	float roll  = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll;
 	float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch;
 	float yaw   = M_DEG_TO_RAD_F * (float)rot_lookup[rot].yaw;
 
-	math::EulerAngles euler(roll, pitch, yaw);
-
-	math::Dcm R(euler);
-
-	for (int i = 0; i < 3; i++) {
-		for (int j = 0; j < 3; j++) {
-			(*rot_matrix)(i, j) = R(i, j);
-		}
-	}
+	rot_matrix->from_euler(roll, pitch, yaw);
 }
diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h
index 85c63c0fc..0c56494c5 100644
--- a/src/lib/conversion/rotation.h
+++ b/src/lib/conversion/rotation.h
@@ -116,6 +116,6 @@ const rot_lookup_t rot_lookup[] = {
  * Get the rotation matrix
  */
 __EXPORT void
-get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix);
+get_rot_matrix(enum Rotation rot, math::Matrix<3,3> *rot_matrix);
 
 #endif /* ROTATION_H_ */
diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
index 27d76f959..fb7ed486d 100644
--- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
+++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
@@ -83,8 +83,8 @@ float ECL_L1_Pos_Controller::crosstrack_error(void)
 	return _crosstrack_error;
 }
 
-void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position,
-				       const math::Vector2f &ground_speed_vector)
+void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector<2> &vector_A, const math::Vector<2> &vector_B, const math::Vector<2> &vector_curr_position,
+				       const math::Vector<2> &ground_speed_vector)
 {
 
 	/* this follows the logic presented in [1] */
@@ -94,7 +94,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
 	float ltrack_vel;
 
 	/* get the direction between the last (visited) and next waypoint */
-	_target_bearing = get_bearing_to_next_waypoint(vector_curr_position.getX(), vector_curr_position.getY(), vector_B.getX(), vector_B.getY());
+	_target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_B(0), vector_B(1));
 
 	/* enforce a minimum ground speed of 0.1 m/s to avoid singularities */
 	float ground_speed = math::max(ground_speed_vector.length(), 0.1f);
@@ -103,7 +103,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
 	_L1_distance = _L1_ratio * ground_speed;
 
 	/* calculate vector from A to B */
-	math::Vector2f vector_AB = get_local_planar_vector(vector_A, vector_B);
+	math::Vector<2> vector_AB = get_local_planar_vector(vector_A, vector_B);
 
 	/*
 	 * check if waypoints are on top of each other. If yes,
@@ -116,7 +116,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
 	vector_AB.normalize();
 
 	/* calculate the vector from waypoint A to the aircraft */
-	math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
+	math::Vector<2> vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
 
 	/* calculate crosstrack error (output only) */
 	_crosstrack_error = vector_AB % vector_A_to_airplane;
@@ -130,7 +130,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
 	float alongTrackDist = vector_A_to_airplane * vector_AB;
 
 	/* estimate airplane position WRT to B */
-	math::Vector2f vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized();
+	math::Vector<2> vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized();
 	
 	/* calculate angle of airplane position vector relative to line) */
 
@@ -143,14 +143,14 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
 		/* calculate eta to fly to waypoint A */
 
 		/* unit vector from waypoint A to current position */
-		math::Vector2f vector_A_to_airplane_unit = vector_A_to_airplane.normalized();
+		math::Vector<2> vector_A_to_airplane_unit = vector_A_to_airplane.normalized();
 		/* velocity across / orthogonal to line */
 		xtrack_vel = ground_speed_vector % (-vector_A_to_airplane_unit);
 		/* velocity along line */
 		ltrack_vel = ground_speed_vector * (-vector_A_to_airplane_unit);
 		eta = atan2f(xtrack_vel, ltrack_vel);
 		/* bearing from current position to L1 point */
-		_nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX());
+		_nav_bearing = atan2f(-vector_A_to_airplane_unit(1) , -vector_A_to_airplane_unit(0));
 
 	/*
 	 * If the AB vector and the vector from B to airplane point in the same
@@ -174,7 +174,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
 		ltrack_vel = ground_speed_vector * (-vector_B_to_P_unit);
 		eta = atan2f(xtrack_vel, ltrack_vel);
 		/* bearing from current position to L1 point */
-		_nav_bearing = atan2f(-vector_B_to_P_unit.getY() , -vector_B_to_P_unit.getX());
+		_nav_bearing = atan2f(-vector_B_to_P_unit(1) , -vector_B_to_P_unit(0));
 
 	} else {
 
@@ -194,7 +194,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
 		float eta1 = asinf(sine_eta1);
 		eta = eta1 + eta2;
 		/* bearing from current position to L1 point */
-		_nav_bearing = atan2f(vector_AB.getY(), vector_AB.getX()) + eta1;
+		_nav_bearing = atan2f(vector_AB(1), vector_AB(0)) + eta1;
 
 	}
 
@@ -209,8 +209,8 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
 	_bearing_error = eta;
 }
 
-void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, const math::Vector2f &vector_curr_position, float radius, int8_t loiter_direction,
-				       const math::Vector2f &ground_speed_vector)
+void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector<2> &vector_A, const math::Vector<2> &vector_curr_position, float radius, int8_t loiter_direction,
+				       const math::Vector<2> &ground_speed_vector)
 {
 	/* the complete guidance logic in this section was proposed by [2] */
 
@@ -220,7 +220,7 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, cons
 	float K_velocity = 2.0f * _L1_damping * omega;
 
 	/* update bearing to next waypoint */
-	_target_bearing = get_bearing_to_next_waypoint(vector_curr_position.getX(), vector_curr_position.getY(), vector_A.getX(), vector_A.getY());
+	_target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_A(0), vector_A(1));
 
 	/* ground speed, enforce minimum of 0.1 m/s to avoid singularities */
 	float ground_speed = math::max(ground_speed_vector.length() , 0.1f);
@@ -229,10 +229,10 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, cons
 	_L1_distance = _L1_ratio * ground_speed;
 
 	/* calculate the vector from waypoint A to current position */
-	math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
+	math::Vector<2> vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
 
 	/* store the normalized vector from waypoint A to current position */
-	math::Vector2f vector_A_to_airplane_unit = (vector_A_to_airplane).normalized();
+	math::Vector<2> vector_A_to_airplane_unit = (vector_A_to_airplane).normalized();
 
 	/* calculate eta angle towards the loiter center */
 
@@ -287,19 +287,19 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, cons
 		/* angle between requested and current velocity vector */
 		_bearing_error = eta;
 		/* bearing from current position to L1 point */
-		_nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX());
+		_nav_bearing = atan2f(-vector_A_to_airplane_unit(1) , -vector_A_to_airplane_unit(0));
 
 	} else {
 		_lateral_accel = lateral_accel_sp_circle;
 		_circle_mode = true;
 		_bearing_error = 0.0f;
 		/* bearing from current position to L1 point */
-		_nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX());
+		_nav_bearing = atan2f(-vector_A_to_airplane_unit(1) , -vector_A_to_airplane_unit(0));
 	}
 }
 
 
-void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading, const math::Vector2f &ground_speed_vector)
+void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading, const math::Vector<2> &ground_speed_vector)
 {
 	/* the complete guidance logic in this section was proposed by [2] */
 
@@ -352,14 +352,11 @@ void ECL_L1_Pos_Controller::navigate_level_flight(float current_heading)
 }
 
 
-math::Vector2f ECL_L1_Pos_Controller::get_local_planar_vector(const math::Vector2f &origin, const math::Vector2f &target) const
+math::Vector<2> ECL_L1_Pos_Controller::get_local_planar_vector(const math::Vector<2> &origin, const math::Vector<2> &target) const
 {
 	/* this is an approximation for small angles, proposed by [2] */
 
-	math::Vector2f out;
-
-	out.setX(math::radians((target.getX() - origin.getX())));
-	out.setY(math::radians((target.getY() - origin.getY())*cosf(math::radians(origin.getX()))));
+	math::Vector<2> out(math::radians((target(0) - origin(0))), math::radians((target(1) - origin(1))*cosf(math::radians(origin(0)))));
 
 	return out * static_cast<float>(CONSTANTS_RADIUS_OF_EARTH);
 }
diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.h b/src/lib/ecl/l1/ecl_l1_pos_controller.h
index 7a3c42a92..5c0804a39 100644
--- a/src/lib/ecl/l1/ecl_l1_pos_controller.h
+++ b/src/lib/ecl/l1/ecl_l1_pos_controller.h
@@ -160,8 +160,8 @@ public:
 	 *
 	 * @return sets _lateral_accel setpoint
 	 */
-	void navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position,
-			   const math::Vector2f &ground_speed);
+	void navigate_waypoints(const math::Vector<2> &vector_A, const math::Vector<2> &vector_B, const math::Vector<2> &vector_curr_position,
+			   const math::Vector<2> &ground_speed);
 
 
 	/**
@@ -172,8 +172,8 @@ public:
 	 *
 	 * @return sets _lateral_accel setpoint
 	 */
-	void navigate_loiter(const math::Vector2f &vector_A, const math::Vector2f &vector_curr_position, float radius, int8_t loiter_direction,
-			   const math::Vector2f &ground_speed_vector);
+	void navigate_loiter(const math::Vector<2> &vector_A, const math::Vector<2> &vector_curr_position, float radius, int8_t loiter_direction,
+			   const math::Vector<2> &ground_speed_vector);
 
 
 	/**
@@ -185,7 +185,7 @@ public:
 	 *
 	 * @return sets _lateral_accel setpoint
 	 */
-	void navigate_heading(float navigation_heading, float current_heading, const math::Vector2f &ground_speed);
+	void navigate_heading(float navigation_heading, float current_heading, const math::Vector<2> &ground_speed);
 
 
 	/**
@@ -260,7 +260,7 @@ private:
 	 * @param wp The point to convert to into the local coordinates, in WGS84 coordinates
 	 * @return The vector in meters pointing from the reference position to the coordinates
 	 */
-	math::Vector2f get_local_planar_vector(const math::Vector2f &origin, const math::Vector2f &target) const;
+	math::Vector<2> get_local_planar_vector(const math::Vector<2> &origin, const math::Vector<2> &target) const;
 
 };
 
diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp
index a733ef1d2..d7f350e5c 100644
--- a/src/lib/external_lgpl/tecs/tecs.cpp
+++ b/src/lib/external_lgpl/tecs/tecs.cpp
@@ -29,7 +29,7 @@ using namespace math;
  *
  */
 
-void TECS::update_50hz(float baro_altitude, float airspeed, const math::Dcm &rotMat, const math::Vector3 &accel_body, const math::Vector3 &accel_earth)
+void TECS::update_50hz(float baro_altitude, float airspeed, const math::Matrix<3,3> &rotMat, const math::Vector<3> &accel_body, const math::Vector<3> &accel_earth)
 {
 	// Implement third order complementary filter for height and height rate
 	// estimted height rate = _integ2_state
@@ -257,7 +257,7 @@ void TECS::_update_energies(void)
 	_SKEdot = _integ5_state * _vel_dot;
 }
 
-void TECS::_update_throttle(float throttle_cruise, const math::Dcm &rotMat)
+void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotMat)
 {
 	// Calculate total energy values
 	_STE_error = _SPE_dem - _SPE_est + _SKE_dem - _SKE_est;
@@ -468,7 +468,7 @@ void TECS::_update_STE_rate_lim(void)
 	_STEdot_min = - _minSinkRate * CONSTANTS_ONE_G;
 }
 
-void TECS::update_pitch_throttle(const math::Dcm &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO,
+void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO,
 				 float throttle_min, float throttle_max, float throttle_cruise,
 				 float pitch_limit_min, float pitch_limit_max)
 {
diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h
index f8f832ed7..68832a039 100644
--- a/src/lib/external_lgpl/tecs/tecs.h
+++ b/src/lib/external_lgpl/tecs/tecs.h
@@ -71,10 +71,10 @@ public:
 	// Update of the estimated height and height rate internal state
 	// Update of the inertial speed rate internal state
 	// Should be called at 50Hz or greater
-	void update_50hz(float baro_altitude, float airspeed, const math::Dcm &rotMat, const math::Vector3 &accel_body, const math::Vector3 &accel_earth);
+	void update_50hz(float baro_altitude, float airspeed, const math::Matrix<3,3> &rotMat, const math::Vector<3> &accel_body, const math::Vector<3> &accel_earth);
 
 	// Update the control loop calculations
-	void update_pitch_throttle(const math::Dcm &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO,
+	void update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO,
 				   float throttle_min, float throttle_max, float throttle_cruise,
 				   float pitch_limit_min, float pitch_limit_max);
 	// demanded throttle in percentage
@@ -338,7 +338,7 @@ private:
 	void _update_energies(void);
 
 	// Update Demanded Throttle
-	void _update_throttle(float throttle_cruise, const math::Dcm &rotMat);
+	void _update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotMat);
 
 	// Detect Bad Descent
 	void _detect_bad_descent(void);
diff --git a/src/lib/mathlib/math/Dcm.cpp b/src/lib/mathlib/math/Dcm.cpp
deleted file mode 100644
index 5c15d4d6d..000000000
--- a/src/lib/mathlib/math/Dcm.cpp
+++ /dev/null
@@ -1,179 +0,0 @@
-/****************************************************************************
- *
- *   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
diff --git a/src/lib/mathlib/math/Dcm.hpp b/src/lib/mathlib/math/Dcm.hpp
deleted file mode 100644
index 38f697c15..000000000
--- a/src/lib/mathlib/math/Dcm.hpp
+++ /dev/null
@@ -1,113 +0,0 @@
-/****************************************************************************
- *
- *   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
-
diff --git a/src/lib/mathlib/math/EulerAngles.cpp b/src/lib/mathlib/math/EulerAngles.cpp
deleted file mode 100644
index e733d23bb..000000000
--- a/src/lib/mathlib/math/EulerAngles.cpp
+++ /dev/null
@@ -1,126 +0,0 @@
-/****************************************************************************
- *
- *   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
diff --git a/src/lib/mathlib/math/EulerAngles.hpp b/src/lib/mathlib/math/EulerAngles.hpp
deleted file mode 100644
index 399eecfa7..000000000
--- a/src/lib/mathlib/math/EulerAngles.hpp
+++ /dev/null
@@ -1,74 +0,0 @@
-/****************************************************************************
- *
- *   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
-
diff --git a/src/lib/mathlib/math/Matrix.cpp b/src/lib/mathlib/math/Matrix.cpp
deleted file mode 100644
index ebd1aeda3..000000000
--- a/src/lib/mathlib/math/Matrix.cpp
+++ /dev/null
@@ -1,193 +0,0 @@
-/****************************************************************************
- *
- *   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
diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp
index f19db15ec..ea0cf4ca1 100644
--- a/src/lib/mathlib/math/Matrix.hpp
+++ b/src/lib/mathlib/math/Matrix.hpp
@@ -1,6 +1,9 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Anton Babushkin <anton.babushkin@me.com>
+ *           Pavel Kirienko <pavel.kirienko@gmail.com>
+ *           Lorenz Meier <lm@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -32,30 +35,401 @@
  ****************************************************************************/
 
 /**
- * @file Matrix.h
+ * @file Matrix.hpp
  *
- * matrix code
+ * Matrix class
  */
 
-#pragma once
+#ifndef MATRIX_HPP
+#define MATRIX_HPP
 
-#include <nuttx/config.h>
-
-#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU)
-#include "arm/Matrix.hpp"
-#else
-#include "generic/Matrix.hpp"
-#endif
+#include <stdio.h>
+#include "../CMSIS/Include/arm_math.h"
 
 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
+
+template <unsigned int M, unsigned int N>
+class __EXPORT Matrix;
+
+// MxN matrix with float elements
+template <unsigned int M, unsigned int N>
+class __EXPORT MatrixBase
+{
+public:
+	/**
+	 * matrix data[row][col]
+	 */
+	float data[M][N];
+
+	/**
+	 * struct for using arm_math functions
+	 */
+	arm_matrix_instance_f32 arm_mat;
+
+	/**
+	 * trivial ctor
+	 * note that this ctor will not initialize elements
+	 */
+	MatrixBase() {
+		arm_mat = {M, N, &data[0][0]};
+	}
+
+	/**
+	 * copyt ctor
+	 */
+	MatrixBase(const MatrixBase<M, N> &m) {
+		arm_mat = {M, N, &data[0][0]};
+		memcpy(data, m.data, sizeof(data));
+	}
+
+	MatrixBase(const float *d) {
+		arm_mat = {M, N, &data[0][0]};
+		memcpy(data, d, sizeof(data));
+	}
+
+	MatrixBase(const float d[M][N]) {
+		arm_mat = {M, N, &data[0][0]};
+		memcpy(data, d, sizeof(data));
+	}
+
+	/**
+	 * set data
+	 */
+	void set(const float *d) {
+		memcpy(data, d, sizeof(data));
+	}
+
+	/**
+	 * set data
+	 */
+	void set(const float d[M][N]) {
+		memcpy(data, d, sizeof(data));
+	}
+
+	/**
+	 * access by index
+	 */
+	float &operator()(const unsigned int row, const unsigned int col) {
+		return data[row][col];
+	}
+
+	/**
+	 * access by index
+	 */
+	float operator()(const unsigned int row, const unsigned int col) const {
+		return data[row][col];
+	}
+
+	/**
+	 * get rows number
+	 */
+	unsigned int get_rows() const {
+		return M;
+	}
+
+	/**
+	 * get columns number
+	 */
+	unsigned int get_cols() const {
+		return N;
+	}
+
+	/**
+	 * test for equality
+	 */
+	bool operator ==(const Matrix<M, N> &m) const {
+		for (unsigned int i = 0; i < M; i++)
+			for (unsigned int j = 0; j < N; j++)
+				if (data[i][j] != m.data[i][j])
+					return false;
+
+		return true;
+	}
+
+	/**
+	 * test for inequality
+	 */
+	bool operator !=(const Matrix<M, N> &m) const {
+		for (unsigned int i = 0; i < M; i++)
+			for (unsigned int j = 0; j < N; j++)
+				if (data[i][j] != m.data[i][j])
+					return true;
+
+		return false;
+	}
+
+	/**
+	 * set to value
+	 */
+	const Matrix<M, N> &operator =(const Matrix<M, N> &m) {
+		memcpy(data, m.data, sizeof(data));
+		return *static_cast<Matrix<M, N>*>(this);
+	}
+
+	/**
+	 * negation
+	 */
+	Matrix<M, N> operator -(void) const {
+		Matrix<M, N> res;
+
+		for (unsigned int i = 0; i < N; i++)
+			for (unsigned int j = 0; j < M; j++)
+				res.data[i][j] = -data[i][j];
+
+		return res;
+	}
+
+	/**
+	 * addition
+	 */
+	Matrix<M, N> operator +(const Matrix<M, N> &m) const {
+		Matrix<M, N> res;
+
+		for (unsigned int i = 0; i < N; i++)
+			for (unsigned int j = 0; j < M; j++)
+				res.data[i][j] = data[i][j] + m.data[i][j];
+
+		return res;
+	}
+
+	Matrix<M, N> &operator +=(const Matrix<M, N> &m) {
+		for (unsigned int i = 0; i < N; i++)
+			for (unsigned int j = 0; j < M; j++)
+				data[i][j] += m.data[i][j];
+
+		return *static_cast<Matrix<M, N>*>(this);
+	}
+
+	/**
+	 * subtraction
+	 */
+	Matrix<M, N> operator -(const Matrix<M, N> &m) const {
+		Matrix<M, N> res;
+
+		for (unsigned int i = 0; i < M; i++)
+			for (unsigned int j = 0; j < N; j++)
+				res.data[i][j] = data[i][j] - m.data[i][j];
+
+		return res;
+	}
+
+	Matrix<M, N> &operator -=(const Matrix<M, N> &m) {
+		for (unsigned int i = 0; i < N; i++)
+			for (unsigned int j = 0; j < M; j++)
+				data[i][j] -= m.data[i][j];
+
+		return *static_cast<Matrix<M, N>*>(this);
+	}
+
+	/**
+	 * uniform scaling
+	 */
+	Matrix<M, N> operator *(const float num) const {
+		Matrix<M, N> res;
+
+		for (unsigned int i = 0; i < M; i++)
+			for (unsigned int j = 0; j < N; j++)
+				res.data[i][j] = data[i][j] * num;
+
+		return res;
+	}
+
+	Matrix<M, N> &operator *=(const float num) {
+		for (unsigned int i = 0; i < M; i++)
+			for (unsigned int j = 0; j < N; j++)
+				data[i][j] *= num;
+
+		return *static_cast<Matrix<M, N>*>(this);
+	}
+
+	Matrix<M, N> operator /(const float num) const {
+		Matrix<M, N> res;
+
+		for (unsigned int i = 0; i < M; i++)
+			for (unsigned int j = 0; j < N; j++)
+				res[i][j] = data[i][j] / num;
+
+		return res;
+	}
+
+	Matrix<M, N> &operator /=(const float num) {
+		for (unsigned int i = 0; i < M; i++)
+			for (unsigned int j = 0; j < N; j++)
+				data[i][j] /= num;
+
+		return *static_cast<Matrix<M, N>*>(this);
+	}
+
+	/**
+	 * multiplication by another matrix
+	 */
+	template <unsigned int P>
+	Matrix<M, P> operator *(const Matrix<N, P> &m) const {
+		Matrix<M, P> res;
+		arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat);
+		return res;
+	}
+
+	/**
+	 * transpose the matrix
+	 */
+	Matrix<N, M> transposed(void) const {
+		Matrix<N, M> res;
+		arm_mat_trans_f32(&this->arm_mat, &res.arm_mat);
+		return res;
+	}
+
+	/**
+	 * invert the matrix
+	 */
+	Matrix<M, N> inversed(void) const {
+		Matrix<M, N> res;
+		arm_mat_inverse_f32(&this->arm_mat, &res.arm_mat);
+		return res;
+	}
+
+	/**
+	 * set zero matrix
+	 */
+	void zero(void) {
+		memset(data, 0, sizeof(data));
+	}
+
+	/**
+	 * set identity matrix
+	 */
+	void identity(void) {
+		memset(data, 0, sizeof(data));
+		unsigned int n = (M < N) ? M : N;
+
+		for (unsigned int i = 0; i < n; i++)
+			data[i][i] = 1;
+	}
+
+	void print(void) {
+		for (unsigned int i = 0; i < M; i++) {
+			printf("[ ");
+
+			for (unsigned int j = 0; j < N; j++)
+				printf("%.3f\t", data[i][j]);
+
+			printf(" ]\n");
+		}
+	}
+};
+
+template <unsigned int M, unsigned int N>
+class __EXPORT Matrix : public MatrixBase<M, N>
+{
+public:
+	using MatrixBase<M, N>::operator *;
+
+	Matrix() : MatrixBase<M, N>() {}
+
+	Matrix(const Matrix<M, N> &m) : MatrixBase<M, N>(m) {}
+
+	Matrix(const float *d) : MatrixBase<M, N>(d) {}
+
+	Matrix(const float d[M][N]) : MatrixBase<M, N>(d) {}
+
+	/**
+	 * set to value
+	 */
+	const Matrix<M, N> &operator =(const Matrix<M, N> &m) {
+		memcpy(this->data, m.data, sizeof(this->data));
+		return *this;
+	}
+
+	/**
+	 * multiplication by a vector
+	 */
+	Vector<M> operator *(const Vector<N> &v) const {
+		Vector<M> res;
+		arm_mat_mult_f32(&this->arm_mat, &v.arm_col, &res.arm_col);
+		return res;
+	}
+};
+
+template <>
+class __EXPORT Matrix<3, 3> : public MatrixBase<3, 3>
+{
+public:
+	using MatrixBase<3, 3>::operator *;
+
+	Matrix() : MatrixBase<3, 3>() {}
+
+	Matrix(const Matrix<3, 3> &m) : MatrixBase<3, 3>(m) {}
+
+	Matrix(const float *d) : MatrixBase<3, 3>(d) {}
+
+	Matrix(const float d[3][3]) : MatrixBase<3, 3>(d) {}
+
+	/**
+	 * set to value
+	 */
+	const Matrix<3, 3> &operator =(const Matrix<3, 3> &m) {
+		memcpy(this->data, m.data, sizeof(this->data));
+		return *this;
+	}
+
+	/**
+	 * multiplication by a vector
+	 */
+	Vector<3> operator *(const Vector<3> &v) const {
+		Vector<3> res(data[0][0] * v.data[0] + data[0][1] * v.data[1] + data[0][2] * v.data[2],
+			      data[1][0] * v.data[0] + data[1][1] * v.data[1] + data[1][2] * v.data[2],
+			      data[2][0] * v.data[0] + data[2][1] * v.data[1] + data[2][2] * v.data[2]);
+		return res;
+	}
+
+	/**
+	 * create a rotation matrix from given euler angles
+	 * based on http://gentlenav.googlecode.com/files/EulerAngles.pdf
+	 */
+	void from_euler(float roll, float pitch, float yaw) {
+		float cp = cosf(pitch);
+		float sp = sinf(pitch);
+		float sr = sinf(roll);
+		float cr = cosf(roll);
+		float sy = sinf(yaw);
+		float cy = cosf(yaw);
+
+		data[0][0] = cp * cy;
+		data[0][1] = (sr * sp * cy) - (cr * sy);
+		data[0][2] = (cr * sp * cy) + (sr * sy);
+		data[1][0] = cp * sy;
+		data[1][1] = (sr * sp * sy) + (cr * cy);
+		data[1][2] = (cr * sp * sy) - (sr * cy);
+		data[2][0] = -sp;
+		data[2][1] = sr * cp;
+		data[2][2] = cr * cp;
+	}
+
+	/**
+	 * get euler angles from rotation matrix
+	 */
+	Vector<3> to_euler(void) const {
+		Vector<3> euler;
+		euler.data[1] = asinf(-data[2][0]);
+
+		if (fabsf(euler.data[1] - M_PI_2_F) < 1.0e-3f) {
+			euler.data[0] = 0.0f;
+			euler.data[2] = atan2f(data[1][2] - data[0][1], data[0][2] + data[1][1]) + euler.data[0];
+
+		} else if (fabsf(euler.data[1] + M_PI_2_F) < 1.0e-3f) {
+			euler.data[0] = 0.0f;
+			euler.data[2] = atan2f(data[1][2] - data[0][1], data[0][2] + data[1][1]) - euler.data[0];
+
+		} else {
+			euler.data[0] = atan2f(data[2][1], data[2][2]);
+			euler.data[2] = atan2f(data[1][0], data[0][0]);
+		}
+
+		return euler;
+	}
+};
+
+}
+
+#endif // MATRIX_HPP
diff --git a/src/lib/mathlib/math/Quaternion.cpp b/src/lib/mathlib/math/Quaternion.cpp
deleted file mode 100644
index 02fec4ca6..000000000
--- a/src/lib/mathlib/math/Quaternion.cpp
+++ /dev/null
@@ -1,174 +0,0 @@
-/****************************************************************************
- *
- *   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
diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp
index 048a55d33..21d05c7ef 100644
--- a/src/lib/mathlib/math/Quaternion.hpp
+++ b/src/lib/mathlib/math/Quaternion.hpp
@@ -1,6 +1,9 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Anton Babushkin <anton.babushkin@me.com>
+ *           Pavel Kirienko <pavel.kirienko@gmail.com>
+ *           Lorenz Meier <lm@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -34,82 +37,129 @@
 /**
  * @file Quaternion.hpp
  *
- * math quaternion lib
+ * Quaternion class
  */
 
-#pragma once
+#ifndef QUATERNION_HPP
+#define QUATERNION_HPP
 
+#include <math.h>
+#include "../CMSIS/Include/arm_math.h"
 #include "Vector.hpp"
 #include "Matrix.hpp"
 
 namespace math
 {
 
-class Dcm;
-class EulerAngles;
-
-class __EXPORT Quaternion : public Vector
+class __EXPORT Quaternion : public Vector<4>
 {
 public:
-
 	/**
-	 * default ctor
+	 * trivial ctor
 	 */
-	Quaternion();
+	Quaternion() : Vector<4>() {}
 
 	/**
-	 * ctor from floats
+	 * copy ctor
 	 */
-	Quaternion(float a, float b, float c, float d);
+	Quaternion(const Quaternion &q) : Vector<4>(q) {}
 
 	/**
-	 * ctor from data
+	 * casting from vector
 	 */
-	Quaternion(const float *data);
+	Quaternion(const Vector<4> &v) : Vector<4>(v) {}
 
 	/**
-	 * ctor from Vector
+	 * setting ctor
 	 */
-	Quaternion(const Vector &v);
+	Quaternion(const float d[4]) : Vector<4>(d) {}
 
 	/**
-	 * ctor from EulerAngles
+	 * setting ctor
 	 */
-	Quaternion(const EulerAngles &euler);
+	Quaternion(const float a0, const float b0, const float c0, const float d0): Vector<4>(a0, b0, c0, d0) {}
+
+	using Vector<4>::operator *;
 
 	/**
-	 * ctor from Dcm
+	 * multiplication
 	 */
-	Quaternion(const Dcm &dcm);
+	const Quaternion operator *(const Quaternion &q) const {
+		return Quaternion(
+			       data[0] * q.data[0] - data[1] * q.data[1] - data[2] * q.data[2] - data[3] * q.data[3],
+			       data[0] * q.data[1] + data[1] * q.data[0] + data[2] * q.data[3] - data[3] * q.data[2],
+			       data[0] * q.data[2] - data[1] * q.data[3] + data[2] * q.data[0] + data[3] * q.data[1],
+			       data[0] * q.data[3] + data[1] * q.data[2] - data[2] * q.data[1] + data[3] * q.data[0]);
+	}
 
 	/**
-	 * deep copy ctor
+	 * derivative
 	 */
-	Quaternion(const Quaternion &right);
+	const Quaternion derivative(const Vector<3> &w) {
+		float dataQ[] = {
+			data[0], -data[1], -data[2], -data[3],
+			data[1],  data[0], -data[3],  data[2],
+			data[2],  data[3],  data[0], -data[1],
+			data[3], -data[2],  data[1],  data[0]
+		};
+		Matrix<4, 4> Q(dataQ);
+		Vector<4> v(0.0f, w.data[0], w.data[1], w.data[2]);
+		return Q * v * 0.5f;
+	}
 
 	/**
-	 * dtor
+	 * imaginary part of quaternion
 	 */
-	virtual ~Quaternion();
+	Vector<3> imag(void) {
+		return Vector<3>(&data[1]);
+	}
 
 	/**
-	 * derivative
+	 * set quaternion to rotation defined by euler angles
 	 */
-	Vector derivative(const Vector &w);
+	void from_euler(float roll, float pitch, float yaw) {
+		double cosPhi_2 = cos(double(roll) / 2.0);
+		double sinPhi_2 = sin(double(roll) / 2.0);
+		double cosTheta_2 = cos(double(pitch) / 2.0);
+		double sinTheta_2 = sin(double(pitch) / 2.0);
+		double cosPsi_2 = cos(double(yaw) / 2.0);
+		double sinPsi_2 = sin(double(yaw) / 2.0);
+		data[0] = cosPhi_2 * cosTheta_2 * cosPsi_2 + sinPhi_2 * sinTheta_2 * sinPsi_2;
+		data[1] = sinPhi_2 * cosTheta_2 * cosPsi_2 - cosPhi_2 * sinTheta_2 * sinPsi_2;
+		data[2] = cosPhi_2 * sinTheta_2 * cosPsi_2 + sinPhi_2 * cosTheta_2 * sinPsi_2;
+		data[3] = cosPhi_2 * cosTheta_2 * sinPsi_2 - sinPhi_2 * sinTheta_2 * cosPsi_2;
+	}
+
+	void from_dcm(const Matrix<3, 3> &m) {
+		// avoiding singularities by not using division equations
+		data[0] = 0.5f * sqrtf(1.0f + m.data[0][0] + m.data[1][1] + m.data[2][2]);
+		data[1] = 0.5f * sqrtf(1.0f + m.data[0][0] - m.data[1][1] - m.data[2][2]);
+		data[2] = 0.5f * sqrtf(1.0f - m.data[0][0] + m.data[1][1] - m.data[2][2]);
+		data[3] = 0.5f * sqrtf(1.0f - m.data[0][0] - m.data[1][1] + m.data[2][2]);
+	}
 
 	/**
-	 * accessors
+	 * create rotation matrix for the quaternion
 	 */
-	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); }
+	Matrix<3, 3> to_dcm(void) const {
+		Matrix<3, 3> R;
+		float aSq = data[0] * data[0];
+		float bSq = data[1] * data[1];
+		float cSq = data[2] * data[2];
+		float dSq = data[3] * data[3];
+		R.data[0][0] = aSq + bSq - cSq - dSq;
+		R.data[0][1] = 2.0f * (data[1] * data[2] - data[0] * data[3]);
+		R.data[0][2] = 2.0f * (data[0] * data[2] + data[1] * data[3]);
+		R.data[1][0] = 2.0f * (data[1] * data[2] + data[0] * data[3]);
+		R.data[1][1] = aSq - bSq + cSq - dSq;
+		R.data[1][2] = 2.0f * (data[2] * data[3] - data[0] * data[1]);
+		R.data[2][0] = 2.0f * (data[1] * data[3] - data[0] * data[2]);
+		R.data[2][1] = 2.0f * (data[0] * data[1] + data[2] * data[3]);
+		R.data[2][2] = aSq - bSq - cSq + dSq;
+		return R;
+	}
 };
 
-int __EXPORT quaternionTest();
-} // math
+}
 
+#endif // QUATERNION_HPP
diff --git a/src/lib/mathlib/math/Vector.cpp b/src/lib/mathlib/math/Vector.cpp
deleted file mode 100644
index 35158a396..000000000
--- a/src/lib/mathlib/math/Vector.cpp
+++ /dev/null
@@ -1,100 +0,0 @@
-/****************************************************************************
- *
- *   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
diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp
index 73de793d5..b7840170c 100644
--- a/src/lib/mathlib/math/Vector.hpp
+++ b/src/lib/mathlib/math/Vector.hpp
@@ -1,6 +1,9 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Anton Babushkin <anton.babushkin@me.com>
+ *           Pavel Kirienko <pavel.kirienko@gmail.com>
+ *           Lorenz Meier <lm@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -32,26 +35,442 @@
  ****************************************************************************/
 
 /**
- * @file Vector.h
+ * @file Vector.hpp
  *
- * math vector
+ * Vector class
  */
 
-#pragma once
+#ifndef VECTOR_HPP
+#define VECTOR_HPP
 
-#include <nuttx/config.h>
-
-#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU)
-#include "arm/Vector.hpp"
-#else
-#include "generic/Vector.hpp"
-#endif
+#include <stdio.h>
+#include <math.h>
+#include "../CMSIS/Include/arm_math.h"
 
 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
+
+template <unsigned int N>
+class __EXPORT Vector;
+
+template <unsigned int N>
+class __EXPORT VectorBase
+{
+public:
+	/**
+	 * vector data
+	 */
+	float data[N];
+
+	/**
+	 * struct for using arm_math functions, represents column vector
+	 */
+	arm_matrix_instance_f32 arm_col;
+
+	/**
+	 * trivial ctor
+	 * note that this ctor will not initialize elements
+	 */
+	VectorBase() {
+		arm_col = {N, 1, &data[0]};
+	}
+
+	/**
+	 * copy ctor
+	 */
+	VectorBase(const VectorBase<N> &v) {
+		arm_col = {N, 1, &data[0]};
+		memcpy(data, v.data, sizeof(data));
+	}
+
+	/**
+	 * setting ctor
+	 */
+	VectorBase(const float d[N]) {
+		arm_col = {N, 1, &data[0]};
+		memcpy(data, d, sizeof(data));
+	}
+
+	/**
+	 * set data
+	 */
+	void set(const float d[N]) {
+		memcpy(data, d, sizeof(data));
+	}
+
+	/**
+	 * access to elements by index
+	 */
+	float &operator()(const unsigned int i) {
+		return data[i];
+	}
+
+	/**
+	 * access to elements by index
+	 */
+	float operator()(const unsigned int i) const {
+		return data[i];
+	}
+
+	/**
+	 * get vector size
+	 */
+	unsigned int get_size() const {
+		return N;
+	}
+
+	/**
+	 * test for equality
+	 */
+	bool operator ==(const Vector<N> &v) const {
+		for (unsigned int i = 0; i < N; i++)
+			if (data[i] != v.data[i])
+				return false;
+
+		return true;
+	}
+
+	/**
+	 * test for inequality
+	 */
+	bool operator !=(const Vector<N> &v) const {
+		for (unsigned int i = 0; i < N; i++)
+			if (data[i] != v.data[i])
+				return true;
+
+		return false;
+	}
+
+	/**
+	 * set to value
+	 */
+	const Vector<N> &operator =(const Vector<N> &v) {
+		memcpy(data, v.data, sizeof(data));
+		return *static_cast<const Vector<N>*>(this);
+	}
+
+	/**
+	 * negation
+	 */
+	const Vector<N> operator -(void) const {
+		Vector<N> res;
+
+		for (unsigned int i = 0; i < N; i++)
+			res.data[i] = -data[i];
+
+		return res;
+	}
+
+	/**
+	 * addition
+	 */
+	const Vector<N> operator +(const Vector<N> &v) const {
+		Vector<N> res;
+
+		for (unsigned int i = 0; i < N; i++)
+			res.data[i] = data[i] + v.data[i];
+
+		return res;
+	}
+
+	/**
+	 * subtraction
+	 */
+	const Vector<N> operator -(const Vector<N> &v) const {
+		Vector<N> res;
+
+		for (unsigned int i = 0; i < N; i++)
+			res.data[i] = data[i] - v.data[i];
+
+		return res;
+	}
+
+	/**
+	 * uniform scaling
+	 */
+	const Vector<N> operator *(const float num) const {
+		Vector<N> res;
+
+		for (unsigned int i = 0; i < N; i++)
+			res.data[i] = data[i] * num;
+
+		return res;
+	}
+
+	/**
+	 * uniform scaling
+	 */
+	const Vector<N> operator /(const float num) const {
+		Vector<N> res;
+
+		for (unsigned int i = 0; i < N; i++)
+			res.data[i] = data[i] / num;
+
+		return res;
+	}
+
+	/**
+	 * addition
+	 */
+	const Vector<N> &operator +=(const Vector<N> &v) {
+		for (unsigned int i = 0; i < N; i++)
+			data[i] += v.data[i];
+
+		return *static_cast<const Vector<N>*>(this);
+	}
+
+	/**
+	 * subtraction
+	 */
+	const Vector<N> &operator -=(const Vector<N> &v) {
+		for (unsigned int i = 0; i < N; i++)
+			data[i] -= v.data[i];
+
+		return *static_cast<const Vector<N>*>(this);
+	}
+
+	/**
+	 * uniform scaling
+	 */
+	const Vector<N> &operator *=(const float num) {
+		for (unsigned int i = 0; i < N; i++)
+			data[i] *= num;
+
+		return *static_cast<const Vector<N>*>(this);
+	}
+
+	/**
+	 * uniform scaling
+	 */
+	const Vector<N> &operator /=(const float num) {
+		for (unsigned int i = 0; i < N; i++)
+			data[i] /= num;
+
+		return *static_cast<const Vector<N>*>(this);
+	}
+
+	/**
+	 * dot product
+	 */
+	float operator *(const Vector<N> &v) const {
+		float res = 0.0f;
+
+		for (unsigned int i = 0; i < N; i++)
+			res += data[i] * v.data[i];
+
+		return res;
+	}
+
+	/**
+	 * gets the length of this vector squared
+	 */
+	float length_squared() const {
+		float res = 0.0f;
+
+		for (unsigned int i = 0; i < N; i++)
+			res += data[i] * data[i];
+
+		return res;
+	}
+
+	/**
+	 * gets the length of this vector
+	 */
+	float length() const {
+		float res = 0.0f;
+
+		for (unsigned int i = 0; i < N; i++)
+			res += data[i] * data[i];
+
+		return sqrtf(res);
+	}
+
+	/**
+	 * normalizes this vector
+	 */
+	void normalize() {
+		*this /= length();
+	}
+
+	/**
+	 * returns the normalized version of this vector
+	 */
+	Vector<N> normalized() const {
+		return *this / length();
+	}
+
+	/**
+	 * set zero vector
+	 */
+	void zero(void) {
+		memset(data, 0, sizeof(data));
+	}
+
+	void print(void) {
+		printf("[ ");
+
+		for (unsigned int i = 0; i < N; i++)
+			printf("%.3f\t", data[i]);
+
+		printf("]\n");
+	}
+};
+
+template <unsigned int N>
+class __EXPORT Vector : public VectorBase<N>
+{
+public:
+	Vector() : VectorBase<N>() {}
+
+	Vector(const Vector<N> &v) : VectorBase<N>(v) {}
+
+	Vector(const float d[N]) : VectorBase<N>(d) {}
+
+	/**
+	 * set to value
+	 */
+	const Vector<N> &operator =(const Vector<N> &v) {
+		memcpy(this->data, v.data, sizeof(this->data));
+		return *this;
+	}
+};
+
+template <>
+class __EXPORT Vector<2> : public VectorBase<2>
+{
+public:
+	Vector() : VectorBase<2>() {}
+
+	// simple copy is 1.6 times faster than memcpy
+	Vector(const Vector<2> &v) : VectorBase<2>() {
+		data[0] = v.data[0];
+		data[1] = v.data[1];
+	}
+
+	Vector(const float d[2]) : VectorBase<2>() {
+		data[0] = d[0];
+		data[1] = d[1];
+	}
+
+	Vector(const float x, const float y) : VectorBase<2>() {
+		data[0] = x;
+		data[1] = y;
+	}
+
+	/**
+	 * set data
+	 */
+	void set(const float d[2]) {
+		data[0] = d[0];
+		data[1] = d[1];
+	}
+
+	/**
+	 * set to value
+	 */
+	const Vector<2> &operator =(const Vector<2> &v) {
+		data[0] = v.data[0];
+		data[1] = v.data[1];
+		return *this;
+	}
+
+	float operator %(const Vector<2> &v) const {
+		return data[0] * v.data[1] - data[1] * v.data[0];
+	}
+};
+
+template <>
+class __EXPORT Vector<3> : public VectorBase<3>
+{
+public:
+	Vector() : VectorBase<3>() {}
+
+	// simple copy is 1.6 times faster than memcpy
+	Vector(const Vector<3> &v) : VectorBase<3>() {
+		for (unsigned int i = 0; i < 3; i++)
+			data[i] = v.data[i];
+	}
+
+	Vector(const float d[3]) : VectorBase<3>() {
+		for (unsigned int i = 0; i < 3; i++)
+			data[i] = d[i];
+	}
+
+	Vector(const float x, const float y, const float z) : VectorBase<3>() {
+		data[0] = x;
+		data[1] = y;
+		data[2] = z;
+	}
+
+	/**
+	 * set data
+	 */
+	void set(const float d[3]) {
+		for (unsigned int i = 0; i < 3; i++)
+			data[i] = d[i];
+	}
+
+	/**
+	 * set to value
+	 */
+	const Vector<3> &operator =(const Vector<3> &v) {
+		for (unsigned int i = 0; i < 3; i++)
+			data[i] = v.data[i];
+
+		return *this;
+	}
+
+	Vector<3> operator %(const Vector<3> &v) const {
+		return Vector<3>(
+			       data[1] * v.data[2] - data[2] * v.data[1],
+			       data[2] * v.data[0] - data[0] * v.data[2],
+			       data[0] * v.data[1] - data[1] * v.data[0]
+		       );
+	}
+};
+
+template <>
+class __EXPORT Vector<4> : public VectorBase<4>
+{
+public:
+	Vector() : VectorBase() {}
+
+	Vector(const Vector<4> &v) : VectorBase<4>() {
+		for (unsigned int i = 0; i < 4; i++)
+			data[i] = v.data[i];
+	}
+
+	Vector(const float d[4]) : VectorBase<4>() {
+		for (unsigned int i = 0; i < 4; i++)
+			data[i] = d[i];
+	}
+
+	Vector(const float x0, const float x1, const float x2, const float x3) : VectorBase() {
+		data[0] = x0;
+		data[1] = x1;
+		data[2] = x2;
+		data[3] = x3;
+	}
+
+	/**
+	 * set data
+	 */
+	void set(const float d[4]) {
+		for (unsigned int i = 0; i < 4; i++)
+			data[i] = d[i];
+	}
+
+	/**
+	 * set to value
+	 */
+	const Vector<4> &operator =(const Vector<4> &v) {
+		for (unsigned int i = 0; i < 4; i++)
+			data[i] = v.data[i];
+
+		return *this;
+	}
+};
+
+}
+
+#endif // VECTOR_HPP
diff --git a/src/lib/mathlib/math/Vector2f.cpp b/src/lib/mathlib/math/Vector2f.cpp
deleted file mode 100644
index 68e741817..000000000
--- a/src/lib/mathlib/math/Vector2f.cpp
+++ /dev/null
@@ -1,103 +0,0 @@
-/****************************************************************************
- *
- *   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
diff --git a/src/lib/mathlib/math/Vector2f.hpp b/src/lib/mathlib/math/Vector2f.hpp
deleted file mode 100644
index ecd62e81c..000000000
--- a/src/lib/mathlib/math/Vector2f.hpp
+++ /dev/null
@@ -1,79 +0,0 @@
-/****************************************************************************
- *
- *   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
-
diff --git a/src/lib/mathlib/math/Vector3.cpp b/src/lib/mathlib/math/Vector3.cpp
deleted file mode 100644
index 3936650c6..000000000
--- a/src/lib/mathlib/math/Vector3.cpp
+++ /dev/null
@@ -1,109 +0,0 @@
-/****************************************************************************
- *
- *   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
diff --git a/src/lib/mathlib/math/Vector3.hpp b/src/lib/mathlib/math/Vector3.hpp
deleted file mode 100644
index 1656e184e..000000000
--- a/src/lib/mathlib/math/Vector3.hpp
+++ /dev/null
@@ -1,79 +0,0 @@
-/****************************************************************************
- *
- *   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;
-	using Vector::operator *;
-
-	/**
-	 * 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
-
diff --git a/src/lib/mathlib/math/arm/Matrix.cpp b/src/lib/mathlib/math/arm/Matrix.cpp
deleted file mode 100644
index 21661622a..000000000
--- a/src/lib/mathlib/math/arm/Matrix.cpp
+++ /dev/null
@@ -1,40 +0,0 @@
-/****************************************************************************
- *
- *   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"
diff --git a/src/lib/mathlib/math/arm/Matrix.hpp b/src/lib/mathlib/math/arm/Matrix.hpp
deleted file mode 100644
index 1945bb02d..000000000
--- a/src/lib/mathlib/math/arm/Matrix.hpp
+++ /dev/null
@@ -1,292 +0,0 @@
-/****************************************************************************
- *
- *   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
diff --git a/src/lib/mathlib/math/arm/Vector.cpp b/src/lib/mathlib/math/arm/Vector.cpp
deleted file mode 100644
index 7ea6496bb..000000000
--- a/src/lib/mathlib/math/arm/Vector.cpp
+++ /dev/null
@@ -1,40 +0,0 @@
-/****************************************************************************
- *
- *   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"
diff --git a/src/lib/mathlib/math/arm/Vector.hpp b/src/lib/mathlib/math/arm/Vector.hpp
deleted file mode 100644
index a2526d0a2..000000000
--- a/src/lib/mathlib/math/arm/Vector.hpp
+++ /dev/null
@@ -1,236 +0,0 @@
-/****************************************************************************
- *
- *   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
diff --git a/src/lib/mathlib/math/generic/Matrix.cpp b/src/lib/mathlib/math/generic/Matrix.cpp
deleted file mode 100644
index 21661622a..000000000
--- a/src/lib/mathlib/math/generic/Matrix.cpp
+++ /dev/null
@@ -1,40 +0,0 @@
-/****************************************************************************
- *
- *   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"
diff --git a/src/lib/mathlib/math/generic/Matrix.hpp b/src/lib/mathlib/math/generic/Matrix.hpp
deleted file mode 100644
index 5601a3447..000000000
--- a/src/lib/mathlib/math/generic/Matrix.hpp
+++ /dev/null
@@ -1,437 +0,0 @@
-/****************************************************************************
- *
- *   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
diff --git a/src/lib/mathlib/math/generic/Vector.cpp b/src/lib/mathlib/math/generic/Vector.cpp
deleted file mode 100644
index 7ea6496bb..000000000
--- a/src/lib/mathlib/math/generic/Vector.cpp
+++ /dev/null
@@ -1,40 +0,0 @@
-/****************************************************************************
- *
- *   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"
diff --git a/src/lib/mathlib/math/generic/Vector.hpp b/src/lib/mathlib/math/generic/Vector.hpp
deleted file mode 100644
index 8cfdc676d..000000000
--- a/src/lib/mathlib/math/generic/Vector.hpp
+++ /dev/null
@@ -1,245 +0,0 @@
-/****************************************************************************
- *
- *   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
diff --git a/src/lib/mathlib/mathlib.h b/src/lib/mathlib/mathlib.h
index 40ffb22bc..9e03855c5 100644
--- a/src/lib/mathlib/mathlib.h
+++ b/src/lib/mathlib/mathlib.h
@@ -41,13 +41,9 @@
 
 #pragma once
 
-#include "math/Dcm.hpp"
-#include "math/EulerAngles.hpp"
+#include "math/Vector.hpp"
 #include "math/Matrix.hpp"
 #include "math/Quaternion.hpp"
-#include "math/Vector.hpp"
-#include "math/Vector3.hpp"
-#include "math/Vector2f.hpp"
 #include "math/Limits.hpp"
 
 #endif
@@ -56,4 +52,4 @@
 
 #include "CMSIS/Include/arm_math.h"
 
-#endif
\ No newline at end of file
+#endif
diff --git a/src/lib/mathlib/module.mk b/src/lib/mathlib/module.mk
index 72bc7db8a..191e2da73 100644
--- a/src/lib/mathlib/module.mk
+++ b/src/lib/mathlib/module.mk
@@ -35,13 +35,6 @@
 # Math library
 #
 SRCS		 = math/test/test.cpp \
-		   math/Vector.cpp \
-		   math/Vector2f.cpp \
-		   math/Vector3.cpp \
-		   math/EulerAngles.cpp \
-		   math/Quaternion.cpp \
-		   math/Dcm.cpp \
-		   math/Matrix.cpp \
 		   math/Limits.cpp
 
 #
@@ -49,13 +42,3 @@ SRCS		 = math/test/test.cpp \
 # current makefile name, since app.mk needs it.
 #
 APP_MAKEFILE	:= $(lastword $(MAKEFILE_LIST))
-
-ifeq ($(CONFIG_ARCH_CORTEXM4)$(CONFIG_ARCH_FPU),yy)
-INCLUDE_DIRS	+= math/arm
-SRCS		+= math/arm/Vector.cpp \
-		   math/arm/Matrix.cpp
-else
-#INCLUDE_DIRS	+= math/generic
-#SRCS		+= math/generic/Vector.cpp \
-#		   math/generic/Matrix.cpp
-endif
diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
index ecca04dd7..aca3fe7b6 100644
--- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
+++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
@@ -53,21 +53,6 @@ static const int8_t ret_error = -1; 	// error occurred
 
 KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	SuperBlock(parent, name),
-	// ekf matrices
-	F(9, 9),
-	G(9, 6),
-	P(9, 9),
-	P0(9, 9),
-	V(6, 6),
-	// attitude measurement ekf matrices
-	HAtt(4, 9),
-	RAtt(4, 4),
-	// position measurement ekf matrices
-	HPos(6, 9),
-	RPos(6, 6),
-	// attitude representations
-	C_nb(),
-	q(),
 	// subscriptions
 	_sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz
 	_gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz
@@ -112,8 +97,17 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 {
 	using namespace math;
 
+	F.zero();
+	G.zero();
+	V.zero();
+	HAtt.zero();
+	RAtt.zero();
+	HPos.zero();
+	RPos.zero();
+
 	// initial state covariance matrix
-	P0 = Matrix::identity(9) * 0.01f;
+	P0.identity();
+	P0 *= 0.01f;
 	P = P0;
 
 	// initial state
@@ -138,7 +132,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 		_sensors.magnetometer_ga[2]);
 
 	// initialize dcm
-	C_nb = Dcm(q);
+	C_nb = q.to_dcm();
 
 	// HPos is constant
 	HPos(0, 3) = 1.0f;
@@ -228,8 +222,8 @@ void KalmanNav::update()
 		if (correctAtt() == ret_ok) _attitudeInitCounter++;
 
 		if (_attitudeInitCounter > 100) {
-			warnx("initialized EKF attitude\n");
-			warnx("phi: %8.4f, theta: %8.4f, psi: %8.4f\n",
+			warnx("initialized EKF attitude");
+			warnx("phi: %8.4f, theta: %8.4f, psi: %8.4f",
 			       double(phi), double(theta), double(psi));
 			_attitudeInitialized = true;
 		}
@@ -259,8 +253,8 @@ void KalmanNav::update()
 		// lat/lon and not have init
 		map_projection_init(lat0, lon0);
 		_positionInitialized = true;
-		warnx("initialized EKF state with GPS\n");
-		warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n",
+		warnx("initialized EKF state with GPS");
+		warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f",
 		       double(vN), double(vE), double(vD),
 		       lat, lon, double(alt));
 	}
@@ -404,28 +398,28 @@ int KalmanNav::predictState(float dt)
 
 	// attitude prediction
 	if (_attitudeInitialized) {
-		Vector3 w(_sensors.gyro_rad_s);
+		Vector<3> w(_sensors.gyro_rad_s);
 
 		// attitude
 		q = q + q.derivative(w) * dt;
 
 		// renormalize quaternion if needed
-		if (fabsf(q.norm() - 1.0f) > 1e-4f) {
-			q = q.unit();
+		if (fabsf(q.length() - 1.0f) > 1e-4f) {
+			q.normalize();
 		}
 
 		// C_nb update
-		C_nb = Dcm(q);
+		C_nb = q.to_dcm();
 
 		// euler update
-		EulerAngles euler(C_nb);
-		phi = euler.getPhi();
-		theta = euler.getTheta();
-		psi = euler.getPsi();
+		Vector<3> euler = C_nb.to_euler();
+		phi = euler.data[0];
+		theta = euler.data[1];
+		psi = euler.data[2];
 
 		// specific acceleration in nav frame
-		Vector3 accelB(_sensors.accelerometer_m_s2);
-		Vector3 accelN = C_nb * accelB;
+		Vector<3> accelB(_sensors.accelerometer_m_s2);
+		Vector<3> accelN = C_nb * accelB;
 		fN = accelN(0);
 		fE = accelN(1);
 		fD = accelN(2);
@@ -549,10 +543,10 @@ int KalmanNav::predictStateCovariance(float dt)
 	G(5, 4) = C_nb(2, 1);
 	G(5, 5) = C_nb(2, 2);
 
-	// continuous predictioon equations
-	// for discrte time EKF
+	// continuous prediction equations
+	// for discrete time EKF
 	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
-	P = P + (F * P + P * F.transpose() + G * V * G.transpose()) * dt;
+	P = P + (F * P + P * F.transposed() + G * V * G.transposed()) * dt;
 
 	return ret_ok;
 }
@@ -577,13 +571,14 @@ int KalmanNav::correctAtt()
 
 	// compensate roll and pitch, but not yaw
 	// XXX take the vectors out of the C_nb matrix to avoid singularities
-	math::Dcm C_rp(math::EulerAngles(phi, theta, 0.0f));//C_nb.transpose();
+	math::Matrix<3,3> C_rp;
+	C_rp.from_euler(phi, theta, 0.0f);//C_nb.transposed();
 
 	// mag measurement
-	Vector3 magBody(_sensors.magnetometer_ga);
+	Vector<3> magBody(_sensors.magnetometer_ga);
 
 	// transform to earth frame
-	Vector3 magNav = C_rp * magBody;
+	Vector<3> magNav = C_rp * magBody;
 
 	// calculate error between estimate and measurement
 	// apply declination correction for true heading as well.
@@ -592,12 +587,12 @@ int KalmanNav::correctAtt()
 	if (yMag < -M_PI_F) yMag += 2*M_PI_F;
 
 	// accel measurement
-	Vector3 zAccel(_sensors.accelerometer_m_s2);
-	float accelMag = zAccel.norm();
-	zAccel = zAccel.unit();
+	Vector<3> zAccel(_sensors.accelerometer_m_s2);
+	float accelMag = zAccel.length();
+	zAccel.normalize();
 
 	// ignore accel correction when accel mag not close to g
-	Matrix RAttAdjust = RAtt;
+	Matrix<4,4> RAttAdjust = RAtt;
 
 	bool ignoreAccel = fabsf(accelMag - _g.get()) > 1.1f;
 
@@ -611,14 +606,10 @@ int KalmanNav::correctAtt()
 	}
 
 	// accel predicted measurement
-	Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -_g.get())).unit();
+	Vector<3> zAccelHat = (C_nb.transposed() * Vector<3>(0, 0, -_g.get())).normalized();
 
 	// calculate residual
-	Vector y(4);
-	y(0) = yMag;
-	y(1) = zAccel(0) - zAccelHat(0);
-	y(2) = zAccel(1) - zAccelHat(1);
-	y(3) = zAccel(2) - zAccelHat(2);
+	Vector<4> y(yMag, zAccel(0) - zAccelHat(0), zAccel(1) - zAccelHat(1), zAccel(2) - zAccelHat(2));
 
 	// HMag
 	HAtt(0, 2) = 1;
@@ -632,17 +623,17 @@ int KalmanNav::correctAtt()
 
 	// compute correction
 	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
-	Matrix S = HAtt * P * HAtt.transpose() + RAttAdjust; // residual covariance
-	Matrix K = P * HAtt.transpose() * S.inverse();
-	Vector xCorrect = K * y;
+	Matrix<4, 4> S = HAtt * P * HAtt.transposed() + RAttAdjust; // residual covariance
+	Matrix<9, 4> K = P * HAtt.transposed() * S.inversed();
+	Vector<9> xCorrect = K * y;
 
 	// check correciton is sane
-	for (size_t i = 0; i < xCorrect.getRows(); i++) {
+	for (size_t i = 0; i < xCorrect.get_size(); i++) {
 		float val = xCorrect(i);
 
 		if (isnan(val) || isinf(val)) {
 			// abort correction and return
-			warnx("numerical failure in att correction\n");
+			warnx("numerical failure in att correction");
 			// reset P matrix to P0
 			P = P0;
 			return ret_error;
@@ -669,7 +660,7 @@ int KalmanNav::correctAtt()
 	P = P - K * HAtt * P;
 
 	// fault detection
-	float beta = y.dot(S.inverse() * y);
+	float beta = y * (S.inversed() * y);
 
 	if (beta > _faultAtt.get()) {
 		warnx("fault in attitude: beta = %8.4f", (double)beta);
@@ -678,7 +669,7 @@ int KalmanNav::correctAtt()
 
 	// update quaternions from euler
 	// angle correction
-	q = Quaternion(EulerAngles(phi, theta, psi));
+	q.from_euler(phi, theta, psi);
 
 	return ret_ok;
 }
@@ -688,7 +679,7 @@ int KalmanNav::correctPos()
 	using namespace math;
 
 	// residual
-	Vector y(6);
+	Vector<6> y;
 	y(0) = _gps.vel_n_m_s - vN;
 	y(1) = _gps.vel_e_m_s - vE;
 	y(2) = double(_gps.lat) - double(lat) * 1.0e7 * M_RAD_TO_DEG;
@@ -698,17 +689,17 @@ int KalmanNav::correctPos()
 
 	// compute correction
 	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
-	Matrix S = HPos * P * HPos.transpose() + RPos; // residual covariance
-	Matrix K = P * HPos.transpose() * S.inverse();
-	Vector xCorrect = K * y;
+	Matrix<6,6> S = HPos * P * HPos.transposed() + RPos; // residual covariance
+	Matrix<9,6> K = P * HPos.transposed() * S.inversed();
+	Vector<9> xCorrect = K * y;
 
 	// check correction is sane
-	for (size_t i = 0; i < xCorrect.getRows(); i++) {
+	for (size_t i = 0; i < xCorrect.get_size(); i++) {
 		float val = xCorrect(i);
 
 		if (!isfinite(val)) {
 			// abort correction and return
-			warnx("numerical failure in gps correction\n");
+			warnx("numerical failure in gps correction");
 			// fallback to GPS
 			vN = _gps.vel_n_m_s;
 			vE = _gps.vel_e_m_s;
@@ -735,7 +726,7 @@ int KalmanNav::correctPos()
 	P = P - K * HPos * P;
 
 	// fault detetcion
-	float beta = y.dot(S.inverse() * y);
+	float beta = y * (S.inversed() * y);
 
 	static int counter = 0;
 	if (beta > _faultPos.get() && (counter % 10 == 0)) {
diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
index a69bde1a6..46ee4b6c8 100644
--- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
+++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
@@ -125,17 +125,17 @@ public:
 	virtual void updateParams();
 protected:
 	// kalman filter
-	math::Matrix F;             /**< Jacobian(f,x), where dx/dt = f(x,u) */
-	math::Matrix G;             /**< noise shaping matrix for gyro/accel */
-	math::Matrix P;             /**< state covariance matrix */
-	math::Matrix P0;            /**< initial state covariance matrix */
-	math::Matrix V;             /**< gyro/ accel noise matrix */
-	math::Matrix HAtt;          /**< attitude measurement matrix */
-	math::Matrix RAtt;          /**< attitude measurement noise matrix */
-	math::Matrix HPos;          /**< position measurement jacobian matrix */
-	math::Matrix RPos;          /**< position measurement noise matrix */
+	math::Matrix<9,9> F;             /**< Jacobian(f,x), where dx/dt = f(x,u) */
+	math::Matrix<9,6> G;             /**< noise shaping matrix for gyro/accel */
+	math::Matrix<9,9> P;             /**< state covariance matrix */
+	math::Matrix<9,9> P0;            /**< initial state covariance matrix */
+	math::Matrix<6,6> V;             /**< gyro/ accel noise matrix */
+	math::Matrix<4,9> HAtt;          /**< attitude measurement matrix */
+	math::Matrix<4,4> RAtt;          /**< attitude measurement noise matrix */
+	math::Matrix<6,9> HPos;          /**< position measurement jacobian matrix */
+	math::Matrix<6,6> RPos;          /**< position measurement noise matrix */
 	// attitude
-	math::Dcm C_nb;             /**< direction cosine matrix from body to nav frame */
+	math::Matrix<3,3> C_nb;             /**< direction cosine matrix from body to nav frame */
 	math::Quaternion q;         /**< quaternion from body to nav frame */
 	// subscriptions
 	control::UOrbSubscription<sensor_combined_s> _sensors;          /**< sensors sub. */
diff --git a/src/modules/att_pos_estimator_ekf/kalman_main.cpp b/src/modules/att_pos_estimator_ekf/kalman_main.cpp
index 372b2d162..3d20d4d2d 100644
--- a/src/modules/att_pos_estimator_ekf/kalman_main.cpp
+++ b/src/modules/att_pos_estimator_ekf/kalman_main.cpp
@@ -107,7 +107,7 @@ int att_pos_estimator_ekf_main(int argc, char *argv[])
 		daemon_task = task_spawn_cmd("att_pos_estimator_ekf",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_MAX - 30,
-					 4096,
+					 8192,
 					 kalman_demo_thread_main,
 					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
 		exit(0);
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
index 983ac7c8d..48a11e7fc 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -267,6 +267,10 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 	float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f };
 	unsigned offset_count = 0;
 
+	/* rotation matrix for magnetic declination */
+	math::Matrix<3, 3> R_decl;
+	R_decl.identity();
+
 	/* register the perf counter */
 	perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf");
 
@@ -301,6 +305,9 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 
 				/* update parameters */
 				parameters_update(&ekf_param_handles, &ekf_params);
+
+				/* update mag declination rotation matrix */
+				R_decl.from_euler(0.0f, 0.0f, ekf_params.mag_decl);
 			}
 
 			/* only run filter if sensor values changed */
@@ -450,17 +457,12 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets));
 
 					/* magnetic declination */
-					math::EulerAngles eulerMagDecl(0.0f, 0.0f, ekf_params.mag_decl);
-					math::Dcm R(eulerMagDecl);
-					math::Dcm R_body(&Rot_matrix[0]);
-					R = R * R_body;
+
+					math::Matrix<3, 3> R_body = (&Rot_matrix[0]);
+					math::Matrix<3, 3> R = R_decl * R_body;
 
 					/* copy rotation matrix */
-					for (int i = 0; i < 3; i++) {
-						for (int j = 0; j < 3; j++) {
-							att.R[i][j] = R(i, j);
-						}
-					}
+					memcpy(&att.R[0][0], &R.data[0][0], sizeof(att.R));
 					att.R_valid = true;
 
 					if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) {
diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp
index 5eeca5a1a..36b75dd58 100644
--- a/src/modules/commander/accelerometer_calibration.cpp
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -194,15 +194,13 @@ int do_accel_calibration(int mavlink_fd)
 		int32_t board_rotation_int;
 		param_get(board_rotation_h, &(board_rotation_int));
 		enum Rotation board_rotation_id = (enum Rotation)board_rotation_int;
-		math::Matrix board_rotation(3, 3);
+		math::Matrix<3,3> board_rotation;
 		get_rot_matrix(board_rotation_id, &board_rotation);
-		math::Matrix board_rotation_t = board_rotation.transpose();
-		math::Vector3 accel_offs_vec;
-		accel_offs_vec.set(&accel_offs[0]);
-		math::Vector3 accel_offs_rotated = board_rotation_t * accel_offs_vec;
-		math::Matrix accel_T_mat(3, 3);
-		accel_T_mat.set(&accel_T[0][0]);
-		math::Matrix accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation;
+		math::Matrix<3,3> board_rotation_t = board_rotation.transposed();
+		math::Vector<3> accel_offs_vec(&accel_offs[0]);
+		math::Vector<3> accel_offs_rotated = board_rotation_t * accel_offs_vec;
+		math::Matrix<3,3> accel_T_mat(&accel_T[0][0]);
+		math::Matrix<3,3> accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation;
 
 		accel_scale.x_offset = accel_offs_rotated(0);
 		accel_scale.x_scale = accel_T_rotated(0, 0);
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index a9648b207..5e32ac32f 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -170,7 +170,7 @@ private:
 	uint64_t _airspeed_last_valid;			///< last time airspeed was valid. Used to detect sensor failures
 	float _groundspeed_undershoot;			///< ground speed error to min. speed in m/s
 	bool _global_pos_valid;				///< global position is valid
-	math::Dcm _R_nb;				///< current attitude
+	math::Matrix<3, 3> _R_nb;				///< current attitude
 
 	ECL_L1_Pos_Controller				_l1_control;
 	TECS						_tecs;
@@ -282,7 +282,7 @@ private:
 	/**
 	 * Control position.
 	 */
-	bool		control_position(const math::Vector2f &global_pos, const math::Vector2f &ground_speed,
+	bool		control_position(const math::Vector<2> &global_pos, const math::Vector<2> &ground_speed,
 					 const struct vehicle_global_position_set_triplet_s &global_triplet);
 
 	float calculate_target_airspeed(float airspeed_demand);
@@ -600,10 +600,10 @@ FixedwingPositionControl::calculate_gndspeed_undershoot()
 
 	if (_global_pos_valid) {
 		/* get ground speed vector */
-		math::Vector2f ground_speed_vector(_global_pos.vx, _global_pos.vy);
+		math::Vector<2> ground_speed_vector(_global_pos.vx, _global_pos.vy);
 
 		/* rotate with current attitude */
-		math::Vector2f yaw_vector(_R_nb(0, 0), _R_nb(1, 0));
+		math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0));
 		yaw_vector.normalize();
 		float ground_speed_body = yaw_vector * ground_speed_vector;
 
@@ -624,7 +624,7 @@ FixedwingPositionControl::calculate_gndspeed_undershoot()
 }
 
 bool
-FixedwingPositionControl::control_position(const math::Vector2f &current_position, const math::Vector2f &ground_speed,
+FixedwingPositionControl::control_position(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed,
 		const struct vehicle_global_position_set_triplet_s &global_triplet)
 {
 	bool setpoint = true;
@@ -637,8 +637,8 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 	float baro_altitude = _global_pos.alt;
 
 	/* filter speed and altitude for controller */
-	math::Vector3 accel_body(_accel.x, _accel.y, _accel.z);
-	math::Vector3 accel_earth = _R_nb.transpose() * accel_body;
+	math::Vector<3> accel_body(_accel.x, _accel.y, _accel.z);
+	math::Vector<3> accel_earth = _R_nb.transposed() * accel_body;
 
 	_tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth);
 	float altitude_error = _global_triplet.current.altitude - _global_pos.alt;
@@ -662,29 +662,29 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 		if (_setpoint_valid) {
 
 			/* current waypoint (the one currently heading for) */
-			math::Vector2f next_wp(global_triplet.current.lat / 1e7f, global_triplet.current.lon / 1e7f);
+			math::Vector<2> next_wp(global_triplet.current.lat / 1e7f, global_triplet.current.lon / 1e7f);
 
 			/* previous waypoint */
-			math::Vector2f prev_wp;
+			math::Vector<2> prev_wp;
 
 			if (global_triplet.previous_valid) {
-				prev_wp.setX(global_triplet.previous.lat / 1e7f);
-				prev_wp.setY(global_triplet.previous.lon / 1e7f);
+				prev_wp(0) = global_triplet.previous.lat / 1e7f;
+				prev_wp(1) = global_triplet.previous.lon / 1e7f;
 
 			} else {
 				/*
 				 * No valid previous waypoint, go for the current wp.
 				 * This is automatically handled by the L1 library.
 				 */
-				prev_wp.setX(global_triplet.current.lat / 1e7f);
-				prev_wp.setY(global_triplet.current.lon / 1e7f);
+				prev_wp(0) = global_triplet.current.lat / 1e7f;
+				prev_wp(1) = global_triplet.current.lon / 1e7f;
 
 			}
 
 			// XXX add RTL switch
 			if (global_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH && _launch_valid) {
 
-				math::Vector2f rtl_pos(_launch_lat, _launch_lon);
+				math::Vector<2> rtl_pos(_launch_lat, _launch_lon);
 
 				_l1_control.navigate_waypoints(rtl_pos, rtl_pos, current_position, ground_speed);
 				_att_sp.roll_body = _l1_control.nav_roll();
@@ -730,7 +730,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 
 				/* switch to heading hold for the last meters, continue heading hold after */
 
-				float wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), current_position.getX(), current_position.getY());
+				float wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), current_position(0), current_position(1));
 				//warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO");
 				if (wp_distance < 15.0f || land_noreturn) {
 
@@ -869,7 +869,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 
 			altitude_error = _loiter_hold_alt - _global_pos.alt;
 
-			math::Vector2f loiter_hold_pos(_loiter_hold_lat, _loiter_hold_lon);
+			math::Vector<2> loiter_hold_pos(_loiter_hold_lat, _loiter_hold_lon);
 
 			/* loiter around current position */
 			_l1_control.navigate_loiter(loiter_hold_pos, current_position, _parameters.loiter_hold_radius,
@@ -1101,8 +1101,8 @@ FixedwingPositionControl::task_main()
 			vehicle_airspeed_poll();
 			// vehicle_baro_poll();
 
-			math::Vector2f ground_speed(_global_pos.vx, _global_pos.vy);
-			math::Vector2f current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f);
+			math::Vector<2> ground_speed(_global_pos.vx, _global_pos.vy);
+			math::Vector<2> current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f);
 
 			/*
 			 * Attempt to control position, on success (= sensors present and not in manual mode),
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 7b6fad658..b4f7f2dfe 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -683,8 +683,8 @@ handle_message(mavlink_message_t *msg)
 
 			/* Calculate Rotation Matrix */
 			math::Quaternion q(hil_state.attitude_quaternion);
-			math::Dcm C_nb(q);
-			math::EulerAngles euler(C_nb);
+			math::Matrix<3,3> C_nb = q.to_dcm();
+			math::Vector<3> euler = C_nb.to_euler();
 
 			/* set rotation matrix */
 			for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
@@ -699,9 +699,9 @@ handle_message(mavlink_message_t *msg)
 			hil_attitude.q[3] = q(3);
 			hil_attitude.q_valid = true;
 
-			hil_attitude.roll = euler.getPhi();
-			hil_attitude.pitch = euler.getTheta();
-			hil_attitude.yaw = euler.getPsi();
+			hil_attitude.roll = euler(0);
+			hil_attitude.pitch = euler(1);
+			hil_attitude.yaw = euler(2);
 			hil_attitude.rollspeed = hil_state.rollspeed;
 			hil_attitude.pitchspeed = hil_state.pitchspeed;
 			hil_attitude.yawspeed = hil_state.yawspeed;
diff --git a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp
index 76f053372..30c073c29 100644
--- a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp
+++ b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp
@@ -133,13 +133,11 @@ private:
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
-	bool	_att_sp_valid;				/**< flag if the attitude setpoint is valid */
+	math::Matrix<3, 3> _K;					/**< diagonal gain matrix for position error */
+	math::Matrix<3, 3> _K_rate_p;				/**< diagonal gain matrix for angular rate error */
+	math::Matrix<3, 3> _K_rate_d;				/**< diagonal gain matrix for angular rate derivative */
 
-	math::Matrix _K;					/**< diagonal gain matrix for position error */
-	math::Matrix _K_rate_p;				/**< diagonal gain matrix for angular rate error */
-	math::Matrix _K_rate_d;				/**< diagonal gain matrix for angular rate derivative */
-
-	math::Vector3 _rates_prev;			/**< angular rates on previous step */
+	math::Vector<3> _rates_prev;			/**< angular rates on previous step */
 
 	struct {
 		param_t att_p;
@@ -222,17 +220,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_actuators_0_pub(-1),
 
 /* performance counters */
-	_loop_perf(perf_alloc(PC_ELAPSED, "fw att control")),
-
-/* states */
-	_att_sp_valid(false),
-
-/* gain matrices */
-	_K(3, 3),
-	_K_rate_p(3, 3),
-	_K_rate_d(3, 3),
-
-	_rates_prev(0.0f, 0.0f, 0.0f)
+	_loop_perf(perf_alloc(PC_ELAPSED, "fw att control"))
 
 {
 	memset(&_att, 0, sizeof(_att));
@@ -241,6 +229,12 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	memset(&_control_mode, 0, sizeof(_control_mode));
 	memset(&_arming, 0, sizeof(_arming));
 
+	_K.zero();
+	_K_rate_p.zero();
+	_K_rate_d.zero();
+
+	_rates_prev.zero();
+
 	_parameter_handles.att_p		= 	param_find("MC_ATT_P");
 	_parameter_handles.att_rate_p	= 	param_find("MC_ATTRATE_P");
 	_parameter_handles.att_rate_d	= 	param_find("MC_ATTRATE_D");
@@ -293,17 +287,14 @@ MulticopterAttitudeControl::parameters_update()
 	param_get(_parameter_handles.yaw_rate_p, &yaw_rate_p);
 	param_get(_parameter_handles.yaw_rate_d, &yaw_rate_d);
 
-	_K.setAll(0.0f);
 	_K(0, 0) = att_p;
 	_K(1, 1) = att_p;
 	_K(2, 2) = yaw_p;
 
-	_K_rate_p.setAll(0.0f);
 	_K_rate_p(0, 0) = att_rate_p;
 	_K_rate_p(1, 1) = att_rate_p;
 	_K_rate_p(2, 2) = yaw_rate_p;
 
-	_K_rate_d.setAll(0.0f);
 	_K_rate_d(0, 0) = att_rate_d;
 	_K_rate_d(1, 1) = att_rate_d;
 	_K_rate_d(2, 2) = yaw_rate_d;
@@ -348,7 +339,6 @@ MulticopterAttitudeControl::vehicle_setpoint_poll()
 
 	if (att_sp_updated) {
 		orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp);
-		_att_sp_valid = true;
 	}
 }
 
@@ -401,6 +391,24 @@ MulticopterAttitudeControl::task_main()
 	vehicle_manual_poll();
 	arming_status_poll();
 
+	/* setpoint rotation matrix */
+	math::Matrix<3, 3> R_sp;
+	R_sp.identity();
+
+	/* rotation matrix for current state */
+	math::Matrix<3, 3> R;
+	R.identity();
+
+	/* current angular rates */
+	math::Vector<3> rates;
+	rates.zero();
+
+	/* identity matrix */
+	math::Matrix<3, 3> I;
+	I.identity();
+
+	math::Quaternion q;
+
 	bool reset_yaw_sp = true;
 
 	/* wakeup source(s) */
@@ -486,6 +494,7 @@ MulticopterAttitudeControl::task_main()
 							/* move yaw setpoint */
 							yaw_sp_move_rate = _manual.yaw;
 							_att_sp.yaw_body = _wrap_pi(_att_sp.yaw_body + yaw_sp_move_rate * dt);
+							_att_sp.R_valid = false;
 							publish_att_sp = true;
 						}
 					}
@@ -494,6 +503,7 @@ MulticopterAttitudeControl::task_main()
 					if (reset_yaw_sp) {
 						reset_yaw_sp = false;
 						_att_sp.yaw_body = _att.yaw;
+						_att_sp.R_valid = false;
 						publish_att_sp = true;
 					}
 
@@ -501,11 +511,10 @@ MulticopterAttitudeControl::task_main()
 						/* update attitude setpoint if not in position control mode */
 						_att_sp.roll_body = _manual.roll;
 						_att_sp.pitch_body = _manual.pitch;
+						_att_sp.R_valid = false;
 						publish_att_sp = true;
 					}
 
-					_att_sp_valid = true;
-
 				} else {
 					/* manual rate inputs (ACRO) */
 					// TODO
@@ -528,17 +537,15 @@ MulticopterAttitudeControl::task_main()
 				reset_yaw_sp = true;
 			}
 
-			if (publish_att_sp || (_att_sp_valid && !_att_sp.R_valid)) {
-				/* controller uses rotation matrix, not euler angles, convert if necessary */
-				math::EulerAngles euler_sp(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body);
-				math::Dcm R_sp(euler_sp);
-
-				for (int i = 0; i < 3; i++) {
-					for (int j = 0; j < 3; j++) {
-						_att_sp.R_body[i][j] = R_sp(i, j);
-					}
-				}
+			if (_att_sp.R_valid) {
+				/* rotation matrix in _att_sp is valid, use it */
+				R_sp.set(&_att_sp.R_body[0][0]);
+			} else {
+				/* rotation matrix in _att_sp is not valid, use euler angles instead */
+				R_sp.from_euler(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body);
 
+				/* copy rotation matrix back to setpoint struct */
+				memcpy(&_att_sp.R_body[0][0], &R_sp.data[0][0], sizeof(_att_sp.R_body));
 				_att_sp.R_valid = true;
 			}
 
@@ -554,41 +561,41 @@ MulticopterAttitudeControl::task_main()
 				}
 			}
 
-			/* desired rotation matrix */
-			math::Dcm R_des(_att_sp.R_body);
-
 			/* rotation matrix for current state */
-			math::Dcm R(_att.R);
+			R.set(_att.R);
+
 			/* current body angular rates */
-			math::Vector3 rates(_att.rollspeed, _att.pitchspeed, _att.yawspeed);
+			rates(0) = _att.rollspeed;
+			rates(1) = _att.pitchspeed;
+			rates(2) = _att.yawspeed;
 
 			/* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */
-			math::Vector3 R_z(R(0, 2), R(1, 2), R(2, 2));
-			math::Vector3 R_des_z(R_des(0, 2), R_des(1, 2), R_des(2, 2));
+			math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2));
+			math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
 
 			/* axis and sin(angle) of desired rotation */
-			math::Vector3 e_R = R.transpose() * R_z.cross(R_des_z);
+			math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z);
 
 			/* calculate angle error */
-			float e_R_z_sin = e_R.norm();
-			float e_R_z_cos = R_z * R_des_z;
+			float e_R_z_sin = e_R.length();
+			float e_R_z_cos = R_z * R_sp_z;
 
 			/* calculate weight for yaw control */
-			float yaw_w = R_des(2, 2) * R_des(2, 2);
+			float yaw_w = R_sp(2, 2) * R_sp(2, 2);
 
 			/* calculate rotation matrix after roll/pitch only rotation */
-			math::Matrix R_rp(3, 3);
+			math::Matrix<3, 3> R_rp;
 
 			if (e_R_z_sin > 0.0f) {
 				/* get axis-angle representation */
 				float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos);
-				math::Vector3 e_R_z_axis = e_R / e_R_z_sin;
+				math::Vector<3> e_R_z_axis = e_R / e_R_z_sin;
 
 				e_R = e_R_z_axis * e_R_z_angle;
 
 				/* cross product matrix for e_R_axis */
-				math::Matrix e_R_cp(3, 3);
-				e_R_cp.setAll(0.0f);
+				math::Matrix<3, 3> e_R_cp;
+				e_R_cp.zero();
 				e_R_cp(0, 1) = -e_R_z_axis(2);
 				e_R_cp(0, 2) = e_R_z_axis(1);
 				e_R_cp(1, 0) = e_R_z_axis(2);
@@ -597,11 +604,6 @@ MulticopterAttitudeControl::task_main()
 				e_R_cp(2, 1) = e_R_z_axis(0);
 
 				/* rotation matrix for roll/pitch only rotation */
-				math::Matrix I(3, 3);
-				I.setAll(0.0f);
-				I(0, 0) = 1.0f;
-				I(1, 1) = 1.0f;
-				I(2, 2) = 1.0f;
 				R_rp = R * (I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));
 
 			} else {
@@ -609,17 +611,17 @@ MulticopterAttitudeControl::task_main()
 				R_rp = R;
 			}
 
-			/* R_rp and R_des has the same Z axis, calculate yaw error */
-			math::Vector3 R_des_x(R_des(0, 0), R_des(1, 0), R_des(2, 0));
-			math::Vector3 R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
-			e_R(2) = atan2f(R_rp_x.cross(R_des_x) * R_des_z, R_rp_x * R_des_x) * yaw_w;
+			/* R_rp and R_sp has the same Z axis, calculate yaw error */
+			math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));
+			math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
+			e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w;
 
 			if (e_R_z_cos < 0.0) {
 				/* for large thrust vector rotations use another rotation method:
-				 * calculate angle and axis for R -> R_des rotation directly */
-				math::Quaternion q(R.transpose() * R_des);
-				float angle_d = 2.0f * atan2f(sqrtf(q.getB() * q.getB() + q.getC() * q.getC() + q.getD() * q.getD()), q.getA());
-				math::Vector3 e_R_d(q.getB(), q.getC(), q.getD());
+				 * calculate angle and axis for R -> R_sp rotation directly */
+				q.from_dcm(R.transposed() * R_sp);
+				math::Vector<3> e_R_d = q.imag();
+				float angle_d = 2.0f * atan2f(e_R_d.length(), q(0));
 
 				/* use fusion of Z axis based rotation and direct rotation */
 				float direct_w = e_R_z_cos * e_R_z_cos * yaw_w;
@@ -627,11 +629,11 @@ MulticopterAttitudeControl::task_main()
 			}
 
 			/* angular rates setpoint*/
-			math::Vector3 rates_sp = _K * e_R;
+			math::Vector<3> rates_sp = _K * e_R;
 
 			/* feed forward yaw setpoint rate */
 			rates_sp(2) += yaw_sp_move_rate * yaw_w;
-			math::Vector3 control = _K_rate_p * (rates_sp - rates) + _K_rate_d * (_rates_prev - rates) / fmaxf(dt, 0.003f);
+			math::Vector<3> control = _K_rate_p * (rates_sp - rates) + _K_rate_d * (_rates_prev - rates) / fmaxf(dt, 0.003f);
 			_rates_prev = rates;
 
 			/* publish the attitude rates setpoint */
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index f6c44444a..04307d96b 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -422,11 +422,11 @@ Navigator::task_main()
 
 			mission_poll();
 
-			math::Vector2f ground_speed(_global_pos.vx, _global_pos.vy);
+			math::Vector<2> ground_speed(_global_pos.vx, _global_pos.vy);
 			// Current waypoint
-			math::Vector2f next_wp(_global_triplet.current.lat / 1e7f, _global_triplet.current.lon / 1e7f);
+			math::Vector<2> next_wp(_global_triplet.current.lat / 1e7f, _global_triplet.current.lon / 1e7f);
 			// Global position
-			math::Vector2f current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f);
+			math::Vector<2> current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f);
 
 			/* AUTONOMOUS FLIGHT */
 
@@ -436,19 +436,19 @@ Navigator::task_main()
 				if (_mission_valid) {
 
 					// Next waypoint
-					math::Vector2f prev_wp;
+					math::Vector<2> prev_wp;
 
 					if (_global_triplet.previous_valid) {
-						prev_wp.setX(_global_triplet.previous.lat / 1e7f);
-						prev_wp.setY(_global_triplet.previous.lon / 1e7f);
+						prev_wp(0) = _global_triplet.previous.lat / 1e7f;
+						prev_wp(1) = _global_triplet.previous.lon / 1e7f;
 
 					} else {
 						/*
 						 * No valid next waypoint, go for heading hold.
 						 * This is automatically handled by the L1 library.
 						 */
-						prev_wp.setX(_global_triplet.current.lat / 1e7f);
-						prev_wp.setY(_global_triplet.current.lon / 1e7f);
+						prev_wp(0) = _global_triplet.current.lat / 1e7f;
+						prev_wp(1) = _global_triplet.current.lon / 1e7f;
 
 					}
 
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index f99312f8c..9e2eeafa4 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -211,8 +211,8 @@ private:
 	struct differential_pressure_s _diff_pres;
 	struct airspeed_s _airspeed;
 
-	math::Matrix	_board_rotation;		/**< rotation matrix for the orientation that the board is mounted */
-	math::Matrix	_external_mag_rotation;		/**< rotation matrix for the orientation that an external mag is mounted */
+	math::Matrix<3,3>	_board_rotation;		/**< rotation matrix for the orientation that the board is mounted */
+	math::Matrix<3,3>	_external_mag_rotation;		/**< rotation matrix for the orientation that an external mag is mounted */
 	bool		_mag_is_external;		/**< true if the active mag is on an external board */
 
 	struct {
@@ -465,8 +465,6 @@ Sensors::Sensors() :
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")),
 
-	_board_rotation(3, 3),
-	_external_mag_rotation(3, 3),
 	_mag_is_external(false)
 {
 
@@ -920,7 +918,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 
 		orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
 
-		math::Vector3 vect = {accel_report.x, accel_report.y, accel_report.z};
+		math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z);
 		vect = _board_rotation * vect;
 
 		raw.accelerometer_m_s2[0] = vect(0);
@@ -946,7 +944,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
 
 		orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);
 
-		math::Vector3 vect = {gyro_report.x, gyro_report.y, gyro_report.z};
+		math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
 		vect = _board_rotation * vect;
 
 		raw.gyro_rad_s[0] = vect(0);
@@ -972,7 +970,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
 
 		orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report);
 
-		math::Vector3 vect = {mag_report.x, mag_report.y, mag_report.z};
+		math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z);
 
 		if (_mag_is_external)
 			vect = _external_mag_rotation * vect;
diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk
index 68a080c77..4fbd296d2 100644
--- a/src/systemcmds/tests/module.mk
+++ b/src/systemcmds/tests/module.mk
@@ -24,6 +24,7 @@ SRCS			 = test_adc.c \
 			   test_uart_loopback.c \
 			   test_uart_send.c \
 			   test_mixer.cpp \
+			   test_mathlib.cpp \
 			   test_file.c \
 			   tests_main.c \
 			   test_param.c \
diff --git a/src/systemcmds/tests/test_mathlib.cpp b/src/systemcmds/tests/test_mathlib.cpp
new file mode 100644
index 000000000..693a208ba
--- /dev/null
+++ b/src/systemcmds/tests/test_mathlib.cpp
@@ -0,0 +1,159 @@
+/****************************************************************************
+ *
+ *   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 test_mathlib.cpp
+ *
+ * Mathlib test
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <string.h>
+#include <time.h>
+#include <mathlib/mathlib.h>
+#include <systemlib/err.h>
+#include <drivers/drv_hrt.h>
+
+#include "tests.h"
+
+#define TEST_OP(_title, _op) { unsigned int n = 60000; hrt_abstime t0, t1; t0 = hrt_absolute_time(); for (unsigned int j = 0; j < n; j++) { _op; }; t1 = hrt_absolute_time(); warnx(_title ": %.6fus", (double)(t1 - t0) / n); }
+
+using namespace math;
+
+const char* formatResult(bool res) {
+	return res ? "OK" : "ERROR";
+}
+
+int test_mathlib(int argc, char *argv[])
+{
+	warnx("testing mathlib");
+
+	{
+		Vector<2> v;
+		Vector<2> v1(1.0f, 2.0f);
+		Vector<2> v2(1.0f, -1.0f);
+		float data[2] = {1.0f, 2.0f};
+		TEST_OP("Constructor Vector<2>()", Vector<2> v3);
+		TEST_OP("Constructor Vector<2>(Vector<2>)", Vector<2> v3(v1));
+		TEST_OP("Constructor Vector<2>(float[])", Vector<2> v3(data));
+		TEST_OP("Constructor Vector<2>(float, float)", Vector<2> v3(1.0f, 2.0f));
+		TEST_OP("Vector<2> = Vector<2>", v = v1);
+		TEST_OP("Vector<2> + Vector<2>", v + v1);
+		TEST_OP("Vector<2> - Vector<2>", v - v1);
+		TEST_OP("Vector<2> += Vector<2>", v += v1);
+		TEST_OP("Vector<2> -= Vector<2>", v -= v1);
+		TEST_OP("Vector<2> * Vector<2>", v * v1);
+		TEST_OP("Vector<2> %% Vector<2>", v1 % v2);
+	}
+
+	{
+		Vector<3> v;
+		Vector<3> v1(1.0f, 2.0f, 0.0f);
+		Vector<3> v2(1.0f, -1.0f, 2.0f);
+		float data[3] = {1.0f, 2.0f, 3.0f};
+		TEST_OP("Constructor Vector<3>()", Vector<3> v3);
+		TEST_OP("Constructor Vector<3>(Vector<3>)", Vector<3> v3(v1));
+		TEST_OP("Constructor Vector<3>(float[])", Vector<3> v3(data));
+		TEST_OP("Constructor Vector<3>(float, float, float)", Vector<3> v3(1.0f, 2.0f, 3.0f));
+		TEST_OP("Vector<3> = Vector<3>", v = v1);
+		TEST_OP("Vector<3> + Vector<3>", v + v1);
+		TEST_OP("Vector<3> - Vector<3>", v - v1);
+		TEST_OP("Vector<3> += Vector<3>", v += v1);
+		TEST_OP("Vector<3> -= Vector<3>", v -= v1);
+		TEST_OP("Vector<3> * float", v1 * 2.0f);
+		TEST_OP("Vector<3> / float", v1 / 2.0f);
+		TEST_OP("Vector<3> *= float", v1 *= 2.0f);
+		TEST_OP("Vector<3> /= float", v1 /= 2.0f);
+		TEST_OP("Vector<3> * Vector<3>", v * v1);
+		TEST_OP("Vector<3> %% Vector<3>", v1 % v2);
+		TEST_OP("Vector<3> length", v1.length());
+		TEST_OP("Vector<3> length squared", v1.length_squared());
+		TEST_OP("Vector<3> element read", volatile float a = v1(0));
+		TEST_OP("Vector<3> element read direct", volatile float a = v1.data[0]);
+		TEST_OP("Vector<3> element write", v1(0) = 1.0f);
+		TEST_OP("Vector<3> element write direct", v1.data[0] = 1.0f);
+	}
+
+	{
+		Vector<4> v;
+		Vector<4> v1(1.0f, 2.0f, 0.0f, -1.0f);
+		Vector<4> v2(1.0f, -1.0f, 2.0f, 0.0f);
+		float data[4] = {1.0f, 2.0f, 3.0f, 4.0f};
+		TEST_OP("Constructor Vector<4>()", Vector<4> v3);
+		TEST_OP("Constructor Vector<4>(Vector<4>)", Vector<4> v3(v1));
+		TEST_OP("Constructor Vector<4>(float[])", Vector<4> v3(data));
+		TEST_OP("Constructor Vector<4>(float, float, float, float)", Vector<4> v3(1.0f, 2.0f, 3.0f, 4.0f));
+		TEST_OP("Vector<4> = Vector<4>", v = v1);
+		TEST_OP("Vector<4> + Vector<4>", v + v1);
+		TEST_OP("Vector<4> - Vector<4>", v - v1);
+		TEST_OP("Vector<4> += Vector<4>", v += v1);
+		TEST_OP("Vector<4> -= Vector<4>", v -= v1);
+		TEST_OP("Vector<4> * Vector<4>", v * v1);
+	}
+
+	{
+		Vector<10> v1;
+		v1.zero();
+		float data[10];
+		TEST_OP("Constructor Vector<10>()", Vector<10> v3);
+		TEST_OP("Constructor Vector<10>(Vector<10>)", Vector<10> v3(v1));
+		TEST_OP("Constructor Vector<10>(float[])", Vector<10> v3(data));
+	}
+
+	{
+		Matrix<3, 3> m1;
+		m1.identity();
+		Matrix<3, 3> m2;
+		m2.identity();
+		Vector<3> v1(1.0f, 2.0f, 0.0f);
+		TEST_OP("Matrix<3, 3> * Vector<3>", m1 * v1);
+		TEST_OP("Matrix<3, 3> + Matrix<3, 3>", m1 + m2);
+		TEST_OP("Matrix<3, 3> * Matrix<3, 3>", m1 * m2);
+	}
+
+	{
+		Matrix<10, 10> m1;
+		m1.identity();
+		Matrix<10, 10> m2;
+		m2.identity();
+		Vector<10> v1;
+		v1.zero();
+		TEST_OP("Matrix<10, 10> * Vector<10>", m1 * v1);
+		TEST_OP("Matrix<10, 10> + Matrix<10, 10>", m1 + m2);
+		TEST_OP("Matrix<10, 10> * Matrix<10, 10>", m1 * m2);
+	}
+
+	return 0;
+}
diff --git a/src/systemcmds/tests/tests.h b/src/systemcmds/tests/tests.h
index a57d04be3..6420d3798 100644
--- a/src/systemcmds/tests/tests.h
+++ b/src/systemcmds/tests/tests.h
@@ -109,6 +109,7 @@ extern int	test_bson(int argc, char *argv[]);
 extern int	test_file(int argc, char *argv[]);
 extern int	test_mixer(int argc, char *argv[]);
 extern int	test_rc(int argc, char *argv[]);
+extern int	test_mathlib(int argc, char *argv[]);
 
 __END_DECLS
 
diff --git a/src/systemcmds/tests/tests_main.c b/src/systemcmds/tests/tests_main.c
index 1088a4407..5fab8f82e 100644
--- a/src/systemcmds/tests/tests_main.c
+++ b/src/systemcmds/tests/tests_main.c
@@ -106,6 +106,7 @@ const struct {
 	{"file",		test_file,	0},
 	{"mixer",		test_mixer,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"rc",			test_rc,	OPT_NOJIGTEST | OPT_NOALLTEST},
+	{"mathlib",		test_mathlib,	0},
 	{"help",		test_help,	OPT_NOALLTEST | OPT_NOHELP | OPT_NOJIGTEST},
 	{NULL,			NULL, 		0}
 };