Merged beta into paul_estimator

40 files changed, 1812 insertions, 2802 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/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
index 2eb58abd6..9584924cc 100644
--- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
@@ -45,39 +45,29 @@
 #include <geo/geo.h>
 #include <ecl/ecl.h>
 #include <mathlib/mathlib.h>
+#include <systemlib/err.h>
 
 ECL_PitchController::ECL_PitchController() :
 	_last_run(0),
+	_tc(0.1f),
+	_k_p(0.0f),
+	_k_i(0.0f),
+	_k_ff(0.0f),
+	_integrator_max(0.0f),
+	_max_rate_pos(0.0f),
+	_max_rate_neg(0.0f),
+	_roll_ff(0.0f),
 	_last_output(0.0f),
 	_integrator(0.0f),
 	_rate_error(0.0f),
 	_rate_setpoint(0.0f),
-	_max_deflection_rad(math::radians(45.0f))
+	_bodyrate_setpoint(0.0f)
 {
 }
 
-float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler,
-				   bool lock_integrator, float airspeed_min, float airspeed_max, float airspeed)
+float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed)
 {
-	/* get the usual dt estimate */
-	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
-	_last_run = ecl_absolute_time();
-	float dt = (float)dt_micros * 1e-6f;
-
-	/* lock integral for long intervals */
-	if (dt_micros > 500000)
-		lock_integrator = true;
 
-	float k_roll_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
-	float k_i_rate = _k_i * _tc;
-
-	/* input conditioning */
-	if (!isfinite(airspeed)) {
-		/* airspeed is NaN, +- INF or not available, pick center of band */
-		airspeed = 0.5f * (airspeed_min + airspeed_max);
-	} else if (airspeed < airspeed_min) {
-		airspeed = airspeed_min;
-	}
 
 	/* flying inverted (wings upside down) ? */
 	bool inverted = false;
@@ -100,34 +90,78 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc
 	}
 
 	/* calculate the offset in the rate resulting from rolling  */
+	//xxx needs explanation and conversion to body angular rates or should be removed
 	float turn_offset = fabsf((CONSTANTS_ONE_G / airspeed) *
 				tanf(roll) * sinf(roll)) * _roll_ff;
 	if (inverted)
 		turn_offset = -turn_offset;
 
+	/* Calculate the error */
 	float pitch_error = pitch_setpoint - pitch;
-	/* rate setpoint from current error and time constant */
-	_rate_setpoint = pitch_error / _tc;
+
+	/*  Apply P controller: rate setpoint from current error and time constant */
+	_rate_setpoint =  pitch_error / _tc;
 
 	/* add turn offset */
 	_rate_setpoint += turn_offset;
 
-	_rate_error = _rate_setpoint - pitch_rate;
+	/* limit the rate */ //XXX: move to body angluar rates
+	if (_max_rate_pos > 0.01f && _max_rate_neg > 0.01f) {
+		if (_rate_setpoint > 0.0f) {
+			_rate_setpoint = (_rate_setpoint > _max_rate_pos) ? _max_rate_pos : _rate_setpoint;
+		} else {
+			_rate_setpoint = (_rate_setpoint < -_max_rate_neg) ? -_max_rate_neg : _rate_setpoint;
+		}
 
-	float ilimit_scaled = _integrator_max * scaler;
+	}
 
-	if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) {
+	return _rate_setpoint;
+}
 
-		float id = _rate_error * k_i_rate * dt * scaler;
+float ECL_PitchController::control_bodyrate(float roll, float pitch,
+		float pitch_rate, float yaw_rate,
+		float yaw_rate_setpoint,
+		float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator)
+{
+	/* get the usual dt estimate */
+	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
+	_last_run = ecl_absolute_time();
+	float dt = (float)dt_micros * 1e-6f;
+
+	/* lock integral for long intervals */
+	if (dt_micros > 500000)
+		lock_integrator = true;
+
+//	float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
+	float k_ff = 0;
+
+	/* input conditioning */
+	if (!isfinite(airspeed)) {
+		/* airspeed is NaN, +- INF or not available, pick center of band */
+		airspeed = 0.5f * (airspeed_min + airspeed_max);
+	} else if (airspeed < airspeed_min) {
+		airspeed = airspeed_min;
+	}
+
+	/* Transform setpoint to body angular rates */
+	_bodyrate_setpoint = cosf(roll) * _rate_setpoint + cosf(pitch) * sinf(roll) * yaw_rate_setpoint; //jacobian
+
+	/* Transform estimation to body angular rates */
+	float pitch_bodyrate = cosf(roll) * pitch_rate + cosf(pitch) * sinf(roll) * yaw_rate; //jacobian
+
+	_rate_error = _bodyrate_setpoint - pitch_bodyrate;
+
+	if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
+
+		float id = _rate_error * dt;
 
 		/*
-		 * anti-windup: do not allow integrator to increase into the
-		 * wrong direction if actuator is at limit
+		 * anti-windup: do not allow integrator to increase if actuator is at limit
 		 */
-		if (_last_output < -_max_deflection_rad) {
+		if (_last_output < -1.0f) {
 			/* only allow motion to center: increase value */
 			id = math::max(id, 0.0f);
-		} else if (_last_output > _max_deflection_rad) {
+		} else if (_last_output > 1.0f) {
 			/* only allow motion to center: decrease value */
 			id = math::min(id, 0.0f);
 		}
@@ -136,11 +170,14 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc
 	}
 
 	/* integrator limit */
-	_integrator = math::constrain(_integrator, -ilimit_scaled, ilimit_scaled);
-	/* store non-limited output */
-	_last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_roll_ff)) * scaler;
-
-	return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad);
+	//xxx: until start detection is available: integral part in control signal is limited here
+	float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
+
+	/* Apply PI rate controller and store non-limited output */
+	_last_output = (_bodyrate_setpoint * _k_ff +_rate_error * _k_p + integrator_constrained) * scaler * scaler;  //scaler is proportional to 1/airspeed
+//	warnx("pitch: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
+//	warnx("roll: _last_output %.4f", (double)_last_output);
+	return math::constrain(_last_output, -1.0f, 1.0f);
 }
 
 void ECL_PitchController::reset_integrator()
diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h
index 1e6cec6a1..30a82a86a 100644
--- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h
+++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h
@@ -36,6 +36,7 @@
  * Definition of a simple orthogonal pitch PID controller.
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  *
  * Acknowledgements:
  *
@@ -51,13 +52,18 @@
 #include <stdbool.h>
 #include <stdint.h>
 
-class __EXPORT ECL_PitchController
+class __EXPORT ECL_PitchController //XXX: create controller superclass
 {
 public:
 	ECL_PitchController();
 
-	float control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler = 1.0f,
-		      bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f));
+	float control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed);
+
+
+	float control_bodyrate(float roll, float pitch,
+			float pitch_rate, float yaw_rate,
+			float yaw_rate_setpoint,
+			float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f), float scaler = 1.0f, bool lock_integrator = false);
 
 	void reset_integrator();
 
@@ -67,21 +73,27 @@ public:
 	void set_k_p(float k_p) {
 		_k_p = k_p;
 	}
+
 	void set_k_i(float k_i) {
 		_k_i = k_i;
 	}
-	void set_k_d(float k_d) {
-		_k_d = k_d;
+
+	void set_k_ff(float k_ff) {
+		_k_ff = k_ff;
 	}
+
 	void set_integrator_max(float max) {
 		_integrator_max = max;
 	}
+
 	void set_max_rate_pos(float max_rate_pos) {
 		_max_rate_pos = max_rate_pos;
 	}
+
 	void set_max_rate_neg(float max_rate_neg) {
 		_max_rate_neg = max_rate_neg;
 	}
+
 	void set_roll_ff(float roll_ff) {
 		_roll_ff = roll_ff;
 	}
@@ -94,13 +106,17 @@ public:
 		return _rate_setpoint;
 	}
 
