diff --git a/src/boards/Main/StateMachines/ADAController/ADAController.cpp b/src/boards/Main/StateMachines/ADAController/ADAController.cpp
index b72730bf351e8a467f4ff6a73d8d0d77319dc84a..dc31b481b0d042938cd2d19f2f25d2bbbbad982e 100644
--- a/src/boards/Main/StateMachines/ADAController/ADAController.cpp
+++ b/src/boards/Main/StateMachines/ADAController/ADAController.cpp
@@ -33,6 +33,7 @@
 #include <drivers/timer/TimestampTimer.h>
 #include <events/EventBroker.h>
 #include <miosix.h>
+#include <utils/AeroUtils/AeroUtils.h>
 
 using namespace miosix;
 using namespace Boardcore;
@@ -223,7 +224,7 @@ void ADAController::calibrate()
 
     for (int i = 0; i < CALIBRATION_SAMPLES_COUNT; i++)
     {
-        auto data = Sensors::getInstance().getMS5803LastSample();
+        auto data = Sensors::getInstance().getStaticPressureLastSample();
         pressure.add(data.pressure);
 
         miosix::Thread::sleep(CALIBRATION_SLEEP_TIME);
@@ -232,12 +233,15 @@ void ADAController::calibrate()
     // Set the pressure and temperature reference
     ReferenceValues reference = ada.getReferenceValues();
     reference.refPressure     = pressure.getStats().mean;
+    reference.refAltitude     = Aeroutils::relAltitude(
+            reference.refPressure, reference.mslPressure, reference.mslTemperature);
 
     // Update the algorithm reference values
     {
         miosix::PauseKernelLock l;
         ada.setReferenceValues(reference);
         ada.setKalmanConfig(getADAKalmanConfig());
+        ada.update(reference.refPressure);
     }
 
     EventBroker::getInstance().post(ADA_READY, TOPIC_ADA);
diff --git a/src/boards/Main/StateMachines/NASController/NASController.cpp b/src/boards/Main/StateMachines/NASController/NASController.cpp
index 2cc7b63b624fe4b871ae77d0a5dd019182787f9e..cf39b26e050e0695362e5ec3edbbb9ce49d0af7e 100644
--- a/src/boards/Main/StateMachines/NASController/NASController.cpp
+++ b/src/boards/Main/StateMachines/NASController/NASController.cpp
@@ -30,6 +30,7 @@
 #include <algorithms/NAS/StateInitializer.h>
 #include <common/ReferenceConfig.h>
 #include <common/events/Events.h>
+#include <utils/AeroUtils/AeroUtils.h>
 
 using namespace std;
 using namespace Eigen;
@@ -104,7 +105,8 @@ void NASController::calibrate()
                      imuData.magneticFieldZ);
 
         // Barometer
-        MS5803Data barometerData = Sensors::getInstance().getMS5803LastSample();
+        auto barometerData =
+            Sensors::getInstance().getStaticPressureLastSample();
         pressure.add(barometerData.pressure);
 
         miosix::Thread::sleep(CALIBRATION_SLEEP_TIME);
@@ -123,6 +125,8 @@ void NASController::calibrate()
     // Set the pressure reference using an already existing reference values
     ReferenceValues reference = nas.getReferenceValues();
     reference.refPressure     = pressure.getStats().mean;
+    reference.refAltitude     = Aeroutils::relAltitude(
+            reference.refPressure, reference.mslPressure, reference.mslTemperature);
 
     // If in this moment the GPS has fix i use that position as starting
     UBXGPSData gps = Sensors::getInstance().getUbxGpsLastSample();