diff --git a/CMakeLists.txt b/CMakeLists.txt
index 437c8fb845d1753c4133613fc7195ae37931e6a7..560f180a11ee6767209b5737ca37ef47fdb891b0 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -180,6 +180,9 @@ sbs_target(test-nas-parafoil stm32f429zi_skyward_parafoil)
 add_executable(test-nas src/tests/algorithms/NAS/test-nas.cpp)
 sbs_target(test-nas stm32f429zi_skyward_death_stack_v3)
 
+add_executable(test-fft src/tests/algorithms/FFT/test-fft.cpp)
+sbs_target(test-fft stm32f407vg_stm32f4discovery)
+
 add_executable(test-nas-with-triad src/tests/algorithms/NAS/test-nas-with-triad.cpp)
 sbs_target(test-nas-with-triad stm32f429zi_skyward_death_stack_x)
 
diff --git a/src/tests/algorithms/FFT/test-fft-data.h b/src/tests/algorithms/FFT/test-fft-data.h
new file mode 100644
index 0000000000000000000000000000000000000000..3da338d51b20bef6738186272c69d0dbae52918e
--- /dev/null
+++ b/src/tests/algorithms/FFT/test-fft-data.h
@@ -0,0 +1,65 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Federico Lolli
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#pragma once
+
+#include <vector>
+
+namespace Boardcore
+{
+
+// ------------------------ INPUT ------------------------
+// result of sin(50 * 2πt) + 2sin(25 * 2πt) with t sampling time
+// that sums to 1 period (64 samples)
+static const std::vector<float> SIGNAL{
+    0.000000,   0.288001,    -2.344254,  2.595312,   -0.058260, -1.136140,
+    0.739060,   -1.795279,   2.414214,   -0.000944,  -2.035020, 1.358310,
+    -1.140652,  1.774263,    0.007497,   -2.526806,  2.000000,  -0.565236,
+    0.772864,   0.111324,    -2.554866,  2.469451,   -0.187261, -0.391125,
+    0.414214,   -2.185460,   2.586819,   -0.024999,  -1.472474, 0.932373,
+    -1.578887,  2.249572,    0.000000,   -2.249572,  1.578887,  -0.932373,
+    1.472474,   0.024999,    -2.586819,  2.185460,   -0.414214, 0.391125,
+    0.187261,   -2.469451,   2.554866,   -0.111324,  -0.772864, 0.565236,
+    -2.000000,  2.526806,    -0.007497,  -1.774263,  1.140652,  -1.358310,
+    2.035020,   0.000944,    -2.414214,  1.795279,   -0.739060, 1.136140,
+    0.05826008, -2.59531214, 2.34425399, -0.28800129};
+
+static const int SAMPLES       = 64;
+static const float SAMPLE_RATE = 64.0;
+
+// ------------------------ EXPECTED OUTPUT ------------------------
+static const std::vector<float> INTENSITY{
+    0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000,
+    0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000,
+    1.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000, 0.000000,
+    0.000000, 0.000000, 0.000000, 0.000000, 2.000000, 0.000000, 0.000000,
+    0.000000, 0.000000, 0.000000, 0.000000};
+
+static const std::vector<float> FREQUENCY{
+    0.0,  1.0,  2.0,  3.0,  4.0,  5.0,  6.0,  7.0,  8.0,  9.0,  10.0, 11.0,
+    12.0, 13.0, 14.0, 15.0, 16.0, 17.0, 18.0, 19.0, 20.0, 21.0, 22.0, 23.0,
+    24.0, 25.0, 26.0, 27.0, 28.0, 29.0, 30.0, 31.0, 32.0, 31.0, 32.0, 33.0,
+    34.0, 35.0, 36.0, 37.0, 38.0, 39.0, 40.0, 41.0, 42.0, 43.0, 44.0, 45.0,
+    46.0, 47.0, 48.0, 47.0, 48.0, 49.0, 50.0, 51.0, 52.0, 53.0, 54.0, 55.0,
+    56.0, 57.0, 58.0, 59.0, 60.0, 61.0, 62.0, 63.0};
+
+}  // namespace Boardcore
diff --git a/src/tests/algorithms/FFT/test-fft.cpp b/src/tests/algorithms/FFT/test-fft.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1e4768573970a4261615a005f0836b6df8f36177
--- /dev/null
+++ b/src/tests/algorithms/FFT/test-fft.cpp
@@ -0,0 +1,73 @@
+/* Copyright (c) 2023 Skyward Experimental Rocketry
+ * Author: Federico Lolli
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <algorithms/FFT.h>
+#include <complex.h>
+#include <math.h>
+
+#include <Eigen/Core>
+
+#include "test-fft-data.h"
+
+using namespace Boardcore;
+using namespace Eigen;
+
+int main()
+{
+    bool failed = false;
+
+    Eigen::Vector<float, SAMPLES> signal_vector =
+        Eigen::Vector<float, SAMPLES>::Zero();
+    for (int i = 0; i < SAMPLES; i++)
+    {
+        signal_vector(i) = SIGNAL[i];
+    }
+
+    Vector<std::complex<float>, SAMPLES> fft_result =
+        FFT<SAMPLES>::fft(signal_vector);
+    Vector<float, SAMPLES / 2> fft_freq = FFT<SAMPLES>::fftfreq(SAMPLE_RATE);
+
+    for (int i = 0; i < fft_result.size() / 2; i++)
+    {
+        if (std::abs(2.0 / SAMPLES * std::abs(fft_result(i)) - INTENSITY[i]) >
+            0.001)
+        {
+            printf("FFT result differs from the correct one [%d]: %f != %f\n",
+                   i, fft_result(i).real(), INTENSITY[i]);
+            failed = true;
+        }
+    }
+
+    for (int i = 0; i < fft_freq.size(); i++)
+    {
+        if (fft_freq(i) != FREQUENCY[i])
+        {
+            printf("FFT freq differs from the correct one [%d]: %f != %f\n", i,
+                   fft_freq(i), FREQUENCY[i]);
+            failed = true;
+        }
+    }
+
+    failed ? printf("FAILED\n") : printf("PASSED\n");
+
+    return 0;
+}