diff --git a/.vscode/settings.json b/.vscode/settings.json
index 85d459346d971d2af82abac3ea5ef87f7c240860..9dc44c462b9bbe8830a9767dd69084beb40a81b3 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -159,6 +159,7 @@
         "MPXH",
         "Nidasio",
         "pitot",
+        "SATS",
         "setb",
         "Setpoint",
         "stateinitializer",
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 73e684ae99af289299c879863a71a4a91ea4cf8d..a69738be5c7a29a21d5931f57bfcf97ab18f8b88 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -188,6 +188,13 @@ sbs_target(main-test-serial-ports stm32f429zi_skyward_death_stack_v3)
 add_executable(main-test-xbee src/tests/Main/test-xbee.cpp)
 sbs_target(main-test-xbee stm32f429zi_skyward_death_stack_v3)
 
+#-----------------------------------------------------------------------------#
+#                               Payload Computer                              #
+#-----------------------------------------------------------------------------#
+
+add_executable(payload-test-nas src/tests/Payload/test-nas.cpp)
+sbs_target(payload-test-nas stm32f429zi_skyward_death_stack_x)
+
 #-----------------------------------------------------------------------------#
 #                              Auxiliary Computer                             #
 #-----------------------------------------------------------------------------#
diff --git a/src/boards/Payload/Configs/SensorsConfig.h b/src/boards/Payload/Configs/SensorsConfig.h
index 2c96b7af450e46eed1c4541b9a393bb232a9233c..b1866cf762bf739c28d1c4f70686a54b8e656624 100644
--- a/src/boards/Payload/Configs/SensorsConfig.h
+++ b/src/boards/Payload/Configs/SensorsConfig.h
@@ -94,7 +94,7 @@ constexpr unsigned int IMU_BMX_FIFO_FILL_TIME =
 
 // Axis rotation
 static const Boardcore::AxisOrthoOrientation BMX160_AXIS_ROTATION = {
-    Boardcore::Direction::POSITIVE_Y, Boardcore::Direction::NEGATIVE_X};
+    Boardcore::Direction::NEGATIVE_Y, Boardcore::Direction::NEGATIVE_Z};
 
 // Correction parameter file
 constexpr char BMX160_CORRECTION_PARAMETERS_FILE[30] = "/sd/bmx160_params.csv";
diff --git a/src/tests/Payload/test-nas.cpp b/src/tests/Payload/test-nas.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..596e71392eff6690bc81b5942285d94c870d329c
--- /dev/null
+++ b/src/tests/Payload/test-nas.cpp
@@ -0,0 +1,186 @@
+/* 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 <drivers/interrupt/external_interrupts.h>
+#include <logger/Logger.h>
+#include <miosix.h>
+#include <sensors/BMX160/BMX160.h>
+#include <sensors/BMX160/BMX160WithCorrection.h>
+#include <sensors/MS5803/MS5803.h>
+#include <sensors/SensorManager.h>
+#include <sensors/UBXGPS/UBXGPSSerial.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;
+
+constexpr uint32_t UPDATE_PERIOD = 20;  // 50 hz
+
+const Eigen::Vector3f nedMag(0.4732, 0.0272, 0.8805);
+
+// static const Boardcore::AxisOrthoOrientation BMX160_AXIS_ROTATION = {
+//     Boardcore::Direction::NEGATIVE_Y, Boardcore::Direction::NEGATIVE_Z};
+static const Boardcore::AxisOrthoOrientation BMX160_AXIS_ROTATION = {
+    Boardcore::Direction::POSITIVE_X, Boardcore::Direction::POSITIVE_Y};
+
+static const Boardcore::ReferenceValues defaultReferenceValues = {
+    135.0,              // [m] Altitude
+    99714.0,            // [Pa] Pressure
+    278.27,             // [K] Temperature
+    45.50106793771145,  // [deg] Start latitude
+    9.156376900740167,  // [deg] Start longitude
+    Boardcore::Constants::MSL_PRESSURE,
+    Boardcore::Constants::MSL_TEMPERATURE,
+};
+
+static const Boardcore::NASConfig defaultNasConfig = {
+    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
+    10.0f,                   // SIGMA_POS
+    10.0f,                   // SIGMA_VEL
+    10.0f,                   // SIGMA_PITOT
+    1.0f,                    // P_POS
+    10.0f,                   // P_POS_VERTICAL
+    1.0f,                    // P_VEL
+    10.0f,                   // P_VEL_VERTICAL
+    0.01f,                   // P_ATT
+    0.01f,                   // P_BIAS
+    6.0f,                    // SATS_NUM
+    nedMag                   // NED_MAG
+};
+
+void init();
+void step();
+void print();
+
+BMX160* imu;
+BMX160WithCorrection* imuCorrected;
+NAS* nas;
+
+SPIBus spi(SPI1);
+
+void __attribute__((used)) EXTI5_IRQHandlerImpl()
+{
+    if (imu)
+        imu->IRQupdateTimestamp(TimestampTimer::getTimestamp());
+}
+
+int main()
+{
+    init();
+
+    TaskScheduler scheduler;
+    scheduler.addTask(step, 20, TaskScheduler::Policy::RECOVER);
+    scheduler.addTask(print, 250);
+    scheduler.start();
+
+    while (true)
+        Thread::sleep(1000);
+}
+
+void init()
+{
+    // Prepare bmx160
+    enableExternalInterrupt(miosix::sensors::bmx160::intr::getPin().getPort(),
+                            miosix::sensors::bmx160::intr::getPin().getNumber(),
+                            InterruptTrigger::FALLING_EDGE);
+
+    SPIBusConfig spiConfig;
+    spiConfig.clockDivider = SPI::ClockDivider::DIV_8;
+
+    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;
+
+    imu = new BMX160(spi, sensors::bmx160::cs::getPin(), bmxConfig, spiConfig);
+    imu->init();
+
+    // Prepare calibration
+    imuCorrected = new BMX160WithCorrection(imu, BMX160_AXIS_ROTATION);
+    imuCorrected->init();
+
+    // NAS
+    nas = new NAS(defaultNasConfig);
+
+    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 step()
+{
+    imu->sample();
+    imuCorrected->sample();
+
+    auto imuData = imuCorrected->getLastSample();
+
+    // Predict step
+    nas->predictGyro({0, 0, 0});
+    nas->predictAcc(imuData);
+
+    // Correct step
+    nas->correctMag(imuData);
+    nas->correctAcc(imuData);
+    nas->correctGPS(Vector4f{0, 0, 0, 0});
+    nas->correctBaro(defaultReferenceValues.refPressure);
+}
+
+void print()
+{
+    auto nasState = nas->getState();
+    auto imuData  = imuCorrected->getLastSample();
+
+    printf("%f, %f, %f\t", imuData.accelerationX, imuData.accelerationY,
+           imuData.accelerationZ);
+    printf("%f, %f, %f\t", imuData.magneticFieldX, imuData.magneticFieldY,
+           imuData.magneticFieldZ);
+
+    printf("w%fwa%fab%fbc%fc\n", nasState.qw, nasState.qx, nasState.qy,
+           nasState.qz);
+}