diff --git a/CMakeLists.txt b/CMakeLists.txt
index 4100936c084b7c836ee5e26555face62957c591f..433a59c0551d9149da2a86e27923e3a1c93fec23 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -132,17 +132,26 @@ sbs_target(test-stepper stm32f429zi_stm32f4discovery)
 add_executable(test-kalman-eigen-benchmark src/tests/algorithms/Kalman/test-kalman-eigen-benchmark.cpp)
 sbs_target(test-kalman-eigen-benchmark stm32f429zi_stm32f4discovery)
 
-add_executable(test-nas src/tests/algorithms/NAS/test-nas.cpp)
-sbs_target(test-nas stm32f429zi_skyward_death_stack_x)
+add_executable(test-tmp src/tests/algorithms/NAS/test-tmp.cpp)
+sbs_target(test-tmp 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_parafoil)
+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-nas-with-logs src/tests/algorithms/NAS/test-nas-with-logs.cpp)
-sbs_target(test-nas-with-logs 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-with-triad src/tests/algorithms/NAS/test-nas-with-triad.cpp)
+sbs_target(test-nas-with-triad stm32f429zi_skyward_death_stack_x)
 
 add_executable(test-triad src/tests/algorithms/NAS/test-triad.cpp)
-sbs_target(test-triad stm32f429zi_parafoil)
+sbs_target(test-triad stm32f429zi_skyward_death_stack_x)
+
+add_executable(test-triad-parafoil src/tests/algorithms/NAS/test-triad-parafoil.cpp)
+sbs_target(test-triad-parafoil stm32f429zi_parafoil)
 
 #-----------------------------------------------------------------------------#
 #                               Tests - Drivers                               #
diff --git a/src/shared/algorithms/NAS/NAS.cpp b/src/shared/algorithms/NAS/NAS.cpp
index efb132cd7d1c500d5150d2e96732e7460bc05c79..e119c361e8a72e7f898725c4b942a720b6b6076e 100644
--- a/src/shared/algorithms/NAS/NAS.cpp
+++ b/src/shared/algorithms/NAS/NAS.cpp
@@ -224,7 +224,7 @@ void NAS::correctMag(const Vector3f& mag)
     P.block<6, 6>(IDX_QUAT, IDX_QUAT) = Pq;
 }
 
-void NAS::correctAcc(const Eigen::Vector3f& acceleration)
+void NAS::correctAcc(const Eigen::Vector3f& acc)
 {
     Vector4f q = x.block<4, 1>(IDX_QUAT, 0);
     Matrix3f A = body2ned(q).transpose();
@@ -249,10 +249,8 @@ void NAS::correctAcc(const Eigen::Vector3f& acceleration)
 
     Matrix<float, 6, 3> K = Pq * H.transpose() * S.inverse();
 
-    Eigen::Vector3f a = acceleration;
-    a.normalize();
     aEst.normalize();
-    Vector3f e             = a - aEst;
+    Vector3f e             = acc - aEst;
     Matrix<float, 6, 1> dx = K * e;
     Vector4f r{0.5f * dx(0), 0.5f * dx(1), 0.5f * dx(2),
                sqrtf(1.0f - 0.25f * dx.head<3>().squaredNorm())};
diff --git a/src/shared/algorithms/NAS/NAS.h b/src/shared/algorithms/NAS/NAS.h
index e5752e3ac530ea15b3fafbcfdabb8e6e0a90f3b8..d232f4beb6f6951c1d8ecded27b3aea6c2c9a77f 100644
--- a/src/shared/algorithms/NAS/NAS.h
+++ b/src/shared/algorithms/NAS/NAS.h
@@ -92,7 +92,7 @@ public:
     /**
      * @brief Correction with accelerometer data.
      *
-     * @param u Vector with acceleration data [x y z][m/s^2]
+     * @param u Normaliezed vector with acceleration data [x y z]
      */
     void correctAcc(const Eigen::Vector3f& acceleration);
 
diff --git a/src/shared/utils/MovingAverage.h b/src/shared/utils/MovingAverage.h
index 94e8fda20d5577c21c9de585a6f3081547c2a0e3..a5f64e04f47bb0a93b1df5e7d071a2397fc10cc3 100644
--- a/src/shared/utils/MovingAverage.h
+++ b/src/shared/utils/MovingAverage.h
@@ -48,9 +48,9 @@ public:
 private:
     T value = 0;
 
-    const int movingAverageN           = 20;
-    const float movingAverageCoeff     = 1 / (float)movingAverageN;
-    const float movingAverageCompCoeff = 1 - movingAverageCoeff;
+    int movingAverageN           = 20;
+    float movingAverageCoeff     = 1 / (float)movingAverageN;
+    float movingAverageCompCoeff = 1 - movingAverageCoeff;
 };
 
 }  // namespace Boardcore