+	float get_desired_bodyrate() {
+		return _bodyrate_setpoint;
+	}
+
 private:
 
 	uint64_t _last_run;
 	float _tc;
 	float _k_p;
 	float _k_i;
-	float _k_d;
+	float _k_ff;
 	float _integrator_max;
 	float _max_rate_pos;
 	float _max_rate_neg;
@@ -109,7 +125,7 @@ private:
 	float _integrator;
 	float _rate_error;
 	float _rate_setpoint;
-	float _max_deflection_rad;
+	float _bodyrate_setpoint;
 };
 
 #endif // ECL_PITCH_CONTROLLER_H
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
index 0b1ffa7a4..2e86c72dc 100644
--- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
@@ -45,21 +45,46 @@
 #include <geo/geo.h>
 #include <ecl/ecl.h>
 #include <mathlib/mathlib.h>
+#include <systemlib/err.h>
 
 ECL_RollController::ECL_RollController() :
 	_last_run(0),
 	_tc(0.1f),
+	_k_p(0.0f),
+	_k_i(0.0f),
+	_k_ff(0.0f),
+	_integrator_max(0.0f),
+	_max_rate(0.0f),
 	_last_output(0.0f),
 	_integrator(0.0f),
 	_rate_error(0.0f),
 	_rate_setpoint(0.0f),
-	_max_deflection_rad(math::radians(45.0f))
+	_bodyrate_setpoint(0.0f)
 {
+}
+
+float ECL_RollController::control_attitude(float roll_setpoint, float roll)
+{
+
+	/* Calculate error */
+	float roll_error = roll_setpoint - roll;
+
+	/* Apply P controller */
+	_rate_setpoint = roll_error / _tc;
+
+	/* limit the rate */ //XXX: move to body angluar rates
+	if (_max_rate > 0.01f) {
+		_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
+		_rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
+	}
 
+	return _rate_setpoint;
 }
 
-float ECL_RollController::control(float roll_setpoint, float roll, float roll_rate,
-				  float scaler, bool lock_integrator, float airspeed_min, float airspeed_max, float airspeed)
+float ECL_RollController::control_bodyrate(float pitch,
+		float roll_rate, float yaw_rate,
+		float yaw_rate_setpoint,
+		float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator)
 {
 	/* get the usual dt estimate */
 	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
@@ -70,10 +95,11 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra
 	if (dt_micros > 500000)
 		lock_integrator = true;
 
-	float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
-	float k_i_rate = _k_i * _tc;
+//	float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
+	float k_ff = 0; //xxx: param
 
 	/* input conditioning */
+//	warnx("airspeed pre %.4f", (double)airspeed);
 	if (!isfinite(airspeed)) {
 		/* airspeed is NaN, +- INF or not available, pick center of band */
 		airspeed = 0.5f * (airspeed_min + airspeed_max);
@@ -81,32 +107,27 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra
 		airspeed = airspeed_min;
 	}
 
-	float roll_error = roll_setpoint - roll;
-	_rate_setpoint = roll_error / _tc;
-
-	/* limit the rate */
-	if (_max_rate > 0.01f) {
-		_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
-		_rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
-	}
 
-	_rate_error = _rate_setpoint - roll_rate;
+	/* Transform setpoint to body angular rates */
+	_bodyrate_setpoint = _rate_setpoint - sinf(pitch) * yaw_rate_setpoint; //jacobian
 
+	/* Transform estimation to body angular rates */
+	float roll_bodyrate = roll_rate - sinf(pitch) * yaw_rate; //jacobian
 
-	float ilimit_scaled = _integrator_max * scaler;
+	/* Calculate body angular rate error */
+	_rate_error = _bodyrate_setpoint - roll_bodyrate; //body angular rate error
 
-	if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) {
+	if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
 
-		float id = _rate_error * k_i_rate * dt * scaler;
+		float id = _rate_error * dt;
 
 		/*
-		 * anti-windup: do not allow integrator to increase into the
-		 * wrong direction if actuator is at limit
+		 * anti-windup: do not allow integrator to increase if actuator is at limit
 		 */
-		if (_last_output < -_max_deflection_rad) {
+		if (_last_output < -1.0f) {
 			/* only allow motion to center: increase value */
 			id = math::max(id, 0.0f);
-		} else if (_last_output > _max_deflection_rad) {
+		} else if (_last_output > 1.0f) {
 			/* only allow motion to center: decrease value */
 			id = math::min(id, 0.0f);
 		}
@@ -115,11 +136,14 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra
 	}
 
 	/* integrator limit */
-	_integrator = math::constrain(_integrator, -ilimit_scaled, ilimit_scaled);
-	/* store non-limited output */
-	_last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_ff)) * scaler;
+	//xxx: until start detection is available: integral part in control signal is limited here
+	float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
+	//warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max);
+
+	/* Apply PI rate controller and store non-limited output */
+	_last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * scaler * scaler;  //scaler is proportional to 1/airspeed
 
-	return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad);
+	return math::constrain(_last_output, -1.0f, 1.0f);
 }
 
 void ECL_RollController::reset_integrator()
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h
index 0d4ea9333..92c64b95f 100644
--- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h
@@ -36,6 +36,7 @@
  * Definition of a simple orthogonal roll PID controller.
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  *
  * Acknowledgements:
  *
@@ -51,13 +52,17 @@
 #include <stdbool.h>
 #include <stdint.h>
 
-class __EXPORT ECL_RollController
+class __EXPORT ECL_RollController //XXX: create controller superclass
 {
 public:
 	ECL_RollController();
 
-	float control(float roll_setpoint, float roll, float roll_rate,
-		      float scaler = 1.0f, bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f));
+	float control_attitude(float roll_setpoint, float roll);
+
+	float control_bodyrate(float pitch,
+			float roll_rate, float yaw_rate,
+			float yaw_rate_setpoint,
+			float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f), float scaler = 1.0f, bool lock_integrator = false);
 
 	void reset_integrator();
 
@@ -66,18 +71,23 @@ public:
 			_tc = time_constant;
 		}
 	}
+
 	void set_k_p(float k_p) {
 		_k_p = k_p;
 	}
+
 	void set_k_i(float k_i) {
 		_k_i = k_i;
 	}
-	void set_k_d(float k_d) {
-		_k_d = k_d;
+
+	void set_k_ff(float k_ff) {
+		_k_ff = k_ff;
 	}
+
 	void set_integrator_max(float max) {
 		_integrator_max = max;
 	}
+
 	void set_max_rate(float max_rate) {
 		_max_rate = max_rate;
 	}
@@ -90,19 +100,23 @@ public:
 		return _rate_setpoint;
 	}
 
+	float get_desired_bodyrate() {
+		return _bodyrate_setpoint;
+	}
+
 private:
 	uint64_t _last_run;
 	float _tc;
 	float _k_p;
 	float _k_i;
-	float _k_d;
+	float _k_ff;
 	float _integrator_max;
 	float _max_rate;
 	float _last_output;
 	float _integrator;
 	float _rate_error;
 	float _rate_setpoint;
-	float _max_deflection_rad;
+	float _bodyrate_setpoint;
 };
 
 #endif // ECL_ROLL_CONTROLLER_H
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
index b786acf24..255776765 100644
--- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
@@ -44,29 +44,125 @@
 #include <geo/geo.h>
 #include <ecl/ecl.h>
 #include <mathlib/mathlib.h>
