diff --git a/include/sta/math/algorithms/kalmanFilter.hpp b/include/sta/math/algorithms/kalmanFilter.hpp
index cbe65b6..b42f853 100644
--- a/include/sta/math/algorithms/kalmanFilter.hpp
+++ b/include/sta/math/algorithms/kalmanFilter.hpp
@@ -62,6 +62,8 @@ public:
 	 */
 	KalmanState predict(KalmanState state, matrix u);
 
+	KalmanState predict(KalmanState state);
+
 	/**
 	 * @brief Corrects the state of the Kalman filter based on a measurement.
 	 * @param state The current state and error convariance matrix.
diff --git a/src/algorithms/kalmanFilter.cpp b/src/algorithms/kalmanFilter.cpp
index 1ea9224..59269d5 100644
--- a/src/algorithms/kalmanFilter.cpp
+++ b/src/algorithms/kalmanFilter.cpp
@@ -37,15 +37,32 @@ KalmanState KalmanFilter::predict(KalmanState state, matrix u)
     return state;
 }
 
+KalmanState KalmanFilter::predict(KalmanState state)
+{
+	state.x = F_ * state.x;
+
+	/*
+	state.x.set(0, 0, state.x[0] + F_.get_idx(0, 3) * state.x[3] + F_.get_idx(0, 6) * state.x[6]);
+	state.x.set(1, 0, state.x[1] + F_.get_idx(1, 4) * state.x[4] + F_.get_idx(1, 7) * state.x[7]);
+	state.x.set(2, 0, state.x[2] + F_.get_idx(2, 5) * state.x[5] + F_.get_idx(2, 8) * state.x[6]);
+	state.x.set(3, 0, state.x[3] + F_.get_idx(3, 6) * state.x[6]);
+	state.x.set(4, 0, state.x[4] + F_.get_idx(4, 7) * state.x[7]);
+	state.x.set(5, 0, state.x[5] + F_.get_idx(5, 8) * state.x[8]);
+	state.x.set(6, 0, state.x[6]);
+	state.x.set(7, 0, state.x[7]);
+	state.x.set(8, 0, state.x[8]);
+	*/
+
+	state.error = F_ * (state.error * F_.T()) + Q_;
+
+	return state;
+}
+
 KalmanState KalmanFilter::correct(KalmanState state, matrix z)
 {
     // Correct step implementation
     // Calculate the Kalman gain
-    matrix K;
-    {
-
-    K = state.error * H_.T() * linalg::inv(H_ * state.error * H_.T() + R_);
-    }
+    matrix K = state.error * H_.T() * linalg::inv(H_ * state.error * H_.T() + R_);
     // Update the state based on the measurement
     state.x = state.x + K * (z - H_ * state.x); //TODO check transpose
     // Update the error covariance matrix
diff --git a/src/quaternion.cpp b/src/quaternion.cpp
index 25e6ea6..c705d20 100644
--- a/src/quaternion.cpp
+++ b/src/quaternion.cpp
@@ -8,6 +8,7 @@
 #include <sta/math/quaternion.hpp>
 
 #include <math.h>
+#include <sta/debug/profile.hpp>
 
 namespace sta
 {
@@ -32,6 +33,7 @@ namespace sta
 			oz *= (M_PI / 180.0f);
 
 			float norm = sqrt(ox*ox + oy*oy + oz*oz);
+
 			if (norm == 0)
 			{
 				return *this;
@@ -42,14 +44,10 @@ namespace sta
 			float cosDt2 = cos(norm * dt2);
 			float sinDt2 = 1/norm * sin(norm * dt2);
 
-			// ox /= norm;
-			// oy /= norm;
-			// oz /= norm;
-
 			float qw = cosDt2*w 	+ -sinDt2*ox*x 	+ -sinDt2*oy*y 	+ -sinDt2*oz*z;
-			float qx = sinDt2*ox*w 	+ cosDt2*x 		+ sinDt2*oz*y 	+ -sinDt2*oy*z;
-			float qy = sinDt2*oy*w 	+ -sinDt2*oz*x 	+ cosDt2*y 		+ sinDt2*ox*z;
-			float qz = sinDt2*oz*w 	+  sinDt2*oy*x 	+ -sinDt2*ox*y 	+ cosDt2*z;
+			float qx = sinDt2*ox*w 	+  cosDt2*x 	+ sinDt2*oz*y 	+ -sinDt2*oy*z;
+			float qy = sinDt2*oy*w 	+ -sinDt2*oz*x 	+  cosDt2*y 	+  sinDt2*ox*z;
+			float qz = sinDt2*oz*w 	+  sinDt2*oy*x 	+ -sinDt2*ox*y 	+  cosDt2*z;
 
 			return Quaternion(qw, qx, qy, qz).normalized();
 	    }