diff --git a/include/sta/math/quaternion.hpp b/include/sta/math/quaternion.hpp
index 6c4095d..0e7e528 100644
--- a/include/sta/math/quaternion.hpp
+++ b/include/sta/math/quaternion.hpp
@@ -8,6 +8,7 @@
 #ifndef STA_PEAK_QUATERNION_HPP
 #define STA_PEAK_QUATERNION_HPP
 
+#include <sta/math/linalg/matrix.hpp>
 namespace sta
 {
 	namespace math {
@@ -55,11 +56,8 @@ namespace sta
 			 */
 			float norm();
 
-			/**
-			 * @brief 
-			 * 
-			 * @return Quaternion 
-			 */
+			matrix toRotationMatrix();
+
 			Quaternion normalized();
 
 			Quaternion conjugate();			
diff --git a/src/quaternion.cpp b/src/quaternion.cpp
index 54c1b16..25e6ea6 100644
--- a/src/quaternion.cpp
+++ b/src/quaternion.cpp
@@ -54,6 +54,23 @@ namespace sta
 			return Quaternion(qw, qx, qy, qz).normalized();
 	    }
 
+		matrix Quaternion::toRotationMatrix(){
+			
+			matrix R = matrix(3, 3);
+			R.set(0, 0, 2 *(w*w + x*x) - 1);
+			R.set(0, 1, 2 *(x*y - w*z));
+			R.set(0, 2, 2 *(x*z + w*y));
+
+			R.set(1, 0, 2 *(x*y + w*z));
+			R.set(1, 1, 2 *(w*w + y*y) - 1);
+			R.set(1, 2, 2 *(y*z - w*x));
+
+			R.set(2, 0, 2 *(x*z - w*y));
+			R.set(2, 1, 2 *(y*z + w*x));
+			R.set(2, 2, 2 *(w*w + z*z) - 1);
+			return R;
+		}
+
 	    Quaternion Quaternion::unit()
 	    {
 	    	return Quaternion();