From: Ankur <ankur29.garg@samsung.com>
Date: Thu, 12 Feb 2015 12:27:12 +0000 (+0530)
Subject: Optimization of template structure - Feature merge from devel/tizen branch
X-Git-Tag: submit/tizen_tv/20150226.040306~14
X-Git-Url: http://review.tizen.org/git/?a=commitdiff_plain;h=734a5743ae8a434c661a90c72478678a5165a2e8;p=platform%2Fcore%2Fsystem%2Fsensord.git

Optimization of template structure - Feature merge from devel/tizen branch

This is one of the four patches being submitted to merge the changes made to devel/tizen to tizen  branch.
This contains the feature where the template structure of the vector and matrix classes was changed.

Patch Set 3: Changes related to coding guidelines as per the comments

Build has been checked for armv7l and aarch64 for public repo.

Change-Id: Ia26d66b1ca351a22de92b858f6b60dbfee242d42
---

diff --git a/src/sensor_fusion/euler_angles.cpp b/src/sensor_fusion/euler_angles.cpp
index 8964dfb..c3bfbe5 100644
--- a/src/sensor_fusion/euler_angles.cpp
+++ b/src/sensor_fusion/euler_angles.cpp
@@ -20,12 +20,11 @@
 
 #include <math.h>
 
-#define EULER_SIZE 3
 #define RAD2DEG 57.2957795
 #define DEG2RAD 0.0174532925
 
 template <typename TYPE>
-euler_angles<TYPE>::euler_angles() : m_ang(EULER_SIZE)
+euler_angles<TYPE>::euler_angles() : m_ang()
 {
 }
 
