diff --git a/CMakeLists.txt b/CMakeLists.txt
index 433a59c0551d9149da2a86e27923e3a1c93fec23..1c07b46355e37135c849ff6faa1c81f512fb09d7 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -135,15 +135,18 @@ sbs_target(test-kalman-eigen-benchmark stm32f429zi_stm32f4discovery)
 add_executable(test-tmp src/tests/algorithms/NAS/test-tmp.cpp)
 sbs_target(test-tmp stm32f429zi_skyward_death_stack_x)
 
-add_executable(test-attitude src/tests/algorithms/NAS/test-attitude.cpp)
-sbs_target(test-attitude stm32f429zi_skyward_death_stack_x)
-
 add_executable(test-attitude-parafoil src/tests/algorithms/NAS/test-attitude-parafoil.cpp)
 sbs_target(test-attitude-parafoil stm32f429zi_parafoil)
 
+add_executable(test-attitude-stack src/tests/algorithms/NAS/test-attitude-stack.cpp)
+sbs_target(test-attitude-stack stm32f429zi_skyward_death_stack_x)
+
 add_executable(test-nas-parafoil src/tests/algorithms/NAS/test-nas-parafoil.cpp)
 sbs_target(test-nas-parafoil stm32f429zi_parafoil)
 
+add_executable(test-nas-stack src/tests/algorithms/NAS/test-nas-stack.cpp)
+sbs_target(test-nas-stack stm32f429zi_skyward_death_stack_x)
+
 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)
 
@@ -347,7 +350,7 @@ add_executable(test-ms5803 src/tests/sensors/test-ms5803.cpp)
 sbs_target(test-ms5803 stm32f429zi_skyward_death_stack_x)
 
 add_executable(test-ubloxgps-serial src/tests/sensors/test-ubloxgps.cpp)
-sbs_target(test-ubloxgps-serial stm32f407vg_stm32f4discovery)
+sbs_target(test-ubloxgps-serial stm32f429zi_skyward_death_stack_x)
 
 add_executable(test-ubloxgps-spi src/tests/sensors/test-ubloxgps.cpp)
 target_compile_definitions(test-ubloxgps-spi PRIVATE USE_SPI)
diff --git a/src/tests/algorithms/NAS/test-attitude.cpp b/src/tests/algorithms/NAS/test-attitude-stack.cpp
similarity index 100%
rename from src/tests/algorithms/NAS/test-attitude.cpp
rename to src/tests/algorithms/NAS/test-attitude-stack.cpp
diff --git a/src/tests/algorithms/NAS/test-nas-parafoil.cpp b/src/tests/algorithms/NAS/test-nas-parafoil.cpp
index eb969d4631f481d59968aa9d5f454da69316752e..57fd167c839ec3bdef2d24c69417ddb7f118aab1 100644
--- a/src/tests/algorithms/NAS/test-nas-parafoil.cpp
+++ b/src/tests/algorithms/NAS/test-nas-parafoil.cpp
@@ -40,6 +40,7 @@ NASConfig getEKConfig();
 void setInitialOrientation();
 void init();
 void step();
+void print();
 
 Vector3f nedMag   = Vector3f(0.4747, 0.0276, 0.8797);
 Vector2f startPos = Vector2f(45.501141, 9.156281);
@@ -58,7 +59,8 @@ int main()
     setInitialOrientation();
 
     TaskScheduler scheduler;
-    scheduler.addTask(step, 20);
+    scheduler.addTask(step, 20, TaskScheduler::Policy::RECOVER);
+    scheduler.addTask(print, 250);
     scheduler.start();
 
     while (true)
@@ -131,9 +133,7 @@ void step()
 
     Vector2f gpsPos(gpsData.latitude, gpsData.longitude);
     gpsPos = Aeroutils::geodetic2NED(gpsPos, startPos);
-    // std::cout << gpsPos.transpose() << " - ";
     Vector2f gpsVel(gpsData.velocityNorth, gpsData.velocityNorth);
