diff --git a/cmake/boardcore.cmake b/cmake/boardcore.cmake
index af7bd7e40ee90659f52b2cc0d0f4aa14b1baa531..49c7e5016601ccf8571c727c9ad078a644ae2e14 100644
--- a/cmake/boardcore.cmake
+++ b/cmake/boardcore.cmake
@@ -26,15 +26,16 @@ include(boardcore-host)
 foreach(OPT_BOARD ${BOARDS})
     set(BOARDCORE_LIBRARY boardcore-${OPT_BOARD})
     add_library(${BOARDCORE_LIBRARY} STATIC EXCLUDE_FROM_ALL
+
         # Actuators
         ${SBS_BASE}/src/shared/actuators/HBridge/HBridge.cpp
         ${SBS_BASE}/src/shared/actuators/Servo/Servo.cpp
 
-
         # Algorithms
         ${SBS_BASE}/src/shared/algorithms/ADA/ADA.cpp
         ${SBS_BASE}/src/shared/algorithms/AirBrakes/AirBrakes.cpp
         ${SBS_BASE}/src/shared/algorithms/NAS/NAS.cpp
+        ${SBS_BASE}/src/shared/algorithms/NAS/StateInitializer.cpp
 
         # Debug
         ${SBS_BASE}/src/shared/utils/Debug.cpp
diff --git a/src/shared/algorithms/NAS/StateInitializer.cpp b/src/shared/algorithms/NAS/StateInitializer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5fa2a13934a647bb348ac064332e5ec735d6465e
--- /dev/null
+++ b/src/shared/algorithms/NAS/StateInitializer.cpp
@@ -0,0 +1,94 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Authors: Marco Cella, Alberto Nidasio
+ *
+ * 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 "StateInitializer.h"
+
+namespace Boardcore
+{
+StateInitializer::StateInitializer() { x_init << Eigen::MatrixXf::Zero(13, 1); }
+
+void StateInitializer::eCompass(const Eigen::Vector3f acc,
+                                const Eigen::Vector3f mag)
+{
+    // ndr: since this method runs only when the rocket is stationary, there's
+    // no need to add the gravity vector because the accelerometers already
+    // measure it. This is not true if we consider the flying rocket.
+
+    Eigen::Vector3f am(acc.cross(mag));
+
+    Eigen::Matrix3f R;
+    R << am.cross(acc), am, acc;
+    R.col(0).normalize();
+    R.col(1).normalize();
+    R.col(2).normalize();
+
+    Eigen::Vector4f x_quat = SkyQuaternion::rotationMatrix2quat(R);
+
+    x_init(NAS::IDX_QUAT)     = x_quat(0);
+    x_init(NAS::IDX_QUAT + 1) = x_quat(1);
+    x_init(NAS::IDX_QUAT + 2) = x_quat(2);
+    x_init(NAS::IDX_QUAT + 3) = x_quat(3);
+}
+
+void StateInitializer::triad(Eigen::Vector3f& acc, Eigen::Vector3f& mag,
+                             Eigen::Vector3f& nedMag)
+{
+    // The gravity vector is expected to be read inversely because
+    // accelerometers read the binding reaction
+    Eigen::Vector3f nedAcc(0.0f, 0.0f, -1.0f);
+
+    // Compute the reference triad
+    Eigen::Vector3f R1 = nedAcc;
+    Eigen::Vector3f R2 = nedAcc.cross(nedMag).normalized();
+    Eigen::Vector3f R3 = R1.cross(R2);
+
+    // Compute the measured triad
+    Eigen::Vector3f r1 = acc;
+    Eigen::Vector3f r2 = acc.cross(mag).normalized();
+    Eigen::Vector3f r3 = r1.cross(r2);
+
+    // Compose the reference and measured matrixes
+    Eigen::Matrix3f M;
+    M << R1, R2, R3;
+    Eigen::Matrix3f m;
+    m << r1, r2, r3;
+
+    // Compute the rotation matrix and the corresponding quaternion
+    Eigen::Matrix3f A = M * m.transpose();
+    Eigen::Vector4f q = SkyQuaternion::rotationMatrix2quat(A);
+
+    // Save the orientation in the state
+    x_init.block<4, 1>(NAS::IDX_QUAT, 0) = q;
+}
+
+void StateInitializer::positionInit(const float pressure,
+                                    const float pressureRef,
+                                    const float temperatureRef)
+{
+    x_init(0) = 0.0;
+    x_init(1) = 0.0;
+    // msl altitude (in NED, so negative)
+    x_init(2) = -Aeroutils::relAltitude(pressure, pressureRef, temperatureRef);
+}
+
+Eigen::Matrix<float, 13, 1> StateInitializer::getInitX() { return x_init; }
+}  // namespace Boardcore
\ No newline at end of file
diff --git a/src/shared/algorithms/NAS/StateInitializer.h b/src/shared/algorithms/NAS/StateInitializer.h
index 959597beb70db8e4b2133eacf12df22d90f10add..8b72cad89b4300e623a849bae9a0d6e29d8d5b5e 100644
--- a/src/shared/algorithms/NAS/StateInitializer.h
+++ b/src/shared/algorithms/NAS/StateInitializer.h
@@ -97,73 +97,4 @@ private:
 
     PrintLogger log = Logging::getLogger("initstates");
 };