+#include <systemlib/err.h>
 
 ECL_YawController::ECL_YawController() :
 	_last_run(0),
-	_last_error(0.0f),
+	_k_p(0.0f),
+	_k_i(0.0f),
+	_k_ff(0.0f),
+	_integrator_max(0.0f),
+	_max_rate(0.0f),
 	_last_output(0.0f),
-	_last_rate_hp_out(0.0f),
-	_last_rate_hp_in(0.0f),
-	_k_d_last(0.0f),
-	_integrator(0.0f)
+	_integrator(0.0f),
+	_rate_error(0.0f),
+	_rate_setpoint(0.0f),
+	_bodyrate_setpoint(0.0f),
+	_coordinated_min_speed(1.0f)
 {
+}
+
+float ECL_YawController::control_attitude(float roll, float pitch,
+		float speed_body_u, float speed_body_v, float speed_body_w,
+		float roll_rate_setpoint, float pitch_rate_setpoint)
+{
+//	static int counter = 0;
+	/* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */
+	_rate_setpoint = 0.0f;
+	if (sqrtf(speed_body_u * speed_body_u + speed_body_v * speed_body_v + speed_body_w * speed_body_w) > _coordinated_min_speed) {
+		float denumerator = (speed_body_u * cosf(roll) * cosf(pitch) + speed_body_w * sinf(pitch));
+		if(denumerator != 0.0f) { //XXX: floating point comparison
+			_rate_setpoint = (speed_body_w * roll_rate_setpoint + 9.81f * sinf(roll) * cosf(pitch) + speed_body_u * pitch_rate_setpoint * sinf(roll)) / denumerator;
+//			warnx("yaw: speed_body_u %.f speed_body_w %1.f roll %.1f pitch %.1f denumerator %.1f _rate_setpoint %.1f", speed_body_u, speed_body_w, denumerator, _rate_setpoint);
+		}
+
+//		if(counter % 20 == 0) {
+//			warnx("denumerator: %.4f, speed_body_u: %.4f, speed_body_w: %.4f, cosf(roll): %.4f, cosf(pitch): %.4f, sinf(pitch): %.4f", (double)denumerator, (double)speed_body_u, (double)speed_body_w, (double)cosf(roll), (double)cosf(pitch), (double)sinf(pitch));
+//		}
+	}
+
+	/* limit the rate */ //XXX: move to body angluar rates
+	if (_max_rate > 0.01f) {
+	_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
+	_rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
+	}
+
 
+//	counter++;
+
+	if(!isfinite(_rate_setpoint)){
+		warnx("yaw rate sepoint not finite");
+		_rate_setpoint = 0.0f;
+	}
+
+	return _rate_setpoint;
 }
 
-float ECL_YawController::control(float roll, float yaw_rate, float accel_y, float scaler, bool lock_integrator,
-				 float airspeed_min, float airspeed_max, float aspeed)
+float ECL_YawController::control_bodyrate(float roll, float pitch,
+		float pitch_rate, float yaw_rate,
+		float pitch_rate_setpoint,
+		float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator)
 {
 	/* get the usual dt estimate */
 	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
 	_last_run = ecl_absolute_time();
+	float dt = (float)dt_micros * 1e-6f;
+
+	/* lock integral for long intervals */
+	if (dt_micros > 500000)
+		lock_integrator = true;
+
+
+//	float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
+	float k_ff = 0;
+
+
+	/* input conditioning */
+	if (!isfinite(airspeed)) {
+	/* airspeed is NaN, +- INF or not available, pick center of band */
+	airspeed = 0.5f * (airspeed_min + airspeed_max);
+	} else if (airspeed < airspeed_min) {
+	airspeed = airspeed_min;
+	}
+
+
+	/* Transform setpoint to body angular rates */
+	_bodyrate_setpoint = -sinf(roll) * pitch_rate_setpoint + cosf(roll)*cosf(pitch) * _rate_setpoint; //jacobian
+
+	/* Transform estimation to body angular rates */
+	float yaw_bodyrate = -sinf(roll) * pitch_rate + cosf(roll)*cosf(pitch) * yaw_rate; //jacobian
+
+	/* Calculate body angular rate error */
+	_rate_error = _bodyrate_setpoint - yaw_bodyrate; //body angular rate error
+
+	if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
+
+	float id = _rate_error * dt;
+
+	/*
+	 * anti-windup: do not allow integrator to increase if actuator is at limit
+	 */
+	if (_last_output < -1.0f) {
+		/* only allow motion to center: increase value */
+		id = math::max(id, 0.0f);
+	} else if (_last_output > 1.0f) {
+		/* only allow motion to center: decrease value */
+		id = math::min(id, 0.0f);
+	}
+
+	_integrator += id;
+	}
+
+	/* integrator limit */
+	//xxx: until start detection is available: integral part in control signal is limited here
+	float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
+
+	/* Apply PI rate controller and store non-limited output */
+	_last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * scaler * scaler;  //scaler is proportional to 1/airspeed
+	//warnx("yaw:_last_output: %.4f, _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_last_output, (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
 
-	float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000;
 
-	return 0.0f;
+	return math::constrain(_last_output, -1.0f, 1.0f);
 }
 
 void ECL_YawController::reset_integrator()
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
index 66b227918..03f3202d0 100644
--- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
+++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
@@ -35,6 +35,15 @@
  * @file ecl_yaw_controller.h
  * Definition of a simple orthogonal coordinated turn yaw PID controller.
  *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ *
+ * Acknowledgements:
+ *
+ *   The control design is based on a design
+ *   by Paul Riseborough and Andrew Tridgell, 2013,
+ *   which in turn is based on initial work of
+ *   Jonathan Challinger, 2012.
  */
 #ifndef ECL_YAW_CONTROLLER_H
 #define ECL_YAW_CONTROLLER_H
@@ -42,47 +51,73 @@
 #include <stdbool.h>
 #include <stdint.h>
 
