mathlib fixes

15 files changed, 428 insertions, 1080 deletions

diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp
index bac594ab9..614877b18 100644
--- a/src/lib/conversion/rotation.cpp
+++ b/src/lib/conversion/rotation.cpp
@@ -41,7 +41,7 @@
 #include "rotation.h"
 
 __EXPORT void
-get_rot_matrix(enum Rotation rot, math::Matrix3f *rot_matrix)
+get_rot_matrix(enum Rotation rot, math::Matrix<3,3> *rot_matrix)
 {
 	float roll  = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll;
 	float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch;
diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h
index 85c63c0fc..0c56494c5 100644
--- a/src/lib/conversion/rotation.h
+++ b/src/lib/conversion/rotation.h
@@ -116,6 +116,6 @@ const rot_lookup_t rot_lookup[] = {
  * Get the rotation matrix
  */
 __EXPORT void
-get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix);
+get_rot_matrix(enum Rotation rot, math::Matrix<3,3> *rot_matrix);
 
 #endif /* ROTATION_H_ */
diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
index 27d76f959..fb7ed486d 100644
--- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
+++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
@@ -83,8 +83,8 @@ float ECL_L1_Pos_Controller::crosstrack_error(void)
 	return _crosstrack_error;
 }
 
-void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position,
-				       const math::Vector2f &ground_speed_vector)
+void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector<2> &vector_A, const math::Vector<2> &vector_B, const math::Vector<2> &vector_curr_position,
+				       const math::Vector<2> &ground_speed_vector)
 {
 
 	/* this follows the logic presented in [1] */
@@ -94,7 +94,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
 	float ltrack_vel;
 
 	/* get the direction between the last (visited) and next waypoint */
-	_target_bearing = get_bearing_to_next_waypoint(vector_curr_position.getX(), vector_curr_position.getY(), vector_B.getX(), vector_B.getY());
+	_target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_B(0), vector_B(1));
 
 	/* enforce a minimum ground speed of 0.1 m/s to avoid singularities */
 	float ground_speed = math::max(ground_speed_vector.length(), 0.1f);
@@ -103,7 +103,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
 	_L1_distance = _L1_ratio * ground_speed;
 
 	/* calculate vector from A to B */
-	math::Vector2f vector_AB = get_local_planar_vector(vector_A, vector_B);
+	math::Vector<2> vector_AB = get_local_planar_vector(vector_A, vector_B);
 
 	/*
 	 * check if waypoints are on top of each other. If yes,
@@ -116,7 +116,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
 	vector_AB.normalize();
 
 	/* calculate the vector from waypoint A to the aircraft */
-	math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
+	math::Vector<2> vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
 
 	/* calculate crosstrack error (output only) */
 	_crosstrack_error = vector_AB % vector_A_to_airplane;
@@ -130,7 +130,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
 	float alongTrackDist = vector_A_to_airplane * vector_AB;
 
 	/* estimate airplane position WRT to B */
-	math::Vector2f vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized();
+	math::Vector<2> vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized();
 	
 	/* calculate angle of airplane position vector relative to line) */
 
@@ -143,14 +143,14 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
 		/* calculate eta to fly to waypoint A */
 
 		/* unit vector from waypoint A to current position */
-		math::Vector2f vector_A_to_airplane_unit = vector_A_to_airplane.normalized();
+		math::Vector<2> vector_A_to_airplane_unit = vector_A_to_airplane.normalized();
 		/* velocity across / orthogonal to line */
 		xtrack_vel = ground_speed_vector % (-vector_A_to_airplane_unit);
 		/* velocity along line */
 		ltrack_vel = ground_speed_vector * (-vector_A_to_airplane_unit);
 		eta = atan2f(xtrack_vel, ltrack_vel);
 		/* bearing from current position to L1 point */
-		_nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX());
+		_nav_bearing = atan2f(-vector_A_to_airplane_unit(1) , -vector_A_to_airplane_unit(0));
 
 	/*
 	 * If the AB vector and the vector from B to airplane point in the same
@@ -174,7 +174,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
 		ltrack_vel = ground_speed_vector * (-vector_B_to_P_unit);
 		eta = atan2f(xtrack_vel, ltrack_vel);
 		/* bearing from current position to L1 point */
-		_nav_bearing = atan2f(-vector_B_to_P_unit.getY() , -vector_B_to_P_unit.getX());
+		_nav_bearing = atan2f(-vector_B_to_P_unit(1) , -vector_B_to_P_unit(0));
 
 	} else {
 
@@ -194,7 +194,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
 		float eta1 = asinf(sine_eta1);
 		eta = eta1 + eta2;
 		/* bearing from current position to L1 point */
-		_nav_bearing = atan2f(vector_AB.getY(), vector_AB.getX()) + eta1;
+		_nav_bearing = atan2f(vector_AB(1), vector_AB(0)) + eta1;
 
 	}
 
@@ -209,8 +209,8 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
 	_bearing_error = eta;
 }
 
-void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, const math::Vector2f &vector_curr_position, float radius, int8_t loiter_direction,
-				       const math::Vector2f &ground_speed_vector)
+void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector<2> &vector_A, const math::Vector<2> &vector_curr_position, float radius, int8_t loiter_direction,
+				       const math::Vector<2> &ground_speed_vector)
 {
 	/* the complete guidance logic in this section was proposed by [2] */
 
@@ -220,7 +220,7 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, cons
 	float K_velocity = 2.0f * _L1_damping * omega;
 
 	/* update bearing to next waypoint */
-	_target_bearing = get_bearing_to_next_waypoint(vector_curr_position.getX(), vector_curr_position.getY(), vector_A.getX(), vector_A.getY());
+	_target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_A(0), vector_A(1));
 
 	/* ground speed, enforce minimum of 0.1 m/s to avoid singularities */
 	float ground_speed = math::max(ground_speed_vector.length() , 0.1f);
@@ -229,10 +229,10 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, cons
 	_L1_distance = _L1_ratio * ground_speed;
 
 	/* calculate the vector from waypoint A to current position */
-	math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
+	math::Vector<2> vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
 
 	/* store the normalized vector from waypoint A to current position */
-	math::Vector2f vector_A_to_airplane_unit = (vector_A_to_airplane).normalized();
+	math::Vector<2> vector_A_to_airplane_unit = (vector_A_to_airplane).normalized();
 
 	/* calculate eta angle towards the loiter center */
 
@@ -287,19 +287,19 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, cons
 		/* angle between requested and current velocity vector */
 		_bearing_error = eta;
 		/* bearing from current position to L1 point */
-		_nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX());
+		_nav_bearing = atan2f(-vector_A_to_airplane_unit(1) , -vector_A_to_airplane_unit(0));
 
 	} else {
 		_lateral_accel = lateral_accel_sp_circle;
 		_circle_mode = true;
 		_bearing_error = 0.0f;
 		/* bearing from current position to L1 point */
-		_nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX());
+		_nav_bearing = atan2f(-vector_A_to_airplane_unit(1) , -vector_A_to_airplane_unit(0));
 	}
 }
 
 
