diff --git a/Makefile b/Makefile
index f78c47d..5af8cfb 100644
--- a/Makefile
+++ b/Makefile
@@ -645,6 +645,7 @@ OBJECTS += ./src/drivers/MPU6000.o
 OBJECTS += ./src/drivers/MS5611.o
 OBJECTS += ./src/drivers/stepper.o
 OBJECTS += ./src/drivers/servo.o
+OBJECTS += ./src/control/ImuFusion.o
 OBJECTS += ./src/control/PID.o
 OBJECTS += ./src/math/Utilities.o
 
diff --git a/src/control/ImuFusion.cpp b/src/control/ImuFusion.cpp
new file mode 100644
index 0000000..e7c9508
--- /dev/null
+++ b/src/control/ImuFusion.cpp
@@ -0,0 +1,31 @@
+#include "src/control/ImuFusion.h"
+
+namespace control {
+
+ImuFusion::ImuFusion(mpu6000_spi* pImu)
+: m_pImu(pImu)
+, m_angle(0)
+{
+}
+
+float ImuFusion::getAngle(float dT)
+{
+    int axis = 0;
+
+    float rot = m_pImu->read_rot(axis);
+
+    m_angle += dT*rot;
+
+    //float ratio = 0.99; 
+    float ratio = 0.95f;
+    //float ratio = 0.9996;
+
+    float rawAngle = m_pImu->read_acc_deg(axis); // conversion from G to Deg
+
+    m_angle = (m_angle * ratio) + (rawAngle * ((float)1.0f-ratio));
+
+    return m_angle;
+}
+   
+
+}
\ No newline at end of file
diff --git a/src/control/ImuFusion.h b/src/control/ImuFusion.h
new file mode 100644
index 0000000..11686b7
--- /dev/null
+++ b/src/control/ImuFusion.h
@@ -0,0 +1,23 @@
+#include "src/drivers/MPU6000.h"
+
+namespace control {
+
+class ImuFusion
+{
+
+public:
+ 
+    ImuFusion(mpu6000_spi* pImu);
+
+    float getAngle(float dT);
+
+private:
+
+    mpu6000_spi* m_pImu;
+
+    float m_angle;
+
+
+};
+
+}
\ No newline at end of file