-class __EXPORT ECL_YawController
+class __EXPORT ECL_YawController //XXX: create controller superclass
 {
 public:
 	ECL_YawController();
 
-	float control(float roll, float yaw_rate, float accel_y, float scaler = 1.0f, bool lock_integrator = false,
-		      float airspeed_min = 0, float airspeed_max = 0, float aspeed = (0.0f / 0.0f));
+	float control_attitude(float roll, float pitch,
+			float speed_body_u, float speed_body_v, float speed_body_w,
+			float roll_rate_setpoint, float pitch_rate_setpoint);
+
+	float control_bodyrate(	float roll, float pitch,
+			float pitch_rate, float yaw_rate,
+			float pitch_rate_setpoint,
+			float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator);
 
 	void reset_integrator();
 
-	void set_k_side(float k_a) {
-		_k_side = k_a;
+	void set_k_p(float k_p) {
+			_k_p = k_p;
 	}
+
 	void set_k_i(float k_i) {
 		_k_i = k_i;
 	}
-	void set_k_d(float k_d) {
-		_k_d = k_d;
-	}
-	void set_k_roll_ff(float k_roll_ff) {
-		_k_roll_ff = k_roll_ff;
+
+	void set_k_ff(float k_ff) {
+		_k_ff = k_ff;
 	}
+
 	void set_integrator_max(float max) {
 		_integrator_max = max;
 	}
 
+	void set_max_rate(float max_rate) {
+		_max_rate = max_rate;
+	}
+
+	void set_coordinated_min_speed(float coordinated_min_speed) {
+		_coordinated_min_speed = coordinated_min_speed;
+	}
+
+
+	float get_rate_error() {
+		return _rate_error;
+	}
+
+	float get_desired_rate() {
+		return _rate_setpoint;
+	}
+
+	float get_desired_bodyrate() {
+		return _bodyrate_setpoint;
+	}
+
 private:
 	uint64_t _last_run;
-
-	float _k_side;
+	float _k_p;
 	float _k_i;
-	float _k_d;
-	float _k_roll_ff;
+	float _k_ff;
 	float _integrator_max;
-
-	float _last_error;
+	float _max_rate;
+	float  _roll_ff;
 	float _last_output;
-	float _last_rate_hp_out;
-	float _last_rate_hp_in;
-	float _k_d_last;
 	float _integrator;
+	float _rate_error;
+	float _rate_setpoint;
+	float _bodyrate_setpoint;
+	float _coordinated_min_speed;
 
 };
 
diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
index 27d76f959..3b68a0a4e 100644
--- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
+++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
@@ -70,7 +70,7 @@ float ECL_L1_Pos_Controller::target_bearing()
 float ECL_L1_Pos_Controller::switch_distance(float wp_radius)
 {
 	/* following [2], switching on L1 distance */
-	return math::max(wp_radius, _L1_distance);
+	return math::min(wp_radius, _L1_distance);
 }
 
 bool ECL_L1_Pos_Controller::reached_loiter_target(void)
@@ -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 */
 
@@ -280,26 +280,26 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, cons
 	 */
 
 	// XXX check switch over
-	if ((lateral_accel_sp_center < lateral_accel_sp_circle && loiter_direction > 0 && xtrack_err_circle > 0.0f) |
+	if ((lateral_accel_sp_center < lateral_accel_sp_circle && loiter_direction > 0 && xtrack_err_circle > 0.0f) ||
 		(lateral_accel_sp_center > lateral_accel_sp_circle && loiter_direction < 0 && xtrack_err_circle > 0.0f)) {
 		_lateral_accel = lateral_accel_sp_center;
 		_circle_mode = false;
 		/* 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 1d5c85699..0f28bccad 100644
--- a/src/lib/external_lgpl/tecs/tecs.cpp
+++ b/src/lib/external_lgpl/tecs/tecs.cpp
@@ -2,6 +2,7 @@
 
 #include "tecs.h"
 #include <ecl/ecl.h>
+#include <systemlib/err.h>
 
 using namespace math;
 
@@ -29,7 +30,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
@@ -168,64 +169,88 @@ void TECS::_update_speed_demand(void)
 	// calculate velocity rate limits based on physical performance limits
 	// provision to use a different rate limit if bad descent or underspeed condition exists
 	// Use 50% of maximum energy rate to allow margin for total energy contgroller
-	float velRateMax;
-	float velRateMin;
-
-	if ((_badDescent) || (_underspeed)) {
-		velRateMax = 0.5f * _STEdot_max / _integ5_state;
-		velRateMin = 0.5f * _STEdot_min / _integ5_state;
-
-	} else {
-		velRateMax = 0.5f * _STEdot_max / _integ5_state;
-		velRateMin = 0.5f * _STEdot_min / _integ5_state;
-	}
-
-	// Apply rate limit
-	if ((_TAS_dem - _TAS_dem_adj) > (velRateMax * 0.1f)) {
-		_TAS_dem_adj = _TAS_dem_adj + velRateMax * 0.1f;
-		_TAS_rate_dem = velRateMax;
-
-	} else if ((_TAS_dem - _TAS_dem_adj) < (velRateMin * 0.1f)) {
-		_TAS_dem_adj = _TAS_dem_adj + velRateMin * 0.1f;
-		_TAS_rate_dem = velRateMin;
-
-	} else {
-		_TAS_dem_adj = _TAS_dem;
-		_TAS_rate_dem = (_TAS_dem - _TAS_dem_last) / 0.1f;
-	}
+//	float velRateMax;
+//	float velRateMin;
+//
+//	if ((_badDescent) || (_underspeed)) {
+//		velRateMax = 0.5f * _STEdot_max / _integ5_state;
+//		velRateMin = 0.5f * _STEdot_min / _integ5_state;
+//
+//	} else {
+//		velRateMax = 0.5f * _STEdot_max / _integ5_state;
+//		velRateMin = 0.5f * _STEdot_min / _integ5_state;
+//	}
+//
+//	// Apply rate limit
+//	if ((_TAS_dem - _TAS_dem_adj) > (velRateMax * 0.1f)) {
+//		_TAS_dem_adj = _TAS_dem_adj + velRateMax * 0.1f;
+//		_TAS_rate_dem = velRateMax;
+//
+//	} else if ((_TAS_dem - _TAS_dem_adj) < (velRateMin * 0.1f)) {
+//		_TAS_dem_adj = _TAS_dem_adj + velRateMin * 0.1f;
+//		_TAS_rate_dem = velRateMin;
+//
+//	} else {
+//		_TAS_dem_adj = _TAS_dem;
+//
+//
+//	_TAS_rate_dem = (_TAS_dem - _TAS_dem_last) / 0.1f;
+//	}
+
+	_TAS_dem_adj = _TAS_dem;
+	_TAS_rate_dem = (_TAS_dem_adj-_integ5_state)*_speedrate_p; //xxx: using a p loop for now
 
 	// Constrain speed demand again to protect against bad values on initialisation.
 	_TAS_dem_adj = constrain(_TAS_dem_adj, _TASmin, _TASmax);
-	_TAS_dem_last = _TAS_dem;
+//	_TAS_dem_last = _TAS_dem;
+
+//	warnx("_TAS_rate_dem: %.1f, _TAS_dem_adj %.1f, _integ5_state %.1f, _badDescent %u , _underspeed %u, velRateMin %.1f",
+//			(double)_TAS_rate_dem, (double)_TAS_dem_adj, (double)_integ5_state, _badDescent, _underspeed, velRateMin);
+//	warnx("_TAS_rate_dem: %.1f, _TAS_dem_adj %.1f, _integ5_state %.1f,  _badDescent %u , _underspeed %u",
+//			(double)_TAS_rate_dem, (double)_TAS_dem_adj, (double)_integ5_state,  _badDescent , _underspeed);
 }
 
-void TECS::_update_height_demand(float demand)
+void TECS::_update_height_demand(float demand, float state)
 {
-	// Apply 2 point moving average to demanded height
-	// This is required because height demand is only updated at 5Hz
-	_hgt_dem = 0.5f * (demand + _hgt_dem_in_old);
-	_hgt_dem_in_old = _hgt_dem;
-
-	// printf("hgt_dem: %6.2f hgt_dem_last: %6.2f max_climb_rate: %6.2f\n", _hgt_dem, _hgt_dem_prev,
-	// 	_maxClimbRate);
+//	// Apply 2 point moving average to demanded height
+//	// This is required because height demand is only updated at 5Hz
+//	_hgt_dem = 0.5f * (demand + _hgt_dem_in_old);
+//	_hgt_dem_in_old = _hgt_dem;
+//
+//	// printf("hgt_dem: %6.2f hgt_dem_last: %6.2f max_climb_rate: %6.2f\n", _hgt_dem, _hgt_dem_prev,
+//	// 	_maxClimbRate);
+//
+//	// Limit height rate of change
+//	if ((_hgt_dem - _hgt_dem_prev) > (_maxClimbRate * 0.1f)) {
+//		_hgt_dem = _hgt_dem_prev + _maxClimbRate * 0.1f;
+//
+//	} else if ((_hgt_dem - _hgt_dem_prev) < (-_maxSinkRate * 0.1f)) {
+//		_hgt_dem = _hgt_dem_prev - _maxSinkRate * 0.1f;
+//	}
+//
+//	_hgt_dem_prev = _hgt_dem;
+//
+//	// Apply first order lag to height demand
+//	_hgt_dem_adj = 0.05f * _hgt_dem + 0.95f * _hgt_dem_adj_last;
+//	_hgt_rate_dem = (_hgt_dem_adj - _hgt_dem_adj_last) / 0.1f;
+//	_hgt_dem_adj_last = _hgt_dem_adj;
+//
+//	// printf("hgt_dem: %6.2f hgt_dem_adj: %6.2f hgt_dem_last: %6.2f hgt_rate_dem: %6.2f\n", _hgt_dem, _hgt_dem_adj, _hgt_dem_adj_last,
+//	// 	_hgt_rate_dem);
+
+	_hgt_dem_adj = demand;//0.025f * demand + 0.975f * _hgt_dem_adj_last;
+	_hgt_dem_adj_last = _hgt_dem_adj;
 
+	_hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p;
 	// Limit height rate of change
-	if ((_hgt_dem - _hgt_dem_prev) > (_maxClimbRate * 0.1f)) {
-		_hgt_dem = _hgt_dem_prev + _maxClimbRate * 0.1f;
+	if (_hgt_rate_dem > _maxClimbRate) {
+		_hgt_rate_dem = _maxClimbRate;
 
-	} else if ((_hgt_dem - _hgt_dem_prev) < (-_maxSinkRate * 0.1f)) {
-		_hgt_dem = _hgt_dem_prev - _maxSinkRate * 0.1f;
+	} else if (_hgt_rate_dem < -_maxSinkRate) {
+		_hgt_rate_dem = -_maxSinkRate;
 	}
 
-	_hgt_dem_prev = _hgt_dem;
-
-	// Apply first order lag to height demand
-	_hgt_dem_adj = 0.05f * _hgt_dem + 0.95f * _hgt_dem_adj_last;
-	_hgt_rate_dem = (_hgt_dem_adj - _hgt_dem_adj_last) / 0.1f;
-	_hgt_dem_adj_last = _hgt_dem_adj;
-
-	// printf("hgt_dem: %6.2f hgt_dem_adj: %6.2f hgt_dem_last: %6.2f hgt_rate_dem: %6.2f\n", _hgt_dem, _hgt_dem_adj, _hgt_dem_adj_last,
-	// 	_hgt_rate_dem);
+	//warnx("_hgt_rate_dem: %.4f, _hgt_dem_adj %.4f", _hgt_rate_dem, _hgt_dem_adj);
 }
 
 void TECS::_detect_underspeed(void)
@@ -257,7 +282,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;
@@ -285,7 +310,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Dcm &rotMat)
 		// additional component which scales with (1/cos(bank angle) - 1) to compensate for induced
 		// drag increase during turns.
 		float cosPhi = sqrtf((rotMat(0, 1) * rotMat(0, 1)) + (rotMat(1, 1) * rotMat(1, 1)));
-		STEdot_dem = STEdot_dem + _rollComp * (1.0f / constrain(cosPhi * cosPhi , 0.1f, 1.0f) - 1.0f);
+		STEdot_dem = STEdot_dem + _rollComp * (1.0f / constrain(cosPhi , 0.1f, 1.0f) - 1.0f);
 
 		if (STEdot_dem >= 0) {
 			ff_throttle = nomThr + STEdot_dem / _STEdot_max * (1.0f - nomThr);
@@ -353,14 +378,18 @@ void TECS::_detect_bad_descent(void)
 	// 1) Underspeed protection not active
 	// 2) Specific total energy error > 0
 	// This mode will produce an undulating speed and height response as it cuts in and out but will prevent the aircraft from descending into the ground if an unachievable speed demand is set
-	float STEdot = _SPEdot + _SKEdot;
-
-	if ((!_underspeed && (_STE_error > 200.0f) && (STEdot < 0.0f) && (_throttle_dem >= _THRmaxf * 0.9f)) || (_badDescent && !_underspeed && (_STE_error > 0.0f))) {
-		_badDescent = true;
-
-	} else {
-		_badDescent = false;
-	}
+//	float STEdot = _SPEdot + _SKEdot;
+//
+//	if ((!_underspeed && (_STE_error > 200.0f) && (STEdot < 0.0f) && (_throttle_dem >= _THRmaxf * 0.9f)) || (_badDescent && !_underspeed && (_STE_error > 0.0f))) {
+//
+//		warnx("bad descent detected: _STE_error %.1f, STEdot %.1f, _throttle_dem %.1f", _STE_error, STEdot, _throttle_dem);
+//		_badDescent = true;
+//
+//	} else {
+//		_badDescent = false;
+//	}
+
+	_badDescent = false;
 }
 
 void TECS::_update_pitch(void)
@@ -476,7 +505,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)
 {
@@ -508,7 +537,7 @@ void TECS::update_pitch_throttle(const math::Dcm &rotMat, float pitch, float bar
 	_update_speed_demand();
 
 	// Calculate the height demand
-	_update_height_demand(hgt_dem);
+	_update_height_demand(hgt_dem, baro_altitude);
 
 	// Detect underspeed condition
 	_detect_underspeed();
diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h
index f8f832ed7..d1ebacda1 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
@@ -180,6 +180,14 @@ public:
 		_indicated_airspeed_max = airspeed;
 	}
 
+	void set_heightrate_p(float heightrate_p) {
+		_heightrate_p = heightrate_p;
+	}
+
+	void set_speedrate_p(float speedrate_p) {
+		_speedrate_p = speedrate_p;
+	}
+
 private:
 	// Last time update_50Hz was called
 	uint64_t _update_50hz_last_usec;
@@ -203,6 +211,8 @@ private:
 	float _vertAccLim;
 	float _rollComp;
 	float _spdWeight;
+	float _heightrate_p;
+	float _speedrate_p;
 
 	// throttle demand in the range from 0.0 to 1.0
 	float _throttle_dem;
@@ -329,7 +339,7 @@ private:
 	void _update_speed_demand(void);
 
 	// Update the demanded height
-	void _update_height_demand(float demand);
+	void _update_height_demand(float demand, float state);
 
 	// Detect an underspeed condition
 	void _detect_underspeed(void);
@@ -338,7 +348,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/geo/geo.c b/src/lib/geo/geo.c
index ce2c64a59..34285e2bf 100644
--- a/src/lib/geo/geo.c
+++ b/src/lib/geo/geo.c
@@ -72,7 +72,6 @@ __EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are
 	/* calculate local scale by using the relation of true distance and the distance on plane */ //TODO: this is a quick solution, there are probably easier ways to determine the scale
 
 	/* 1) calculate true distance d on sphere to a point: http://www.movable-type.co.uk/scripts/latlong.html */
-	const double r_earth = 6371000;
 
 	double lat1 = phi_1;
 	double lon1 = lambda_0;
@@ -81,7 +80,7 @@ __EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are
 	double lon2 = lambda_0 + 0.5 / 180 * M_PI;
 	double sin_lat_2 = sin(lat2);
 	double cos_lat_2 = cos(lat2);
-	double d = acos(sin(lat1) * sin_lat_2 + cos(lat1) * cos_lat_2 * cos(lon2 - lon1)) * r_earth;
+	double d = acos(sin(lat1) * sin_lat_2 + cos(lat1) * cos_lat_2 * cos(lon2 - lon1)) * CONSTANTS_RADIUS_OF_EARTH;
 
 	/* 2) calculate distance rho on plane */
 	double k_bar = 0;
@@ -188,8 +187,7 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou
 	double a = sin(d_lat / 2.0d) * sin(d_lat / 2.0d) + sin(d_lon / 2.0d) * sin(d_lon / 2.0d) * cos(lat_now_rad) * cos(lat_next_rad);
 	double c = 2.0d * atan2(sqrt(a), sqrt(1.0d - a));
 
-	const double radius_earth = 6371000.0d;
-	return radius_earth * c; 
+	return CONSTANTS_RADIUS_OF_EARTH * c;
 }
 
 __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
@@ -209,7 +207,7 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub
 	return theta;
 }
 
-__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy)
+__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e)
 {
 	double lat_now_rad = lat_now * M_DEG_TO_RAD;
 	double lon_now_rad = lon_now * M_DEG_TO_RAD;
@@ -220,11 +218,11 @@ __EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double
 	double d_lon = lon_next_rad - lon_now_rad;
 
 	/* conscious mix of double and float trig function to maximize speed and efficiency */
-	*vy = CONSTANTS_RADIUS_OF_EARTH * sin(d_lon) * cos(lat_next_rad);
-	*vx = CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon);
+	*v_n = CONSTANTS_RADIUS_OF_EARTH * (cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon));
+	*v_e = CONSTANTS_RADIUS_OF_EARTH * sin(d_lon) * cos(lat_next_rad);
 }
 
