diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json
index 3f878454cccbfc2b3c134578294a896f6e53c46e..0f530eb0bb42c7e438451bfb5f7c8f48ac500d1f 100755
--- a/.vscode/c_cpp_properties.json
+++ b/.vscode/c_cpp_properties.json
@@ -131,7 +131,7 @@
             "defines": [
                 "DEBUG",
                 "_ARCH_CORTEXM4_STM32F4",
-                "_BOARD_STM32F429ZI_SKYWARD_DEATH_STACK_X",
+                "_BOARD_STM32F429ZI_SKYWARD_DEATHST_X",
                 "_MIOSIX_BOARDNAME=stm32f429zi_skyward_death_stack_x",
                 "HSE_VALUE=8000000",
                 "SYSCLK_FREQ_168MHz=168000000",
@@ -195,7 +195,7 @@
                 "DEBUG",
                 "HILSimulation",
                 "_ARCH_CORTEXM4_STM32F4",
-                "_BOARD_STM32F429ZI_SKYWARD_DEATH_STACK_X_MAKER_FAIRE",
+                "_BOARD_STM32F429ZI_SKYWARD_DEATHST_X_MAKER_FAIRE",
                 "_MIOSIX_BOARDNAME=stm32f429zi_skyward_death_stack_x_maker_faire",
                 "HSE_VALUE=8000000",
                 "SYSCLK_FREQ_168MHz=168000000",
diff --git a/.vscode/settings.json b/.vscode/settings.json
index 743392d0cbad610129652fe8bcf299b7831f3e7f..85d459346d971d2af82abac3ea5ef87f7c240860 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -113,7 +113,8 @@
         "xstring": "cpp",
         "xtr1common": "cpp",
         "xtree": "cpp",
-        "xutility": "cpp"
+        "xutility": "cpp",
+        "BiasCorrector.C": "cpp"
     },
     "editor.formatOnSave": true,
     "[c]": {
@@ -152,15 +153,18 @@
         "GPIOF",
         "KALM",
         "kalman",
+        "magcal",
         "mavlink",
         "miosix",
         "MPXH",
         "Nidasio",
         "pitot",
+        "setb",
         "Setpoint",
         "stateinitializer",
         "telecommands",
         "TMTC",
+        "tparam",
         "UBXGPS",
         "usart",
         "VREF",
@@ -171,4 +175,4 @@
         "CMAKE_C_COMPILER_LAUNCHER": "ccache",
         "CMAKE_CXX_COMPILER_LAUNCHER": "ccache"
     }
-}
\ No newline at end of file
+}
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 859b26d6f03a33685ddb376ac00fb9c68709b112..73e684ae99af289299c879863a71a4a91ea4cf8d 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -49,7 +49,7 @@ sbs_target(main-entry-roccaraso stm32f429zi_skyward_death_stack_v3)
 
 add_executable(main-entry-milano src/entrypoints/Main/main-entry.cpp ${MAIN_COMPUTER})
 target_include_directories(main-entry-milano PRIVATE ${OBSW_INCLUDE_DIRS})
-target_compile_definitions(main-entry-milano PRIVATE MILANO)
+target_compile_definitions(main-entry-milano PRIVATE MILANO BUZZER_LOW INTERP USE_SERIAL_TRANSCEIVER)
 sbs_target(main-entry-milano stm32f429zi_skyward_death_stack_v3)
 
 add_executable(main-entry-hil-euroc src/entrypoints/Main/main-entry.cpp ${MAIN_COMPUTER} ${HIL})