diff --git a/src/tests/algorithms/NAS/test-attitude-parafoil.cpp b/src/tests/algorithms/NAS/test-attitude-parafoil.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4f8af2b692b965d8873f721c62d57308931475a8
--- /dev/null
+++ b/src/tests/algorithms/NAS/test-attitude-parafoil.cpp
@@ -0,0 +1,141 @@
+/* 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 <miosix.h>
+#include <sensors/MPU9250/MPU9250.h>
+#include <sensors/SensorManager.h>
+#include <utils/SkyQuaternion/SkyQuaternion.h>
+
+#include <iostream>
+
+using namespace miosix;
+using namespace Boardcore;
+using namespace Eigen;
+
+NASConfig getEKConfig();
+void setInitialOrientation();
+void imuInit();
+void imuStep();
+
+Vector3f nedMag = Vector3f(0.4747, 0.0276, 0.8797);
+
+NAS* nas;
+
+SPIBus spi1(SPI1);
+MPU9250* imu = nullptr;
+
+int main()
+{
+    imuInit();
+
+    nas = new NAS(getEKConfig());
+    setInitialOrientation();
+
+    TaskScheduler scheduler;
+    scheduler.addTask(imuStep, 20);
+    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()
+{
+    Eigen::Matrix<float, 13, 1> x;
+
+    // Set quaternions
+    Eigen::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 imuInit()
+{
+    imu = new MPU9250(spi1, sensors::mpu9250::cs::getPin());
+    imu->init();
+}
+
+void imuStep()
+{
+    imu->sample();
+    auto data = imu->getLastSample();
+    Vector3f acceleration(data.accelerationX, data.accelerationY,
+                          data.accelerationZ);
+    Vector3f angularVelocity(data.angularVelocityX, data.angularVelocityY,
+                             data.angularVelocityZ);
+    Vector3f magneticField(data.magneticFieldX, data.magneticFieldY,
+                           data.magneticFieldZ);
+
+    // Calibration
+    {
+        Vector3f bias(0.0218462708018154, 0.0281755574886535,
+                      0.0264119470499244);
+        angularVelocity -= bias;
+        Vector3f offset(15.9850903462129, -15.6775071377074, -33.8438469147423);
+        magneticField -= offset;
+        magneticField = {magneticField[1], magneticField[0], -magneticField[2]};
+    }
+
+    acceleration.normalize();
+    magneticField.normalize();
+
+    // Predict step
+    nas->predictGyro(angularVelocity);
+
+    // Correct step
+    nas->correctMag(magneticField);
+    nas->correctAcc(acceleration);
+
+    auto nasState = nas->getState();
+    printf("w%fwa%fab%fbc%fc\n", nasState.qw, nasState.qx, nasState.qy,
+           nasState.qz);
+}
diff --git a/src/tests/algorithms/NAS/test-nas.cpp b/src/tests/algorithms/NAS/test-attitude.cpp
similarity index 65%
rename from src/tests/algorithms/NAS/test-nas.cpp
rename to src/tests/algorithms/NAS/test-attitude.cpp
index 1d6e5a5cfcfb155a8ed0f155abf0cb285fdc0a6e..beacab318c26c6005805ca9f7bc24aa7e1147e6b 100644
--- a/src/tests/algorithms/NAS/test-nas.cpp
+++ b/src/tests/algorithms/NAS/test-attitude.cpp
@@ -21,9 +21,9 @@
  */
 
 #include <algorithms/NAS/NAS.h>
+#include <algorithms/NAS/StateInitializer.h>
 #include <miosix.h>
 #include <sensors/BMX160/BMX160.h>
-#include <sensors/LIS3MDL/LIS3MDL.h>
 #include <sensors/SensorManager.h>
 #include <utils/SkyQuaternion/SkyQuaternion.h>
 
@@ -35,26 +35,26 @@ using namespace Eigen;
 
 NASConfig getEKConfig();
 void setInitialOrientation();
-void bmxInit();
-void bmxCallback();
+void imuInit();
+void imuStep();
 
-NAS* kalman;
+Vector3f nedMag = Vector3f(0.4747, 0.0276, 0.8797);
 
-SPIBus spi1(SPI1);
-BMX160* bmx160 = nullptr;
+NAS* nas;
 
-Boardcore::SensorManager::SensorMap_t sensorsMap;
-Boardcore::SensorManager* sensorManager = nullptr;
+SPIBus spi1(SPI1);
+BMX160* imu = nullptr;
 
 int main()
 {
-    bmxInit();
+    imuInit();
 
-    sensorManager = new SensorManager(sensorsMap);
-    kalman        = new NAS(getEKConfig());
+    nas = new NAS(getEKConfig());
     setInitialOrientation();
 
-    sensorManager->start();
+    TaskScheduler scheduler;
+    scheduler.addTask(imuStep, 20);
+    scheduler.start();
 
     while (true)
         Thread::sleep(1000);
@@ -70,8 +70,8 @@ NASConfig getEKConfig()
     config.SIGMA_MAG      = 0.1f;
     config.SIGMA_GPS      = 10.0f;
     config.SIGMA_BAR      = 4.3f;
-    config.SIGMA_POS      = 10.0f;
-    config.SIGMA_VEL      = 10.0f;
+    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;
@@ -79,9 +79,7 @@ NASConfig getEKConfig()
     config.P_ATT          = 0.01f;
     config.P_BIAS         = 0.01f;
     config.SATS_NUM       = 6.0f;
-
-    // Normalized magnetic field in Milan
-    config.NED_MAG = Vector3f(0.0812241, -0.963085, 0.256649);
+    config.NED_MAG        = nedMag;
 
     return config;
 }
