diff --git a/CMakeLists.txt b/CMakeLists.txt
index 89dae27..506df5c 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -17,6 +17,7 @@ add_library(${PROJECT_NAME}
     src/math/functions.cpp
     src/math/foc.cpp
     src/math/operators.cpp
+    src/math/cordic.cpp
     src/observers/tracker.cpp
     src/observers/dq_update.cpp
     src/observers/bemf_observer.cpp
@@ -31,6 +32,10 @@ add_executable(bemf_observer_test tests/bemf_observer_test.cpp)
 target_link_libraries(bemf_observer_test ${PROJECT_NAME})
 add_test(NAME bemf_observer_test COMMAND "${CMAKE_SOURCE_DIR}/bin/bemf_observer_test")
 
+add_executable(cordic_algo_test tests/cordic_algo_test.cpp)
+target_link_libraries(cordic_algo_test ${PROJECT_NAME})
+add_test(NAME cordic_algo_test COMMAND "${CMAKE_SOURCE_DIR}/bin/cordic_algo_test" 45)
+
 # Include Header files
 include_directories(
     include
diff --git a/include/math/cordic.hpp b/include/math/cordic.hpp
new file mode 100644
index 0000000..6e7db91
--- /dev/null
+++ b/include/math/cordic.hpp
@@ -0,0 +1,13 @@
+#pragma once
+
+#include <cstdint>
+
+namespace CORDIC
+{
+  struct Trig
+  {
+    int sin;
+    int cos;
+  };
+  Trig trig(int angle);
+}
diff --git a/src/math/cordic.cpp b/src/math/cordic.cpp
new file mode 100644
index 0000000..cb63883
--- /dev/null
+++ b/src/math/cordic.cpp
@@ -0,0 +1,16 @@
+#include<math/cordic.hpp>
+
+CORDIC::Trig CORDIC::trig(int angle)
+{
+  int32_t atan2[14] = {8192, 4836, 2555, 1297, 651, 325, 162, 81, 40, 20, 10, 5, 2, 1};
+  int32_t x = 9949, y = 0, theta = 0;
+  for(int32_t i = 0; i < 14; i++)
+  {
+    int32_t sigma = (theta < angle) ? 1 : -1;
+    theta = theta + sigma * atan2[i];
+    int32_t nx = x - sigma * (y >> i);
+    y = y + sigma * (x >> i);
+    x = nx;
+  }
+  return {y, x};
+}
diff --git a/tests/cordic_algo_test.cpp b/tests/cordic_algo_test.cpp
new file mode 100644
index 0000000..b60243d
--- /dev/null
+++ b/tests/cordic_algo_test.cpp
@@ -0,0 +1,22 @@
+#include <math/cordic.hpp>
+#include <iostream>
+#include <cmath>
+#include <math/functions.hpp>
+
+int main(int argc, char **argv)
+{
+  if(argc != 2)
+  {
+    std::cerr << "Usage: " << argv[0] << " <angle>" << std::endl;
+    return 1;
+  }
+  int angle = atoi(argv[1]);
+  int angle_cordic = (angle << 16) / 360;
+  CORDIC::Trig trig = CORDIC::trig(angle_cordic);
+  std::cout << "sin(x) in cru = " << trig.sin << " " << "cos(x) in cru = " << trig.cos << std::endl;
+  float sine = (float)trig.sin/16384.0F;
+  float cosine = (float)trig.cos/16384.0F;
+  std::cout << "std::sin(x) error = " << std::sin(angle*PI/180) - sine << " " << "std::cos(x) error = " << std::cos(angle*PI/180) - cosine << std::endl;
+  std::cout << "math::sin(x) error = " << math::sin(angle*PI/180) - sine << " " << "math::cos(x) error = " << math::cos(angle*PI/180) - cosine << std::endl;
+  return 0;
+}