@@ -34,12 +33,12 @@ euler_angles<TYPE>::euler_angles(const TYPE roll, const TYPE pitch, const TYPE a
 {
 	TYPE euler_data[EULER_SIZE] = {roll, pitch, azimuth};
 
-	vect<TYPE> v(EULER_SIZE, euler_data);
+	vect<TYPE, EULER_SIZE> v(euler_data);
 	m_ang = v;
 }
 
 template <typename TYPE>
-euler_angles<TYPE>::euler_angles(const vect<TYPE> v)
+euler_angles<TYPE>::euler_angles(const vect<TYPE, EULER_SIZE> v)
 {
 	m_ang = v;
 }
@@ -94,15 +93,35 @@ euler_angles<T> quat2euler(const quaternion<T> q)
 template <typename T>
 euler_angles<T> rad2deg(const euler_angles<T> e)
 {
-	euler_angles<T> result(e.m_ang * (T) RAD2DEG);
-	return result;
+	return (e.m_ang * (T) RAD2DEG);
 }
 
 template <typename T>
 euler_angles<T> deg2rad(const euler_angles<T> e)
 {
-	euler_angles<T> result(e.m_ang * (T) DEG2RAD);
-	return result;
+	return (e.m_ang * (T) DEG2RAD);
+}
+
+template<typename T>
+quaternion<T> euler2quat(euler_angles<T> euler) {
+	T theta = euler.m_ang.m_vec[0];
+	T phi = euler.m_ang.m_vec[1];
+	T psi = euler.m_ang.m_vec[2];
+
+	T R[ROT_MAT_ROWS][ROT_MAT_COLS];
+	R[0][0] = cos(psi)*cos(theta);
+	R[0][1] = -sin(psi)*cos(phi) + cos(psi)*sin(theta)*sin(phi);
+	R[0][2] = sin(psi)*sin(phi) + cos(psi)*sin(theta)*cos(phi);
+	R[1][0] = sin(psi)*cos(theta);
+	R[1][1] = cos(psi)*cos(phi) + sin(psi)*sin(theta)*sin(phi);
+	R[1][2] = -cos(psi)*sin(phi) + sin(psi)*sin(theta)*cos(phi);
+	R[2][0] = -sin(theta);
+	R[2][1] = cos(theta)*sin(phi);
+	R[2][2] = cos(theta)*cos(phi);
+
+	rotation_matrix<T> rot_mat(R);
+	quaternion<T> q = rot_mat2quat(rot_mat);
+	return q;
 }
 
 #endif  //_EULER_ANGLES_H_
diff --git a/src/sensor_fusion/euler_angles.h b/src/sensor_fusion/euler_angles.h
index 9c5af06..061dfb1 100644
--- a/src/sensor_fusion/euler_angles.h
+++ b/src/sensor_fusion/euler_angles.h
@@ -22,21 +22,25 @@
 
 #include "vector.h"
 #include "quaternion.h"
+#include "rotation_matrix.h"
+
+#define EULER_SIZE 3
 
 template <typename TYPE>
 class euler_angles {
 public:
-	vect<TYPE> m_ang;
+	vect<TYPE,EULER_SIZE> m_ang;
 
 	euler_angles();
 	euler_angles(const TYPE roll, const TYPE pitch, const TYPE azimuth);
-	euler_angles(const vect<TYPE> v);
+	euler_angles(const vect<TYPE, EULER_SIZE> v);
 	euler_angles(const euler_angles<TYPE>& e);
 	~euler_angles();
 
 	euler_angles<TYPE> operator =(const euler_angles<TYPE>& e);
 
 	template<typename T> friend euler_angles<T> quat2euler(const quaternion<T> q);
+	template<typename T> friend quaternion<T> euler2quat(euler_angles<T> euler);
 	template<typename T> friend euler_angles<T> rad2deg(const euler_angles<T> e);
 	template<typename T> friend euler_angles<T> deg2rad(const euler_angles<T> e);
 };
diff --git a/src/sensor_fusion/matrix.cpp b/src/sensor_fusion/matrix.cpp
index 9eabcea..9d483f2 100755
--- a/src/sensor_fusion/matrix.cpp
+++ b/src/sensor_fusion/matrix.cpp
@@ -19,111 +19,50 @@
 
 #ifdef _MATRIX_H_
 
-template <typename TYPE>
-matrix<TYPE>::matrix(void)
+TYPE_ROW_COL matrix<TYPE, ROW, COL>::matrix(void)
 {
-	m_mat = NULL;
+	for (int i = 0; i < ROW; i++)
+		for (int j = 0; j < COL; j++)
+			m_mat[i][j] = 0;
 }
 
-template <typename TYPE>
-matrix<TYPE>::matrix(const int rows, const int cols)
+TYPE_ROW_COL matrix<TYPE, ROW, COL>::matrix(const matrix<TYPE, ROW, COL>& m)
 {
-	m_rows = rows;
-	m_cols = cols;
-	m_mat = new TYPE *[m_rows];
-
-	for (int i = 0; i < m_rows; i++)
-		m_mat[i] = new TYPE [m_cols]();
-}
-
-template <typename TYPE>
-matrix<TYPE>::matrix(const matrix<TYPE>& m)
-{
-	m_rows = m.m_rows;
-	m_cols = m.m_cols;
-	m_mat = new TYPE *[m_rows];
-
-	for (int i = 0; i < m_rows; i++)
-		m_mat[i] = new TYPE [m_cols];
-
-	for (int p = 0; p < m_rows; p++)
-		for (int q = 0; q < m_cols; q++)
+	for (int p = 0; p < ROW; p++)
+		for (int q = 0; q < COL; q++)
 			m_mat[p][q] = m.m_mat[p][q];
 }
 
-template <typename TYPE>
-matrix<TYPE>::matrix(const int rows, const int cols, TYPE *mat_data)
+TYPE_ROW_COL matrix<TYPE, ROW, COL>::matrix(TYPE mat_data[ROW][COL])
 {
-	m_rows = rows;
-	m_cols = cols;
-	m_mat = new TYPE *[m_rows];
-
-	for (int i = 0; i < m_rows; i++)
-		m_mat[i] = new TYPE [m_cols];
-
-	for (int i = 0; i < m_rows; i++)
-		for (int j = 0; j < m_cols; j++)
-			m_mat[i][j] = *mat_data++;
+	for (int i = 0; i < ROW; i++)
+		for (int j = 0; j < COL; j++)
+			m_mat[i][j] = mat_data[i][j];
 }
 
-template <typename TYPE>
-matrix<TYPE>::~matrix()
+TYPE_ROW_COL matrix<TYPE, ROW, COL>::~matrix()
 {
-	if (m_mat != NULL)
-	{
-		for (int i = 0; i < m_rows; i++)
-			delete[] m_mat[i];
-		delete[] m_mat;
-	}
 }
 
-template <typename TYPE>
-matrix<TYPE> matrix<TYPE>::operator =(const matrix<TYPE>& m)
+TYPE_ROW_COL matrix<TYPE, ROW, COL> matrix<TYPE, ROW, COL>::operator =(const matrix<TYPE, ROW, COL>& m)
 {
 	if (this == &m)
 	{
 		return *this;
 	}
 
-	if (m_mat == NULL)
-	{
-		m_rows = m.m_rows;
-		m_cols = m.m_cols;
-		m_mat = new TYPE *[m_rows];
-
-		for (int i = 0; i < m_rows; i++)
-			m_mat[i] = new TYPE [m_cols];
-	}
-	else
-	{
-		if ((m_rows != m.m_rows) || (m_cols != m.m_cols))
-		{
-			for (int i = 0; i < m_rows; i++)
-				delete[] m_mat[i];
-			delete[] m_mat;
-
-			m_rows = m.m_rows;
-			m_cols = m.m_cols;
-			m_mat = new TYPE *[m_rows];
-
-			for (int i = 0; i < m_rows; i++)
-				m_mat[i] = new TYPE [m_cols];
-		}
-	}
-
-	for (int p = 0; p < m_rows; p++)
-		for (int q = 0; q < m_cols; q++)
+	for (int p = 0; p < ROW; p++)
+		for (int q = 0; q < COL; q++)
 			m_mat[p][q] = m.m_mat[p][q];
 
 	return *this;
 }
 
-template <typename T>
-ostream& operator <<(ostream& dout, matrix<T>& m)
+T_R_C ostream& operator <<(ostream& dout, matrix<T, R, C>& m)
 {
-	for (int i = 0; i < m.m_rows; i++)
+	for (int i = 0; i < R; i++)
 	{
-		for (int j = 0; j < m.m_cols; j++)
+		for (int j = 0; j < C; j++)
 		{
 			dout << m.m_mat[i][j] << "\t";
 		}
@@ -132,74 +71,60 @@ ostream& operator <<(ostream& dout, matrix<T>& m)
 	return dout;
 }
 
-template <typename T>
-matrix<T> operator +(const matrix<T> m1, const matrix<T> m2)
+T_R_C matrix<T, R, C> operator +(const matrix<T, R, C> m1, const matrix<T, R, C> m2)
 {
-	assert(m1.m_rows == m2.m_rows);
-	assert(m1.m_cols == m2.m_cols);
-
-	matrix<T> m3(m1.m_rows, m1.m_cols);
+	matrix<T, R, C> m3;
 
-	for (int i = 0; i < m1.m_rows; i++)
-		for (int j = 0; j < m1.m_cols; j++)
+	for (int i = 0; i < R; i++)
+		for (int j = 0; j < C; j++)
 			m3.m_mat[i][j] = m1.m_mat[i][j] + m2.m_mat[i][j];
 
 	return m3;
 }
 
-template <typename T>
-matrix<T> operator +(const matrix<T> m, const T val)
+T_R_C matrix<T, R, C> operator +(const matrix<T, R, C> m, const T val)
 {
-	matrix<T> m1(m.m_rows, m.m_cols);
+	matrix<T, R, C> m1;
 
-	for (int i = 0; i < m.m_rows; i++)
-		for (int j = 0; j < m.m_cols; j++)
+	for (int i = 0; i < R; i++)
+		for (int j = 0; j < C; j++)
 			m1.m_mat[i][j] = m.m_mat[i][j] + val;
 
 	return m1;
 }
 
-template <typename T>
-matrix<T> operator -(const matrix<T> m1, const matrix<T> m2)
+T_R_C matrix<T, R, C> operator -(const matrix<T, R, C> m1, const matrix<T, R, C> m2)
 {
-	assert(m1.m_rows == m2.m_rows);
-	assert(m1.m_cols == m2.m_cols);
+	matrix<T, R, C> m3;
 
-	matrix<T> m3(m1.m_rows, m1.m_cols);
-
-	for (int i = 0; i < m1.m_rows; i++)
-		for (int j = 0; j < m1.m_cols; j++)
+	for (int i = 0; i < R; i++)
+		for (int j = 0; j < C; j++)
 			m3.m_mat[i][j] = m1.m_mat[i][j] - m2.m_mat[i][j];
 
 	return m3;
 }
 
-template <typename T>
-matrix<T> operator -(const matrix<T> m, const T val)
+T_R_C matrix<T, R, C> operator -(const matrix<T, R, C> m, const T val)
 {
-	matrix<T> m1(m.m_rows, m.m_cols);
+	matrix<T, R, C> m1;
 
-	for (int i = 0; i < m.m_rows; i++)
-		for (int j = 0; j < m.m_cols; j++)
+	for (int i = 0; i < R; i++)
+		for (int j = 0; j < C; j++)
 			m1.m_mat[i][j] = m.m_mat[i][j] - val;
 
 	return m1;
 }
 
-template <typename T>
-matrix<T> operator *(const matrix<T> m1, const matrix<T> m2)
+T_R_C_C2 matrix<T, R, C2> operator *(const matrix<T, R, C> m1, const matrix<T, C, C2> m2)
 {
-	assert(m1.m_rows == m2.m_cols);
-	assert(m1.m_cols == m2.m_rows);
-
-	matrix<T> m3(m1.m_rows, m2.m_cols);
+	matrix<T, R, C2> m3;
 
-	for (int i = 0; i < m1.m_rows; i++)
+	for (int i = 0; i < R; i++)
 	{
-		for (int j = 0; j < m2.m_cols; j++)
+		for (int j = 0; j < C2; j++)
 		{
 			m3.m_mat[i][j] = 0;
-			for (int k = 0; k < m2.m_rows; k++)
+			for (int k = 0; k < C; k++)
 				m3.m_mat[i][j] += m1.m_mat[i][k] * m2.m_mat[k][j];
 		}
 	}
@@ -207,37 +132,34 @@ matrix<T> operator *(const matrix<T> m1, const matrix<T> m2)
 	return m3;
 }
 
-template <typename T>
-matrix<T> operator *(const matrix<T> m, const T val)
+T_R_C matrix<T, R, C> operator *(const matrix<T, R, C> m, const T val)
 {
-	matrix<T> m1(m.m_rows, m.m_cols);
+	matrix<T, R, C> m1;
 
-	for (int i = 0; i < m.m_rows; i++)
-		for (int j = 0; j < m.m_cols; j++)
+	for (int i = 0; i < R; i++)
+		for (int j = 0; j < C; j++)
 			m1.m_mat[i][j] = m.m_mat[i][j] * val;
 
 	return m1;
 }
 
-template <typename T>
-matrix<T> operator /(const matrix<T> m1, const T val)
+T_R_C matrix<T, R, C> operator /(const matrix<T, R, C> m1, const T val)
 {
-	matrix<T> m3(m1.m_rows, m1.m_cols);
+	matrix<T, R, C> m3;
 
-	for (int i = 0; i < m1.m_rows; i++)
-		for (int j = 0; j < m1.m_cols; j++)
+	for (int i = 0; i < R; i++)
+		for (int j = 0; j < C; j++)
 			m3.m_mat[i][j] = m1.m_mat[i][j] / val;
 
 	return m3;
 }
 
-template <typename T>
-bool operator ==(const matrix<T> m1, const matrix<T> m2)
+T_R1_C1_R2_C2 bool operator ==(const matrix<T, R1, C1> m1, const matrix<T, R2, C2> m2)
 {
-	if ((m1.m_rows == m2.m_rows) && (m1.m_cols == m2.m_cols))
+	if ((R1 == R2) && (C1 == C2))
 	{
-		for (int i = 0; i < m1.m_rows; i++)
-			for (int j = 0; j < m2.m_cols; j++)
+		for (int i = 0; i < R1; i++)
+			for (int j = 0; j < C2; j++)
 				if (m1.m_mat[i][j] != m2.m_mat[i][j])
 					return false;
 	}
@@ -247,41 +169,20 @@ bool operator ==(const matrix<T> m1, const matrix<T> m2)
 	return true;
 }
 
-template <typename T>
-bool operator !=(const matrix<T> m1, const matrix<T> m2)
+T_R1_C1_R2_C2 bool operator !=(const matrix<T, R1, C1> m1, const matrix<T, R2, C2> m2)
 {
 	return (!(m1 == m2));
 }
 
-template <typename T>
-matrix<T> tran(const matrix<T> m)
+T_R_C matrix<T, R, C> tran(const matrix<T, R, C> m)
 {
-	matrix<T> m1(m.m_cols, m.m_rows);
+	matrix<T, R, C> m1;
 
-	for (int i = 0; i < m.m_rows; i++)
-		for (int j = 0; j < m.m_cols; j++)
+	for (int i = 0; i < R; i++)
+		for (int j = 0; j < C; j++)
 			m1.m_mat[j][i] = m.m_mat[i][j];
 
 	return m1;
 }
 
-
-template <typename T>
-matrix<T> mul(const matrix<T> m1, const matrix<T> m2)
-{
-	assert(m2.m_cols == 1);
-	assert(m1.m_cols == m2.m_rows);
-
-	matrix<T> m3(m1.m_rows, 1);
-
-	for (int i = 0; i < m1.m_rows; i++)
-	{
-			m3.m_mat[i][0] = 0;
-			for (int k = 0; k < m2.m_rows; k++)
-				m3.m_mat[i][0] += m1.m_mat[i][k] * m2.m_mat[k][0];
-	}
-
-	return m3;
-}
-
 #endif //_MATRIX_H_
diff --git a/src/sensor_fusion/matrix.h b/src/sensor_fusion/matrix.h
index 05030a0..b2c47bb 100755
--- a/src/sensor_fusion/matrix.h
+++ b/src/sensor_fusion/matrix.h
@@ -24,45 +24,33 @@
 #include <iostream>
 using namespace std;
 
-template <typename TYPE>
-class matrix {
+#define TYPE_ROW_COL template<typename TYPE, int ROW, int COL>
+#define T_R_C template<typename T, int R, int C>
+#define T_R_C_C2 template<typename T, int R, int C, int C2>
+#define T_R1_C1_R2_C2 template<typename T, int R1, int C1, int R2, int C2>
+
+TYPE_ROW_COL class matrix {
 public:
-	int m_rows;
-	int m_cols;
-	TYPE **m_mat;
+	TYPE m_mat[ROW][COL];
 
 	matrix(void);
-	matrix(const int rows, const int cols);
-	matrix(const int rows, const int cols, TYPE *mat_data);
-	matrix(const matrix<TYPE>& m);
+	matrix(TYPE mat_data[ROW][COL]);
+	matrix(const matrix<TYPE, ROW, COL>& m);
 	~matrix();
 
-	matrix<TYPE> operator =(const matrix<TYPE>& m);
-
-	template<typename T> friend ostream& operator << (ostream& dout,
-			matrix<T>& m);
-	template<typename T> friend matrix<T> operator +(const matrix<T> m1,
-			const matrix<T> m2);
-	template<typename T> friend matrix<T> operator +(const matrix<T> m,
-			const T val);
-	template<typename T> friend matrix<T> operator -(const matrix<T> m1,
-			const matrix<T> m2);
-	template<typename T> friend matrix<T> operator -(const matrix<T> m,
-			const T val);
-	template<typename T> friend matrix<T> operator *(const matrix<T> m1,
-			const matrix<T> m2);
-	template<typename T> friend matrix<T> operator *(const matrix<T> m,
-			const T val);
-	template<typename T> friend matrix<T> operator /(const matrix<T> m1,
-			 const T val);
-	template<typename T> friend bool operator ==(const matrix<T> m1,
-			const matrix<T> m2);
-	template<typename T> friend bool operator !=(const matrix<T> m1,
-			const matrix<T> m2);
-
-	template<typename T> friend matrix<T> tran(const matrix<T> m);
-	template <typename T> friend matrix<T> mul(const matrix<T> m1,
-			const matrix<T> m2);
+	matrix<TYPE, ROW, COL> operator =(const matrix<TYPE, ROW, COL>& m);
+
+	T_R_C friend ostream& operator << (ostream& dout, matrix<T, R, C>& m);
+	T_R_C friend matrix<T, R, C> operator +(const matrix<T, R, C> m1, const matrix<T, R, C> m2);
+	T_R_C friend matrix<T, R, C> operator +(const matrix<T, R, C> m, const T val);
+	T_R_C friend matrix<T, R, C> operator -(const matrix<T, R, C> m1, const matrix<T, R, C> m2);
+	T_R_C friend matrix<T, R, C> operator -(const matrix<T, R, C> m, const T val);
+	T_R_C_C2 friend matrix<T, R, C2> operator *(const matrix<T, R, C> m1, const matrix<T, C, C2> m2);
+	T_R_C friend matrix<T, R, C> operator *(const matrix<T, R, C> m, const T val);
+	T_R_C friend matrix<T, R, C> operator /(const matrix<T, R, C> m1, const T val);
+	T_R1_C1_R2_C2 friend bool operator ==(const matrix<T, R1, C1> m1, const matrix<T, R2, C2> m2);
+	T_R1_C1_R2_C2 friend bool operator !=(const matrix<T, R1, C1> m1, const matrix<T, R2, C2> m2);
+	T_R_C friend matrix<T, R, C> tran(const matrix<T, R, C> m);
 };
 
 #include "matrix.cpp"
diff --git a/src/sensor_fusion/orientation_filter.cpp b/src/sensor_fusion/orientation_filter.cpp
index b197f91..d9e353f 100644
--- a/src/sensor_fusion/orientation_filter.cpp
+++ b/src/sensor_fusion/orientation_filter.cpp
@@ -44,14 +44,6 @@
 
 #define ABS(val) (((val) < 0) ? -(val) : (val))
 
-// M-matrix, V-vector, MxN=> matrix dimension, R-RowCount, C-Column count
-#define M3X3R	3
-#define M3X3C	3
-#define M6X6R	6
-#define M6X6C	6
-#define V1x3S	3
-#define V1x4S	4
-#define V1x6S	6
 
 template <typename TYPE>
 orientation_filter<TYPE>::orientation_filter()
@@ -60,10 +52,7 @@ orientation_filter<TYPE>::orientation_filter()
 
 	std::fill_n(arr, MOVING_AVERAGE_WINDOW_LENGTH, NON_ZERO_VAL);
 
-	vect<TYPE> vec(MOVING_AVERAGE_WINDOW_LENGTH, arr);
-	vect<TYPE> vec1x3(V1x3S);
-	vect<TYPE> vec1x6(V1x6S);
-	matrix<TYPE> mat6x6(M6X6R, M6X6C);
+	vect<TYPE, MOVING_AVERAGE_WINDOW_LENGTH> vec(arr);
 
 	m_var_gyr_x = vec;
 	m_var_gyr_y = vec;
@@ -72,17 +61,6 @@ orientation_filter<TYPE>::orientation_filter()
 	m_var_pitch = vec;
 	m_var_azimuth = vec;
 
-	m_tran_mat = mat6x6;
-	m_measure_mat = mat6x6;
-	m_pred_cov = mat6x6;
-	m_driv_cov = mat6x6;
-	m_aid_cov = mat6x6;
-
-	m_bias_correction = vec1x3;
-	m_state_new = vec1x6;
-	m_state_old = vec1x6;
-	m_state_error = vec1x6;
-
 	m_pitch_phase_compensation = 1;
 	m_roll_phase_compensation = 1;
 	m_azimuth_phase_compensation = 1;
@@ -97,13 +75,12 @@ orientation_filter<TYPE>::~orientation_filter()
 }
 
 template <typename TYPE>
-inline void orientation_filter<TYPE>::initialize_sensor_data(const sensor_data<TYPE> accel,
+inline void orientation_filter<TYPE>::init_accel_gyro_mag_data(const sensor_data<TYPE> accel,
 		const sensor_data<TYPE> gyro, const sensor_data<TYPE> magnetic)
 {
 	unsigned long long sample_interval_gyro = SAMPLE_INTV;
 
 	m_accel.m_data = accel.m_data;
-	m_gyro.m_data = gyro.m_data;
 	m_magnetic.m_data = magnetic.m_data;
 
 	if (m_gyro.m_time_stamp != 0 && gyro.m_time_stamp != 0)
@@ -115,11 +92,11 @@ inline void orientation_filter<TYPE>::initialize_sensor_data(const sensor_data<T
 	m_gyro.m_time_stamp = gyro.m_time_stamp;
 	m_magnetic.m_time_stamp = magnetic.m_time_stamp;
 
-	m_gyro.m_data = m_gyro.m_data - m_bias_correction;
+	m_gyro.m_data = gyro.m_data - m_bias_correction;
 }
 
 template <typename TYPE>
-inline void orientation_filter<TYPE>::initialize_sensor_data(const sensor_data<TYPE> accel,
+inline void orientation_filter<TYPE>::init_accel_mag_data(const sensor_data<TYPE> accel,
 		const sensor_data<TYPE> magnetic)
 {
 	m_accel.m_data = accel.m_data;
@@ -130,22 +107,41 @@ inline void orientation_filter<TYPE>::initialize_sensor_data(const sensor_data<T
 }
 
 template <typename TYPE>
+inline void orientation_filter<TYPE>::init_accel_gyro_data(const sensor_data<TYPE> accel,
+		const sensor_data<TYPE> gyro)
+{
+	unsigned long long sample_interval_gyro = SAMPLE_INTV;
+
+	m_accel.m_data = accel.m_data;
+
+	if (m_gyro.m_time_stamp != 0 && gyro.m_time_stamp != 0)
+		sample_interval_gyro = gyro.m_time_stamp - m_gyro.m_time_stamp;
+
+	m_gyro_dt = sample_interval_gyro * US2S;
+
+	m_accel.m_time_stamp = accel.m_time_stamp;
+	m_gyro.m_time_stamp = gyro.m_time_stamp;
+
+	m_gyro.m_data = gyro.m_data - m_bias_correction;
+}
+
+template <typename TYPE>
 inline void orientation_filter<TYPE>::orientation_triad_algorithm()
 {
 	TYPE arr_acc_e[V1x3S] = {0.0, 0.0, 1.0};
 	TYPE arr_mag_e[V1x3S] = {0.0, (TYPE) m_magnetic_alignment_factor, 0.0};
 
-	vect<TYPE> acc_e(V1x3S, arr_acc_e);
-	vect<TYPE> mag_e(V1x3S, arr_mag_e);
+	vect<TYPE, V1x3S> acc_e(arr_acc_e);
+	vect<TYPE, V1x3S> mag_e(arr_mag_e);
 
-	vect<TYPE> acc_b_x_mag_b = cross(m_accel.m_data, m_magnetic.m_data);
-	vect<TYPE> acc_e_x_mag_e = cross(acc_e, mag_e);
+	vect<TYPE, SENSOR_DATA_SIZE> acc_b_x_mag_b = cross(m_accel.m_data, m_magnetic.m_data);
+	vect<TYPE, V1x3S> acc_e_x_mag_e = cross(acc_e, mag_e);
 
-	vect<TYPE> cross1 = cross(acc_b_x_mag_b, m_accel.m_data);
-	vect<TYPE> cross2 = cross(acc_e_x_mag_e, acc_e);
+	vect<TYPE, SENSOR_DATA_SIZE> cross1 = cross(acc_b_x_mag_b, m_accel.m_data);
+	vect<TYPE, V1x3S> cross2 = cross(acc_e_x_mag_e, acc_e);
 
-	matrix<TYPE> mat_b(M3X3R, M3X3C);
-	matrix<TYPE> mat_e(M3X3R, M3X3C);
+	matrix<TYPE, M3X3R, M3X3C> mat_b;
+	matrix<TYPE, M3X3R, M3X3C> mat_e;
 
 	for(int i = 0; i < M3X3R; i++)
 	{
@@ -157,13 +153,23 @@ inline void orientation_filter<TYPE>::orientation_triad_algorithm()
 		mat_e.m_mat[i][2] = cross2.m_vec[i];
 	}
 
-	matrix<TYPE> mat_b_t = tran(mat_b);
+	matrix<TYPE, M3X3R, M3X3C> mat_b_t = tran(mat_b);
 	rotation_matrix<TYPE> rot_mat(mat_e * mat_b_t);
 
 	m_quat_aid = rot_mat2quat(rot_mat);
 }
 
 template <typename TYPE>
+inline void orientation_filter<TYPE>::compute_accel_orientation()
+{
+	TYPE arr_acc_e[V1x3S] = {0.0, 0.0, 1.0};
+
+	vect<TYPE, V1x3S> acc_e(arr_acc_e);
+
+	m_quat_aid = sensor_data2quat(m_accel, acc_e);
+}
+
+template <typename TYPE>
 inline void orientation_filter<TYPE>::compute_covariance()
 {
 	TYPE var_gyr_x, var_gyr_y, var_gyr_z;
@@ -217,19 +223,10 @@ inline void orientation_filter<TYPE>::time_update()
 	m_measure_mat.m_mat[2][2] = 1;
 
 	if (is_initialized(m_state_old))
-		m_state_new = transpose(mul(m_tran_mat, transpose(m_state_old)));
+		m_state_new = transpose(m_tran_mat * transpose(m_state_old));
 
 	m_pred_cov = (m_tran_mat * m_pred_cov * tran(m_tran_mat)) + m_driv_cov;
 
-	for (int j=0; j<M6X6C; ++j) {
-		for (int i=0; i<M6X6R; ++i)	{
-			if (ABS(m_pred_cov.m_mat[i][j]) < NEGLIGIBLE_VAL)
-				m_pred_cov.m_mat[i][j] = NEGLIGIBLE_VAL;
-		}
-		if (ABS(m_state_new.m_vec[j]) < NEGLIGIBLE_VAL)
-			m_state_new.m_vec[j] = NEGLIGIBLE_VAL;
-	}
-
 	if(!is_initialized(m_quat_driv.m_quat))
 		m_quat_driv = m_quat_aid;
 
@@ -277,36 +274,32 @@ inline void orientation_filter<TYPE>::time_update()
 template <typename TYPE>
 inline void orientation_filter<TYPE>::measurement_update()
 {
-	matrix<TYPE> gain(M6X6R, M6X6C);
-	matrix<TYPE> iden(M6X6R, M6X6C);
-	iden.m_mat[0][0] = iden.m_mat[1][1] = iden.m_mat[2][2] = 1;
-	iden.m_mat[3][3] = iden.m_mat[4][4] = iden.m_mat[5][5] = 1;
+	matrix<TYPE, M6X6R, M6X6C> gain;
+	TYPE iden = 0;
 
 	for (int j=0; j<M6X6C; ++j) {
-		for (int i=0; i<M6X6R; ++i) {
+		for (int i=0; i<M6X6R; ++i)	{
 			gain.m_mat[i][j] = m_pred_cov.m_mat[j][i] / (m_pred_cov.m_mat[j][j] + m_aid_cov.m_mat[j][j]);
-			m_state_new.m_vec[i] = m_state_new.m_vec[i] + gain.m_mat[i][j] * m_state_error.m_vec[j];
-		}
 
-		matrix<TYPE> temp = iden;
+			m_state_new.m_vec[i] = m_state_new.m_vec[i] + gain.m_mat[i][j] * m_state_error.m_vec[j];
 
-		for (int i=0; i<M6X6R; ++i)
-			temp.m_mat[i][j] = iden.m_mat[i][j] - (gain.m_mat[i][j] * m_measure_mat.m_mat[j][i]);
+			if (i == j)
+				iden = 1;
+			else
+				iden = 0;
 
-		m_pred_cov = temp * m_pred_cov;
-	}
+			m_pred_cov.m_mat[j][i] = (iden - (gain.m_mat[i][j] * m_measure_mat.m_mat[j][i])) *
+					m_pred_cov.m_mat[j][i];
 
-	for (int j=0; j<M6X6C; ++j) {
-		for (int i=0; i<M6X6R; ++i) {
-			if (ABS(m_pred_cov.m_mat[i][j]) < NEGLIGIBLE_VAL)
-				m_pred_cov.m_mat[i][j] = NEGLIGIBLE_VAL;
+			if (ABS(m_pred_cov.m_mat[j][i]) < NEGLIGIBLE_VAL)
+				m_pred_cov.m_mat[j][i] = NEGLIGIBLE_VAL;
 		}
 	}
 
 	m_state_old = m_state_new;
 
 	TYPE arr_bias[V1x3S] = {m_state_new.m_vec[3], m_state_new.m_vec[4], m_state_new.m_vec[5]};
-	vect<TYPE> vec(V1x3S, arr_bias);
+	vect<TYPE, V1x3S> vec(arr_bias);
 
 	m_bias_correction = vec;
 }
@@ -317,7 +310,7 @@ euler_angles<TYPE> orientation_filter<TYPE>::get_orientation(const sensor_data<T
 {
 	euler_angles<TYPE> cor_euler_ang;
 
-	initialize_sensor_data(accel, gyro, magnetic);
+	init_accel_gyro_mag_data(accel, gyro, magnetic);
 
 	normalize(m_accel);
 	m_gyro.m_data = m_gyro.m_data * (TYPE) PI;
@@ -357,7 +350,7 @@ template <typename TYPE>
 quaternion<TYPE> orientation_filter<TYPE>::get_geomagnetic_quaternion(const sensor_data<TYPE> accel,
 		const sensor_data<TYPE> magnetic)
 {
-	initialize_sensor_data(accel, magnetic);
+	init_accel_mag_data(accel, magnetic);
 
 	normalize(m_accel);
 	normalize(m_magnetic);
diff --git a/src/sensor_fusion/orientation_filter.h b/src/sensor_fusion/orientation_filter.h
index 09f5b90..bf89c96 100644
--- a/src/sensor_fusion/orientation_filter.h
+++ b/src/sensor_fusion/orientation_filter.h
@@ -27,27 +27,37 @@
 #include "euler_angles.h"
 #include "rotation_matrix.h"
 
+#define MOVING_AVERAGE_WINDOW_LENGTH	20
+
+#define M3X3R	3
+#define M3X3C	3
+#define M6X6R	6
+#define M6X6C	6
+#define V1x3S	3
+#define V1x4S	4
+#define V1x6S	6
+
 template <typename TYPE>
 class orientation_filter {
 public:
 	sensor_data<TYPE> m_accel;
 	sensor_data<TYPE> m_gyro;
 	sensor_data<TYPE> m_magnetic;
-	vect<TYPE> m_var_gyr_x;
-	vect<TYPE> m_var_gyr_y;
-	vect<TYPE> m_var_gyr_z;
-	vect<TYPE> m_var_roll;
-	vect<TYPE> m_var_pitch;
-	vect<TYPE> m_var_azimuth;
-	matrix<TYPE> m_driv_cov;
-	matrix<TYPE> m_aid_cov;
-	matrix<TYPE> m_tran_mat;
-	matrix<TYPE> m_measure_mat;
-	matrix<TYPE> m_pred_cov;
-	vect<TYPE> m_state_new;
-	vect<TYPE> m_state_old;
-	vect<TYPE> m_state_error;
-	vect<TYPE> m_bias_correction;
+	vect<TYPE, MOVING_AVERAGE_WINDOW_LENGTH> m_var_gyr_x;
+	vect<TYPE, MOVING_AVERAGE_WINDOW_LENGTH> m_var_gyr_y;
+	vect<TYPE, MOVING_AVERAGE_WINDOW_LENGTH> m_var_gyr_z;
+	vect<TYPE, MOVING_AVERAGE_WINDOW_LENGTH> m_var_roll;
+	vect<TYPE, MOVING_AVERAGE_WINDOW_LENGTH> m_var_pitch;
+	vect<TYPE, MOVING_AVERAGE_WINDOW_LENGTH> m_var_azimuth;
+	matrix<TYPE, M6X6R, M6X6C> m_driv_cov;
+	matrix<TYPE, M6X6R, M6X6C> m_aid_cov;
+	matrix<TYPE, M6X6R, M6X6C> m_tran_mat;
+	matrix<TYPE, M6X6R, M6X6C> m_measure_mat;
+	matrix<TYPE, M6X6R, M6X6C> m_pred_cov;
+	vect<TYPE, V1x6S> m_state_new;
+	vect<TYPE, V1x6S> m_state_old;
+	vect<TYPE, V1x6S> m_state_error;
+	vect<TYPE, V1x3S> m_bias_correction;
 	quaternion<TYPE> m_quat_aid;
 	quaternion<TYPE> m_quat_driv;
 	rotation_matrix<TYPE> m_rot_matrix;
@@ -63,11 +73,14 @@ public:
 	orientation_filter();
 	~orientation_filter();
 
-	inline void initialize_sensor_data(const sensor_data<TYPE> accel,
+	inline void init_accel_gyro_mag_data(const sensor_data<TYPE> accel,
 			const sensor_data<TYPE> gyro, const sensor_data<TYPE> magnetic);
-	inline void initialize_sensor_data(const sensor_data<TYPE> accel,
+	inline void init_accel_mag_data(const sensor_data<TYPE> accel,
 			const sensor_data<TYPE> magnetic);
+	inline void init_accel_gyro_data(const sensor_data<TYPE> accel,
+			const sensor_data<TYPE> gyro);
 	inline void orientation_triad_algorithm();
+	inline void compute_accel_orientation();
 	inline void compute_covariance();
 	inline void time_update();
 	inline void measurement_update();
diff --git a/src/sensor_fusion/quaternion.cpp b/src/sensor_fusion/quaternion.cpp
index 7578326..caf2950 100755
--- a/src/sensor_fusion/quaternion.cpp
+++ b/src/sensor_fusion/quaternion.cpp
@@ -21,8 +21,6 @@
 
 #include <math.h>
 
-#define QUAT_SIZE 4
-
 template <typename T> int sgn(T val) {
 	if (val >= 0)
 		return 1;
@@ -38,7 +36,7 @@ template <typename T> T mag(T val) {
 }
 
 template <typename TYPE>
-quaternion<TYPE>::quaternion() : m_quat(QUAT_SIZE)
+quaternion<TYPE>::quaternion() : m_quat()
 {
 }
 
@@ -47,12 +45,12 @@ quaternion<TYPE>::quaternion(const TYPE w, const TYPE x, const TYPE y, const TYP
 {
 	TYPE vec_data[QUAT_SIZE] = {w, x, y, z};
 
-	vect<TYPE> v(QUAT_SIZE, vec_data);
+	vect<TYPE, QUAT_SIZE> v(vec_data);
 	m_quat = v;
 }
 
 template <typename TYPE>
-quaternion<TYPE>::quaternion(const vect<TYPE> v)
+quaternion<TYPE>::quaternion(const vect<TYPE, QUAT_SIZE> v)
 {
 	m_quat = v;
 }
@@ -95,9 +93,7 @@ void quaternion<TYPE>::quat_normalize()
 template <typename T>
 quaternion<T> operator *(const quaternion<T> q, const T val)
 {
-	quaternion<T> res;
-	res = q.m_quat * val;
-	return (res);
+	return (q.m_quat * val);
 }
 
 template <typename T>
@@ -147,4 +143,18 @@ quaternion<T> phase_correction(const quaternion<T> q1, const quaternion<T> q2)
 
 	return q;
 }
+
+template<typename T>
+quaternion<T> axis2quat(const vect<T, REF_VEC_SIZE> axis, const T angle)
+{
+	T w;
+	vect<T, REF_VEC_SIZE> imag;
+
+	w = cos(angle/2);
+	imag = axis * (T)(-sin(angle/2));
+
+	quaternion<T> q(w, imag.m_vec[0], imag.m_vec[1], imag.m_vec[2]);
+
+	return q;
+}
 #endif  //_QUATERNION_H_
diff --git a/src/sensor_fusion/quaternion.h b/src/sensor_fusion/quaternion.h
index 225b129..539a952 100755
--- a/src/sensor_fusion/quaternion.h
+++ b/src/sensor_fusion/quaternion.h
@@ -22,14 +22,17 @@
 
 #include "vector.h"
 
+#define QUAT_SIZE 4
+#define REF_VEC_SIZE 3
+
 template <typename TYPE>
 class quaternion {
 public:
-	vect<TYPE> m_quat;
+	vect<TYPE, QUAT_SIZE> m_quat;
 
 	quaternion();
 	quaternion(const TYPE w, const TYPE x, const TYPE y, const TYPE z);
-	quaternion(const vect<TYPE> v);
+	quaternion(const vect<TYPE, QUAT_SIZE> v);
 	quaternion(const quaternion<TYPE>& q);
 	~quaternion();
 
@@ -44,6 +47,8 @@ public:
 			const quaternion<T> q2);
 	template<typename T> friend quaternion<T> phase_correction(const quaternion<T> q1,
 			const quaternion<T> q2);
+	template<typename T> friend quaternion<T> axis2quat(const vect<T, REF_VEC_SIZE> axis,
+			const T angle);
 };
 
 #include "quaternion.cpp"
diff --git a/src/sensor_fusion/rotation_matrix.cpp b/src/sensor_fusion/rotation_matrix.cpp
index f29e669..83ff5d3 100644
--- a/src/sensor_fusion/rotation_matrix.cpp
+++ b/src/sensor_fusion/rotation_matrix.cpp
@@ -20,8 +20,6 @@
 #if defined (_ROTATION_MATRIX_H_) && defined (_MATRIX_H_)
 
 #define QUAT_LEN 4
-#define ROT_MAT_ROWS 3
-#define ROT_MAT_COLS 3
 
 template <typename T> T get_sign(T val)
 {
@@ -29,21 +27,21 @@ template <typename T> T get_sign(T val)
 }
 
 template <typename TYPE>
-rotation_matrix<TYPE>::rotation_matrix() : m_rot_mat(ROT_MAT_ROWS, ROT_MAT_COLS)
+rotation_matrix<TYPE>::rotation_matrix() : m_rot_mat()
 {
 
 }
 
 template <typename TYPE>
-rotation_matrix<TYPE>::rotation_matrix(const matrix<TYPE> m)
+rotation_matrix<TYPE>::rotation_matrix(const matrix<TYPE, ROT_MAT_ROWS, ROT_MAT_COLS> m)
 {
 	m_rot_mat = m;
 }
 
 template <typename TYPE>
-rotation_matrix<TYPE>::rotation_matrix(const int rows, const int cols, TYPE *mat_data)
+rotation_matrix<TYPE>::rotation_matrix(TYPE mat_data[ROT_MAT_ROWS][ROT_MAT_COLS])
 {
-	matrix<TYPE> m(rows, cols, mat_data);
+	matrix<TYPE, ROT_MAT_ROWS, ROT_MAT_COLS> m(mat_data);
 	m_rot_mat = m;
 }
 
@@ -88,7 +86,7 @@ rotation_matrix<T> quat2rot_mat(quaternion<T> q)
 	R[2][1] = 2 * ((y * z) - (w * x));
 	R[2][2] = (2 * w * w) - 1 + (2 * z * z);
 
-	rotation_matrix<T> rm(ROT_MAT_ROWS, ROT_MAT_COLS, &R[0][0]);
+	rotation_matrix<T> rm(R);
 
 	return rm;
 }
diff --git a/src/sensor_fusion/rotation_matrix.h b/src/sensor_fusion/rotation_matrix.h
index bb4aa78..5cdf430 100644
--- a/src/sensor_fusion/rotation_matrix.h
+++ b/src/sensor_fusion/rotation_matrix.h
@@ -23,14 +23,17 @@
 #include "matrix.h"
 #include "quaternion.h"
 
+#define ROT_MAT_ROWS 3
+#define ROT_MAT_COLS 3
+
 template <typename TYPE>
 class rotation_matrix {
 public:
-	matrix<TYPE> m_rot_mat;
+	matrix<TYPE, ROT_MAT_ROWS, ROT_MAT_COLS> m_rot_mat;
 
 	rotation_matrix();
-	rotation_matrix(const matrix<TYPE> m);
-	rotation_matrix(const int rows, const int cols, TYPE *mat_data);
+	rotation_matrix(const matrix<TYPE, ROT_MAT_ROWS, ROT_MAT_COLS> m);
+	rotation_matrix(TYPE mat_data[ROT_MAT_ROWS][ROT_MAT_COLS]);
 	rotation_matrix(const rotation_matrix<TYPE>& rm);
 	~rotation_matrix();
 
diff --git a/src/sensor_fusion/sensor_data.cpp b/src/sensor_fusion/sensor_data.cpp
index 4794b5e..7272800 100644
--- a/src/sensor_fusion/sensor_data.cpp
+++ b/src/sensor_fusion/sensor_data.cpp
@@ -21,10 +21,8 @@
 
 #include "math.h"
 
-#define SENSOR_DATA_SIZE 3
-
 template <typename TYPE>
-sensor_data<TYPE>::sensor_data() : m_data(SENSOR_DATA_SIZE), m_time_stamp(0)
+sensor_data<TYPE>::sensor_data() : m_data(), m_time_stamp(0)
 {
 }
 
@@ -34,13 +32,13 @@ sensor_data<TYPE>::sensor_data(const TYPE x, const TYPE y,
 {
 	TYPE vec_data[SENSOR_DATA_SIZE] = {x, y, z};
 
-	vect<TYPE> v(SENSOR_DATA_SIZE, vec_data);
+	vect<TYPE, SENSOR_DATA_SIZE> v(vec_data);
 	m_data = v;
 	m_time_stamp = time_stamp;
 }
 
 template <typename TYPE>
-sensor_data<TYPE>::sensor_data(const vect<TYPE> v,
+sensor_data<TYPE>::sensor_data(const vect<TYPE, SENSOR_DATA_SIZE> v,
 		const unsigned long long time_stamp)
 {
 	m_data = v;
@@ -71,7 +69,7 @@ sensor_data<TYPE> sensor_data<TYPE>::operator =(const sensor_data<TYPE>& s)
 template <typename T>
 sensor_data<T> operator +(sensor_data<T> data1, sensor_data<T> data2)
 {
-	sensor_data<T> result(data1.m_data + data2.m_data, data1.m_time_stamp);
+	sensor_data<T> result(data1.m_data + data2.m_data, 0);
 	return result;
 }
 
@@ -105,5 +103,18 @@ sensor_data<T> scale_data(sensor_data<T> data, T scaling_factor)
 	return s;
 }
 
+
+template<typename T>
+quaternion<T> sensor_data2quat(const sensor_data<T> data, const vect<T, REF_VEC_SIZE> ref_vec)
+{
+	vect<T, REF_VEC_SIZE> axis;
+	T angle;
+
+	axis = cross(data.m_data, ref_vec);
+	angle = acos(dot(data.m_data, ref_vec));
+
+	return axis2quat(axis, angle);
+}
+
 #endif /* _SENSOR_DATA_H_ */
 
diff --git a/src/sensor_fusion/sensor_data.h b/src/sensor_fusion/sensor_data.h
index c95e5b4..7998b36 100644
--- a/src/sensor_fusion/sensor_data.h
+++ b/src/sensor_fusion/sensor_data.h
@@ -21,17 +21,20 @@
 #define _SENSOR_DATA_H_
 
 #include "vector.h"
+#include "quaternion.h"
+
+#define SENSOR_DATA_SIZE 3
 
 template <typename TYPE>
 class sensor_data {
 public:
-	vect<TYPE> m_data;
+	vect<TYPE, SENSOR_DATA_SIZE> m_data;
 	unsigned long long m_time_stamp;
 
 	sensor_data();
 	sensor_data(const TYPE x, const TYPE y, const TYPE z,
 			const unsigned long long time_stamp);
-	sensor_data(const vect<TYPE> v,
+	sensor_data(const vect<TYPE, SENSOR_DATA_SIZE> v,
 			const unsigned long long time_stamp);
 	sensor_data(const sensor_data<TYPE>& s);
 	~sensor_data();
@@ -44,6 +47,8 @@ public:
 	template<typename T> friend void normalize(sensor_data<T>& data);
 	template<typename T> friend sensor_data<T> scale_data(sensor_data<T> data,
 			T scaling_factor);
+	template<typename T> friend quaternion<T> sensor_data2quat(const sensor_data<T> data,
+			const vect<T, REF_VEC_SIZE> ref_vec);
 };
 
 #include "sensor_data.cpp"
diff --git a/src/sensor_fusion/test/test_projects/euler_angles_test/euler_angles_main.cpp b/src/sensor_fusion/test/test_projects/euler_angles_test/euler_angles_main.cpp
index 06e449d..4a3104d 100644
--- a/src/sensor_fusion/test/test_projects/euler_angles_test/euler_angles_main.cpp
+++ b/src/sensor_fusion/test/test_projects/euler_angles_test/euler_angles_main.cpp
@@ -26,10 +26,10 @@ int main()
 	float arr2[4] = {0.6, 0.6, -.18, -.44};
 	float arr3[4] = {-0.5, -0.36, .43, .03};
 
-	vect<float> v0(3, arr0);
-	vect<float> v1(3, arr1);
-	vect<float> v2(4, arr2);
-	vect<float> v3(4, arr3);
+	vect<float,3> v0(arr0);
+	vect<float,3> v1(arr1);
+	vect<float,4> v2(arr2);
+	vect<float,4> v3(arr3);
 
 	quaternion<float> q1(v2);
 	quaternion<float> q2(v3);
@@ -60,6 +60,11 @@ int main()
 	cout << "input\t" << q2.m_quat << "\n";
 	cout << "output\t" << e8.m_ang << "\n\n";
 
+	cout << "Euler to Quaternion\n";
+	quaternion<float> q3 = euler2quat(e8);
+	cout << "input\t" << e8.m_ang << "\n";
+	cout << "output\t" << q3.m_quat << "\n\n";
+
 	cout << "Radians to Degrees\n";
 	euler_angles<float> e6 = deg2rad(e0);
 	cout << "input\t" << e0.m_ang << "\n";
diff --git a/src/sensor_fusion/test/test_projects/matrix_test/matrix_main.cpp b/src/sensor_fusion/test/test_projects/matrix_test/matrix_main.cpp
index d1a2153..8227c3a 100644
--- a/src/sensor_fusion/test/test_projects/matrix_test/matrix_main.cpp
+++ b/src/sensor_fusion/test/test_projects/matrix_test/matrix_main.cpp
@@ -32,19 +32,19 @@ int main()
 	float arr8[3][3] = {{4.75, 0.65, 0.123}, {0.075, 5.302, 0.56}, {1.113, 0.475, 2.362}};
 	float arr9[3][3] = {{1, 2, 3}, {1, 2, 3}, {1, 2, 3}};
 
-	matrix<float> m1(2, 2, (float *) arr0);
-	matrix<float> m2(2, 2, (float *) arr1);
-	matrix<float> m3(2, 2);
-	matrix<float> m10(3, 3, (float *) arr3);
-	matrix<float> m11(3, 2, (float *) arr5);
-	matrix<float> m6(3, 3);
-	matrix<float> m13(3, 2);
-	matrix<float> m12(3, 3, (float *) arr4);
-	matrix<float> m15(3, 3, (float *) arr8);
-	matrix<float> m20(1, 3, (float *) arr11);
-	matrix<float> m21(3, 1, (float *) arr12);
-	matrix<float> m22(2, 3, (float *) arr15);
-	matrix<float> m9(3, 3, (float *) arr9);
+	matrix<float, 2, 2> m1(arr0);
+	matrix<float, 2, 2> m2(arr1);
+	matrix<float, 2, 2> m3;
+	matrix<float, 3, 3> m10(arr3);
+	matrix<float, 3, 2> m11(arr5);
+	matrix<float, 3, 3> m6;
+	matrix<float, 3, 2> m13;
+	matrix<float, 3, 3> m12(arr4);
+	matrix<float, 3, 3> m15(arr8);
+	matrix<float, 1, 3> m20(arr11);
+	matrix<float, 3, 1> m21(arr12);
+	matrix<float, 2, 3> m22(arr15);
+	matrix<float, 3, 3> m9(arr9);
 
 	cout<< "Constructor Test\n";
 	cout<< "\n" << m6;
@@ -65,7 +65,7 @@ int main()
 	cout<< "\n\n\nMultiplication\n";
 	m6 = m10 * m12;
 	m3 = m1 * m2;
-	matrix<float> m7(m20.m_rows, m21.m_cols);
+	matrix<float, 1, 1> m7;
 	m7 = m20 * m21;
 	cout<< "\n" << m10 << "\n" << m12;
 	cout<< "\nProduct:\n" << m6 << endl;
@@ -74,7 +74,7 @@ int main()
 	cout<< "\n" << m20 << "\n" << m21;
 	cout<< "\nProduct:\n" << m7 << endl;
 	cout<< "\n" << m9 << "\n" << m21;
-	m21 = mul(m9, m21);
+	m21 = m9 * m21;
 	cout<< "\nProduct:\n" << m21 << endl;
 
 	cout<< "\n\n\nDivision\n";
@@ -148,8 +148,8 @@ int main()
 
 
 	cout<< "\n\nAssignment\n";
-	m3 = m12;
+	matrix<float, 3, 3> m30 = m12;
 	cout<< "Input \n" << m12;
-	cout<< "\nOutput:\n" << m3 << endl;
+	cout<< "\nOutput:\n" << m30 << endl;
 
 }
diff --git a/src/sensor_fusion/test/test_projects/quaternion_test/quaternion_main.cpp b/src/sensor_fusion/test/test_projects/quaternion_test/quaternion_main.cpp
index 330a1ad..09f980f 100644
--- a/src/sensor_fusion/test/test_projects/quaternion_test/quaternion_main.cpp
+++ b/src/sensor_fusion/test/test_projects/quaternion_test/quaternion_main.cpp
@@ -23,9 +23,12 @@ int main()
 {
 	float arr0[4] = {2344.98, 345.24, 456.12, 98.33};
 	float arr1[4] = {0.056, 0.34, -0.0076, 0.001};
+	float axis1[3] = {6.5, 7.5, 8.3};
+	float ang1 = 8.4;
 
-	vect<float> v0(4, arr0);
-	vect<float> v1(4, arr1);
+	vect<float, 4> v0(arr0);
+	vect<float, 4> v1(arr1);
+	vect<float, 3> v2(axis1);
 
 	quaternion<float> q0(v0);
 	quaternion<float> q1(v1);
@@ -63,5 +66,13 @@ int main()
 	cout << "input\t" << q1.m_quat << "\n";
 	q1.quat_normalize();
 	cout << "output\t" << q1.m_quat << "\n\n";
+
+	cout << "Axis2quat\n";
+	cout << "input\t" << " " << v2 << endl;
+	cout << endl;
+	quaternion<float> q11 = axis2quat(v2, ang1);
+	cout << "output\t" << q11.m_quat << "\n\n";
+	cout << endl;
+
 }
 
diff --git a/src/sensor_fusion/test/test_projects/rotation_matrix_test/rotation_matrix_main.cpp b/src/sensor_fusion/test/test_projects/rotation_matrix_test/rotation_matrix_main.cpp
index 21ecadf..9ab8c35 100644
--- a/src/sensor_fusion/test/test_projects/rotation_matrix_test/rotation_matrix_main.cpp
+++ b/src/sensor_fusion/test/test_projects/rotation_matrix_test/rotation_matrix_main.cpp
@@ -25,15 +25,15 @@ int main()
 	float arr2[3][3] = {{2.24, 0.5, 0.023}, {3.675, 5.32, 0.556}, {1.023, 45.75, 621.6}};
 	float arr3[3][3] = {{4.75, 0.65, 0.123}, {0.075, 5.302, 0.56}, {1.113, 0.475, 2.362}};
 
-	matrix<float> m1(3, 3, (float *) arr1);
-	matrix<float> m2(3, 3, (float *) arr2);
-	matrix<float> m3(3, 3, (float *) arr3);
+	matrix<float, 3, 3> m1(arr1);
+	matrix<float, 3, 3> m2(arr2);
+	matrix<float, 3, 3> m3(arr3);
 
 	rotation_matrix<float> rm0, rm5;
 	rotation_matrix<float> rm1(m1);
 	rotation_matrix<float> rm2(m2);
 	rotation_matrix<float> rm3(m3);
-	rotation_matrix<float> rm4(3, 3, (float *) arr1);
+	rotation_matrix<float> rm4(arr1);
 
 	quaternion<float> q0(-0.612372, 0.456436, 0.456436, 0.456436);
 	quaternion<float> q1;
diff --git a/src/sensor_fusion/test/test_projects/sensor_data_test/sensor_data_main.cpp b/src/sensor_fusion/test/test_projects/sensor_data_test/sensor_data_main.cpp
index 5f94e53..4477daa 100644
--- a/src/sensor_fusion/test/test_projects/sensor_data_test/sensor_data_main.cpp
+++ b/src/sensor_fusion/test/test_projects/sensor_data_test/sensor_data_main.cpp
@@ -22,13 +22,16 @@
 int main()
 {
 	float arr1[3] = {1.04, -4.678, -2.34};
-
-	vect<float> v1(3, arr1);
+	float arr2[3] = {0, 0, 1};
+	vect<float,3> v1(arr1);
+	vect<float,3> v2(arr2);
 
 	sensor_data<float> sd1(2.0, 3.0, 4.0, 140737488355328);
 	sensor_data<float> sd2(1.04, -4.678, -2.34, 0);
 	sensor_data<float> sd3(0.054, 1.097, 4.456, 140737488355328);
+
 	sensor_data<float> sd10(v1, 140737488355328);
+	sensor_data<float> sd11(0.2, 0.1, 0.3, 140737488355328);
 
 	cout << "Constructor tests\n";
 	cout << "input\t" << v1 << "\n";
@@ -57,5 +60,10 @@ int main()
 	sensor_data<float> sd8 = scale_data(sd2, xx);
 	cout<< "\n" << sd2.m_data << "\n" << xx;
 	cout<< "\nResult:\n" << sd8.m_data << endl;
+
+	cout<<"\n\n\nConvert Sensor Data to Quaternion:\n";
+	quaternion<float> q = sensor_data2quat(sd11, v2);
+	cout<< "\n" << sd11.m_data << "\n" << v2;
+	cout<< "\nResult:\n" << q.m_quat << endl;
 }
 
diff --git a/src/sensor_fusion/test/test_projects/vector_test/vector_main.cpp b/src/sensor_fusion/test/test_projects/vector_test/vector_main.cpp
index befdeee..bca9d2d 100644
--- a/src/sensor_fusion/test/test_projects/vector_test/vector_main.cpp
+++ b/src/sensor_fusion/test/test_projects/vector_test/vector_main.cpp
@@ -31,24 +31,32 @@ int main()
 	float arr43[6] = {2.3454,-0.0384,-8.90,3.455,6.785,21.345};
 	float arr5[5] = {0.2,-0.4,0.6,-0.8,1.0};
 
-	vect<float> v1(5, arr0);
-	vect<float> v2(5, arr8);
-	vect<float> v10(4, arr3);
-	vect<float> v12(4, arr4);
-	vect<float> v15(6, arr1);
-	vect<float> v20(6, arr43);
-	vect<float> v21(3, arr2);
-	vect<float> v22(3, arr15);
-	vect<float> v3(4);
-	vect<float> v6(3);
-	vect<float> v13(5);
-	vect<float> v95(6);
-	vect<float> v35(5, arr5);
+	vect<float,5> v1(arr0);
+	vect<float,5> v2(arr8);
+	vect<float,4> v10(arr3);
+	vect<float,4> v12(arr4);
+	vect<float,6> v15(arr1);
+	vect<float,6> v20(arr43);
+	vect<float,3> v21(arr2);
+	vect<float,3> v22(arr15);
+	vect<float,5> v31(arr0);
+	vect<float,3> v3;
+	vect<float,2> vn;
+	vect<float,4> v61(arr4);
+	vect<float,3> v6;
+	vect<float,4> vm;
+	vect<float,5> v13;
+	vect<float,6> v95;
+	vect<float,5> v35(arr5);
+	vect<float,5> vl;
+	vect<float,4> vp;
+	vect<float,4> vr(arr4);
+	vect<float,3> vf;
 
 	float arr57[3][3] = {{2.24, 0.5, 0.023}, {3.675, 5.32, 0.556}, {1.023, 45.75, 621.6}};
-	matrix<float> m12(3, 3, (float *) arr57);
+	matrix<float,3 ,3> m12(arr57);
 	float arr67[3][1] = {{2.0}, {3.0}, {4.0}};
-	matrix<float> m32(3, 1, (float *) arr67);
+	matrix<float,3,1> m32(arr67);
 
 	cout<< "Constructor Test\n";
 	cout<< "\n" << v3;
@@ -56,39 +64,41 @@ int main()
 	cout<< "\n\nAddition\n";
 	v3 = v21 + v22;
 	v95 = v15 + v20;
+	cout<< "\n\nNumbers added\n";
 	cout<< "\n" << v21 << "\n" << v22;
 	cout<< "\nSum:\n" << v3 << endl;
+	cout<< "\n\nNumbers added\n";
 	cout<< "\n" << v15 << "\n" << v20;
 	cout<< "\nSum:\n" << v95 << endl;
 
 	float num = 5.5650;
 	float num1 = -2.32;
 	cout<< "\n\n\nScalar addition\n";
-	v3 = v2 + num;
-	v6 = v10 + num1;
+	vl = v2 + num;
+	vm = v10 + num1;
 	cout<< "\nNumber added:" << num;
 	cout<< "\n\n" << v2;
-	cout<< "\nResult:\n\n" << v3;
+	cout<< "\nResult:\n\n" << vl;
 	cout<< "\nNumber added:"<< num1;
 	cout<< "\n\n" << v10;
-	cout<< "\nResult:\n\n"<< v6;
+	cout<< "\nResult:\n\n"<< vm;
 
 	cout<< "\n\n\nSubtraction\n";
-	v6 = v10 - v12;
+	vp = v10 - v12;
 	cout<< "\n" << v10 << "\n" << v12;
-	cout<< "\nDifference:\n" << v6 << endl;
+	cout<< "\nDifference:\n" << vp << endl;
 
 	float x = 4.0;
 	float x1 = -2.5;
 	cout<< "\n\n\nScalar subtraction\n";
 	v13 = v1 - x;
-	v6 = v10 - x1;
+	vp = v10 - x1;
 	cout<< "\nNumber Subtracted:" << x;
 	cout<< "\n\n" << v1;
 	cout<< "\nResult:\n\n" << v13;
 	cout<< "\nNumber Subtracted:" << x1;
 	cout<< "\n\n" << v10;
-	cout<< "\nResult:\n\n" << v6;
+	cout<< "\nResult:\n\n" << vp;
 
 	float xx = 7.2;
 	cout<<"\n\n\nMultiplication\n";
@@ -97,23 +107,20 @@ int main()
 	cout<< "\nProduct:\n" << v13 << endl;
 
 	cout<< "\n\n\nMultiplication matrix x vector:\n";
-	matrix<float> m102 = m32 * v22;
+	matrix<float,3,3> m102 = m32 * v22;
 	cout<< "\n" << m32 <<"\n"<< v22;
 	cout<< "\nProduct:\n"<< m102 << endl;
 
 	cout<< "\n\n\nVector x Multiplication matrix:\n";
-	vect<float> v102 = (v22 * m12);
+	vect<float,3> v102 = (v22 * m12);
 	cout<< "\n" << v22 << "\n" << m12;
 	cout<< "\nProduct:\n" << v102 << endl;
-	float val = mul(v22, m32);
-	cout<< "\n" << v22 << "\n" << m32;
-	cout<< "\nProduct:\n" << val << endl;
 
 	float z = 3.50;
 	float z1 = -5.567;
 	cout<< "\n\n\nScalar multiplication\n";
 	v13 = v1 * z;
-	v6 = v12 * z1;
+	v61 = v12 * z1;
 	cout<< "\nNumber Multiplied:" << z;
 	cout<< "\n\n" << v1;
 	cout<< "\nResult:\n\n" << v13;
@@ -123,18 +130,18 @@ int main()
 
 	float num2 = 5.5;
 	cout<< "\n\n\nDivision\n";
-	v3 = v1 / num2;
+	v31 = v1 / num2;
 	cout<< "\n" << v1 << "\n" << num2;
 	cout<< "\nResult:\n" << v3 << endl;
 
 	cout<< "\n\n\nTranspose:";
 	cout << "\n\n" << v20;
 	cout << "\nResult:\n\n";
-	matrix<float> m101 = (transpose(v20));
+	matrix<float,6,1> m101 = transpose(v20);
 	cout << m101;
 	cout << "\n\n" << m101;
 	cout << "\nResult:\n\n";
-	v20 = (transpose(m101));
+	v20 = transpose(m101);
 	cout << v20;
 
 	cout << "\n\nv1:\n\n" << v1;
@@ -148,9 +155,9 @@ int main()
 	cout << (v10 == v10);
 
 	cout << "\n\nv12:\n\n" << v12;
-	cout << "\n\nv15:\n\n" << v15;
+	cout << "\n\nv15:\n\n" << vr;
 	cout << "\n\n\nv12 != v15 :";
-	cout << (v12 != v15);
+	cout << (v12 != vr);
 
 	cout << "\n\nv15:\n\n" << v15;
 	cout << "\n\nv15:\n\n" << v15;
@@ -158,16 +165,20 @@ int main()
 	cout << (v15 != v15);
 
 	cout<< "\n\nAssignment\n";
-	v3 = v1;
+	v3 = vf;
 	cout<< "Input \n" << v1;
 	cout<< "\nOutput:\n" << v3 << endl;
 
-
-	vect<float> v111 = cross(v21, v22);
+	vect<float,3> v111 = cross(v21, v22);
 	cout<< "\n\n\nCross Product:";
 	cout << "\n\n" << v21 << "\n\n" << v22;
 	cout << "\nResult:\n\n" << v111;
 
+	float val = dot(v21, v22);
+	cout<< "\n\n\nDot Product:";
+	cout << "\n\n" << v21 << "\n\n" << v22;
+	cout << "\nResult:\n\n" << val;
+
 	cout <<  "\n\n\nQueue insert function:";
 	cout << "\nInput:\n\n" << v111;
 	insert_end(v111, (float) 0.9191919);
@@ -180,6 +191,6 @@ int main()
 
 	cout <<  "\n\n\nIs Initialized:";
 	cout << "\nInput:\n\n" << v35;
-	cout << "\nResult:\n\n" << is_initialized(v35);
+	cout << "\nResult:\n\n" << is_initialized(v35) << endl;
 }
 
diff --git a/src/sensor_fusion/vector.cpp b/src/sensor_fusion/vector.cpp
index 5ab9862..2fc932a 100644
--- a/src/sensor_fusion/vector.cpp
+++ b/src/sensor_fusion/vector.cpp
@@ -19,82 +19,44 @@
 
 #if defined (_VECTOR_H_) && defined (_MATRIX_H_)
 
-template <typename TYPE>
-vect<TYPE>::vect(void)
+TYPE_SIZE vect<TYPE, SIZE>::vect(void)
 {
-	m_vec = NULL;
+	for(int i=0;i<SIZE;i++)
+		m_vec[i] = 0;
 }
 
-template <typename TYPE>
-vect<TYPE>::vect(const int size)
+TYPE_SIZE vect<TYPE, SIZE>::vect(TYPE vec_data[SIZE])
 {
-	m_size = size;
-	m_vec = new TYPE [m_size]();
-}
-
-template <typename TYPE>
-vect<TYPE>::vect(const int size, TYPE *vec_data)
-{
-	m_size = size;
-	m_vec = new TYPE [m_size];
 
-	for (int j = 0; j < m_size; j++)
-		m_vec[j] = *vec_data++;
+	for (int j = 0; j < SIZE; j++)
+		m_vec[j] = vec_data[j];
 }
 
-template <typename TYPE>
-vect<TYPE>::vect(const vect<TYPE>& v)
+TYPE_SIZE vect<TYPE, SIZE>::vect(const vect<TYPE, SIZE>& v)
 {
-	m_size = v.m_size;
-	m_vec = new TYPE [m_size];
-
-	for (int q = 0; q < m_size; q++)
+	for (int q = 0; q < SIZE; q++)
 		m_vec[q] = v.m_vec[q];
 }
 
-template <typename TYPE>
-vect<TYPE>::~vect()
+
+TYPE_SIZE vect<TYPE, SIZE>::~vect()
 {
-	if (m_vec != NULL)
-		delete[] m_vec;
 }
 
-template <typename TYPE>
-vect<TYPE> vect<TYPE>::operator =(const vect<TYPE>& v)
+TYPE_SIZE vect<TYPE, SIZE> vect<TYPE, SIZE>::operator =(const vect<TYPE, SIZE>& v)
 {
 	if (this == &v)
 	{
 		return *this;
 	}
-
-	if (m_vec == NULL)
-	{
-		m_size = v.m_size;
-		m_vec = new TYPE [m_size];
-	}
-	else
-	{
-		if (m_size != v.m_size)
-		{
-			delete[] m_vec;
-
-			m_size = v.m_size;
-			m_vec = new TYPE [m_size];
-		}
-	}
-
-
-	for (int q = 0; q < m_size; q++)
+	for (int q = 0; q < SIZE; q++)
 		m_vec[q] = v.m_vec[q];
-
-
-	return *this;
+			return *this;
 }
 
-template <typename TYPE>
-ostream& operator <<(ostream& dout, vect<TYPE>& v)
+T_S ostream& operator <<(ostream& dout, vect<T, S>& v)
 {
-	for (int j = 0; j < v.m_size; j++)
+	for (int j = 0; j < S; j++)
 	{
 		dout << v.m_vec[j] << "\t";
 	}
@@ -104,65 +66,56 @@ ostream& operator <<(ostream& dout, vect<TYPE>& v)
 	return dout;
 }
 
-template <typename T>
-vect<T> operator +(const vect<T> v1, const vect<T> v2)
+T_S vect<T, S> operator +(const vect<T, S> v1, const vect<T, S> v2)
 {
-	assert(v1.m_size == v2.m_size);
+	vect<T, S> v3;
 
-	vect<T> v3(v1.m_size);
-
-	for (int j = 0; j < v1.m_size; j++)
+	for (int j = 0; j < S; j++)
 		v3.m_vec[j] = v1.m_vec[j] + v2.m_vec[j];
 
 	return v3;
 }
 
-template <typename T>
-vect<T> operator +(const vect<T> v, const T val)
+T_S vect<T, S> operator +(const vect<T, S> v, const T val)
 {
-	vect<T> v1(v.m_size);
+	vect<T, S> v1;
 
-	for (int j = 0; j < v.m_size; j++)
+	for (int j = 0; j < S; j++)
 		v1.m_vec[j] = v.m_vec[j] + val;
 
 	return v1;
 }
 
-template <typename T>
-vect<T> operator -(const vect<T> v1, const vect<T> v2)
+T_S vect<T, S> operator -(const vect<T, S> v1, const vect<T, S> v2)
 {
-	assert(v1.m_size == v2.m_size);
-
-	vect<T> v3(v1.m_size);
+	vect<T, S> v3;
 
-	for (int j = 0; j < v1.m_size; j++)
+	for (int j = 0; j < S; j++)
 		v3.m_vec[j] = v1.m_vec[j] - v2.m_vec[j];
 
 	return v3;
 }
 
-template <typename T>
-vect<T> operator -(const vect<T> v, const T val)
+T_S vect<T, S> operator -(const vect<T, S> v, const T val)
 {
-	vect<T> v1(v.m_size);
+	vect<T, S> v1;
 
-	for (int j = 0; j < v.m_size; j++)
+	for (int j = 0; j < S; j++)
 		v1.m_vec[j] = v.m_vec[j] - val;
 
 	return v1;
 }
 
-template <typename T>
-matrix<T> operator *(const matrix<T> m, const vect<T> v)
+T_S_R_C matrix<T, R, S> operator *(const matrix<T, R, C> m, const vect<T, S> v)
 {
-	assert(m.m_rows == v.m_size);
-	assert(m.m_cols == 1);
+	assert(R == S);
+	assert(C == 1);
 
-	matrix<T> m1(m.m_rows, v.m_size);
+	matrix<T,R,S> m1;
 
-	for (int i = 0; i < m1.m_rows; i++)
+	for (int i = 0; i < R; i++)
 	{
-		for (int j = 0; j < m1.m_cols; j++)
+		for (int j = 0; j < S; j++)
 		{
 			m1.m_mat[i][j] = m.m_mat[i][0] * v.m_vec[j];
 		}
@@ -171,52 +124,47 @@ matrix<T> operator *(const matrix<T> m, const vect<T> v)
 	return m1;
 }
 
-template <typename T>
-vect<T> operator *(const vect<T> v, const matrix<T> m)
+T_S_R_C vect<T, S> operator *(const vect<T, S> v, const matrix<T, R, C> m)
 {
-	assert(m.m_rows == v.m_size);
-	assert(m.m_cols != 1);
+	assert(R == S);
+	assert(C != 1);
+	vect<T,C> v1;
 
-	vect<T> v1(m.m_cols);
-
-	for (int j = 0; j < m.m_cols; j++)
+	for (int j = 0; j < C; j++)
 	{
 		v1.m_vec[j] = 0;
-		for (int k = 0; k < m.m_rows; k++)
+		for (int k = 0; k < R; k++)
 			v1.m_vec[j] += v.m_vec[k] * m.m_mat[k][j];
 	}
 
 	return v1;
 }
 
-template <typename T>
-vect<T> operator *(const vect<T> v, const T val)
+T_S vect<T, S> operator *(const vect<T, S> v, const T val)
 {
-	vect<T> v1(v.m_size);
+	vect<T, S> v1;
 
-	for (int j = 0; j < v.m_size; j++)
+	for (int j = 0; j < S; j++)
 		v1.m_vec[j] = v.m_vec[j] * val;
 
 	return v1;
 }
 
-template <typename T>
-vect<T> operator /(const vect<T> v, const T val)
+T_S vect<T, S> operator /(const vect<T, S> v, const T val)
 {
-	vect<T> v1(v.m_size);
+	vect<T, S> v1;
 
-	for (int j = 0; j < v.m_size; j++)
+	for (int j = 0; j < S; j++)
 		v1.m_vec[j] = v.m_vec[j] / val;
 
 	return v1;
 }
 
-template <typename T>
-bool operator ==(const vect<T> v1, const vect<T> v2)
+T_S1_S2 bool operator ==(const vect<T, S1> v1, const vect<T, S2> v2)
 {
-	if (v1.m_size == v2.m_size)
+	if (S1==S2)
 	{
-		for (int i = 0; i < v1.m_size; i++)
+		for (int i = 0; i < S1; i++)
 			if (v1.m_vec[i] != v2.m_vec[i])
 				return false;
 	}
@@ -226,62 +174,42 @@ bool operator ==(const vect<T> v1, const vect<T> v2)
 	return true;
 }
 
-template <typename T>
-bool operator !=(const vect<T> v1, const vect<T> v2)
+T_S1_S2 bool operator !=(const vect<T, S1> v1, const vect<T, S2> v2)
 {
 	return (!(v1 == v2));
 }
 
-template <typename T>
-matrix<T> transpose(const vect<T> v)
+T_S matrix<T, S, 1> transpose(const vect<T, S> v)
 {
-	matrix<T> m(v.m_size, 1);
+	matrix<T, S, 1> m;
 
-	for (int i = 0; i < v.m_size; i++)
+	for (int i = 0; i < S; i++)
 		m.m_mat[i][0] = v.m_vec[i];
 
 	return m;
 }
 
-template <typename T>
-vect<T> transpose(const matrix<T> m)
+T_S vect<T, S> transpose(const matrix<T, S, 1> m)
 {
-	vect<T> v(m.m_rows);
+	vect<T, S> v;
 
-	for (int i = 0; i < m.m_rows; i++)
+	for (int i = 0; i < S; i++)
 		v.m_vec[i] = m.m_mat[i][0];
 
 	return v;
 }
 
-template <typename T>
-T mul(const vect<T> v, const matrix<T> m)
+T_S void insert_end(vect<T, S>& v, T val)
 {
-	assert(m.m_rows == v.m_size);
-	assert(m.m_cols == 1);
-
-	T result = (T) 0;
-
-	for (int k = 0; k < v.m_size; k++)
-		result += v.m_vec[k] * m.m_mat[k][0];
-
-	return result;
-}
-
-
-template <typename T>
-void insert_end(vect<T>& v, T val)
-{
-	for (int i = 0; i < (v.m_size - 1); i++)
+	for (int i = 0; i < (S - 1); i++)
 		v.m_vec[i] = v.m_vec[i+1];
 
-	v.m_vec[v.m_size-1] = val;
+	v.m_vec[S-1] = val;
 }
 
-template <typename T>
-vect<T> cross(const vect<T> v1, const vect<T> v2)
+T_S vect<T, S> cross(const vect<T, S> v1, const vect<T, S> v2)
 {
-	vect<T> v3(v1.m_size);
+	vect<T, S> v3;
 
 	v3.m_vec[0] = ((v1.m_vec[1] * v2.m_vec[2]) - (v1.m_vec[2] * v2.m_vec[1]));
 	v3.m_vec[1] = ((v1.m_vec[2] * v2.m_vec[0]) - (v1.m_vec[0] * v2.m_vec[2]));
@@ -290,10 +218,14 @@ vect<T> cross(const vect<T> v1, const vect<T> v2)
 	return v3;
 }
 
-template <typename T>
-bool is_initialized(const vect<T> v)
+T_S T dot(const vect<T, S> v1, const vect<T, S> v2)
+{
+	return (v1.m_vec[0] * v2.m_vec[0] + v1.m_vec[1] * v2.m_vec[1] + v1.m_vec[2] * v2.m_vec[2]);
+}
+
+T_S bool is_initialized(const vect<T, S> v)
 {
-	vect<T> v1(v.m_size);
+	vect<T, S> v1;
 	bool retval;
 
 	retval = (v == v1) ? false : true;
@@ -301,25 +233,24 @@ bool is_initialized(const vect<T> v)
 	return retval;
 }
 
-template <typename T>
-T var(const vect<T> v)
+T_S T var(const vect<T, S> v)
 {
 	T val = 0;
 	T mean, var, diff;
 
-	for (int i = 0; i < v.m_size; i++)
+	for (int i = 0; i < S; i++)
 		val += v.m_vec[i];
 
-	mean = val / v.m_size;
+	mean = val / S;
 
 	val = 0;
-	for (int i = 0; i < v.m_size; i++)
+	for (int i = 0; i < S; i++)
 	{
 		diff = (v.m_vec[i] - mean);
 		val += diff * diff;
 	}
 
-	var = val / (v.m_size - 1);
+	var = val / (S - 1);
 
 	return var;
 }
diff --git a/src/sensor_fusion/vector.h b/src/sensor_fusion/vector.h
index 5578f3c..c976ecc 100644
--- a/src/sensor_fusion/vector.h
+++ b/src/sensor_fusion/vector.h
@@ -22,53 +22,43 @@
 
 #include "matrix.h"
 
-template <typename TYPE>
-class vect {
-public:
-	int m_size;
-	TYPE *m_vec;
+#define TYPE_SIZE template<typename TYPE, int SIZE>
+#define T_S template<typename T, int S>
+#define T_S_R_C template<typename T, int S, int R, int C>
+#define T_S1_S2 template<typename T, int S1, int S2>
 
+TYPE_SIZE class vect {
+public:
+	TYPE m_vec[SIZE];
 	vect(void);
-	vect(const int size);
-	vect(const int size, TYPE *vec_data);
-	vect(const vect<TYPE>& v);
+	vect(TYPE vec_data[SIZE]);
+	vect(const vect<TYPE, SIZE>& v);
 	~vect();
 
-	vect<TYPE> operator =(const vect<TYPE>& v);
+	vect<TYPE, SIZE> operator =(const vect<TYPE, SIZE>& v);
 
-	template<typename T> friend ostream& operator << (ostream& dout,
-			vect<T>& v);
-	template<typename T> friend vect<T> operator +(const vect<T> v1,
-			const vect<T> v2);
-	template<typename T> friend vect<T> operator +(const vect<T> v,
-			const T val);
-	template<typename T> friend vect<T> operator -(const vect<T> v1,
-			const vect<T> v2);
-	template<typename T> friend vect<T> operator -(const vect<T> v,
-			const T val);
-	template<typename T> friend matrix<T> operator *(const matrix<T> v1,
-			const vect<T> v2);
-	template<typename T> friend vect<T> operator *(const vect<T> v,
-			const matrix<T> m);
-	template<typename T> friend vect<T> operator *(const vect<T> v,
-			const T val);
-	template<typename T> friend vect<T> operator /(const vect<T> v1,
-			const T val);
-	template<typename T> friend bool operator ==(const vect<T> v1,
-			const vect<T> v2);
-	template<typename T> friend bool operator !=(const vect<T> v1,
-			const vect<T> v2);
+	T_S friend ostream& operator << (ostream& dout, vect<T, S>& v);
+	T_S friend vect<T, S> operator +(const vect<T, S> v1, const vect<T, S> v2);
+	T_S friend vect<T, S> operator +(const vect<T, S> v, const T val);
+	T_S friend vect<T, S> operator -(const vect<T,S> v1, const vect<T,S> v2);
+	T_S friend vect<T, S> operator -(const vect<T,S> v, const T val);
+	T_S_R_C friend matrix<T, R, S> operator *(const matrix<T, R, C> v1, const vect<T, S> v2);
+	T_S_R_C friend vect<T, S> operator *(const vect<T, S> v, const matrix<T, R, C> m);
+	T_S friend vect<T, S> operator *(const vect<T, S> v, const T val);
+	T_S friend vect<T, S> operator /(const vect<T,S> v1, const T val);
+	T_S1_S2 friend bool operator ==(const vect<T, S1> v1, const vect<T, S2> v2);
+	T_S1_S2 friend bool operator !=(const vect<T, S1> v1, const vect<T, S2> v2);
 
-	template<typename T> friend T mul(const vect<T> v, const matrix<T> m);
-	template<typename T> friend void insert_end(vect<T>& v, T val);
-	template<typename T> friend matrix<T> transpose(const vect<T> v);
-	template <typename T> friend vect<T> transpose(const matrix<T> m);
-	template<typename T> friend vect<T> cross(const vect<T> v1,
-			const vect<T> v2);
-	template <typename T> friend T var(const vect<T> v);
-	template <typename T> friend bool is_initialized(const vect<T> v);
+	T_S friend void insert_end(vect<T, S>& v, T val);
+	T_S friend matrix<T, S, 1> transpose(const vect<T, S> v);
+	T_S friend vect<T, S> transpose(const matrix<T, S, 1> m);
+	T_S friend vect<T, S> cross(const vect<T, S> v1, const vect<T, S> v2);
+	T_S friend T dot(const vect<T, S> v1, const vect<T, S> v2);
+	T_S friend T var(const vect<T, S> v);
+	T_S friend bool is_initialized(const vect<T, S> v);
 };
 
 #include "vector.cpp"
 
 #endif /* _VECTOR_H_ */
+