@@ -97,10 +95,10 @@ void setInitialOrientation()
     x(8)              = q(2);
     x(9)              = q(3);
 
-    kalman->setX(x);
+    nas->setX(x);
 }
 
-void bmxInit()
+void imuInit()
 {
     SPIBusConfig spiConfig;
     spiConfig.clockDivider = SPI::ClockDivider::DIV_8;
@@ -122,55 +120,48 @@ void bmxInit()
 
     bmx_config.gyroscopeUnit = BMX160Config::GyroscopeMeasureUnit::RAD;
 
-    bmx160 = new BMX160(spi1, miosix::sensors::bmx160::cs::getPin(), bmx_config,
-                        spiConfig);
-
-    SensorInfo info("BMX160", 20, &bmxCallback);
-
-    sensorsMap.emplace(std::make_pair(bmx160, info));
+    imu = new BMX160(spi1, miosix::sensors::bmx160::cs::getPin(), bmx_config,
+                     spiConfig);
+    imu->init();
 }
 
-void bmxCallback()
+void imuStep()
 {
-    auto data = bmx160->getLastSample();
-
-    // Predict step
+    imu->sample();
+    auto data = imu->getLastSample();
+    Vector3f acceleration(data.accelerationX, data.accelerationY,
+                          data.accelerationZ);
+    Vector3f angularVelocity(data.angularVelocityX, data.angularVelocityY,
+                             data.angularVelocityZ);
+    Vector3f magneticField(data.magneticFieldX, data.magneticFieldY,
+                           data.magneticFieldZ);
+
+    // Calibration
     {
-        Vector3f angularVelocity(data.angularVelocityX, data.angularVelocityY,
-                                 data.angularVelocityZ);
         Vector3f offset{-1.63512255486542, 3.46523431469979, -3.08516033954451};
         angularVelocity = angularVelocity - offset;
         angularVelocity = angularVelocity / 180 * Constants::PI / 10;
-
-        kalman->predictGyro(angularVelocity);
+        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;
     }
 
-    // Correct with acclelerometer
-    // {
-    //     Vector3f angularVelocity(data.accelerationX, data.accelerationY,
-    //                              data.accelerationZ);
+    acceleration.normalize();
+    magneticField.normalize();
 
-    //     kalman->correctAcc(angularVelocity);
-    // }
+    // Predict step
+    nas->predictGyro(angularVelocity);
 
     // Correct step
-    {
-        Vector3f magneticField(data.magneticFieldX, data.magneticFieldY,
-                               data.magneticFieldZ);
-
-        // Apply correction
-        Vector3f b{21.0730033648425, -24.3997259703105, -2.32621524742862};
-        Matrix3f A{{0.659926504672263, 0, 0},
-                   {0, 0.662442130094073, 0},
-                   {0, 0, 2.28747567094359}};
-        magneticField = (magneticField - b).transpose() * A;
-
-        magneticField.normalize();
-        kalman->correctMag(magneticField);
-    }
+    nas->correctMag(magneticField);
+    nas->correctAcc(acceleration);
 
-    auto kalmanState = kalman->getState();
+    // std::cout << acceleration.transpose() << angularVelocity.transpose()
+    //           << magneticField.transpose() << std::endl;
 
-    printf("w%fwa%fab%fbc%fc\n", kalmanState.qw, kalmanState.qx, kalmanState.qy,
-           kalmanState.qz);
+    auto nasState = nas->getState();
+    printf("w%fwa%fab%fbc%fc\n", nasState.qw, nasState.qx, nasState.qy,
+           nasState.qz);
 }