-__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy)
+__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e)
 {
 	double lat_now_rad = lat_now * M_DEG_TO_RAD;
 	double lon_now_rad = lon_now * M_DEG_TO_RAD;
@@ -235,8 +233,17 @@ __EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, d
 	double d_lon = lon_next_rad - lon_now_rad;
 
 	/* conscious mix of double and float trig function to maximize speed and efficiency */
-	*vy = CONSTANTS_RADIUS_OF_EARTH * d_lon;
-	*vx = CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad);
+	*v_n = CONSTANTS_RADIUS_OF_EARTH * d_lat;
+	*v_e = CONSTANTS_RADIUS_OF_EARTH * d_lon * cos(lat_now_rad);
+}
+
+__EXPORT void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res, double *lon_res)
+{
+	double lat_now_rad = lat_now * M_DEG_TO_RAD;
+	double lon_now_rad = lon_now * M_DEG_TO_RAD;
+
+	*lat_res = (lat_now_rad + v_n / CONSTANTS_RADIUS_OF_EARTH) * M_RAD_TO_DEG;
+	*lon_res = (lon_now_rad + v_e / (CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad))) * M_RAD_TO_DEG;
 }
 
 // Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
@@ -386,25 +393,64 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, d
 	return return_value;
 }
 
+__EXPORT float get_distance_to_point_global_wgs84(double lat_now, double lon_now, float alt_now,
+	                                          double lat_next, double lon_next, float alt_next,
+	                                          float *dist_xy, float *dist_z)
+{
+	double current_x_rad = lat_next / 180.0 * M_PI;
+	double current_y_rad = lon_next / 180.0 * M_PI;
+	double x_rad = lat_now / 180.0 * M_PI;
+	double y_rad = lon_now / 180.0 * M_PI;
+
+	double d_lat = x_rad - current_x_rad;
+	double d_lon = y_rad - current_y_rad;
+
+	double a = sin(d_lat / 2.0) * sin(d_lat / 2.0) + sin(d_lon / 2.0) * sin(d_lon / 2.0f) * cos(current_x_rad) * cos(x_rad);
+	double c = 2 * atan2(sqrt(a), sqrt(1 - a));
+
+	float dxy = CONSTANTS_RADIUS_OF_EARTH * c;
+	float dz = alt_now - alt_next;
+
+	*dist_xy = fabsf(dxy);
+	*dist_z = fabsf(dz);
+
+	return sqrtf(dxy * dxy + dz * dz);
+}
+
+
+__EXPORT float mavlink_wpm_distance_to_point_local(float x_now, float y_now, float z_now,
+	                                           float x_next, float y_next, float z_next,
+	                                           float *dist_xy, float *dist_z)
+{
+	float dx = x_now - x_next;
+	float dy = y_now - y_next;
+	float dz = z_now - z_next;
+
+	*dist_xy = sqrtf(dx * dx + dy * dy);
+	*dist_z = fabsf(dz);
+
+	return sqrtf(dx * dx + dy * dy + dz * dz);
+}
+
 __EXPORT float _wrap_pi(float bearing)
 {
 	/* value is inf or NaN */
-	if (!isfinite(bearing) || bearing == 0) {
+	if (!isfinite(bearing)) {
 		return bearing;
 	}
 
 	int c = 0;
-
-	while (bearing > M_PI_F && c < 30) {
+	while (bearing > M_PI_F) {
 		bearing -= M_TWOPI_F;
-		c++;
+		if (c++ > 3)
+			return NAN;
 	}
 
 	c = 0;
-
-	while (bearing <=  -M_PI_F && c < 30) {
+	while (bearing <= -M_PI_F) {
 		bearing += M_TWOPI_F;
-		c++;
+		if (c++ > 3)
+			return NAN;
 	}
 
 	return bearing;
@@ -417,12 +463,18 @@ __EXPORT float _wrap_2pi(float bearing)
 		return bearing;
 	}
 
-	while (bearing >= M_TWOPI_F) {
-		bearing = bearing - M_TWOPI_F;
+	int c = 0;
+	while (bearing > M_TWOPI_F) {
+		bearing -= M_TWOPI_F;
+		if (c++ > 3)
+			return NAN;
 	}
 
-	while (bearing <  0.0f) {
-		bearing = bearing + M_TWOPI_F;
+	c = 0;
+	while (bearing <= 0.0f) {
+		bearing += M_TWOPI_F;
+		if (c++ > 3)
+			return NAN;
 	}
 
 	return bearing;
@@ -435,12 +487,18 @@ __EXPORT float _wrap_180(float bearing)
 		return bearing;
 	}
 
+	int c = 0;
 	while (bearing > 180.0f) {
-		bearing = bearing - 360.0f;
+		bearing -= 360.0f;
+		if (c++ > 3)
+			return NAN;
 	}
 
-	while (bearing <=  -180.0f) {
-		bearing = bearing + 360.0f;
+	c = 0;
+	while (bearing <= -180.0f) {
+		bearing += 360.0f;
+		if (c++ > 3)
+			return NAN;
 	}
 
 	return bearing;
@@ -453,15 +511,19 @@ __EXPORT float _wrap_360(float bearing)
 		return bearing;
 	}
 
-	while (bearing >= 360.0f) {
-		bearing = bearing - 360.0f;
+	int c = 0;
+	while (bearing > 360.0f) {
+		bearing -= 360.0f;
+		if (c++ > 3)
+			return NAN;
 	}
 
-	while (bearing <  0.0f) {
-		bearing = bearing + 360.0f;
+	c = 0;
+	while (bearing <= 0.0f) {
+		bearing += 360.0f;
+		if (c++ > 3)
+			return NAN;
 	}
 
 	return bearing;
 }
-
-
diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h
index 123ff80f1..94afb4df0 100644
--- a/src/lib/geo/geo.h
+++ b/src/lib/geo/geo.h
@@ -47,6 +47,9 @@
 
 #pragma once
 
+#include "uORB/topics/fence.h"
+#include "uORB/topics/vehicle_global_position.h"
+
 __BEGIN_DECLS
 
 #include <stdbool.h>
@@ -112,15 +115,31 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou
  */
 __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
 
-__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy);
+__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e);
+
+__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e);
 
-__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy);
+__EXPORT void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res, double *lon_res);
 
 __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end);
 
 __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center,
 		float radius, float arc_start_bearing, float arc_sweep);
 