diff --git a/skyward-boardcore b/skyward-boardcore
index e60ca34f7557877779f3da84d33a76ca0e1d84d5..a757b8dc34e0b160ddaf3ed33408d5aa2a22a4fc 160000
--- a/skyward-boardcore
+++ b/skyward-boardcore
@@ -1 +1 @@
-Subproject commit e60ca34f7557877779f3da84d33a76ca0e1d84d5
+Subproject commit a757b8dc34e0b160ddaf3ed33408d5aa2a22a4fc
diff --git a/src/boards/Main/Configs/NASConfig.h b/src/boards/Main/Configs/NASConfig.h
index 0da418ccf9fdd0b1620e72bbb034244703d45576..445449e860808815818a0728857c88fda7cd5ca6 100644
--- a/src/boards/Main/Configs/NASConfig.h
+++ b/src/boards/Main/Configs/NASConfig.h
@@ -41,6 +41,7 @@ static const Boardcore::NASConfig config = {
     UPDATE_PERIOD / 1000.0,          // T
     0.0001f,                         // SIGMA_BETA
     0.3f,                            // SIGMA_W
+    0.1f,                            // SIGMA_ACC
     0.1f,                            // SIGMA_MAG
     10.0f,                           // SIGMA_GPS
     4.3f,                            // SIGMA_BAR
diff --git a/src/boards/Main/Configs/SensorsConfig.h b/src/boards/Main/Configs/SensorsConfig.h
index 3b5cf64b9518fe37d833d9d003feb607345036b1..1d886dbe0612a1ac3f55140e5ec869470acdebaf 100644
--- a/src/boards/Main/Configs/SensorsConfig.h
+++ b/src/boards/Main/Configs/SensorsConfig.h
@@ -29,7 +29,6 @@
 #include <sensors/LIS3MDL/LIS3MDL.h>
 #include <sensors/MPU9250/MPU9250.h>
 #include <sensors/calibration/AxisOrientation.h>
-#include <sensors/calibration/Calibration.h>
 
 namespace Main
 {
diff --git a/src/boards/Main/Sensors/Sensors.h b/src/boards/Main/Sensors/Sensors.h
index ac0227e53c2ce4dcc6a143fdc0b23850ce226bb8..81fd67b5873f38df541a5f00d0ddb226a6439a81 100644
--- a/src/boards/Main/Sensors/Sensors.h
+++ b/src/boards/Main/Sensors/Sensors.h
@@ -64,12 +64,9 @@ public:
      */
     struct StateComplete
     {
-        HILAccelerometer *accelerometer;
         HILBarometer *barometer;
         HILPitot *pitot;
         HILGps *gps;
-        HILGyroscope *gyro;
-        HILMagnetometer *magnetometer;
         HILTemperature *temperature;
         HILImu *imu;
         HILKalman *kalman;
diff --git a/src/boards/Main/StateMachines/NASController/NASController.cpp b/src/boards/Main/StateMachines/NASController/NASController.cpp
index a212199ae2ec6846e83acf3aed227fb95f853df9..4fa5114b71bf861447b5cc075072a464035d4122 100644
--- a/src/boards/Main/StateMachines/NASController/NASController.cpp
+++ b/src/boards/Main/StateMachines/NASController/NASController.cpp
@@ -72,6 +72,9 @@ void NASController::update()
         nas.correctGPS(gpsData);
         nas.correctBaro(pressureData.pressure);
 
+        // TODO: Add accelerometer correction until the acceleration goes out of
+        // specs
+
         Logger::getInstance().log(nas.getState());
         FlightStatsRecorder::getInstance().update(nas.getState());
     }
diff --git a/src/boards/Main/TMRepository/TMRepository.cpp b/src/boards/Main/TMRepository/TMRepository.cpp
index f0f5c087ea9cba36ee14b78f1c78df548a54c208..83e639981ba8f11429036ab1eb6c804d5ea54715 100644
--- a/src/boards/Main/TMRepository/TMRepository.cpp
+++ b/src/boards/Main/TMRepository/TMRepository.cpp
@@ -35,6 +35,8 @@
 #include <Main/StateMachines/Deployment/Deployment.h>
 #include <Main/StateMachines/FlightModeManager/FlightModeManager.h>
 #include <Main/StateMachines/NASController/NASController.h>
+#include <algorithms/NAS/StateInitializer.h>
+#include <common/ReferenceConfig.h>
 #include <diagnostic/CpuMeter/CpuMeter.h>
 #include <drivers/timer/TimestampTimer.h>
 #include <events/EventBroker.h>
@@ -276,14 +278,24 @@ mavlink_message_t TMRepository::packSystemTm(SystemTMList tmId, uint8_t msgId,
             tm.nas_vn     = nasState.vn;
             tm.nas_ve     = nasState.ve;
             tm.nas_vd     = nasState.vd;
-            tm.nas_qx     = nasState.qx;
-            tm.nas_qy     = nasState.qy;
-            tm.nas_qz     = nasState.qz;
-            tm.nas_qw     = nasState.qw;
             tm.nas_bias_x = nasState.bx;
             tm.nas_bias_y = nasState.by;
             tm.nas_bias_z = nasState.bz;
 
+            // Test triad
+            StateInitializer state;
+            state.triad({imuData.accelerationX, imuData.accelerationY,
+                         imuData.accelerationZ},
+                        {imuData.magneticFieldX, imuData.magneticFieldY,
+                         imuData.magneticFieldZ},
+                        Common::ReferenceConfig::nedMag);
+            auto testState = state.getInitX();
+
+            tm.nas_qw = testState(9);
+            tm.nas_qx = testState(6);
+            tm.nas_qy = testState(7);
+            tm.nas_qz = testState(8);
+
             // Sensing pins statuses
             tm.pin_launch =
                 PinHandler::getInstance().getPinsData()[LAUNCH_PIN].lastState;
diff --git a/src/boards/Parafoil/Configs/NASConfig.h b/src/boards/Parafoil/Configs/NASConfig.h
index 66ffb9fd50b0d6c4ead4f2febd75c0b16b947dc4..1c075cd60edb10bdf4012759a1ab3006757a8f7d 100644
--- a/src/boards/Parafoil/Configs/NASConfig.h
+++ b/src/boards/Parafoil/Configs/NASConfig.h
@@ -39,6 +39,7 @@ static const Boardcore::NASConfig config = {
     1.0f / UPDATE_PERIOD,  // T
     0.0001f,               // SIGMA_BETA
     0.3f,                  // SIGMA_W
+    0.1f,                  // SIGMA_ACC
     0.1f,                  // SIGMA_MAG
     10.0f,                 // SIGMA_GPS
     4.3f,                  // SIGMA_BAR
diff --git a/src/boards/Payload/Configs/NASConfig.h b/src/boards/Payload/Configs/NASConfig.h
index c99dde138646802d2e55355e44f9e2fa946338c4..dda4d519f9dd5e44993ac32457e56d15f69033c2 100644
--- a/src/boards/Payload/Configs/NASConfig.h
+++ b/src/boards/Payload/Configs/NASConfig.h
@@ -41,6 +41,7 @@ static const Boardcore::NASConfig config = {
     UPDATE_PERIOD / 1000.0,          // T
     0.0001f,                         // SIGMA_BETA
     0.3f,                            // SIGMA_W
+    0.1f,                            // SIGMA_ACC
     0.1f,                            // SIGMA_MAG
     10.0f,                           // SIGMA_GPS
     4.3f,                            // SIGMA_BAR
diff --git a/src/boards/Payload/Configs/SensorsConfig.h b/src/boards/Payload/Configs/SensorsConfig.h
index 57f31660a3a3f578463a6877978ee87423f418a3..2c96b7af450e46eed1c4541b9a393bb232a9233c 100644
--- a/src/boards/Payload/Configs/SensorsConfig.h
+++ b/src/boards/Payload/Configs/SensorsConfig.h
@@ -27,7 +27,6 @@
 #include <sensors/BMX160/BMX160Config.h>
 #include <sensors/LIS3MDL/LIS3MDL.h>
 #include <sensors/calibration/AxisOrientation.h>
-#include <sensors/calibration/Calibration.h>
 
 namespace Payload
 {
diff --git a/src/boards/Payload/Sensors/Sensors.cpp b/src/boards/Payload/Sensors/Sensors.cpp
index fc476a5892740fe3c2e4ae61a2e11a94b7ff3d44..99617f16397dffcfe2ff6b37aedd2e27728a7689 100644
--- a/src/boards/Payload/Sensors/Sensors.cpp
+++ b/src/boards/Payload/Sensors/Sensors.cpp
@@ -384,13 +384,8 @@ void Sensors::bmx160Callback()
 
 void Sensors::bmx160WithCorrectionInit()
 {
-    // Read the correction parameters
-    BMX160CorrectionParameters correctionParameters =
-        BMX160WithCorrection::readCorrectionParametersFromFile(
-            BMX160_CORRECTION_PARAMETERS_FILE);
-
-    bmx160WithCorrection = new BMX160WithCorrection(
-        bmx160, correctionParameters, BMX160_AXIS_ROTATION);
+    bmx160WithCorrection =
+        new BMX160WithCorrection(bmx160, BMX160_AXIS_ROTATION);
 
     SensorInfo info(
         "BMX160WithCorrection", SAMPLE_PERIOD_IMU_BMX,
diff --git a/src/boards/Payload/StateMachines/NASController/NASController.cpp b/src/boards/Payload/StateMachines/NASController/NASController.cpp
index 8ff21963b71aa4b429713de80c47f6dfd2358c9d..380bc2c0b51dd68c2d40d488abfffc986e26b73b 100644
--- a/src/boards/Payload/StateMachines/NASController/NASController.cpp
+++ b/src/boards/Payload/StateMachines/NASController/NASController.cpp
@@ -70,6 +70,9 @@ void NASController::update()
         nas.correctGPS(gpsData);
         nas.correctBaro(pressureData.pressure);
 
+        // TODO: Add accelerometer correction until the acceleration goes out of
+        // specs
+
         Logger::getInstance().log(nas.getState());
         FlightStatsRecorder::getInstance().update(nas.getState());
     }
diff --git a/src/tests/Main/calibration/test-bmx160-calibration.cpp b/src/tests/Main/calibration/test-bmx160-calibration.cpp
index 1fc701e86ca8202509c47e616f74ef6c51aaaa30..07f5f0b323369418582b7a5a49374f04195fa5f3 100644
--- a/src/tests/Main/calibration/test-bmx160-calibration.cpp
+++ b/src/tests/Main/calibration/test-bmx160-calibration.cpp
@@ -26,6 +26,8 @@
 #include <sensors/BMX160/BMX160.h>
 #include <sensors/BMX160/BMX160WithCorrection.h>
 #include <sensors/calibration/AxisOrientation.h>
+#include <sensors/calibration/BiasCalibration/BiasCalibration.h>
+#include <sensors/calibration/SixParameterCalibration/SixParameterCalibration.h>
 #include <sensors/calibration/SoftAndHardIronCalibration/SoftAndHardIronCalibration.h>
 #include <utils/Debug.h>
 
@@ -34,18 +36,21 @@
 
 using namespace std;
 using namespace miosix;
+using namespace Eigen;
 using namespace Boardcore;
 
 constexpr const char* CORRECTION_PARAMETER_FILE = "/sd/bmx160_params.csv";
 constexpr const char* MAG_CALIBRATION_DATA_FILE =
     "/sd/bmx160_mag_calibration_data.csv";
-constexpr int ACC_CALIBRATION_SAMPLES        = 200;
-constexpr int ACC_CALIBRATION_SLEEP_TIME     = 25;  // [us]
+
+constexpr int ACC_CALIBRATION_SLEEP_TIME     = 10;  // [s]
 constexpr int ACC_CALIBRATION_N_ORIENTATIONS = 6;
 
-constexpr int MAG_CALIBRATION_DURATION           = 10;  // [s]
+constexpr int MAG_CALIBRATION_DURATION           = 30;  // [s]
 constexpr uint32_t MAG_CALIBRATION_SAMPLE_PERIOD = 20;  // [ms]
 
+constexpr int GYRO_CALIBRATION_DURATION = 10;  // [s]
+
 BMX160* bmx160;
 
 /**
@@ -62,20 +67,30 @@ BMX160* bmx160;
  */
 AxisOrthoOrientation orientations[ACC_CALIBRATION_N_ORIENTATIONS]{
     {Direction::POSITIVE_X, Direction::POSITIVE_Y},  // Z up
-    {Direction::POSITIVE_Y, Direction::POSITIVE_Z},  // X up
-    {Direction::POSITIVE_Z, Direction::POSITIVE_X},  // Y up
-    {Direction::POSITIVE_Y, Direction::POSITIVE_X},  // Z down
-    {Direction::POSITIVE_Z, Direction::POSITIVE_Y},  // X down
-    {Direction::POSITIVE_X, Direction::POSITIVE_Z},  // Y down
+    {Direction::POSITIVE_Z, Direction::POSITIVE_Y},  // X up
+    {Direction::POSITIVE_X, Direction::POSITIVE_Z},  // Y up
+    {Direction::POSITIVE_X, Direction::NEGATIVE_Y},  // Z down
+    {Direction::NEGATIVE_Z, Direction::POSITIVE_Y},  // X down
+    {Direction::POSITIVE_X, Direction::NEGATIVE_Z},  // Y down
 };
 
 constexpr const char* testHumanFriendlyDirection[]{
     "Z up", "X up", "Y up", "Z down", "X down", "Y down",
 };
 
-// BMX160 Watermark interrupt
+#if defined(_BOARD_STM32F429ZI_SKYWARD_DEATHST_X)
+SPIBus bus(SPI1);
+
+void __attribute__((used)) EXTI5_IRQHandlerImpl()
+{
+#elif defined(_BOARD_STM32F429ZI_SKYWARD_DEATHST_V3)
+SPIBus bus(SPI4);
+
 void __attribute__((used)) EXTI3_IRQHandlerImpl()
 {
+#else
+#error "Board not supported"
+#endif
     if (bmx160)
         bmx160->IRQupdateTimestamp(TimestampTimer::getTimestamp());
 }
@@ -83,47 +98,45 @@ void __attribute__((used)) EXTI3_IRQHandlerImpl()
 int menu();
 void waitForInput();
 
-BMX160CorrectionParameters calibrateAccelerometer(
-    BMX160CorrectionParameters correctionParameters);
-BMX160CorrectionParameters calibrateMagnetometer(
-    BMX160CorrectionParameters correctionParameters);
+void calibrateAccelerometer();
+void calibrateMagnetometer();
+void calibrateGyroscope();
 
 int main()
 {
-    // Enable interrupt from BMX pin
-    enableExternalInterrupt(GPIOE_BASE, 3, InterruptTrigger::FALLING_EDGE);
+// Enable interrupt from BMX pin
+#if defined(_BOARD_STM32F429ZI_SKYWARD_DEATHST_X)
+    enableExternalInterrupt(miosix::sensors::bmx160::intr::getPin().getPort(),
+                            miosix::sensors::bmx160::intr::getPin().getNumber(),
+                            InterruptTrigger::FALLING_EDGE);
+#elif defined(_BOARD_STM32F429ZI_SKYWARD_DEATHST_V3)
+    enableExternalInterrupt(miosix::sensors::bmx160::intr::getPin().getPort(),
+                            miosix::sensors::bmx160::intr::getPin().getNumber(),
+                            InterruptTrigger::FALLING_EDGE);
+#else
+#error "Board not supported"
+#endif
 
     // Greet the user
     printf("\nWelcome to the calibration procedure!\n");
 
-    // Read the current correction parameters
-    BMX160CorrectionParameters correctionParameters;
-    correctionParameters =
-        BMX160WithCorrection::readCorrectionParametersFromFile(
-            CORRECTION_PARAMETER_FILE);
-
     // Make the user choose what to do
     switch (menu())
     {
         case 1:
-            correctionParameters = calibrateAccelerometer(correctionParameters);
+            calibrateAccelerometer();
             break;
         case 2:
-            correctionParameters = calibrateMagnetometer(correctionParameters);
+            calibrateMagnetometer();
+            break;
+        case 3:
+            calibrateGyroscope();
             break;
 
         default:
             break;
     }
 
-    // Save the correction parameters in the file
-    {
-        std::ofstream correctionParametersFile(CORRECTION_PARAMETER_FILE);
-        correctionParametersFile << BMX160CorrectionParameters::header()
-                                 << std::endl;
-        correctionParameters.print(correctionParametersFile);
-    }
-
     return 0;
 }
 
@@ -134,6 +147,7 @@ int menu()
     printf("\nWhat do you want to do?\n");
     printf("1. Calibrate accelerometer\n");
     printf("2. Calibrate magnetometer\n");
+    printf("3. Calibrate gyroscope\n");
     printf("\n>> ");
     scanf("%d", &choice);
 
@@ -150,25 +164,16 @@ void waitForInput()
     } while (temp != "c");
 }
 
-/**
- * The accelerometer calibration consists in finding the bias and coefficient
- * against target 9.8m/s^2 on the three axis positioning the sensor in all the 6
- * positions (+x, -x, +y, -y, +z, -z).
- */
-BMX160CorrectionParameters calibrateAccelerometer(
-    BMX160CorrectionParameters correctionParameters)
+void calibrateAccelerometer()
 {
-    Eigen::Matrix<float, 3, 2> m;
-    auto* calibrationModel =
-        new SixParameterCalibration<AccelerometerData,
-                                    ACC_CALIBRATION_SAMPLES *
-                                        ACC_CALIBRATION_N_ORIENTATIONS>;
+    SixParametersCorrector correction;
+    correction.fromFile("/sd/bmx160_accelerometer_correction.csv");
 
-    SPIBus bus(SPI4);
+    SixParameterCalibration calibrationModel({0, 0, 9.8});
 
     BMX160Config bmxConfig;
     bmxConfig.fifoMode      = BMX160Config::FifoMode::HEADER;
-    bmxConfig.fifoWatermark = 100;
+    bmxConfig.fifoWatermark = 20;
     bmxConfig.fifoInterrupt = BMX160Config::FifoInterruptPin::PIN_INT1;
 
     bmxConfig.temperatureDivider = 0;
@@ -176,58 +181,38 @@ BMX160CorrectionParameters calibrateAccelerometer(
     bmxConfig.accelerometerRange = BMX160Config::AccelerometerRange::G_16;
     bmxConfig.gyroscopeRange     = BMX160Config::GyroscopeRange::DEG_2000;
 
-    bmxConfig.accelerometerDataRate = BMX160Config::OutputDataRate::HZ_1600;
-    bmxConfig.gyroscopeDataRate     = BMX160Config::OutputDataRate::HZ_1600;
-    bmxConfig.magnetometerRate      = BMX160Config::OutputDataRate::HZ_50;
+    bmxConfig.accelerometerDataRate = BMX160Config::OutputDataRate::HZ_100;
+    bmxConfig.gyroscopeDataRate     = BMX160Config::OutputDataRate::HZ_100;
+    bmxConfig.magnetometerRate      = BMX160Config::OutputDataRate::HZ_100;
 
     bmxConfig.gyroscopeUnit = BMX160Config::GyroscopeMeasureUnit::RAD;
 
     bmx160 = new BMX160(bus, miosix::sensors::bmx160::cs::getPin(), bmxConfig);
 
-    TRACE("Initializing BMX160...\n");
+    printf("Initializing BMX160...\n");
 
     if (!bmx160->init())
-    {
-        TRACE("Init failed! (code: %d)\n", bmx160->getLastError());
-        return correctionParameters;
-    }
+        printf("Init failed! (code: %d)\n", bmx160->getLastError());
 
-    TRACE("Performing self-test...\n");
+    printf("Performing self-test...\n");
 
     if (!bmx160->selfTest())
-    {
-        TRACE("Self-test failed! (code: %d)\n", bmx160->getLastError());
-        return correctionParameters;
-    }
-
-    TRACE("Initialization and self-test completed!\n");
+        printf("Self-test failed! (code: %d)\n", bmx160->getLastError());
 
-    Eigen::Vector3f avgAccel, vec;
-    int fifoAccSampleCount;
-    uint64_t accelTimestamp = 0;
-
-    // Set reference vector, 1g on third axis
-    calibrationModel->setReferenceVector({0, 0, 9.8});
+    printf("Initialization and self-test completed!\n");
 
     // Show the user the current correction values
-    printf("\nCurrent bias vector\n");
-    printf("b = [    % 2.5f    % 2.5f    % 2.5f    ]\n\n",
-           correctionParameters.accelParams(0, 1),
-           correctionParameters.accelParams(1, 1),
-           correctionParameters.accelParams(2, 1));
-    printf("Matrix to be multiplied to the input vector\n");
-    printf("    |    % 2.5f    % 2.5f    % 2.5f    |\n",
-           correctionParameters.accelParams(0, 0), 0.f, 0.f);
-    printf("M = |    % 2.5f    % 2.5f    % 2.5f    |\n", 0.f,
-           correctionParameters.accelParams(1, 0), 0.f);
-    printf("    |    % 2.5f    % 2.5f    % 2.5f    |\n", 0.f, 0.f,
-           correctionParameters.accelParams(2, 0));
+    printf("Current correction parameters:\n");
+    printf("A = |% 2.5f    % 2.5f    % 2.5f|\n\n", correction.getA()(0),
+           correction.getA()(1), correction.getA()(2));
+    printf("b = |% 2.5f    % 2.5f    % 2.5f|\n\n", correction.getb()(0),
+           correction.getb()(1), correction.getb()(2));
 
     waitForInput();
 
     printf(
-        "Please note that the BMX axis, viewed facing the death stack x, "
-        "are as follows:\n");
+        "Please note that the BMX axis, viewed facing the ELC bay, are as "
+        "follows:\n");
     printf("         z   x\n");
     printf("         ^   ^\n");
     printf("         |  /\n");
@@ -247,70 +232,52 @@ BMX160CorrectionParameters calibrateAccelerometer(
 
         printf("Reding data and feeding the model...\n");
 
-        // Sample the sensor and feed the data to the calibration model
-        int count = 0;
-        while (count < ACC_CALIBRATION_SAMPLES)
-        {
-            bmx160->sample();
-            avgAccel = {0, 0, 0};
-
-            // Read all the data contained in the fifo
-            uint8_t size       = bmx160->getLastFifoSize();
-            fifoAccSampleCount = 0;
-            for (int ii = 0; ii < size; ii++)
+        TaskScheduler scheduler;
+        scheduler.addTask(
+            [&]()
             {
-                BMX160Data fifoElement = bmx160->getFifoElement(ii);
+                bmx160->sample();
 
-                // Read acceleration data
-                if (fifoElement.accelerationTimestamp > accelTimestamp)
-                {
-                    static_cast<AccelerometerData>(fifoElement) >> vec;
-                    avgAccel += vec;
+                uint8_t fifoSize = bmx160->getLastFifoSize();
+                auto& fifo       = bmx160->getLastFifo();
 
-                    accelTimestamp = fifoElement.accelerationTimestamp;
-                    fifoAccSampleCount++;
+                for (uint8_t ii = 0; ii < fifoSize; ii++)
+                {
+                    Logger::getInstance().log(fifo.at(ii));
+                    calibrationModel.feed(
+                        static_cast<AccelerometerData>(fifo.at(ii)),
+                        orientations[i]);
                 }
-            }
-            avgAccel /= fifoAccSampleCount;
-
-            AccelerometerData data;
-            data << avgAccel;
+            },
+            200);
+        Logger::getInstance().start();
+        scheduler.start();
 
-            miosix::Thread::sleep(ACC_CALIBRATION_SLEEP_TIME);
+        Thread::sleep(ACC_CALIBRATION_SLEEP_TIME * 1000);
 
-            calibrationModel->feed(data, orientations[i]);
-            count++;
-        }
-
-        printf("Orientation calibration completed\n");
+        scheduler.stop();
+        Logger::getInstance().stop();
     }
 
-    printf("Computing the result....");
-    SixParameterCorrector<AccelerometerData> corrector =
-        calibrationModel->computeResult();
-
-    corrector >> m;
-    corrector >> correctionParameters.accelParams;
+    printf("Computing the result....\n");
+    auto newCorrector = calibrationModel.computeResult();
 
-    printf("Bias vector\n");
-    printf("b = [    % 2.5f    % 2.5f    % 2.5f    ]\n\n", m(0, 1), m(1, 1),
-           m(2, 1));
-    printf("Matrix to be multiplied to the input vector\n");
-    printf("    |    % 2.5f    % 2.5f    % 2.5f    |\n", m(0, 0), 0.f, 0.f);
-    printf("M = |    % 2.5f    % 2.5f    % 2.5f    |\n", 0.f, m(1, 0), 0.f);
-    printf("    |    % 2.5f    % 2.5f    % 2.5f    |\n", 0.f, 0.f, m(2, 0));
+    printf("New correction parameters:\n");
+    printf("A = |% 2.5f    % 2.5f    % 2.5f|\n\n", newCorrector.getA()(0),
+           newCorrector.getA()(1), newCorrector.getA()(2));
+    printf("b = |% 2.5f    % 2.5f    % 2.5f|\n\n", newCorrector.getb()(0),
+           newCorrector.getb()(1), newCorrector.getb()(2));
 
-    return correctionParameters;
+    newCorrector.toFile("/sd/bmx160_accelerometer_correction.csv");
 }
 
-BMX160CorrectionParameters calibrateMagnetometer(
-    BMX160CorrectionParameters correctionParameters)
+void calibrateMagnetometer()
 {
-    Eigen::Matrix<float, 3, 2> m;
-    auto* calibrationModel = new SoftAndHardIronCalibration;
-    Eigen::Vector3f avgMag{0, 0, 0}, vec;
+    SixParametersCorrector corrector;
+    corrector.fromFile("/sd/bmx160_magnetometer_correction.csv");
 
-    SPIBus bus(SPI4);
+    SoftAndHardIronCalibration calibrationModel;
+    Vector3f avgMag{0, 0, 0}, vec;
 
     BMX160Config bmxConfig;
     bmxConfig.fifoMode      = BMX160Config::FifoMode::HEADER;
@@ -330,37 +297,24 @@ BMX160CorrectionParameters calibrateMagnetometer(
 
     bmx160 = new BMX160(bus, miosix::sensors::bmx160::cs::getPin(), bmxConfig);
 
-    TRACE("Initializing BMX160...\n");
+    printf("Initializing BMX160...\n");
 
     if (!bmx160->init())
-    {
-        TRACE("Init failed! (code: %d)\n", bmx160->getLastError());
-        return correctionParameters;
-    }
+        printf("Init failed! (code: %d)\n", bmx160->getLastError());
 
-    TRACE("Performing self-test...\n");
+    printf("Performing self-test...\n");
 
     if (!bmx160->selfTest())
-    {
-        TRACE("Self-test failed! (code: %d)\n", bmx160->getLastError());
-        return correctionParameters;
-    }
+        printf("Self-test failed! (code: %d)\n", bmx160->getLastError());
 
-    TRACE("Initialization and self-test completed!\n");
+    printf("Initialization and self-test completed!\n");
 
     // Show the user the current correction values
-    printf("Current bias vector\n");
-    printf("b = |% 2.5f    % 2.5f    % 2.5f|\n\n",
-           correctionParameters.magnetoParams(0, 1),
-           correctionParameters.magnetoParams(1, 1),
-           correctionParameters.magnetoParams(2, 1));
-    printf("Matrix to be multiplied to the input vector\n");
-    printf("    |% 2.5f    % 2.5f    % 2.5f|\n",
-           correctionParameters.magnetoParams(0, 0), 0.f, 0.f);
-    printf("M = |% 2.5f    % 2.5f    % 2.5f|\n", 0.f,
-           correctionParameters.magnetoParams(1, 0), 0.f);
-    printf("    |% 2.5f    % 2.5f    % 2.5f|\n", 0.f, 0.f,
-           correctionParameters.magnetoParams(2, 0));
+    printf("Current correction parameters:\n");
+    printf("A = |% 2.5f    % 2.5f    % 2.5f|\n\n", corrector.getA()(0),
+           corrector.getA()(1), corrector.getA()(2));
+    printf("b = |% 2.5f    % 2.5f    % 2.5f|\n\n", corrector.getb()(0),
+           corrector.getb()(1), corrector.getb()(2));
 
     printf("Now I will calibrate the magnetometer\n");
     printf(
@@ -386,7 +340,7 @@ BMX160CorrectionParameters calibrateMagnetometer(
             for (uint8_t i = 0; i < fifoSize; i++)
             {
                 Logger::getInstance().log(fifo.at(i));
-                calibrationModel->feed(fifo.at(i));
+                calibrationModel.feed(fifo.at(i));
             }
         },
         200);
@@ -399,17 +353,89 @@ BMX160CorrectionParameters calibrateMagnetometer(
     Logger::getInstance().stop();
 
     printf("Computing the result....\n");
-    auto corrector = calibrationModel->computeResult();
+    auto newCorrector = calibrationModel.computeResult();
+
+    printf("New correction parameters:\n");
+    printf("A = |% 2.5f    % 2.5f    % 2.5f|\n\n", newCorrector.getA()(0),
+           newCorrector.getA()(1), newCorrector.getA()(2));
+    printf("b = |% 2.5f    % 2.5f    % 2.5f|\n\n", newCorrector.getb()(0),
+           newCorrector.getb()(1), newCorrector.getb()(2));
+
+    newCorrector.toFile("/sd/bmx160_magnetometer_correction.csv");
+}
+
+void calibrateGyroscope()
+{
+    BiasCalibration calibrationModel;
+    int count = 0;
+
+    BMX160Config bmxConfig;
+    bmxConfig.fifoMode      = BMX160Config::FifoMode::HEADER;
+    bmxConfig.fifoWatermark = 20;
+    bmxConfig.fifoInterrupt = BMX160Config::FifoInterruptPin::PIN_INT1;
+
+    bmxConfig.temperatureDivider = 0;
+
+    bmxConfig.accelerometerRange = BMX160Config::AccelerometerRange::G_16;
+    bmxConfig.gyroscopeRange     = BMX160Config::GyroscopeRange::DEG_2000;
+
+    bmxConfig.accelerometerDataRate = BMX160Config::OutputDataRate::HZ_100;
+    bmxConfig.gyroscopeDataRate     = BMX160Config::OutputDataRate::HZ_100;
+    bmxConfig.magnetometerRate      = BMX160Config::OutputDataRate::HZ_100;
+
+    bmxConfig.gyroscopeUnit = BMX160Config::GyroscopeMeasureUnit::RAD;
+
+    bmx160 = new BMX160(bus, miosix::sensors::bmx160::cs::getPin(), bmxConfig);
+
+    printf("Initializing BMX160...\n");
+
+    if (!bmx160->init())
+        printf("Init failed! (code: %d)\n", bmx160->getLastError());
+
+    printf("Performing self-test...\n");
+
+    if (!bmx160->selfTest())
+        printf("Self-test failed! (code: %d)\n", bmx160->getLastError());
+
+    printf("Initialization and self-test completed!\n");
+
+    printf(
+        "Now starting the gyroscope calibration, leave the stack perfectly "
+        "still\n");
 
-    corrector >> m;
-    corrector >> correctionParameters.magnetoParams;
+    TaskScheduler scheduler;
+    scheduler.addTask(
+        [&]()
+        {
+            bmx160->sample();
+
+            uint8_t fifoSize = bmx160->getLastFifoSize();
+            auto& fifo       = bmx160->getLastFifo();
+
+            for (uint8_t i = 0; i < fifoSize; i++)
+            {
+                auto data = fifo.at(i);
+                Logger::getInstance().log(data);
+
+                calibrationModel.feed({data.angularVelocityX,
+                                       data.angularVelocityY,
+                                       data.angularVelocityZ});
+                count++;
+            }
+        },
+        200);
+
+    Logger::getInstance().start();
+    scheduler.start();
+
+    Thread::sleep(GYRO_CALIBRATION_DURATION * 1000);
+
+    scheduler.stop();
+    Logger::getInstance().stop();
 
-    printf("b: the bias vector\n");
-    printf("b = |% 2.5f    % 2.5f    % 2.5f|\n\n", m(0, 1), m(1, 1), m(2, 1));
-    printf("M: the matrix to be multiplied to the input vector\n");
-    printf("    |% 2.5f    % 2.5f    % 2.5f|\n", m(0, 0), 0.f, 0.f);
-    printf("M = |% 2.5f    % 2.5f    % 2.5f|\n", 0.f, m(1, 0), 0.f);
-    printf("    |% 2.5f    % 2.5f    % 2.5f|\n", 0.f, 0.f, m(2, 0));
+    auto corrector = calibrationModel.computeResult();
 
-    return correctionParameters;
+    printf("New correction parameters:\n");
+    printf("b = |% 2.5f    % 2.5f    % 2.5f|\n\n", corrector.getb()(0),
+           corrector.getb()(1), corrector.getb()(2));
 }
diff --git a/src/tests/Main/test-nas.cpp b/src/tests/Main/test-nas.cpp
index ab81cf0c51367e93acbd0f72caced349ce7dac71..d9849ad79ffe3cb3806a07ddea357b08acf63fb2 100644
--- a/src/tests/Main/test-nas.cpp
+++ b/src/tests/Main/test-nas.cpp
@@ -61,8 +61,7 @@ int main()
 {
     init();
 
-    nas = new NAS(getEKConfig());
-    nas->setReferenceValues({0, 0, 0, 110000, 20 + 273.5});
+    nas              = new NAS(getEKConfig());
     stateInitializer = new StateInitializer();
     setInitialOrientation();
 
@@ -82,6 +81,7 @@ NASConfig getEKConfig()
     config.T              = 0.02f;
     config.SIGMA_BETA     = 0.0001f;
     config.SIGMA_W        = 0.3f;
+    config.SIGMA_ACC      = 0.1f;
     config.SIGMA_MAG      = 0.1f;
     config.SIGMA_GPS      = 10.0f;
     config.SIGMA_BAR      = 4.3f;
@@ -166,24 +166,11 @@ void step()
 
     // Calibration
     {
-        Vector3f offset{-0.0082, 0.0036, 0.0131};
-        angularVelocity = angularVelocity - offset;
-        Vector3f b{9.41150, -6.49408, 19.60433};
-        Matrix3f A{{0.66017, 0, 0}, {0, 0.66299, 0}, {0, 0, 2.28477}};
-        magneticField = (magneticField + b).transpose() * A;
-    }
+        angularVelocity -= Vector3f{-0.00863, 0.00337, 0.01284};
 
-    // Rotate
-    {
-        acceleration(0)    = -acceleration(0);
-        acceleration(1)    = acceleration(1);
-        acceleration(2)    = -acceleration(2);
-        angularVelocity(0) = -angularVelocity(0);
-        angularVelocity(1) = angularVelocity(1);
-        angularVelocity(2) = -angularVelocity(2);
-        magneticField(0)   = -magneticField(0);
-        magneticField(1)   = magneticField(1);
-        magneticField(2)   = -magneticField(2);
+        Matrix3f A{{0.73726, 0, 0}, {0, 0.59599, 0}, {0, 0, 2.27584}};
+        Vector3f b{39.22325, -17.47903, -13.81505};
+        magneticField = (magneticField - b).transpose() * A;
     }
 
     if (calibrating)
@@ -209,13 +196,15 @@ void step()
 
     // Predict step
     nas->predictGyro(angularVelocity);
-    // nas->predictAcc(acceleration);
+    nas->predictAcc(acceleration);
 
     // Correct step
     magneticField.normalize();
     nas->correctMag(magneticField);
     acceleration.normalize();
     nas->correctAcc(acceleration);
+    nas->correctGPS(
+        Vector4f{Constants::B21_LATITUDE, Constants::B21_LONGITUDE, 0, 0});
     nas->correctBaro(100000);
 
     // auto nasState = nas->getState();