-void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading, const math::Vector2f &ground_speed_vector)
+void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading, const math::Vector<2> &ground_speed_vector)
 {
 	/* the complete guidance logic in this section was proposed by [2] */
 
@@ -352,14 +352,11 @@ void ECL_L1_Pos_Controller::navigate_level_flight(float current_heading)
 }
 
 
-math::Vector2f ECL_L1_Pos_Controller::get_local_planar_vector(const math::Vector2f &origin, const math::Vector2f &target) const
+math::Vector<2> ECL_L1_Pos_Controller::get_local_planar_vector(const math::Vector<2> &origin, const math::Vector<2> &target) const
 {
 	/* this is an approximation for small angles, proposed by [2] */
 
-	math::Vector2f out;
-
-	out.setX(math::radians((target.getX() - origin.getX())));
-	out.setY(math::radians((target.getY() - origin.getY())*cosf(math::radians(origin.getX()))));
+	math::Vector<2> out(math::radians((target(0) - origin(0))), math::radians((target(1) - origin(1))*cosf(math::radians(origin(0)))));
 
 	return out * static_cast<float>(CONSTANTS_RADIUS_OF_EARTH);
 }
diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.h b/src/lib/ecl/l1/ecl_l1_pos_controller.h
index 7a3c42a92..5c0804a39 100644
--- a/src/lib/ecl/l1/ecl_l1_pos_controller.h
+++ b/src/lib/ecl/l1/ecl_l1_pos_controller.h
@@ -160,8 +160,8 @@ public:
 	 *
 	 * @return sets _lateral_accel setpoint
 	 */
-	void navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position,
-			   const math::Vector2f &ground_speed);
+	void navigate_waypoints(const math::Vector<2> &vector_A, const math::Vector<2> &vector_B, const math::Vector<2> &vector_curr_position,
+			   const math::Vector<2> &ground_speed);
 
 
 	/**
@@ -172,8 +172,8 @@ public:
 	 *
 	 * @return sets _lateral_accel setpoint
 	 */
-	void navigate_loiter(const math::Vector2f &vector_A, const math::Vector2f &vector_curr_position, float radius, int8_t loiter_direction,
-			   const math::Vector2f &ground_speed_vector);
+	void navigate_loiter(const math::Vector<2> &vector_A, const math::Vector<2> &vector_curr_position, float radius, int8_t loiter_direction,
+			   const math::Vector<2> &ground_speed_vector);
 
 
 	/**
@@ -185,7 +185,7 @@ public:
 	 *
 	 * @return sets _lateral_accel setpoint
 	 */
-	void navigate_heading(float navigation_heading, float current_heading, const math::Vector2f &ground_speed);
+	void navigate_heading(float navigation_heading, float current_heading, const math::Vector<2> &ground_speed);
 
 
 	/**
@@ -260,7 +260,7 @@ private:
 	 * @param wp The point to convert to into the local coordinates, in WGS84 coordinates
 	 * @return The vector in meters pointing from the reference position to the coordinates
 	 */
-	math::Vector2f get_local_planar_vector(const math::Vector2f &origin, const math::Vector2f &target) const;
+	math::Vector<2> get_local_planar_vector(const math::Vector<2> &origin, const math::Vector<2> &target) const;
 
 };
 
diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp
index a733ef1d2..d7f350e5c 100644
--- a/src/lib/external_lgpl/tecs/tecs.cpp
+++ b/src/lib/external_lgpl/tecs/tecs.cpp
@@ -29,7 +29,7 @@ using namespace math;
  *
  */
 
-void TECS::update_50hz(float baro_altitude, float airspeed, const math::Dcm &rotMat, const math::Vector3 &accel_body, const math::Vector3 &accel_earth)
+void TECS::update_50hz(float baro_altitude, float airspeed, const math::Matrix<3,3> &rotMat, const math::Vector<3> &accel_body, const math::Vector<3> &accel_earth)
 {
 	// Implement third order complementary filter for height and height rate
 	// estimted height rate = _integ2_state
@@ -257,7 +257,7 @@ void TECS::_update_energies(void)
 	_SKEdot = _integ5_state * _vel_dot;
 }
 
-void TECS::_update_throttle(float throttle_cruise, const math::Dcm &rotMat)
+void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotMat)
 {
 	// Calculate total energy values
 	_STE_error = _SPE_dem - _SPE_est + _SKE_dem - _SKE_est;
@@ -468,7 +468,7 @@ void TECS::_update_STE_rate_lim(void)
 	_STEdot_min = - _minSinkRate * CONSTANTS_ONE_G;
 }
 
-void TECS::update_pitch_throttle(const math::Dcm &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO,
+void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO,
 				 float throttle_min, float throttle_max, float throttle_cruise,
 				 float pitch_limit_min, float pitch_limit_max)
 {
diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h
index f8f832ed7..68832a039 100644
--- a/src/lib/external_lgpl/tecs/tecs.h
+++ b/src/lib/external_lgpl/tecs/tecs.h
@@ -71,10 +71,10 @@ public:
 	// Update of the estimated height and height rate internal state
 	// Update of the inertial speed rate internal state
 	// Should be called at 50Hz or greater
-	void update_50hz(float baro_altitude, float airspeed, const math::Dcm &rotMat, const math::Vector3 &accel_body, const math::Vector3 &accel_earth);
+	void update_50hz(float baro_altitude, float airspeed, const math::Matrix<3,3> &rotMat, const math::Vector<3> &accel_body, const math::Vector<3> &accel_earth);
 
 	// Update the control loop calculations
-	void update_pitch_throttle(const math::Dcm &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO,
+	void update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO,
 				   float throttle_min, float throttle_max, float throttle_cruise,
 				   float pitch_limit_min, float pitch_limit_max);
 	// demanded throttle in percentage
@@ -338,7 +338,7 @@ private:
 	void _update_energies(void);
 
 	// Update Demanded Throttle
-	void _update_throttle(float throttle_cruise, const math::Dcm &rotMat);
+	void _update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotMat);
 
 	// Detect Bad Descent
 	void _detect_bad_descent(void);
diff --git a/src/lib/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp
index 1849f58be..67214133a 100644
--- a/src/lib/mathlib/math/Matrix.hpp
+++ b/src/lib/mathlib/math/Matrix.hpp
@@ -42,16 +42,19 @@
 #ifndef MATRIX_HPP
 #define MATRIX_HPP
 