+/*
+ * Calculate distance in global frame
+ */
+__EXPORT float get_distance_to_point_global_wgs84(double lat_now, double lon_now, float alt_now,
+	                                          double lat_next, double lon_next, float alt_next,
+	                                          float *dist_xy, float *dist_z);
+
+/*
+ * Calculate distance in local frame (NED)
+ */
+__EXPORT float mavlink_wpm_distance_to_point_local(float x_now, float y_now, float z_now,
+	                                           float x_next, float y_next, float z_next,
+	                                           float *dist_xy, float *dist_z);
+
 __EXPORT float _wrap_180(float bearing);
 __EXPORT float _wrap_360(float bearing);
 __EXPORT float _wrap_pi(float bearing);
diff --git a/src/lib/launchdetection/CatapultLaunchMethod.cpp b/src/lib/launchdetection/CatapultLaunchMethod.cpp
new file mode 100644
index 000000000..d5c759b17
--- /dev/null
+++ b/src/lib/launchdetection/CatapultLaunchMethod.cpp
@@ -0,0 +1,90 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). 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 documentation4 and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name ECL 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 CatapultLaunchMethod.cpp
+ * Catpult Launch detection
+ *
+ * Authors and acknowledgements in header.
+ */
+
+#include "CatapultLaunchMethod.h"
+#include <systemlib/err.h>
+
+CatapultLaunchMethod::CatapultLaunchMethod() :
+	last_timestamp(0),
+	integrator(0.0f),
+	launchDetected(false),
+	threshold_accel(NULL, "LAUN_CAT_A", false),
+	threshold_time(NULL, "LAUN_CAT_T", false)
+{
+
+}
+
+CatapultLaunchMethod::~CatapultLaunchMethod() {
+
+}
+
+void CatapultLaunchMethod::update(float accel_x)
+{
+	float dt = (float)hrt_elapsed_time(&last_timestamp) * 1e-6f;
+	last_timestamp = hrt_absolute_time();
+
+	if (accel_x > threshold_accel.get()) {
+		integrator += accel_x * dt;
+//		warnx("*** integrator %.3f, threshold_accel %.3f, threshold_time %.3f, accel_x %.3f, dt %.3f",
+//				(double)integrator, (double)threshold_accel.get(),  (double)threshold_time.get(), (double)accel_x, (double)dt);
+		if (integrator > threshold_accel.get() * threshold_time.get()) {
+			launchDetected = true;
+		}
+
+	} else {
+//		warnx("integrator %.3f, threshold_accel %.3f, threshold_time %.3f, accel_x %.3f, dt %.3f",
+//				(double)integrator, (double)threshold_accel.get(),  (double)threshold_time.get(), (double)accel_x, (double)dt);
+		/* reset integrator */
+		integrator = 0.0f;
+		launchDetected = false;
+	}
+
+}
+
+bool CatapultLaunchMethod::getLaunchDetected()
+{
+	return launchDetected;
+}
+
+void CatapultLaunchMethod::updateParams()
+{
+	threshold_accel.update();
+	threshold_time.update();
+}
diff --git a/src/lib/mathlib/math/EulerAngles.hpp b/src/lib/launchdetection/CatapultLaunchMethod.h
index 399eecfa7..e943f11e9 100644
--- a/src/lib/mathlib/math/EulerAngles.hpp
+++ b/src/lib/launchdetection/CatapultLaunchMethod.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -10,9 +10,9 @@
  *    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