-
-StateInitializer::StateInitializer() { x_init << Eigen::MatrixXf::Zero(13, 1); }
-
-void StateInitializer::eCompass(const Eigen::Vector3f acc,
-                                const Eigen::Vector3f mag)
-{
-    // ndr: since this method runs only when the rocket is stationary, there's
-    // no need to add the gravity vector because the accelerometers already
-    // measure it. This is not true if we consider the flying rocket.
-
-    Eigen::Vector3f am(acc.cross(mag));
-
-    Eigen::Matrix3f R;
-    R << am.cross(acc), am, acc;
-    R.col(0).normalize();
-    R.col(1).normalize();
-    R.col(2).normalize();
-
-    Eigen::Vector4f x_quat = SkyQuaternion::rotationMatrix2quat(R);
-
-    x_init(NAS::IDX_QUAT)     = x_quat(0);
-    x_init(NAS::IDX_QUAT + 1) = x_quat(1);
-    x_init(NAS::IDX_QUAT + 2) = x_quat(2);
-    x_init(NAS::IDX_QUAT + 3) = x_quat(3);
-}
-
-void StateInitializer::triad(Eigen::Vector3f& acc, Eigen::Vector3f& mag,
-                             Eigen::Vector3f& nedMag)
-{
-    // The gravity vector is expected to be read inversely because
-    // accelerometers read the binding reaction
-    Eigen::Vector3f nedAcc(0.0f, 0.0f, -1.0f);
-
-    // Compute the reference triad
-    Eigen::Vector3f R1 = nedAcc;
-    Eigen::Vector3f R2 = nedAcc.cross(nedMag).normalized();
-    Eigen::Vector3f R3 = R1.cross(R2);
-
-    // Compute the measured triad
-    Eigen::Vector3f r1 = acc;
-    Eigen::Vector3f r2 = acc.cross(mag).normalized();
-    Eigen::Vector3f r3 = r1.cross(r2);
-
-    // Compose the reference and measured matrixes
-    Eigen::Matrix3f M;
-    M << R1, R2, R3;
-    Eigen::Matrix3f m;
-    m << r1, r2, r3;
-
-    // Compute the rotation matrix and the corresponding quaternion
-    Eigen::Matrix3f A = M * m.transpose();
-    Eigen::Vector4f q = SkyQuaternion::rotationMatrix2quat(A);
-
-    // Save the orientation in the state
-    x_init.block<4, 1>(NAS::IDX_QUAT, 0) = q;
-}
-
-void StateInitializer::positionInit(const float pressure,
-                                    const float pressureRef,
-                                    const float temperatureRef)
-{
-    x_init(0) = 0.0;
-    x_init(1) = 0.0;
-    // msl altitude (in NED, so negative)
-    x_init(2) = -Aeroutils::relAltitude(pressure, pressureRef, temperatureRef);
-}
-
-Eigen::Matrix<float, 13, 1> StateInitializer::getInitX() { return x_init; }
-
 }  // namespace Boardcore