+#include <stdio.h>
 #include "../CMSIS/Include/arm_math.h"
 
 namespace math
 {
 
-template <unsigned int N, unsigned int M>
+template <unsigned int M, unsigned int N>
 class Matrix;
 
+class Quaternion;
+
 // MxN matrix with float elements
-template <unsigned int N, unsigned int M>
+template <unsigned int M, unsigned int N>
 class MatrixBase {
 public:
 	/**
@@ -69,10 +72,14 @@ public:
 	 * note that this ctor will not initialize elements
 	 */
 	MatrixBase() {
-		//arm_mat_init_f32(&arm_mat, M, N, data);
 		arm_mat = {M, N, &data[0][0]};
 	}
 
+	MatrixBase(const float *d) {
+		arm_mat = {M, N, &data[0][0]};
+		memcpy(data, d, sizeof(data));
+	}
+
 	/**
 	 * access by index
 	 */
@@ -98,10 +105,10 @@ public:
 	/**
 	 * test for equality
 	 */
-	bool operator ==(const MatrixBase<M, N> &m) {
+	bool operator ==(const Matrix<M, N> &m) {
  		for (unsigned int i = 0; i < M; i++)
  			for (unsigned int j = 0; j < N; j++)
- 				if (data[i][j] != m(i, j))
+ 				if (data[i][j] != m.data[i][j])
  					return false;
 		return true;
 	}
@@ -109,10 +116,10 @@ public:
 	/**
 	 * test for inequality
 	 */
-	bool operator !=(const MatrixBase<M, N> &m) {
+	bool operator !=(const Matrix<M, N> &m) {
  		for (unsigned int i = 0; i < M; i++)
  			for (unsigned int j = 0; j < N; j++)
- 				if (data[i][j] != m(i, j))
+ 				if (data[i][j] != m.data[i][j])
  					return true;
 		return false;
 	}
@@ -120,85 +127,97 @@ public:
 	/**
 	 * set to value
 	 */
-	const MatrixBase<M, N> &operator =(const MatrixBase<M, N> &m) {
+	const Matrix<M, N> &operator =(const Matrix<M, N> &m) {
 		memcpy(data, m.data, sizeof(data));
-		return *this;
+		return *reinterpret_cast<Matrix<M, N>*>(this);
 	}
 
 	/**
 	 * negation
 	 */
-	MatrixBase<M, N> operator -(void) const {
-		MatrixBase<M, N> res;
+	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[i][j] = -data[i][j];
+ 				res.data[i][j] = -data[i][j];
 		return res;
 	}
 
 	/**
 	 * addition
 	 */
-	MatrixBase<M, N> operator +(const MatrixBase<M, N> &m) const {
-		MatrixBase<M, N> res;
+	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[i][j] = data[i][j] + m(i, j);
+ 				res.data[i][j] = data[i][j] + m.data[i][j];
 		return res;
 	}
 
-	MatrixBase<M, N> &operator +=(const MatrixBase<M, N> &m) {
-		return *this = *this + m;
+	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 *reinterpret_cast<Matrix<M, N>*>(this);
 	}
 
 	/**
 	 * subtraction
 	 */
-	MatrixBase<M, N> operator -(const MatrixBase<M, N> &m) const {
-		MatrixBase<M, N> res;
+	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[i][j] = data[i][j] - m(i, j);
+ 				res.data[i][j] = data[i][j] - m.data[i][j];
 		return res;
 	}
 
-	MatrixBase<M, N> &operator -=(const MatrixBase<M, N> &m) {
-		return *this = *this - m;
+	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 *reinterpret_cast<Matrix<M, N>*>(this);
 	}
 
 	/**
 	 * uniform scaling
 	 */
-	MatrixBase<M, N> operator *(const float num) const {
-		MatrixBase<M, N> res;
+	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;
+ 				res.data[i][j] = data[i][j] * num;
 		return res;
 	}
 
-	MatrixBase<M, N> &operator *=(const float num) {
-		return *this = *this * num;
+	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 *reinterpret_cast<Matrix<M, N>*>(this);
 	}
 
-	MatrixBase<M, N> operator /(const float num) const {
-		MatrixBase<M, N> res;
+	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;
 	}
 
-	MatrixBase<M, N> &operator /=(const float num) {
-		return *this = *this / num;
+	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 *this;
 	}
 
 	/**
 	 * multiplication by another matrix
 	 */
 	template <unsigned int P>
-	Matrix<N, P> operator *(const Matrix<M, P> &m) const {
-		Matrix<N, P> res;
+	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;
 	}
@@ -230,7 +249,7 @@ public:
 			data[i][i] = 1;
 	}
 
-	void dump(void) {
+	void print(void) {
  		for (unsigned int i = 0; i < M; i++) {
  			for (unsigned int j = 0; j < N; j++)
 				printf("%.3f\t", data[i][j]);
@@ -244,6 +263,12 @@ class Matrix : public MatrixBase<M, N> {
 public:
 	using MatrixBase<M, N>::operator *;
 
+	Matrix() : MatrixBase<M, N>() {
+	}
+
+	Matrix(const float *d) : MatrixBase<M, N>(d) {
+	}
+
 	/**
 	 * set to value
 	 */
@@ -255,18 +280,28 @@ public:
 	/**
 	 * multiplication by a vector
 	 */
-	Vector<N> operator *(const Vector<N> &v) const {
+	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;
 	}
 };
+}
+
+#include "Quaternion.hpp"
 
+namespace math {
 template <>
 class Matrix<3, 3> : public MatrixBase<3, 3> {
 public:
 	using MatrixBase<3, 3>::operator *;
 
+	Matrix() : MatrixBase<3, 3>() {
+	}
+
+	Matrix(const float *d) : MatrixBase<3, 3>(d) {
+	}
+
 	/**
 	 * set to value
 	 */
@@ -279,9 +314,70 @@ public:
 	 * multiplication by a vector
 	 */
 	Vector<3> operator *(const Vector<3> &v) const {
-	    return Vector<3>(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]);
+	    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 quaternion
+	 */
+	void from_quaternion(const Quaternion &q) {
+		float aSq = q.data[0] * q.data[0];
+		float bSq = q.data[1] * q.data[1];
+		float cSq = q.data[2] * q.data[2];
+		float dSq = q.data[3] * q.data[3];
+		data[0][0] = aSq + bSq - cSq - dSq;
+		data[0][1] = 2.0f * (q.data[1] * q.data[2] - q.data[0] * q.data[3]);
+		data[0][2] = 2.0f * (q.data[0] * q.data[2] + q.data[1] * q.data[3]);
+		data[1][0] = 2.0f * (q.data[1] * q.data[2] + q.data[0] * q.data[3]);
+		data[1][1] = aSq - bSq + cSq - dSq;
+		data[1][2] = 2.0f * (q.data[2] * q.data[3] - q.data[0] * q.data[1]);
+		data[2][0] = 2.0f * (q.data[1] * q.data[3] - q.data[0] * q.data[2]);
+		data[2][1] = 2.0f * (q.data[0] * q.data[1] + q.data[2] * q.data[3]);
+		data[2][2] = aSq - bSq - cSq + dSq;
+	}
+
+	/**
+	 * 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;
+	}
+
+	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]);
+		}
 	}
 };
 
diff --git a/src/lib/mathlib/math/Matrix3.cpp b/src/lib/mathlib/math/Matrix3.cpp
deleted file mode 100644
index 3f1259dc4..000000000
--- a/src/lib/mathlib/math/Matrix3.cpp
+++ /dev/null
@@ -1,46 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Will Perone <will.perone@gmail.com>
- *           Anton Babushkin <anton.babushkin@me.com>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file Matrix3.cpp
- *
- * 3x3 Matrix
- */
-
-#include "Matrix3.hpp"
-
-namespace math
-{
-}
diff --git a/src/lib/mathlib/math/Matrix3.hpp b/src/lib/mathlib/math/Matrix3.hpp
deleted file mode 100644
index c7eb67ba7..000000000
--- a/src/lib/mathlib/math/Matrix3.hpp
+++ /dev/null
@@ -1,356 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Will Perone <will.perone@gmail.com>
- *           Anton Babushkin <anton.babushkin@me.com>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file Matrix3.hpp
- *
- * 3x3 Matrix
- */
-
-#ifndef MATRIX3_HPP
-#define MATRIX3_HPP
-
-#include "Vector3.hpp"
-#include "../CMSIS/Include/arm_math.h"
-
-namespace math
-{
-
-// 3x3 matrix with elements of type T
-template <typename T>
-class Matrix3 {
-public:
-	/**
-	 * matrix data[row][col]
-	 */
-	T data[3][3];
-
-	/**
-	 * struct for using arm_math functions
-	 */
-	arm_matrix_instance_f32 arm_mat;
-
-	/**
-	 * trivial ctor
-	 * note that this ctor will not initialize elements
-	 */
-	Matrix3<T>() {
-		arm_mat = {3, 3, &data[0][0]};
-	}
-
-	/**
-	 * setting ctor
-	 */
-	Matrix3<T>(const T d[3][3]) {
-		arm_mat = {3, 3, &data[0][0]};
-		memcpy(data, d, sizeof(data));
-	}
-
-	/**
-	 * setting ctor
-	 */
-	Matrix3<T>(const T ax, const T ay, const T az, const T bx, const T by, const T bz, const T cx, const T cy, const T cz) {
-		arm_mat = {3, 3, &data[0][0]};
-		data[0][0] = ax;
-		data[0][1] = ay;
-		data[0][2] = az;
-		data[1][0] = bx;
-		data[1][1] = by;
-		data[1][2] = bz;
-		data[2][0] = cx;
-		data[2][1] = cy;
-		data[2][2] = cz;
-	}
-
-	/**
-	 * casting from a vector3f to a matrix is the tilde operator
-	 */
-	Matrix3<T>(const Vector3<T> &v) {
-		arm_mat = {3, 3, &data[0][0]};
-		data[0][0] = 0;
-		data[0][1] = -v.z;
-		data[0][2] = v.y;
-		data[1][0] = v.z;
-		data[1][1] = 0;
-		data[1][2] = -v.x;
-		data[2][0] = -v.y;
-		data[2][1] = v.x;
-		data[2][2] = 0;
-	}
-
-	/**
-	 * access by index
-	 */
-	inline T &operator ()(unsigned int row, unsigned int col) {
-		return data[row][col];
-	}
-
-	/**
-	 * access to elements by index
-	 */
-	inline const T &operator ()(unsigned int row, unsigned int col) const {
-		return data[row][col];
-	}
-
-	/**
-	 * set to value
-	 */
-	const Matrix3<T> &operator =(const Matrix3<T> &m) {
-		memcpy(data, m.data, sizeof(data));
-		return *this;
-	}
-
-	/**
-	 * test for equality
-	 */
-	bool operator ==(const Matrix3<T> &m) {
- 		for (int i = 0; i < 3; i++)
- 			for (int j = 0; j < 3; j++)
- 				if (data[i][j] != m(i, j))
- 					return false;
-		return true;
-	}
-
-	/**
-	 * test for inequality
-	 */
-	bool operator !=(const Matrix3<T> &m) {
- 		for (int i = 0; i < 3; i++)
- 			for (int j = 0; j < 3; j++)
- 				if (data[i][j] != m(i, j))
- 					return true;
-		return false;
-	}
-
-	/**
-	 * negation
-	 */
-	Matrix3<T> operator -(void) const {
-		Matrix3<T> res;
- 		for (int i = 0; i < 3; i++)
- 			for (int j = 0; j < 3; j++)
- 				res[i][j] = -data[i][j];
-		return res;
-	}
-
-	/**
-	 * addition
-	 */
-	Matrix3<T> operator +(const Matrix3<T> &m) const {
-		Matrix3<T> res;
- 		for (int i = 0; i < 3; i++)
- 			for (int j = 0; j < 3; j++)
- 				res[i][j] = data[i][j] + m(i, j);
-		return res;
-	}
-
-	Matrix3<T> &operator +=(const Matrix3<T> &m) {
-		return *this = *this + m;
-	}
-
-	/**
-	 * subtraction
-	 */
-	Matrix3<T> operator -(const Matrix3<T> &m) const {
-		Matrix3<T> res;
- 		for (int i = 0; i < 3; i++)
- 			for (int j = 0; j < 3; j++)
- 				res[i][j] = data[i][j] - m(i, j);
-		return res;
-	}
-
-	Matrix3<T> &operator -=(const Matrix3<T> &m) {
-		return *this = *this - m;
-	}
-
-	/**
-	 * uniform scaling
-	 */
-	Matrix3<T> operator *(const T num) const {
-		Matrix3<T> res;
- 		for (int i = 0; i < 3; i++)
- 			for (int j = 0; j < 3; j++)
- 				res[i][j] = data[i][j] * num;
-		return res;
-	}
-
-	Matrix3<T> &operator *=(const T num) {
-		return *this = *this * num;
-	}
-
-	Matrix3<T> operator /(const T num) const {
-		Matrix3<T> res;
- 		for (int i = 0; i < 3; i++)
- 			for (int j = 0; j < 3; j++)
- 				res[i][j] = data[i][j] / num;
-		return res;
-	}
-
-	Matrix3<T> &operator /=(const T num) {
-		return *this = *this / num;
-	}
-
-	/**
-	 * multiplication by a vector
-	 */
-	Vector3<T> operator *(const Vector3<T> &v) const {
-	    return Vector3<T>(
-	    		data[0][0] * v.x + data[0][1] * v.y + data[0][2] * v.z,
-	    		data[1][0] * v.x + data[1][1] * v.y + data[1][2] * v.z,
-	    		data[2][0] * v.x + data[2][1] * v.y + data[2][2] * v.z);
-	}
-
-	/**
-	 * multiplication of transpose by a vector
-	 */
-	Vector3<T> mul_transpose(const Vector3<T> &v) const {
-	    return Vector3<T>(
-	    		data[0][0] * v.x + data[1][0] * v.y + data[2][0] * v.z,
-	    		data[0][1] * v.x + data[1][1] * v.y + data[2][1] * v.z,
-	    		data[0][2] * v.x + data[1][2] * v.y + data[2][2] * v.z);
-	}
-
-	/**
-	 * multiplication by another matrix
-	 */
-	Matrix3<T> operator *(const Matrix3<T> &m) const {
-#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU)
-		Matrix3<T> res;
-		arm_mat_mult_f32(&arm_mat, &m.arm_mat, &res.arm_mat);
-		return res;
-#else
-	    return Matrix3<T>(data[0][0] * m(0, 0) + data[0][1] * m(1, 0) + data[0][2] * m(2, 0),
-	                     data[0][0] * m(0, 1) + data[0][1] * m(1, 1) + data[0][2] * m(2, 1),
-	                     data[0][0] * m(0, 2) + data[0][1] * m(1, 2) + data[0][2] * m(2, 2),
-	                     data[1][0] * m(0, 0) + data[1][1] * m(1, 0) + data[1][2] * m(2, 0),
-	                     data[1][0] * m(0, 1) + data[1][1] * m(1, 1) + data[1][2] * m(2, 1),
-	                     data[1][0] * m(0, 2) + data[1][1] * m(1, 2) + data[1][2] * m(2, 2),
-	                     data[2][0] * m(0, 0) + data[2][1] * m(1, 0) + data[2][2] * m(2, 0),
-	                     data[2][0] * m(0, 1) + data[2][1] * m(1, 1) + data[2][2] * m(2, 1),
-	                     data[2][0] * m(0, 2) + data[2][1] * m(1, 2) + data[2][2] * m(2, 2));
-#endif
-	}
-
-	Matrix3<T> &operator *=(const Matrix3<T> &m) {
-		return *this = *this * m;
-	}
-
-	/**
-	 * transpose the matrix
-	 */
-	Matrix3<T> transposed(void) const {
-#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU) && T == float
-        Matrix3<T> res;
-        arm_mat_trans_f32(&arm_mat, &res.arm_mat);
-        return res;
-#else
-		return Matrix3<T>(data[0][0], data[1][0], data[2][0],
-						  data[0][1], data[1][1], data[2][1],
-						  data[0][2], data[1][2], data[2][2]);
-#endif
-	}
-
-	/**
-	 * inverse the matrix
-	 */
-	Matrix3<T> inversed(void) const {
-        Matrix3<T> res;
-        arm_mat_inverse_f32(&arm_mat, &res.arm_mat);
-        return res;
-	}
-
-	/**
-	 * zero the matrix
-	 */
-	void zero(void) {
-		memset(data, 0, sizeof(data));
-	}
-
-	/**
-	 * setup the identity matrix
-	 */
-	void identity(void) {
-		memset(data, 0, sizeof(data));
-		data[0][0] = 1;
-		data[1][1] = 1;
-		data[2][2] = 1;
-	}
-
-	/**
-	 * check if any elements are NAN
-	 */
- 	bool is_nan(void) {
- 		for (int i = 0; i < 3; i++)
- 			for (int j = 0; j < 3; j++)
- 				if (isnan(data[i][j]))
- 					return true;
- 		return false;
-	}
-
-	/**
-	 * create a rotation matrix from given euler angles
-	 * based on http://gentlenav.googlecode.com/files/EulerAngles.pdf
-	 */
-	void from_euler(T roll, T pitch, T yaw) {
-		T cp = (T)cosf(pitch);
-		T sp = (T)sinf(pitch);
-		T sr = (T)sinf(roll);
-		T cr = (T)cosf(roll);
-		T sy = (T)sinf(yaw);
-		T cy = (T)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;
-	}
-
-    // create eulers from a rotation matrix
-	//void to_euler(float *roll, float *pitch, float *yaw);
-
-    // apply an additional rotation from a body frame gyro vector
-    // to a rotation matrix.
-	//void rotate(const Vector3<T> &g);
-};
-
-typedef Matrix3<float> Matrix3f;
-}
-
-#endif // MATRIX3_HPP
diff --git a/src/lib/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp
index 64b980ad8..1b0cb3c41 100644
--- a/src/lib/mathlib/math/Quaternion.hpp
+++ b/src/lib/mathlib/math/Quaternion.hpp
@@ -44,15 +44,17 @@
 
 #include <math.h>
 #include "../CMSIS/Include/arm_math.h"
+#include "Vector.hpp"
+#include "Matrix.hpp"
 
 namespace math
 {
 
-class Quaternion {
-public:
-	float real;
-	Vector3<float> imag;
+template <unsigned int N, unsigned int M>
+class Matrix;
 
+class Quaternion : public Vector<4> {
+public:
 	/**
 	 * trivial ctor
 	 */
@@ -62,7 +64,74 @@ public:
 	/**
 	 * setting ctor
 	 */
-	Quaternion(const float a, const float b, const float c, const float d): real(a), imag(b, c, d) {
+	Quaternion(const float a0, const float b0, const float c0, const float d0): Vector(a0, b0, c0, d0) {
+	}
+
+	Quaternion(const Vector<4> &v) : Vector(v) {
+	}
+
+	Quaternion(const float *v) : Vector(v) {
+	}
+
+	/**
+	 * access to elements by index
+	 */
+	/*
+	inline float &operator ()(unsigned int i) {
+		return *(&a + i);
+	}
+	*/
+
+	/**
+	 * access to elements by index
+	 */
+	/*
+	inline const float &operator ()(unsigned int i) const {
+		return *(&a + i);
+	}
+	*/
+
+	/**
+	 * addition
+	 */
+	/*
+	const Quaternion operator +(const Quaternion &q) const {
+		return Quaternion(a + q.a, b + q.b, c + q.c, d + q.d);
+	}
+	*/
+
+	/**
+	 * subtraction
+	 */
+	/*
+	const Quaternion operator -(const Quaternion &q) const {
+		return Quaternion(a - q.a, b - q.b, c - q.c, d - q.d);
+	}
+	*/
+
+	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;
+	}
+
+	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;
 	}
 };
 }
diff --git a/src/lib/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp
index cd00058b5..2df87c74b 100644
--- a/src/lib/mathlib/math/Vector.hpp
+++ b/src/lib/mathlib/math/Vector.hpp
@@ -42,6 +42,7 @@
 #ifndef VECTOR_HPP
 #define VECTOR_HPP
 
+#include <stdio.h>
 #include <math.h>
 #include "../CMSIS/Include/arm_math.h"
 
@@ -49,31 +50,42 @@ namespace math
 {
 
 template <unsigned int N>
+class Vector;
+
+template <unsigned int N>
 class VectorBase {
 public:
+	/**
+	 * vector data
+	 */
 	float data[N];
+
+	/**
+	 * struct for using arm_math functions, represents column vector
+	 */
 	arm_matrix_instance_f32 arm_col;
 
 	/**
 	 * trivial ctor
 	 */
-	VectorBase<N>() {
+	VectorBase() {
 		arm_col = {N, 1, &data[0]};
 	}
 
 	/**
-	 * setting ctor
+	 * copy ctor
 	 */
-//	VectorBase<N>(const float *d) {
-	//	memcpy(data, d, sizeof(data));
-		//arm_col = {N, 1, &data[0]};
-	//}
+	VectorBase(const VectorBase<N> &v) {
+		arm_col = {N, 1, &data[0]};
+		memcpy(data, v.data, sizeof(data));
+	}
 
 	/**
 	 * setting ctor
 	 */
-	VectorBase<N>(const float d[]) : data(d) {
+	VectorBase(const float *d) {
 		arm_col = {N, 1, &data[0]};
+		memcpy(data, d, sizeof(data));
 	}
 
 	/**
@@ -90,10 +102,18 @@ public:
 		return data[i];
 	}
 
+	unsigned int getRows() {
+		return N;
+	}
+
+	unsigned int getCols() {
+		return 1;
+	}
+
 	/**
 	 * test for equality
 	 */
-	bool operator ==(const VectorBase<N> &v) {
+	bool operator ==(const Vector<N> &v) {
  		for (unsigned int i = 0; i < N; i++)
  			if (data[i] != v(i))
  				return false;
@@ -103,7 +123,7 @@ public:
 	/**
 	 * test for inequality
 	 */
-	bool operator !=(const VectorBase<N> &v) {
+	bool operator !=(const Vector<N> &v) {
  		for (unsigned int i = 0; i < N; i++)
  			if (data[i] != v(i))
  				return true;
@@ -113,98 +133,98 @@ public:
 	/**
 	 * set to value
 	 */
-	const VectorBase<N> &operator =(const VectorBase<N> &v) {
+	const Vector<N> &operator =(const Vector<N> &v) {
 		memcpy(data, v.data, sizeof(data));
-		return *this;
+		return *reinterpret_cast<const Vector<N>*>(this);
 	}
 
 	/**
 	 * negation
 	 */
-	const VectorBase<N> operator -(void) const {
-		VectorBase<N> res;
+	const Vector<N> operator -(void) const {
+		Vector<N> res;
  		for (unsigned int i = 0; i < N; i++)
- 			res[i] = -data[i];
+ 			res.data[i] = -data[i];
  		return res;
 	}
 
 	/**
 	 * addition
 	 */
-	const VectorBase<N> operator +(const VectorBase<N> &v) const {
-		VectorBase<N> res;
+	const Vector<N> operator +(const Vector<N> &v) const {
+		Vector<N> res;
  		for (unsigned int i = 0; i < N; i++)
- 			res[i] = data[i] + v(i);
+ 			res.data[i] = data[i] + v(i);
  		return res;
 	}
 
 	/**
 	 * subtraction
 	 */
-	const VectorBase<N> operator -(const VectorBase<N> &v) const {
-		VectorBase<N> res;
+	const Vector<N> operator -(const Vector<N> &v) const {
+		Vector<N> res;
  		for (unsigned int i = 0; i < N; i++)
- 			res[i] = data[i] - v(i);
+ 			res.data[i] = data[i] - v(i);
  		return res;
 	}
 
 	/**
 	 * uniform scaling
 	 */
-	const VectorBase<N> operator *(const float num) const {
-		VectorBase<N> temp(*this);
+	const Vector<N> operator *(const float num) const {
+		Vector<N> temp(*this);
 		return temp *= num;
 	}
 
 	/**
 	 * uniform scaling
 	 */
-	const VectorBase<N> operator /(const float num) const {
-		VectorBase<N> temp(*this);
+	const Vector<N> operator /(const float num) const {
+		Vector<N> temp(*reinterpret_cast<const Vector<N>*>(this));
 		return temp /= num;
 	}
 
 	/**
 	 * addition
 	 */
-	const VectorBase<N> &operator +=(const VectorBase<N> &v) {
+	const Vector<N> &operator +=(const Vector<N> &v) {
  		for (unsigned int i = 0; i < N; i++)
  			data[i] += v(i);
-		return *this;
+		return *reinterpret_cast<const Vector<N>*>(this);
 	}
 
 	/**
 	 * subtraction
 	 */
-	const VectorBase<N> &operator -=(const VectorBase<N> &v) {
+	const Vector<N> &operator -=(const Vector<N> &v) {
  		for (unsigned int i = 0; i < N; i++)
  			data[i] -= v(i);
-		return *this;
+		return *reinterpret_cast<const Vector<N>*>(this);
 	}
 
 	/**
 	 * uniform scaling
 	 */
-	const VectorBase<N> &operator *=(const float num) {
+	const Vector<N> &operator *=(const float num) {
  		for (unsigned int i = 0; i < N; i++)
  			data[i] *= num;
-		return *this;
+		return *reinterpret_cast<const Vector<N>*>(this);
 	}
 
 	/**
 	 * uniform scaling
 	 */
-	const VectorBase<N> &operator /=(const float num) {
+	const Vector<N> &operator /=(const float num) {
  		for (unsigned int i = 0; i < N; i++)
  			data[i] /= num;
-		return *this;
+		return *reinterpret_cast<const Vector<N>*>(this);
 	}
 
 	/**
 	 * dot product
 	 */
-	float operator *(const VectorBase<N> &v) const {
-		float res;
+	float operator *(const Vector<N> &v) const {
+		float res = 0.0f;
  		for (unsigned int i = 0; i < N; i++)
  			res += data[i] * v(i);
  		return res;
@@ -221,7 +241,7 @@ public:
 	 * gets the length of this vector
 	 */
 	float length() const {
-		return sqrtf(*this * *this);
+		return sqrtf(*this * *reinterpret_cast<const Vector<N>*>(this));
 	}
 
 	/**
@@ -234,46 +254,174 @@ public:
 	/**
 	 * returns the normalized version of this vector
 	 */
-	VectorBase<N> normalized() const {
+	Vector<N> normalized() const {
 		return *this / length();
 	}
+
+	void print(void) {
+		printf("[ ");
+ 		for (unsigned int i = 0; i < N; i++)
+			printf("%.3f\t", data[i]);
+		printf("]\n");
+	}
 };
 
 template <unsigned int N>
 class Vector : public VectorBase<N> {
 public:
+	using VectorBase<N>::operator *;
+
+	Vector() : VectorBase<N>() {
+	}
+
+	Vector(const float d[]) : VectorBase<N>(d) {
+	}
+
+	Vector(const Vector<N> &v) : VectorBase<N>(v) {
+	}
+
+	Vector(const VectorBase<N> &v) : VectorBase<N>(v) {
+	}
+
 	/**
 	 * set to value
 	 */
 	const Vector<N> &operator =(const Vector<N> &v) {
+		this->arm_col = {N, 1, &this->data[0]};
 		memcpy(this->data, v.data, sizeof(this->data));
 		return *this;
 	}
 };
 
 template <>
+class Vector<2> : public VectorBase<2> {
+public:
+	Vector() : VectorBase<2>() {
+	}
+
+	Vector(const float x, const float y) : VectorBase() {
+		data[0] = x;
+		data[1] = y;
+	}
+
+	Vector(const Vector<2> &v) : VectorBase() {
+		data[0] = v.data[0];
+		data[1] = v.data[1];
+	}
+
+	Vector(const VectorBase<2> &v) : VectorBase() {
+		data[0] = v.data[0];
+		data[1] = v.data[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 cross(const Vector<2> &b) const {
+	        return data[0]*b.data[1] - data[1]*b.data[0];
+	}
+
+	float operator %(const Vector<2> &v) const {
+	        return cross(v);
+	}
+
+};
+
+template <>
 class Vector<3> : public VectorBase<3> {
 public:
-	Vector<3>() {
+	Vector() {
 		arm_col = {3, 1, &this->data[0]};
 	}
 
-	Vector<3>(const float x, const float y, const float z) {
+	Vector(const float x, const float y, const float z) {
+		arm_col = {3, 1, &this->data[0]};
 		data[0] = x;
 		data[1] = y;
 		data[2] = z;
+	}
+
+	Vector(const Vector<3> &v) : VectorBase<3>() {
+		data[0] = v.data[0];
+		data[1] = v.data[1];
+		data[2] = v.data[2];
+	}
+
+	/**
+	 * setting ctor
+	 */
+	Vector(const float d[]) {
 		arm_col = {3, 1, &this->data[0]};
+		data[0] = d[0];
+		data[1] = d[1];
+		data[2] = d[2];
+	}
+
+	/**
+	 * set to value
+	 */
+	const Vector<3> &operator =(const Vector<3> &v) {
+		data[0] = v.data[0];
+		data[1] = v.data[1];
+		data[2] = v.data[2];
+		return *this;
+	}
+};
+
+template <>
+class Vector<4> : public VectorBase<4> {
+public:
+	Vector() : VectorBase() {}
+
+	Vector(const float x, const float y, const float z, const float t) : VectorBase() {
+		data[0] = x;
+		data[1] = y;
+		data[2] = z;
+		data[3] = t;
 	}
 
+	Vector(const Vector<4> &v) : VectorBase() {
+		data[0] = v.data[0];
+		data[1] = v.data[1];
+		data[2] = v.data[2];
+		data[3] = v.data[3];
+	}
+
+	Vector(const VectorBase<4> &v) : VectorBase() {
+		data[0] = v.data[0];
+		data[1] = v.data[1];
+		data[2] = v.data[2];
+		data[3] = v.data[3];
+	}
+
+	/**
+	 * setting ctor
+	 */
+	/*
+	Vector(const float d[]) {
+		arm_col = {3, 1, &this->data[0]};
+		data[0] = d[0];
+		data[1] = d[1];
+		data[2] = d[2];
+	}
+*/
 	/**
 	 * set to value
 	 */
+	/*
 	const Vector<3> &operator =(const Vector<3> &v) {
 		data[0] = v.data[0];
 		data[1] = v.data[1];
 		data[2] = v.data[2];
 		return *this;
 	}
+	*/
 };
 
 }
diff --git a/src/lib/mathlib/math/Vector2.hpp b/src/lib/mathlib/math/Vector2.hpp
deleted file mode 100644
index 501740614..000000000
--- a/src/lib/mathlib/math/Vector2.hpp
+++ /dev/null
@@ -1,269 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Will Perone <will.perone@gmail.com>
- *           Anton Babushkin <anton.babushkin@me.com>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file Vector2.hpp
- *
- * 2D Vector
- */
-
-#ifndef VECTOR2_HPP
-#define VECTOR2_HPP
-
-#include <math.h>
-
-namespace math
-{
-
-template <typename T>
-class Vector2 {
-public:
-	T x, y;
-
-	/**
-	 * trivial ctor
-	 */
-	Vector2<T>() {
-	}
-
-	/**
-	 * setting ctor
-	 */
-	Vector2<T>(const T x0, const T y0): x(x0), y(y0) {
-	}
-
-	/**
-	 * setter
-	 */
-	void set(const T x0, const T y0) {
-		x = x0;
-		y = y0;
-	}
-
-	/**
-	 * access to elements by index
-	 */
-	T operator ()(unsigned int i) {
-		return *(&x + i);
-	}
-
-	/**
-	 * access to elements by index
-	 */
-	const T operator ()(unsigned int i) const {
-		return *(&x + i);
-	}
-
-	/**
-	 * test for equality
-	 */
-	bool operator ==(const Vector2<T> &v) {
-		return (x == v.x && y == v.y);
-	}
-
-	/**
-	 * test for inequality
-	 */
-	bool operator !=(const Vector2<T> &v) {
-		return (x != v.x || y != v.y);
-	}
-
-	/**
-	 * set to value
-	 */
-	const Vector2<T> &operator =(const Vector2<T> &v) {
-		x = v.x;
-		y = v.y;
-		return *this;
-	}
-
-	/**
-	 * negation
-	 */
-	const Vector2<T> operator -(void) const {
-		return Vector2<T>(-x, -y);
-	}
-
-	/**
-	 * addition
-	 */
-	const Vector2<T> operator +(const Vector2<T> &v) const {
-		return Vector2<T>(x + v.x, y + v.y);
-	}
-
-	/**
-	 * subtraction
-	 */
-	const Vector2<T> operator -(const Vector2<T> &v) const {
-		return Vector2<T>(x - v.x, y - v.y);
-	}
-
-	/**
-	 * uniform scaling
-	 */
-	const Vector2<T> operator *(const T num) const {
-		Vector2<T> temp(*this);
-		return temp *= num;
-	}
-
-	/**
-	 * uniform scaling
-	 */
-	const Vector2<T> operator /(const T num) const {
-		Vector2<T> temp(*this);
-		return temp /= num;
-	}
-
-	/**
-	 * addition
-	 */
-	const Vector2<T> &operator +=(const Vector2<T> &v) {
-		x += v.x;
-		y += v.y;
-		return *this;
-	}
-
-	/**
-	 * subtraction
-	 */
-	const Vector2<T> &operator -=(const Vector2<T> &v) {
-		x -= v.x;
-		y -= v.y;
-		return *this;
-	}
-
-	/**
-	 * uniform scaling
-	 */
-	const Vector2<T> &operator *=(const T num) {
-		x *= num;
-		y *= num;
-		return *this;
-	}
-
-	/**
-	 * uniform scaling
-	 */
-	const Vector2<T> &operator /=(const T num) {
-		x /= num;
-		y /= num;
-		return *this;
-	}
-
-	/**
-	 * dot product
-	 */
-	T operator *(const Vector2<T> &v) const {
-		return x * v.x + y * v.y;
-	}
-
-	/**
-	 * cross product
-	 */
-	const float operator %(const Vector2<T> &v) const {
-		return x * v.y - y * v.x;
-	}
-
-	/**
-	 * gets the length of this vector squared
-	 */
-	float length_squared() const {
-		return (*this * *this);
-	}
-
-	/**
-	 * gets the length of this vector
-	 */
-	float length() const {
-		return (T)sqrt(*this * *this);
-	}
-
-	/**
-	 * normalizes this vector
-	 */
-	void normalize() {
-		*this /= length();
-	}
-
-	/**
-	 * returns the normalized version of this vector
-	 */
-	Vector2<T> normalized() const {
-		return *this / length();
-	}
-
-	/**
-	 * reflects this vector about n
-	 */
-	void reflect(const Vector2<T> &n)
-	{
-		Vector2<T> orig(*this);
-		project(n);
-		*this = *this * 2 - orig;
-	}
-
-	/**
-	 * projects this vector onto v
-	 */
-	void project(const Vector2<T> &v) {
-		*this = v * (*this * v) / (v * v);
-	}
-
-	/**
-	 * returns this vector projected onto v
-	 */
-	Vector2<T> projected(const Vector2<T> &v) {
-		return v * (*this * v) / (v * v);
-	}
-
-	/**
-	 * computes the angle between 2 arbitrary vectors
-	 */
-	static inline float angle(const Vector2<T> &v1, const Vector2<T> &v2) {
-		return acosf((v1 * v2) / (v1.length() * v2.length()));
-	}
-
-	/**
-	 * computes the angle between 2 arbitrary normalized vectors
-	 */
-	static inline float angle_normalized(const Vector2<T> &v1, const Vector2<T> &v2) {
-		return acosf(v1 * v2);
-	}
-};
-
-typedef Vector2<float> Vector2f;
-}
-
-#endif // VECTOR2_HPP
diff --git a/src/lib/mathlib/math/Vector3.hpp b/src/lib/mathlib/math/Vector3.hpp
deleted file mode 100644
index a0c3b9290..000000000
--- a/src/lib/mathlib/math/Vector3.hpp
+++ /dev/null
@@ -1,287 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Will Perone <will.perone@gmail.com>
- *           Anton Babushkin <anton.babushkin@me.com>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file Vector3.hpp
- *
- * 3D Vector
- */
-
-#ifndef VECTOR3_HPP
-#define VECTOR3_HPP
-
-#include <math.h>
-#include "../CMSIS/Include/arm_math.h"
-
-namespace math
-{
-
-template <typename T>
-class Vector3 {
-public:
-	T x, y, z;
-	arm_matrix_instance_f32 arm_col;
-
-	/**
-	 * trivial ctor
-	 */
-	Vector3<T>() {
-		arm_col = {3, 1, &x};
-	}
-
-	/**
-	 * setting ctor
-	 */
-	Vector3<T>(const T x0, const T y0,  const T z0): x(x0), y(y0), z(z0) {
-		arm_col = {3, 1, &x};
-	}
-
-	/**
-	 * setting ctor
-	 */
-	Vector3<T>(const T data[3]): x(data[0]), y(data[1]), z(data[2]) {
-		arm_col = {3, 1, &x};
-	}
-
-	/**
-	 * setter
-	 */
-	void set(const T x0, const T y0, const T z0) {
-		x = x0;
-		y = y0;
-		z = z0;
-	}
-
-	/**
-	 * access to elements by index
-	 */
-	T operator ()(unsigned int i) {
-		return *(&x + i);
-	}
-
-	/**
-	 * access to elements by index
-	 */
-	const T operator ()(unsigned int i) const {
-		return *(&x + i);
-	}
-
-	/**
-	 * test for equality
-	 */
-	bool operator ==(const Vector3<T> &v) {
-		return (x == v.x && y == v.y && z == v.z);
-	}
-
-	/**
-	 * test for inequality
-	 */
-	bool operator !=(const Vector3<T> &v) {
-		return (x != v.x || y != v.y || z != v.z);
-	}
-
-	/**
-	 * set to value
-	 */
-	const Vector3<T> &operator =(const Vector3<T> &v) {
-		x = v.x;
-		y = v.y;
-		z = v.z;
-		return *this;
-	}
-
-	/**
-	 * negation
-	 */
-	const Vector3<T> operator -(void) const {
-		return Vector3<T>(-x, -y, -z);
-	}
-
-	/**
-	 * addition
-	 */
-	const Vector3<T> operator +(const Vector3<T> &v) const {
-		return Vector3<T>(x + v.x, y + v.y, z + v.z);
-	}
-
-	/**
-	 * subtraction
-	 */
-	const Vector3<T> operator -(const Vector3<T> &v) const {
-		return Vector3<T>(x - v.x, y - v.y, z - v.z);
-	}
-
-	/**
-	 * uniform scaling
-	 */
-	const Vector3<T> operator *(const T num) const {
-		Vector3<T> temp(*this);
-		return temp *= num;
-	}
-
-	/**
-	 * uniform scaling
-	 */
-	const Vector3<T> operator /(const T num) const {
-		Vector3<T> temp(*this);
-		return temp /= num;
-	}
-
-	/**
-	 * addition
-	 */
-	const Vector3<T> &operator +=(const Vector3<T> &v) {
-		x += v.x;
-		y += v.y;
-		z += v.z;
-		return *this;
-	}
-
-	/**
-	 * subtraction
-	 */
-	const Vector3<T> &operator -=(const Vector3<T> &v) {
-		x -= v.x;
-		y -= v.y;
-		z -= v.z;
-		return *this;
-	}
-
-	/**
-	 * uniform scaling
-	 */
-	const Vector3<T> &operator *=(const T num) {
-		x *= num;
-		y *= num;
-		z *= num;
-		return *this;
-	}
-
-	/**
-	 * uniform scaling
-	 */
-	const Vector3<T> &operator /=(const T num) {
-		x /= num;
-		y /= num;
-		z /= num;
-		return *this;
-	}
-
-	/**
-	 * dot product
-	 */
-	T operator *(const Vector3<T> &v) const {
-		return x * v.x + y * v.y + z * v.z;
-	}
-
-	/**
-	 * cross product
-	 */
-	const Vector3<T> operator %(const Vector3<T> &v) const {
-		Vector3<T> temp(y * v.z - z * v.y, z * v.x - x * v.z, x * v.y - y * v.x);
-		return temp;
-	}
-
-	/**
-	 * gets the length of this vector squared
-	 */
-	float length_squared() const {
-		return (*this * *this);
-	}
-
-	/**
-	 * gets the length of this vector
-	 */
-	float length() const {
-		return (T)sqrt(*this * *this);
-	}
-
-	/**
-	 * normalizes this vector
-	 */
-	void normalize() {
-		*this /= length();
-	}
-
-	/**
-	 * returns the normalized version of this vector
-	 */
-	Vector3<T> normalized() const {
-		return *this / length();
-	}
-
-	/**
-	 * reflects this vector about n
-	 */
-	void reflect(const Vector3<T> &n)
-	{
-		Vector3<T> orig(*this);
-		project(n);
-		*this = *this * 2 - orig;
-	}
-
-	/**
-	 * projects this vector onto v
-	 */
-	void project(const Vector3<T> &v) {
-		*this = v * (*this * v) / (v * v);
-	}
-
-	/**
-	 * returns this vector projected onto v
-	 */
-	Vector3<T> projected(const Vector3<T> &v) {
-		return v * (*this * v) / (v * v);
-	}
-
-	/**
-	 * computes the angle between 2 arbitrary vectors
-	 */
-	static inline float angle(const Vector3<T> &v1, const Vector3<T> &v2) {
-		return acosf((v1 * v2) / (v1.length() * v2.length()));
-	}
-
-	/**
-	 * computes the angle between 2 arbitrary normalized vectors
-	 */
-	static inline float angle_normalized(const Vector3<T> &v1, const Vector3<T> &v2) {
-		return acosf(v1 * v2);
-	}
-};
-
-typedef Vector3<float> Vector3f;
-}
-
-#endif // VECTOR3_HPP
diff --git a/src/lib/mathlib/mathlib.h b/src/lib/mathlib/mathlib.h
index d8e95b46b..9e03855c5 100644
--- a/src/lib/mathlib/mathlib.h
+++ b/src/lib/mathlib/mathlib.h
@@ -42,10 +42,7 @@
 #pragma once
 
 #include "math/Vector.hpp"
-#include "math/Vector2.hpp"
-#include "math/Vector3.hpp"
 #include "math/Matrix.hpp"
-#include "math/Matrix3.hpp"
 #include "math/Quaternion.hpp"
 #include "math/Limits.hpp"
 
diff --git a/src/lib/mathlib/module.mk b/src/lib/mathlib/module.mk
index 50951c617..191e2da73 100644
--- a/src/lib/mathlib/module.mk
+++ b/src/lib/mathlib/module.mk
@@ -35,7 +35,6 @@
 # Math library
 #
 SRCS		 = math/test/test.cpp \
-		   math/Matrix3.cpp \
 		   math/Limits.cpp
 
 #