+ *    the documentation4 and/or other materials provided with the
  *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
+ * 3. Neither the name ECL nor the names of its contributors may be
  *    used to endorse or promote products derived from this software
  *    without specific prior written permission.
  *
@@ -32,43 +32,40 @@
  ****************************************************************************/
 
 /**
- * @file Vector.h
+ * @file CatapultLaunchMethod.h
+ * Catpult Launch detection
  *
- * math vector
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
-#pragma once
+#ifndef CATAPULTLAUNCHMETHOD_H_
+#define CATAPULTLAUNCHMETHOD_H_
 
-#include "Vector.hpp"
+#include "LaunchMethod.h"
 
-namespace math
-{
-
-class Quaternion;
-class Dcm;
+#include <drivers/drv_hrt.h>
+#include <controllib/block/BlockParam.hpp>
 
-class __EXPORT EulerAngles : public Vector
+class CatapultLaunchMethod : public LaunchMethod
 {
 public:
-	EulerAngles();
-	EulerAngles(float phi, float theta, float psi);
-	EulerAngles(const Quaternion &q);
-	EulerAngles(const Dcm &dcm);
-	virtual ~EulerAngles();
+	CatapultLaunchMethod();
+	~CatapultLaunchMethod();
 
-	// alias
-	void setPhi(float phi) { (*this)(0) = phi; }
-	void setTheta(float theta) { (*this)(1) = theta; }
-	void setPsi(float psi) { (*this)(2) = psi; }
+	void update(float accel_x);
+	bool getLaunchDetected();
+	void updateParams();
 
-	// const accessors
-	const float &getPhi() const { return (*this)(0); }
-	const float &getTheta() const { return (*this)(1); }
-	const float &getPsi() const { return (*this)(2); }
+private:
+	hrt_abstime last_timestamp;
+//	float threshold_accel_raw;
+//	float threshold_time;
+	float integrator;
+	bool launchDetected;
 
-};
+	control::BlockParamFloat threshold_accel;
+	control::BlockParamFloat threshold_time;
 
-int __EXPORT eulerAnglesTest();
-
-} // math
+};
 
+#endif /* CATAPULTLAUNCHMETHOD_H_ */
diff --git a/src/lib/mathlib/math/Vector.cpp b/src/lib/launchdetection/LaunchDetector.cpp
index 35158a396..df9f2fe95 100644
--- a/src/lib/mathlib/math/Vector.cpp
+++ b/src/lib/launchdetection/LaunchDetector.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -12,7 +12,7 @@
  *    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
+ * 3. Neither the name ECL nor the names of its contributors may be
  *    used to endorse or promote products derived from this software
  *    without specific prior written permission.
  *
@@ -32,69 +32,61 @@
  ****************************************************************************/
 
 /**
- * @file Vector.cpp
+ * @file launchDetection.cpp
+ * Auto Detection for different launch methods (e.g. catapult)
  *
- * math vector
+ * Authors and acknowledgements in header.
  */
 
-#include "test/test.hpp"
+#include "LaunchDetector.h"
+#include "CatapultLaunchMethod.h"
+#include <systemlib/err.h>
 
-#include "Vector.hpp"
-
-namespace math
+LaunchDetector::LaunchDetector() :
+	launchdetection_on(NULL, "LAUN_ALL_ON", false),
+	throttlePreTakeoff(NULL, "LAUN_THR_PRE", false)
 {
+	/* init all detectors */
+	launchMethods[0] = new CatapultLaunchMethod();
 
-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;
+	/* update all parameters of all detectors */
+	updateParams();
 }
 
-int vectorAddTest()
+LaunchDetector::~LaunchDetector()
 {
-	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)
+void LaunchDetector::update(float accel_x)
 {
-	if (a.getRows() != b.getRows()) {
-		printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows());
-		return false;
+	if (launchdetection_on.get() == 1) {
+		for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
+			launchMethods[i]->update(accel_x);
+		}
 	}
+}
 
-	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;
+bool LaunchDetector::getLaunchDetected()
+{
+	if (launchdetection_on.get() == 1) {
+		for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
+			if(launchMethods[i]->getLaunchDetected()) {
+				return true;
+			}
 		}
 	}
 
-	return ret;
+	return false;
 }
 