diff --git a/src/tests/algorithms/NAS/test-nas-parafoil.cpp b/src/tests/algorithms/NAS/test-nas-parafoil.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..eb969d4631f481d59968aa9d5f454da69316752e
--- /dev/null
+++ b/src/tests/algorithms/NAS/test-nas-parafoil.cpp
@@ -0,0 +1,185 @@
+/* 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/MPU9250/MPU9250.h>
+#include <sensors/SensorManager.h>
+#include <sensors/UbloxGPS/UbloxGPS.h>
+#include <utils/AeroUtils/AeroUtils.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();
+
+Vector3f nedMag   = Vector3f(0.4747, 0.0276, 0.8797);
+Vector2f startPos = Vector2f(45.501141, 9.156281);
+
+NAS* nas;
+
+SPIBus spi1(SPI1);
+MPU9250* imu  = nullptr;
+UbloxGPS* gps = nullptr;
+
+int main()
+{
+    init();
+
+    nas = new NAS(getEKConfig());
+    setInitialOrientation();
+
+    TaskScheduler scheduler;
+    scheduler.addTask(step, 20);
+    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()
+{
+    imu = new MPU9250(spi1, sensors::mpu9250::cs::getPin());
+    imu->init();
+
+    gps = new UbloxGPS(38400, 10, 2, "gps", 38400);
+    gps->init();
+    gps->start();
+
+    Logger::getInstance().start();
+}
+
+void step()
+{
+    imu->sample();
+    gps->sample();
+    auto imuData = imu->getLastSample();
+    auto gpsData = gps->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);
+    // std::cout << gpsPos.transpose() << " - ";
+    Vector2f gpsVel(gpsData.velocityNorth, gpsData.velocityNorth);
+    // std::cout << gpsVel.transpose() << " - ";
+    Vector4f gpsCorrection;
+    // cppcheck-suppress constStatement
+    gpsCorrection << gpsPos, gpsVel;
+
+    // Calibration
+    {
+        Vector3f biasAcc(-0.1255, 0.2053, -0.2073);
+        acceleration -= biasAcc;
+        Vector3f bias(-0.0291, 0.0149, 0.0202);
+        angularVelocity -= bias;
+        Vector3f offset(15.9850903462129, -15.6775071377074, -33.8438469147423);
+        magneticField -= offset;
+        magneticField = {magneticField[1], magneticField[0], -magneticField[2]};
+    }
+
+    // Predict step
+    nas->predictGyro(angularVelocity);
+    if (gpsPos[0] < 1e3 && gpsPos[0] > -1e3 && gpsPos[1] < 1e3 &&
+        gpsPos[1] > -1e3)
+        nas->predictAcc(acceleration);
+
+    // 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)
+        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);
+}
diff --git a/src/tests/algorithms/NAS/test-nas-with-logs.cpp b/src/tests/algorithms/NAS/test-nas-with-logs.cpp
deleted file mode 100644
index d85bb459f5c86bb65a1b22635d08ae1b4431072b..0000000000000000000000000000000000000000
--- a/src/tests/algorithms/NAS/test-nas-with-logs.cpp
+++ /dev/null
@@ -1,206 +0,0 @@
-/* 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 <sensors/BMX160/BMX160.h>
-#include <utils/CSVReader/CSVReader.h>
-#include <utils/SkyQuaternion/SkyQuaternion.h>
-
-#include <iostream>
-
-using namespace miosix;
-using namespace Boardcore;
-using namespace Eigen;
-
-NASConfig getEKConfig();
-NASState readInitialState();
-void updateKalman(BMX160Data data);
-void printState();
-std::istream& operator>>(std::istream& input, BMX160Data& data);
-std::istream& operator>>(std::istream& input, NASState& data);
-
-// Normalized NED magnetic field in Milan
-Vector3f nedMag = Vector3f(0.47338841, 0.02656764, 0.88045305);
-
-NAS* kalman;
-bool triad = false;
-
-int main()
-{
-    // Prepare the Kalman
-    kalman = new NAS(getEKConfig());
-    kalman->setState(readInitialState());
-
-    // Retrieve all the data
-    auto logData = CSVParser<BMX160Data>("/sd/imu_data.csv", true).collect();
-    printf("Log size: %d\n", logData.size());
-
-    // Wait to start
-    printf("Waiting...\n");
-    std::cin.get();
-
-    // Loop through the logs
-    for (size_t index = 0; index < logData.size(); index++)
-    {
-        printf("Iteration %lu\n", (unsigned long)index);
-        auto iterationData = logData.at(index);
-
-        // Predict and correct
-        updateKalman(iterationData);
-
-        // Print results
-        printState();
-
-        // Wait to continue
-        std::cin.get();
-    }
-}
-
-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;
-
-    // Normalized magnetic field in Milan
-    config.NED_MAG = nedMag;
-
-    return config;
-}
-
-NASState readInitialState()
-{
-    auto initialState =
-        CSVParser<NASState>("/sd/initial_state.csv", true).collect();
-
-    printf("Initial state count: %d\n", initialState.size());
-
-    if (initialState.size() == 0)
-        return NASState();
-    else
-        return initialState.at(0);
-}
-
-void updateKalman(BMX160Data data)
-{
-    // Vector3f acceleration(data.accelerationX, data.accelerationY,
-    //                       data.accelerationZ);
-    // kalman->predict(acceleration);
-
-    Vector3f angularVelocity(data.angularVelocityX, data.angularVelocityY,
-                             data.angularVelocityZ);
-    kalman->predictGyro(angularVelocity);
-
-    Vector3f magneticField(data.magneticFieldX, data.magneticFieldY,
-                           data.magneticFieldZ);
-    magneticField.normalize();
-    kalman->correctMag(magneticField);
-}
-
-void printState()
-{
-    auto kalmanState = kalman->getState().getX();
-    auto kalmanRotation =
-        SkyQuaternion::quat2eul(kalmanState.block<4, 1>(NAS::IDX_QUAT, 0));
-
-    std::cout << "Kalman state:" << std::endl;
-    std::cout << kalmanState << std::endl;
-    printf("Orientation: %6.4f, %6.2f, %6.4f\n", kalmanRotation(0),
-           kalmanRotation(1), kalmanRotation(2));
-}
-
-std::istream& operator>>(std::istream& input, BMX160Data& data)
-{
-    input >> data.accelerationTimestamp;
-    input.ignore(1, ',');
-    input >> data.accelerationX;
-    input.ignore(1, ',');
-    input >> data.accelerationY;
-    input.ignore(1, ',');
-    input >> data.accelerationZ;
-    input.ignore(1, ',');
-    input >> data.angularVelocityTimestamp;
-    input.ignore(1, ',');
-    input >> data.angularVelocityX;
-    input.ignore(1, ',');
-    input >> data.angularVelocityY;
-    input.ignore(1, ',');
-    input >> data.angularVelocityZ;
-    input.ignore(1, ',');
-    input >> data.magneticFieldTimestamp;
-    input.ignore(1, ',');
-    input >> data.magneticFieldX;
-    input.ignore(1, ',');
-    input >> data.magneticFieldY;
-    input.ignore(1, ',');
-    input >> data.magneticFieldZ;
-
-    return input;
-}
-
-std::istream& operator>>(std::istream& input, NASState& data)
-{
-    input >> data.timestamp;
-    input.ignore(1, ',');
-    input >> data.n;
-    input.ignore(1, ',');
-    input >> data.e;
-    input.ignore(1, ',');
-    input >> data.d;
-    input.ignore(1, ',');
-    input >> data.vn;
-    input.ignore(1, ',');
-    input >> data.ve;
-    input.ignore(1, ',');
-    input >> data.vd;
-    input.ignore(1, ',');
-    input >> data.qx;
-    input.ignore(1, ',');
-    input >> data.qy;
-    input.ignore(1, ',');
-    input >> data.qz;
-    input.ignore(1, ',');
-    input >> data.qw;
-    input.ignore(1, ',');
-    input >> data.bx;
-    input.ignore(1, ',');
-    input >> data.by;
-    input.ignore(1, ',');
-    input >> data.bz;
-
-    return input;
-}
diff --git a/src/tests/algorithms/NAS/test-nas-with-triad.cpp b/src/tests/algorithms/NAS/test-nas-with-triad.cpp
index 942b7f9d9b248954f0eac29847f35a2eb233327f..59baeb1d1e37a36035e2fe6208ec22617ea8ad2d 100644
--- a/src/tests/algorithms/NAS/test-nas-with-triad.cpp
+++ b/src/tests/algorithms/NAS/test-nas-with-triad.cpp
@@ -22,12 +22,9 @@
 
 #include <algorithms/NAS/NAS.h>
 #include <algorithms/NAS/StateInitializer.h>
-#include <drivers/timer/TimestampTimer.h>
 #include <miosix.h>
-#include <sensors/MPU9250/MPU9250.h>
+#include <sensors/BMX160/BMX160.h>
 #include <sensors/SensorManager.h>
-#include <sensors/calibration/SensorDataExtra.h>
-#include <sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.h>
 #include <utils/SkyQuaternion/SkyQuaternion.h>
 
 #include <iostream>
@@ -45,10 +42,10 @@ constexpr uint64_t CALIBRATION_TIMEOUT = 5 * 1e6;
 Vector3f nedMag = Vector3f(0.47472049, 0.02757190, 0.87970463);
 
 StateInitializer* stateInitializer;
-NAS* kalman;
+NAS* nas;
 
 SPIBus spi1(SPI1);
-MPU9250* mpu = nullptr;
+BMX160* bmx160 = nullptr;
 
 Boardcore::SensorManager::SensorMap_t sensorsMap;
 Boardcore::SensorManager* sensorManager = nullptr;
@@ -59,7 +56,7 @@ int main()
 
     sensorManager    = new SensorManager(sensorsMap);
     stateInitializer = new StateInitializer();
-    kalman           = new NAS(getEKConfig());
+    nas              = new NAS(getEKConfig());
 
     // Logger::getInstance().start();
     TimestampTimer::getInstance().resetTimestamp();
@@ -97,11 +94,27 @@ NASConfig getEKConfig()
 
 void bmxInit()
 {
-    mpu = new MPU9250(spi1, miosix::sensors::mpu9250::cs::getPin());
-
-    SensorInfo info("MPU9250", 20, &bmxCallback);
-
-    sensorsMap.emplace(std::make_pair(mpu, info));
+    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;
+
+    bmx160 = new BMX160(spi1, miosix::sensors::bmx160::cs::getPin(), bmx_config,
+                        spiConfig);
+
+    SensorInfo info("BMX160", 20, &bmxCallback);
+
+    sensorsMap.emplace(std::make_pair(bmx160, info));
 }
 
 void bmxCallback()
@@ -111,19 +124,25 @@ void bmxCallback()
     static Vector3f accMean = Vector3f::Zero();
     static Vector3f magMean = Vector3f::Zero();
 
-    auto data = mpu->getLastSample();
-
+    auto data = bmx160->getLastSample();
     Vector3f acceleration(data.accelerationX, data.accelerationY,
                           data.accelerationZ);
-
     Vector3f angularVelocity(data.angularVelocityX, data.angularVelocityY,
                              data.angularVelocityZ);
-    angularVelocity = angularVelocity * Constants::DEGREES_TO_RADIANS;
-
-    SoftAndHardIronCorrector corrector({-5.15221, -56.5428, 38.2193},
-                                       {0.962913, 0.987191, 1.05199});
-    Vector3f magneticField;
-    magneticField << corrector.correct(data);
+    Vector3f magneticField(data.magneticFieldX, data.magneticFieldY,
+                           data.magneticFieldZ);
+
+    // Apply correction
+    Vector3f acc_b(0.3763 + 0.094, -0.1445 - 0.0229, -0.1010 + 0.0150);
+    acceleration -= acc_b;
+    Vector3f gyro_b{-1.63512255486542, 3.46523431469979, -3.08516033954451};
+    angularVelocity = angularVelocity - gyro_b;
+    angularVelocity = angularVelocity / 180 * Constants::PI / 10;
+    Vector3f mag_b{21.0730033648425, -24.3997259703105, -2.32621524742862};
+    Matrix3f A{{0.659926504672263, 0, 0},
+               {0, 0.662442130094073, 0},
+               {0, 0, 2.28747567094359}};
+    magneticField = (magneticField - mag_b).transpose() * A;
 
     if (calibrating)
     {
@@ -135,15 +154,15 @@ void bmxCallback()
         }
         else
         {
-            // Now the calibration has ended, compute and log the kalman state
+            // Now the calibration has ended, compute and log the nas state
             calibrating = false;
 
-            // Compute the initial kalman state
+            // Compute the initial nas state
             stateInitializer->triad(accMean, magMean, nedMag);
-            kalman->setX(stateInitializer->getInitX());
+            nas->setX(stateInitializer->getInitX());
 
             // Save the state and the IMU data
-            // Logger::getInstance().log(kalman->getState());
+            // Logger::getInstance().log(nas->getState());
             data.accelerationX  = accMean[0];
             data.accelerationY  = accMean[1];
             data.accelerationZ  = accMean[2];
@@ -161,8 +180,8 @@ void bmxCallback()
     {
         // Predict step
         {
-            // kalman->predictAcc(acceleration);
-            kalman->predictGyro(angularVelocity);
+            // nas->predictAcc(acceleration);
+            nas->predictGyro(angularVelocity);
 
             data.angularVelocityX = angularVelocity[0];
             data.angularVelocityY = angularVelocity[1];
@@ -172,23 +191,23 @@ void bmxCallback()
         // Correct step
         {
             magneticField.normalize();
-            kalman->correctMag(magneticField);
+            nas->correctMag(magneticField);
 
             data.magneticFieldX = magneticField[0];
             data.magneticFieldY = magneticField[1];
             data.magneticFieldZ = magneticField[2];
         }
 
-        auto kalmanState = kalman->getState();
+        auto nasState = nas->getState();
 
-        kalmanState.timestamp = TimestampTimer::getInstance().getTimestamp();
-        data.accelerationTimestamp    = kalmanState.timestamp;
-        data.magneticFieldTimestamp   = kalmanState.timestamp;
-        data.angularVelocityTimestamp = kalmanState.timestamp;
+        nasState.timestamp = TimestampTimer::getInstance().getTimestamp();
+        data.accelerationTimestamp    = nasState.timestamp;
+        data.magneticFieldTimestamp   = nasState.timestamp;
+        data.angularVelocityTimestamp = nasState.timestamp;
 
-        // Logger::getInstance().log(kalmanState);
+        // Logger::getInstance().log(nasState);
         // Logger::getInstance().log(data);
-        printf("w%fwa%fab%fbc%fc\n", kalmanState.qw, kalmanState.qx,
-               kalmanState.qy, kalmanState.qz);
+        printf("w%fwa%fab%fbc%fc\n", nasState.qw, nasState.qx, nasState.qy,
+               nasState.qz);
     }
 }
diff --git a/src/tests/algorithms/NAS/test-tmp.cpp b/src/tests/algorithms/NAS/test-tmp.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..39fcdbd511705c39361f2a926ef92c15ea662ff9
--- /dev/null
+++ b/src/tests/algorithms/NAS/test-tmp.cpp
@@ -0,0 +1,89 @@
+/* Copyright (c) 2021 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 <drivers/spi/SPIDriver.h>
+#include <drivers/timer/TimestampTimer.h>
+#include <miosix.h>
+#include <sensors/BMX160/BMX160.h>
+#include <sensors/MPU9250/MPU9250.h>
+#include <utils/Debug.h>
+
+using namespace miosix;
+
+using namespace Boardcore;
+
+GpioPin mpuCs = GpioPin(GPIOG_BASE, 3);
+SPIBus spi1(SPI1);
+
+MPU9250* mpu = nullptr;
+BMX160* bmx  = nullptr;
+
+void initBoard()
+{
+    // Chip select pin as output starting high
+    mpuCs.mode(miosix::Mode::OUTPUT);
+    mpuCs.high();
+
+    mpu = new MPU9250(spi1, mpuCs);
+
+    SPIBusConfig spiConfig;
+    spiConfig.clockDivider = SPI::ClockDivider::DIV_8;
+
+    BMX160Config bmxConfig;
+    bmxConfig.fifoMode              = BMX160Config::FifoMode::HEADER;
+    bmxConfig.fifoWatermark         = 80;
+    bmxConfig.fifoInterrupt         = BMX160Config::FifoInterruptPin::PIN_INT1;
+    bmxConfig.temperatureDivider    = 1;
+    bmxConfig.accelerometerRange    = BMX160Config::AccelerometerRange::G_16;
+    bmxConfig.gyroscopeRange        = BMX160Config::GyroscopeRange::DEG_1000;
+    bmxConfig.accelerometerDataRate = BMX160Config::OutputDataRate::HZ_100;
+    bmxConfig.gyroscopeDataRate     = BMX160Config::OutputDataRate::HZ_100;
+    bmxConfig.magnetometerRate      = BMX160Config::OutputDataRate::HZ_100;
+    bmxConfig.gyroscopeUnit         = BMX160Config::GyroscopeMeasureUnit::RAD;
+    bmx = new BMX160(spi1, miosix::sensors::bmx160::cs::getPin(), bmxConfig,
+                     spiConfig);
+
+    mpu->init();
+    bmx->init();
+}
+
+int main()
+{
+    initBoard();
+
+    auto lastTick = getTick();
+    while (true)
+    {
+        mpu->sample();
+        bmx->sample();
+        auto mpuData = mpu->getLastSample();
+        auto bmxData = bmx->getLastSample();
+
+        printf("%f,%f,%f,", mpuData.magneticFieldX, mpuData.magneticFieldY,
+               mpuData.magneticFieldZ);
+        printf("%f,%f,%f\n", bmxData.magneticFieldX, bmxData.magneticFieldY,
+               bmxData.magneticFieldZ);
+
+        Thread::sleepUntil(lastTick + 20);
+        lastTick = getTick();
+    }
+}
diff --git a/src/tests/algorithms/NAS/test-triad-parafoil.cpp b/src/tests/algorithms/NAS/test-triad-parafoil.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ed93635ffcb04004de8ce7810051b48a21bb08bb
--- /dev/null
+++ b/src/tests/algorithms/NAS/test-triad-parafoil.cpp
@@ -0,0 +1,79 @@
+/* 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/StateInitializer.h>
+#include <miosix.h>
+#include <sensors/MPU9250/MPU9250.h>
+#include <sensors/SensorManager.h>
+#include <sensors/calibration/SensorDataExtra.h>
+#include <sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.h>
+
+#include <cmath>
+#include <iostream>
+
+using namespace miosix;
+using namespace Boardcore;
+using namespace Eigen;
+
+void mpuInit();
+void bmxCallback();
+
+Vector3f nedMag = Vector3f(0.4747, 0.0276, 0.8797);
+
+SPIBus spi1(SPI1);
+MPU9250* mpu = nullptr;
+
+int main()
+{
+    mpu = new MPU9250(spi1, sensors::mpu9250::cs::getPin());
+    mpu->init();
+
+    auto lastTick = getTick();
+    while (true)
+    {
+        mpu->sample();
+        auto data = mpu->getLastSample();
+
+        Vector3f acceleration(data.accelerationX, data.accelerationY,
+                              data.accelerationZ);
+
+        Vector3f offset(15.9850903462129, -15.6775071377074, -33.8438469147423);
+        Vector3f magneticField(data.magneticFieldX, data.magneticFieldY,
+                               data.magneticFieldZ);
+        magneticField -= offset;
+        magneticField = {magneticField[1], magneticField[0], -magneticField[2]};
+
+        acceleration.normalize();
+        magneticField.normalize();
+
+        StateInitializer state;
+        state.triad(acceleration, magneticField, nedMag);
+
+        auto kalmanState = state.getInitX();
+        if (!std::isnan(kalmanState(9)))
+            printf("w%fwa%fab%fbc%fc\n", kalmanState(9), kalmanState(6),
+                   kalmanState(7), kalmanState(8));
+
+        Thread::sleepUntil(lastTick + 20);
+        lastTick = getTick();
+    }
+}
diff --git a/src/tests/algorithms/NAS/test-triad.cpp b/src/tests/algorithms/NAS/test-triad.cpp
index a11b8f3272b8b3a741ae902ab535b1adba91ffc2..f5104a1e0f2cb838a1b14f7b58ec072fbabcdcab 100644
--- a/src/tests/algorithms/NAS/test-triad.cpp
+++ b/src/tests/algorithms/NAS/test-triad.cpp
@@ -22,7 +22,7 @@
 
 #include <algorithms/NAS/StateInitializer.h>
 #include <miosix.h>
-#include <sensors/MPU9250/MPU9250.h>
+#include <sensors/BMX160/BMX160.h>
 #include <sensors/SensorManager.h>
 #include <sensors/calibration/SensorDataExtra.h>
 #include <sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.h>
@@ -34,60 +34,77 @@ using namespace miosix;
 using namespace Boardcore;
 using namespace Eigen;
 
-void bmxInit();
-void bmxCallback();
+void imuInit();
 
 Vector3f nedMag = Vector3f(0.4747, 0.0276, 0.8797);
 
 SPIBus spi1(SPI1);
-MPU9250* mpu = nullptr;
-
-Boardcore::SensorManager::SensorMap_t sensorsMap;
-Boardcore::SensorManager* sensorManager = nullptr;
+BMX160* bmx = nullptr;
 
 int main()
 {
-    bmxInit();
-
-    sensorManager = new SensorManager(sensorsMap);
-    sensorManager->start();
+    imuInit();
+    bmx->init();
 
+    auto lastTick = getTick();
     while (true)
-        Thread::sleep(1000);
+    {
+        bmx->sample();
+        auto data = bmx->getLastSample();
+
+        Vector3f acceleration(data.accelerationX, data.accelerationY,
+                              data.accelerationZ);
+        Vector3f angularVelocity(data.angularVelocityX, data.angularVelocityY,
+                                 data.angularVelocityZ);
+        Vector3f offset{-1.63512255486542, 3.46523431469979, -3.08516033954451};
+        angularVelocity = angularVelocity - offset;
+        angularVelocity = angularVelocity / 180 * Constants::PI / 10;
+        Vector3f magneticField(data.magneticFieldX, data.magneticFieldY,
+                               data.magneticFieldZ);
+        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;
+
+        acceleration.normalize();
+        magneticField.normalize();
+
+        StateInitializer state;
+        state.triad(acceleration, magneticField, nedMag);
+
+        auto kalmanState = state.getInitX();
+        if (!std::isnan(kalmanState(9)))
+            printf("w%fwa%fab%fbc%fc\n", kalmanState(9), kalmanState(6),
+                   kalmanState(7), kalmanState(8));
+
+        Thread::sleepUntil(lastTick + 20);
+        lastTick = getTick();
+    }
 }
 
-void bmxInit()
+void imuInit()
 {
-    mpu = new MPU9250(spi1, miosix::sensors::mpu9250::cs::getPin());
-
-    SensorInfo info("MPU9250", 20, &bmxCallback);
+    SPIBusConfig spiConfig;
+    spiConfig.clockDivider = SPI::ClockDivider::DIV_8;
 
-    sensorsMap.emplace(std::make_pair(mpu, info));
-}
-
-void bmxCallback()
-{
-    auto data = mpu->getLastSample();
+    BMX160Config bmx_config;
+    bmx_config.fifoMode      = BMX160Config::FifoMode::HEADER;
+    bmx_config.fifoWatermark = 80;
+    bmx_config.fifoInterrupt = BMX160Config::FifoInterruptPin::PIN_INT1;
 
-    Vector3f acceleration(data.accelerationX, data.accelerationY,
-                          data.accelerationZ);
+    bmx_config.temperatureDivider = 1;
 
-    SoftAndHardIronCorrector corrector({-5.15221, -56.5428, 38.2193},
-                                       {0.962913, 0.987191, 1.05199});
-    Vector3f magneticField;
-    magneticField << (MagnetometerData)data;
+    bmx_config.accelerometerRange = BMX160Config::AccelerometerRange::G_16;
 
-    magneticField << corrector.correct(data);
+    bmx_config.gyroscopeRange = BMX160Config::GyroscopeRange::DEG_1000;
 
-    acceleration.normalize();
-    magneticField.normalize();
+    bmx_config.accelerometerDataRate = BMX160Config::OutputDataRate::HZ_100;
+    bmx_config.gyroscopeDataRate     = BMX160Config::OutputDataRate::HZ_100;
+    bmx_config.magnetometerRate      = BMX160Config::OutputDataRate::HZ_100;
 
-    StateInitializer state;
-    state.triad(acceleration, magneticField, nedMag);
+    bmx_config.gyroscopeUnit = BMX160Config::GyroscopeMeasureUnit::RAD;
 
-    std::cout << acceleration.transpose();
-    auto kalmanState = state.getInitX();
-    if (!std::isnan(kalmanState(9)))
-        printf("w%fwa%fab%fbc%fc\n", kalmanState(9), kalmanState(6),
-               kalmanState(7), kalmanState(8));
+    bmx = new BMX160(spi1, miosix::sensors::bmx160::cs::getPin(), bmx_config,
+                     spiConfig);
 }