-    // std::cout << gpsVel.transpose() << " - ";
     Vector4f gpsCorrection;
     // cppcheck-suppress constStatement
     gpsCorrection << gpsPos, gpsVel;
@@ -158,28 +158,26 @@ void step()
     // Correct step
     magneticField.normalize();
     nas->correctMag(magneticField);
-    // acceleration.normalize();
-    // nas->correctAcc(acceleration);
-    if (gpsPos[0] < 1e3 && gpsPos[0] > -1e3 && gpsPos[1] < 1e3 &&
-        gpsPos[1] > -1e3)
+    acceleration.normalize();
+    nas->correctAcc(acceleration);
+    if (gpsData.fix)
         nas->correctGPS(gpsCorrection);
     nas->correctBaro(100000, 110000, 20 + 273.5);
 
-    // printf(
-    //     "[gps] timestamp: % 4.3f, fix: %01d lat: % f lon: % f "
-    //     "height: %4.1f nsat: %2d speed: %3.2f velN: % 3.2f velE: % 3.2f "
-    //     "track %3.1f\n",
-    //     (float)gpsData.gpsTimestamp / 1000000, gpsData.fix, gpsData.latitude,
-    //     gpsData.longitude, gpsData.height, gpsData.satellites, gpsData.speed,
-    //     gpsData.velocityNorth, gpsData.velocityEast, gpsData.track);
-
     auto nasState = nas->getState();
-    // printf("w%fwa%fab%fbc%fc\n", nasState.qw, nasState.qx, nasState.qy,
-    //        nasState.qz);
-    // printf("%f, %f, %f, %f, %f, %f, %f\n", nasState.n, nasState.e,
-    // nasState.d,
-    //        nasState.qw, nasState.qx, nasState.qy, nasState.qz);
     Logger::getInstance().log(imuData);
     Logger::getInstance().log(gpsData);
     Logger::getInstance().log(nasState);
 }