-} // namespace math
+void LaunchDetector::updateParams() {
+
+	launchdetection_on.update();
+	throttlePreTakeoff.update();
+
+	for (uint8_t i = 0; i < sizeof(launchMethods)/sizeof(LaunchMethod); i++) {
+		launchMethods[i]->updateParams();
+	}
+}
diff --git a/src/lib/mathlib/math/Vector2f.hpp b/src/lib/launchdetection/LaunchDetector.h
index ecd62e81c..7c2ff826c 100644
--- a/src/lib/mathlib/math/Vector2f.hpp
+++ b/src/lib/launchdetection/LaunchDetector.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -10,9 +10,9 @@
  *    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
+ *    the documentation4 and/or other materials provided with the
  *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
+ * 3. Neither the name ECL nor the names of its contributors may be
  *    used to endorse or promote products derived from this software
  *    without specific prior written permission.
  *
@@ -32,48 +32,44 @@
  ****************************************************************************/
 
 /**
- * @file Vector2f.hpp
+ * @file LaunchDetector.h
+ * Auto Detection for different launch methods (e.g. catapult)
  *
- * math 3 vector
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
-#pragma once
+#ifndef LAUNCHDETECTOR_H
+#define LAUNCHDETECTOR_H
 
-#include "Vector.hpp"
+#include <stdbool.h>
+#include <stdint.h>
 
-namespace math
-{
+#include "LaunchMethod.h"
+
+#include <controllib/block/BlockParam.hpp>
 
-class __EXPORT Vector2f :
-	public Vector
+class __EXPORT LaunchDetector
 {
 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);
-	}
+	LaunchDetector();
+	~LaunchDetector();
+
+	void update(float accel_x);
+	bool getLaunchDetected();
+	void updateParams();
+	bool launchDetectionEnabled() { return (bool)launchdetection_on.get(); };
+
+	float getThrottlePreTakeoff() {return throttlePreTakeoff.get(); }
+
+//	virtual bool getLaunchDetected();
+protected:
+private:
+	LaunchMethod* launchMethods[1];
+	control::BlockParamInt launchdetection_on;
+	control::BlockParamFloat throttlePreTakeoff;
+
 
-	/**
-	 * 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
 
+#endif // LAUNCHDETECTOR_H
diff --git a/src/lib/mathlib/math/arm/Vector.cpp b/src/lib/launchdetection/LaunchMethod.h
index 7ea6496bb..bfb5f8cb4 100644
--- a/src/lib/mathlib/math/arm/Vector.cpp
+++ b/src/lib/launchdetection/LaunchMethod.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -10,9 +10,9 @@
  *    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
+ *    the documentation4 and/or other materials provided with the
  *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
+ * 3. Neither the name ECL nor the names of its contributors may be
  *    used to endorse or promote products derived from this software
  *    without specific prior written permission.
  *
@@ -32,9 +32,23 @@
  ****************************************************************************/
 
 /**
- * @file Vector.cpp
+ * @file LaunchMethod.h
+ * Base class for different launch methods
  *
- * math vector
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
-#include "Vector.hpp"
+#ifndef LAUNCHMETHOD_H_
+#define LAUNCHMETHOD_H_
+
+class LaunchMethod
+{
+public:
+	virtual void update(float accel_x) = 0;
+	virtual bool getLaunchDetected() = 0;
+	virtual void updateParams() = 0;
+protected:
+private:
+};
+
+#endif /* LAUNCHMETHOD_H_ */
diff --git a/src/lib/mathlib/math/Vector2f.cpp b/src/lib/launchdetection/launchdetection_params.c
index 68e741817..45d7957f1 100644
--- a/src/lib/mathlib/math/Vector2f.cpp
+++ b/src/lib/launchdetection/launchdetection_params.c
@@ -1,6 +1,7 @@
 /****************************************************************************
  *
  *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: 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
@@ -17,7 +18,7 @@
  *    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
+ * 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,
@@ -32,72 +33,58 @@
  ****************************************************************************/
 
 /**
- * @file Vector2f.cpp
+ * @file launchdetection_params.c
  *
- * math vector
+ * Parameters for launchdetection
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
  */
 
-#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);
-}
+#include <nuttx/config.h>
 
-Vector2f::Vector2f(const float *data) :
-	Vector(2, data)
-{
-}
+#include <systemlib/param/param.h>
 
-Vector2f::~Vector2f()
-{
-}
+/*
+ * Catapult launch detection parameters, accessible via MAVLink
+ *
+ */
 
-float Vector2f::cross(const Vector2f &b) const
-{
-	const Vector2f &a = *this;
-	return a(0)*b(1) - a(1)*b(0);
-}
+/**
+ * Enable launch detection.
+ *
+ * @min 0
+ * @max 1
+ * @group Launch detection
+ */
+PARAM_DEFINE_INT32(LAUN_ALL_ON, 0);
 
-float Vector2f::operator %(const Vector2f &v) const
-{
-	return cross(v);
-}
-    
-float Vector2f::operator *(const Vector2f &v) const
-{
-    return dot(v);
-}
+/**
+ * Catapult accelerometer theshold.
+ *
+ * LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection.
+ *
+ * @min 0
+ * @group Launch detection
+ */
+PARAM_DEFINE_FLOAT(LAUN_CAT_A, 30.0f);
 
-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;
-}
+/**
+ * Catapult time theshold.
+ *
+ * LAUN_CAT_A * LAUN_CAT_T serves as threshold to trigger launch detection.
+ *
+ * @min 0
+ * @group Launch detection
+ */
+PARAM_DEFINE_FLOAT(LAUN_CAT_T, 0.05f);
 
-} // namespace math
+/**
+ * Throttle setting while detecting launch.
+ *
+ * The throttle is set to this value while the system is waiting for the take-off.
+ *
+ * @min 0
+ * @max 1
+ * @group Launch detection
+ */
+PARAM_DEFINE_FLOAT(LAUN_THR_PRE, 0.0f);
diff --git a/src/lib/launchdetection/module.mk b/src/lib/launchdetection/module.mk
new file mode 100644
index 000000000..13648b74c
--- /dev/null
+++ b/src/lib/launchdetection/module.mk
@@ -0,0 +1,40 @@
+############################################################################
+#
+#   Copyright (c) 2013 Estimation and Control Library (ECL). 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.
+#
+############################################################################
+
+#
+# Launchdetection Library
+#
+
+SRCS		 = 	LaunchDetector.cpp \
+			CatapultLaunchMethod.cpp \
+			launchdetection_params.c
diff --git a/src/lib/mathlib/math/Dcm.cpp b/src/lib/mathlib/math/Dcm.cpp
deleted file mode 100644
index f509f7081..000000000
--- a/src/lib/mathlib/math/Dcm.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 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()
-{
-}
-
-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 df8970d3a..000000000
--- a/src/lib/mathlib/math/Dcm.hpp
+++ /dev/null
@@ -1,108 +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);
-
-	/**
-	 * 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/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.hpp b/src/lib/mathlib/math/Vector.hpp
index 73de793d5..c7323c215 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,466 @@
  ****************************************************************************/
 
 /**
- * @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;
+	}
+
+	/**
+	 * element by element multiplication
+	 */
+	const Vector<N> emult(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;
+	}
+
+	/**
+	 * element by element division
+	 */
+	const Vector<N> edivide(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;
+	}
+
+	/**
+	 * 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/Vector3.cpp b/src/lib/mathlib/math/Vector3.cpp
deleted file mode 100644
index dcb85600e..000000000
--- a/src/lib/mathlib/math/Vector3.cpp
+++ /dev/null
@@ -1,99 +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;
-}
-
-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 568d9669a..000000000
--- a/src/lib/mathlib/math/Vector3.hpp
+++ /dev/null
@@ -1,76 +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;
-
-	/**
-	 * 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.hpp b/src/lib/mathlib/math/arm/Vector.hpp
deleted file mode 100644
index 52220fc15..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*(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