diff --git a/components/core/crazyflie/hal/src/sensors_mpu6050_hm5883L_ms5611.c b/components/core/crazyflie/hal/src/sensors_mpu6050_hm5883L_ms5611.c
index ff9258b..bc906cc 100644
--- a/components/core/crazyflie/hal/src/sensors_mpu6050_hm5883L_ms5611.c
+++ b/components/core/crazyflie/hal/src/sensors_mpu6050_hm5883L_ms5611.c
@@ -305,18 +305,21 @@ void sensorsMpu6050Hmc5883lMs5611WaitDataReady(void)
 
 void processBarometerMeasurements(const uint8_t *buffer)
 {
-    //TODO: replace it to MS5611
-    DEBUG_PRINTW("processBarometerMeasurements NEED TODO");
-//   static uint32_t rawPressure = 0;
-//   static int16_t rawTemp = 0;
+    static uint32_t rawPressure = 0;
+    static int16_t rawTemp = 0;
 
-// Check if there is a new pressure update
-
-// Check if there is a new temp update
+    // Check if there is a new pressure update
+    if (buffer[0] & 0x02) {
+        rawPressure = ((uint32_t) buffer[3] << 16) | ((uint32_t) buffer[2] << 8) | buffer[1];
+    }
+    // Check if there is a new temp update
+    if (buffer[0] & 0x01) {
+        rawTemp = ((int16_t) buffer[5] << 8) | buffer[4];
+    }
 
-//   sensorData.baro.pressure = (float) rawPressure / LPS25H_LSB_PER_MBAR;
-//   sensorData.baro.temperature = LPS25H_TEMP_OFFSET + ((float) rawTemp / LPS25H_LSB_PER_CELSIUS);
-//   sensorData.baro.asl = lps25hPressureToAltitude(&sensorData.baro.pressure);
+    sensorData.baro.pressure = (float) rawPressure / MS5611_LSB_PER_MBAR;
+    sensorData.baro.temperature = MS5611_TEMP_OFFSET + ((float) rawTemp / MS5611_LSB_PER_CELSIUS);
+    sensorData.baro.asl = ms5611PressureToAltitude(&sensorData.baro.pressure);
 }
 
 void processMagnetometerMeasurements(const uint8_t *buffer)
diff --git a/components/core/crazyflie/modules/src/position_estimator_altitude.c b/components/core/crazyflie/modules/src/position_estimator_altitude.c
index ae73df9..1110771 100644
--- a/components/core/crazyflie/modules/src/position_estimator_altitude.c
+++ b/components/core/crazyflie/modules/src/position_estimator_altitude.c
@@ -44,6 +44,7 @@ struct selfState_s {
   float vAccDeadband; // Vertical acceleration deadband
   float velZAlpha;   // Blending factor to avoid vertical speed to accumulate error
   float estimatedVZ;
+  float baroAlpha;   // Blending factor for barometer data
 };
 
 static struct selfState_s state = {
@@ -55,6 +56,7 @@ static struct selfState_s state = {
   .vAccDeadband = 0.04f,
   .velZAlpha = 0.995f,
   .estimatedVZ = 0.0f,
+  .baroAlpha = 0.9f,
 };
 
 static void positionEstimateInternal(state_t* estimate, const sensorData_t* sensorData, const tofMeasurement_t* tofMeasurement, float dt, uint32_t tick, struct selfState_s* state);
@@ -103,6 +105,9 @@ static void positionEstimateInternal(state_t* estimate, const sensorData_t* sens
     state->estimatedZ = filteredZ + (state->velocityFactor * state->velocityZ * dt);
   }
 
+  // Incorporate barometer data for altitude estimation
+  state->estimatedZ = (state->baroAlpha * state->estimatedZ) + ((1.0f - state->baroAlpha) * sensorData->baro.asl);
+
   estimate->position.x = 0.0f;
   estimate->position.y = 0.0f;
   estimate->position.z = state->estimatedZ;
@@ -114,6 +119,8 @@ static void positionEstimateInternal(state_t* estimate, const sensorData_t* sens
 static void positionUpdateVelocityInternal(float accWZ, float dt, struct selfState_s* state) {
   state->velocityZ += deadband(accWZ, state->vAccDeadband) * dt * G;
   state->velocityZ *= state->velZAlpha;
+  // Update vertical velocity using barometer data
+  state->velocityZ = (state->baroAlpha * state->velocityZ) + ((1.0f - state->baroAlpha) * state->estimatedVZ);
 }
 
 LOG_GROUP_START(posEstAlt)
@@ -128,4 +135,5 @@ PARAM_ADD(PARAM_FLOAT, estAlphaZr, &state.estAlphaZrange)
 PARAM_ADD(PARAM_FLOAT, velFactor, &state.velocityFactor)
 PARAM_ADD(PARAM_FLOAT, velZAlpha, &state.velZAlpha)
 PARAM_ADD(PARAM_FLOAT, vAccDeadband, &state.vAccDeadband)
+PARAM_ADD(PARAM_FLOAT, baroAlpha, &state.baroAlpha)
 PARAM_GROUP_STOP(posEstAlt)