+
+void print()
+{
+    auto gpsData  = gps->getLastSample();
+    auto nasState = nas->getState();
+
+    Vector2f gpsPos(gpsData.latitude, gpsData.longitude);
+    gpsPos = Aeroutils::geodetic2NED(gpsPos, startPos);
+
+    printf("%d, %f, %f, %f, %f, %f, %f\n", gpsData.fix, gpsPos[0], gpsPos[1],
+           nasState.n, nasState.e, nasState.vn, nasState.ve);
+}
diff --git a/src/tests/algorithms/NAS/test-nas-stack.cpp b/src/tests/algorithms/NAS/test-nas-stack.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e72a1c9830b63682cffff7968cbc27106b85a61f
--- /dev/null
+++ b/src/tests/algorithms/NAS/test-nas-stack.cpp
@@ -0,0 +1,216 @@
+/* Copyright (c) 2022 Skyward Experimental Rocketry
+ * Author: 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 <algorithms/NAS/NAS.h>
+#include <algorithms/NAS/StateInitializer.h>
+#include <logger/Logger.h>
+#include <miosix.h>
+#include <sensors/BMX160/BMX160.h>
+#include <sensors/MS5803/MS5803.h>
+#include <sensors/SensorManager.h>
+#include <sensors/UbloxGPS/UbloxGPS.h>
+#include <utils/AeroUtils/AeroUtils.h>
+#include <utils/Constants.h>
+#include <utils/SkyQuaternion/SkyQuaternion.h>
+
+#include <iostream>
+
+using namespace miosix;
+using namespace Boardcore;
+using namespace Eigen;
+
+NASConfig getEKConfig();
+void setInitialOrientation();
+void init();
+void step();
+void print();
+
+Vector3f nedMag   = Vector3f(0.4747, 0.0276, 0.8797);
+Vector2f startPos = Vector2f(45.501141, 9.156281);
+
+NAS* nas;
+
+SPIBus spi1(SPI1);
+BMX160* imu   = nullptr;
+UbloxGPS* gps = nullptr;
+MS5803* baro  = nullptr;
+
+int main()
+{
+    init();
+
+    nas = new NAS(getEKConfig());
+    setInitialOrientation();
+
+    TaskScheduler scheduler;
+    scheduler.addTask(step, 20, TaskScheduler::Policy::RECOVER);
+    // scheduler.addTask(print, 250);
+    scheduler.start();
+
+    while (true)
+        Thread::sleep(1000);
+}
+
+NASConfig getEKConfig()
+{
+    NASConfig config;
+
+    config.T              = 0.02f;
+    config.SIGMA_BETA     = 0.0001f;
+    config.SIGMA_W        = 0.3f;
+    config.SIGMA_MAG      = 0.1f;
+    config.SIGMA_GPS      = 10.0f;
+    config.SIGMA_BAR      = 4.3f;
+    config.SIGMA_POS      = 10.0;
+    config.SIGMA_VEL      = 10.0;
+    config.P_POS          = 1.0f;
+    config.P_POS_VERTICAL = 10.0f;
+    config.P_VEL          = 1.0f;
+    config.P_VEL_VERTICAL = 10.0f;
+    config.P_ATT          = 0.01f;
+    config.P_BIAS         = 0.01f;
+    config.SATS_NUM       = 6.0f;
+    config.NED_MAG        = nedMag;
+
+    return config;
+}
+
+void setInitialOrientation()
+{
+    Matrix<float, 13, 1> x = Matrix<float, 13, 1>::Zero();
+
+    // Set quaternions
+    Vector4f q = SkyQuaternion::eul2quat({0, 0, 0});
+    x(6)       = q(0);
+    x(7)       = q(1);
+    x(8)       = q(2);
+    x(9)       = q(3);
+
+    nas->setX(x);
+}
+
+void init()
+{
+    SPIBusConfig spiConfig;
+    spiConfig.clockDivider = SPI::ClockDivider::DIV_8;
+
+    BMX160Config bmx_config;
+    bmx_config.fifoMode      = BMX160Config::FifoMode::HEADER;
+    bmx_config.fifoWatermark = 80;
+    bmx_config.fifoInterrupt = BMX160Config::FifoInterruptPin::PIN_INT1;
+
+    bmx_config.temperatureDivider = 1;
+
+    bmx_config.accelerometerRange = BMX160Config::AccelerometerRange::G_16;
+
+    bmx_config.gyroscopeRange = BMX160Config::GyroscopeRange::DEG_1000;
+
+    bmx_config.accelerometerDataRate = BMX160Config::OutputDataRate::HZ_100;
+    bmx_config.gyroscopeDataRate     = BMX160Config::OutputDataRate::HZ_100;
+    bmx_config.magnetometerRate      = BMX160Config::OutputDataRate::HZ_100;
+
+    bmx_config.gyroscopeUnit = BMX160Config::GyroscopeMeasureUnit::RAD;
+
+    imu =
+        new BMX160(spi1, sensors::bmx160::cs::getPin(), bmx_config, spiConfig);
+    imu->init();
+
+    gps = new UbloxGPS(921600, 10, 2, "gps", 38400);
+    gps->init();
+    gps->start();
+
+    baro = new MS5803(spi1, sensors::ms5803::cs::getPin());
+    baro->init();
+
+    Logger::getInstance().start();
+}
+
+void step()
+{
+    imu->sample();
+    gps->sample();
+    baro->sample();
+    auto imuData  = imu->getLastSample();
+    auto gpsData  = gps->getLastSample();
+    auto baroData = baro->getLastSample();
+
+    Vector3f acceleration(imuData.accelerationX, imuData.accelerationY,
+                          imuData.accelerationZ);
+    Vector3f angularVelocity(imuData.angularVelocityX, imuData.angularVelocityY,
+                             imuData.angularVelocityZ);
+    Vector3f magneticField(imuData.magneticFieldX, imuData.magneticFieldY,
+                           imuData.magneticFieldZ);
+
+    Vector2f gpsPos(gpsData.latitude, gpsData.longitude);
+    gpsPos = Aeroutils::geodetic2NED(gpsPos, startPos);
+    Vector2f gpsVel(gpsData.velocityNorth, gpsData.velocityEast);
+    Vector4f gpsCorrection;
+    // cppcheck-suppress constStatement
+    gpsCorrection << gpsPos, gpsVel;
+
+    // Calibration
+    {
+        Vector3f offset{-1.63512255486542, 3.46523431469979, -3.08516033954451};
+        angularVelocity = angularVelocity - offset;
+        angularVelocity = angularVelocity / 180 * Constants::PI / 10;
+        Vector3f b{21.5356818859811, -22.7697302909894, -2.68219304319269};
+        Matrix3f A{{0.688760050772712, 0, 0},
+                   {0, 0.637715211784480, 0},
+                   {0, 0, 2.27669720320908}};
+        magneticField = (magneticField - b).transpose() * A;
+    }
+
+    // Predict step
+    nas->predictGyro(angularVelocity);
+    nas->predictAcc(acceleration);
+
+    // Correct step
+    magneticField.normalize();
+    nas->correctMag(magneticField);
+    acceleration.normalize();
+    nas->correctAcc(acceleration);
+    if (gpsData.fix)
+        nas->correctGPS(gpsCorrection);
+    nas->correctBaro(100000, Constants::MSL_PRESSURE,
+                     Constants::MSL_TEMPERATURE);
+
+    auto nasState = nas->getState();
+
+    Logger::getInstance().log(imuData);
+    Logger::getInstance().log(gpsData);
+    Logger::getInstance().log(baroData);
+    Logger::getInstance().log(nasState);
+}
+
+void print()
+{
+    auto gpsData  = gps->getLastSample();
+    auto baroData = baro->getLastSample();
+    auto nasState = nas->getState();
+
+    Vector2f gpsPos(gpsData.latitude, gpsData.longitude);
+    gpsPos = Aeroutils::geodetic2NED(gpsPos, startPos);
+
+    printf("%d, %f, %f, %f, %f, %f, %f, %f, %f\n", gpsData.fix, gpsPos[0],
+           gpsPos[1], nasState.n, nasState.e, nasState.d, nasState.vn,
+           nasState.ve, baroData.pressure);
+}
diff --git a/src/tests/sensors/test-ms5803.cpp b/src/tests/sensors/test-ms5803.cpp
index f3a8bcc1539d5b4eb9a7255f9056a85d043f6fec..4c00318899a0fd596e002c0baa3e350203ecd876 100644
--- a/src/tests/sensors/test-ms5803.cpp
+++ b/src/tests/sensors/test-ms5803.cpp
@@ -35,12 +35,9 @@ using namespace miosix;
 
 int main()
 {
-    SPIBusConfig spiConfig;
+    // Sample temperature every 10 pressure samples
     SPIBus spiBus(SPI1);
-    SPISlave spiSlave(spiBus, miosix::sensors::ms5803::cs::getPin(), spiConfig);
-
-    // Sample temperature every 5 pressure samples
-    MS5803 sensor(spiSlave, 10);
+    MS5803 sensor(spiBus, miosix::sensors::ms5803::cs::getPin(), {}, 10);
 
     Thread::sleep(100);
 
diff --git a/src/tests/sensors/test-ubloxgps.cpp b/src/tests/sensors/test-ubloxgps.cpp
index 0c611daf4e7ffc2ccabcc56da70d989496e1d45f..24b6cc092a73e531765fbbe4897a29133a827014 100644
--- a/src/tests/sensors/test-ubloxgps.cpp
+++ b/src/tests/sensors/test-ubloxgps.cpp
@@ -69,15 +69,17 @@ int main()
 
     while (true)
     {
+        printf("a\n");
         // Give time to the thread
         Thread::sleep(1000 / RATE);
 
         // Sample
         gps.sample();
+        printf("b\n");
         dataGPS = gps.getLastSample();
 
         // Print out the latest sample
-        TRACE(
+        printf(
             "[gps] timestamp: % 4.3f, fix: %01d lat: % f lon: % f "
             "height: %4.1f nsat: %2d speed: %3.2f velN: % 3.2f velE: % 3.2f "
             "track %3.1f\n",