diff --git a/make/source.mk b/make/source.mk
index 519fbe8865..f976b0380d 100644
--- a/make/source.mk
+++ b/make/source.mk
@@ -107,7 +107,7 @@ FC_SRC = \
             fc/rc_adjustments.c \
             fc/rc_controls.c \
             fc/rc_modes.c \
-            flight/altitude.c \
+            flight/position.c \
             flight/failsafe.c \
             flight/imu.c \
             flight/mixer.c \
diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c
index 8460d95975..9324ca5d40 100644
--- a/src/main/fc/fc_core.c
+++ b/src/main/fc/fc_core.c
@@ -83,7 +83,7 @@
 
 #include "telemetry/telemetry.h"
 
-#include "flight/altitude.h"
+#include "flight/position.h"
 #include "flight/failsafe.h"
 #include "flight/imu.h"
 #include "flight/mixer.h"
@@ -866,17 +866,7 @@ static NOINLINE void subTaskMainSubprocesses(timeUs_t currentTimeUs)
         updateMagHold();
     }
 #endif
-
-#if defined(USE_ALT_HOLD)
-    // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState
-    updateRcCommands();
-    if (sensors(SENSOR_BARO) || sensors(SENSOR_RANGEFINDER)) {
-        if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(RANGEFINDER_MODE)) {
-            applyAltHold();
-        }
-    }
-#endif
-
+    
     // If we're armed, at minimum throttle, and we do arming via the
     // sticks, do not process yaw input from the rx.  We do this so the
     // motors do not spin up while we are trying to arm or disarm.
diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c
index aad2d51c88..2f1e01a6fc 100644
--- a/src/main/fc/fc_tasks.c
+++ b/src/main/fc/fc_tasks.c
@@ -49,7 +49,7 @@
 #include "fc/rc_controls.h"
 #include "fc/runtime_config.h"
 
-#include "flight/altitude.h"
+#include "flight/position.h"
 #include "flight/imu.h"
 #include "flight/mixer.h"
 #include "flight/pid.h"
@@ -81,8 +81,8 @@
 #include "sensors/compass.h"
 #include "sensors/esc_sensor.h"
 #include "sensors/gyro.h"
-#include "sensors/rangefinder.h"
 #include "sensors/sensors.h"
+#include "sensors/rangefinder.h"
 
 #include "scheduler/scheduler.h"
 
@@ -168,25 +168,9 @@ static void taskUpdateRxMain(timeUs_t currentTimeUs)
     }
 #endif
 
-#if !defined(USE_ALT_HOLD)
     // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState
     updateRcCommands();
-#endif
     updateArmingStatus();
-
-#ifdef USE_ALT_HOLD
-#ifdef USE_BARO
-    if (sensors(SENSOR_BARO)) {
-        updateAltHoldState();
-    }
-#endif
-
-#ifdef USE_RANGEFINDER
-    if (sensors(SENSOR_RANGEFINDER)) {
-        updateRangefinderAltHoldState();
-    }
-#endif
-#endif // USE_ALT_HOLD
 }
 #endif
 
@@ -204,20 +188,12 @@ static void taskUpdateBaro(timeUs_t currentTimeUs)
 }
 #endif
 
-#if defined(USE_BARO) || defined(USE_RANGEFINDER)
+#if defined(USE_BARO) || defined(USE_GPS)
 static void taskCalculateAltitude(timeUs_t currentTimeUs)
 {
-    if (false
-#if defined(USE_BARO)
-        || (sensors(SENSOR_BARO) && isBaroReady())
-#endif
-#if defined(USE_RANGEFINDER)
-        || sensors(SENSOR_RANGEFINDER)
-#endif
-        ) {
-        calculateEstimatedAltitude(currentTimeUs);
-    }}
-#endif // USE_BARO || USE_RANGEFINDER
+    calculateEstimatedAltitude(currentTimeUs);
+}
+#endif // USE_BARO || USE_GPS
 
 #ifdef USE_TELEMETRY
 static void taskTelemetry(timeUs_t currentTimeUs)
@@ -296,11 +272,8 @@ void fcTasksInit(void)
 #ifdef USE_BARO
     setTaskEnabled(TASK_BARO, sensors(SENSOR_BARO));
 #endif
-#ifdef USE_RANGEFINDER
-    setTaskEnabled(TASK_RANGEFINDER, sensors(SENSOR_RANGEFINDER));
-#endif
-#if defined(USE_BARO) || defined(USE_RANGEFINDER)
-    setTaskEnabled(TASK_ALTITUDE, sensors(SENSOR_BARO) || sensors(SENSOR_RANGEFINDER));
+#if defined(USE_BARO) || defined(USE_GPS)
+    setTaskEnabled(TASK_ALTITUDE, sensors(SENSOR_BARO) || sensors(SENSOR_GPS));
 #endif
 #ifdef USE_DASHBOARD
     setTaskEnabled(TASK_DASHBOARD, feature(FEATURE_DASHBOARD));
@@ -502,16 +475,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
     },
 #endif
 
-#ifdef USE_RANGEFINDER
-    [TASK_RANGEFINDER] = {
-        .taskName = "RANGEFINDER",
-        .taskFunc = rangefinderUpdate,
-        .desiredPeriod = TASK_PERIOD_MS(70), // XXX HCSR04 sonar specific value.
-        .staticPriority = TASK_PRIORITY_LOW,
-    },
-#endif
-
-#if defined(USE_BARO) || defined(USE_RANGEFINDER)
+#if defined(USE_BARO) || defined(USE_GPS)
     [TASK_ALTITUDE] = {
         .taskName = "ALTITUDE",
         .taskFunc = taskCalculateAltitude,
diff --git a/src/main/flight/altitude.c b/src/main/flight/altitude.c
deleted file mode 100644
index 260664b427..0000000000
--- a/src/main/flight/altitude.c
+++ /dev/null
@@ -1,317 +0,0 @@
-/*
- * This file is part of Cleanflight and Betaflight.
- *
- * Cleanflight and Betaflight are free software. You can redistribute
- * this software and/or modify this software under the terms of the
- * GNU General Public License as published by the Free Software
- * Foundation, either version 3 of the License, or (at your option)
- * any later version.
- *
- * Cleanflight and Betaflight are distributed in the hope that they
- * will be useful, but WITHOUT ANY WARRANTY; without even the implied
- * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
- * See the GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with this software.
- *
- * If not, see <http://www.gnu.org/licenses/>.
- */
-
-#include <stdbool.h>
-#include <stdint.h>
-#include <stdlib.h>
-#include <math.h>
-
-#include "platform.h"
-
-#include "build/debug.h"
-
-#include "common/axis.h"
-#include "common/maths.h"
-#include "common/utils.h"
-
-#include "pg/pg.h"
-#include "pg/pg_ids.h"
-
-#include "fc/config.h"
-#include "fc/rc_controls.h"
-#include "fc/rc_modes.h"
-#include "fc/runtime_config.h"
-
-#include "flight/altitude.h"
-#include "flight/imu.h"
-#include "flight/pid.h"
-
-#include "rx/rx.h"
-
-#include "sensors/sensors.h"
-#include "sensors/barometer.h"
-#include "sensors/rangefinder.h"
-
-
-int32_t AltHold;
-static int32_t estimatedVario = 0;                      // variometer in cm/s
-static int32_t estimatedAltitude = 0;                // in cm
-
-
-
-enum {
-    DEBUG_ALTITUDE_ACC,
-    DEBUG_ALTITUDE_VEL,
-    DEBUG_ALTITUDE_HEIGHT
-};
-// 40hz update rate (20hz LPF on acc)
-#define BARO_UPDATE_FREQUENCY_40HZ (1000 * 25)
-
-#if defined(USE_ALT_HOLD)
-
-PG_REGISTER_WITH_RESET_TEMPLATE(airplaneConfig_t, airplaneConfig, PG_AIRPLANE_CONFIG, 0);
-
-PG_RESET_TEMPLATE(airplaneConfig_t, airplaneConfig,
-    .fixedwing_althold_reversed = false
-);
-
-static int32_t setVelocity = 0;
-static uint8_t velocityControl = 0;
-static int32_t errorVelocityI = 0;
-static int32_t altHoldThrottleAdjustment = 0;
-static int16_t initialThrottleHold;
-
-
-#define DEGREES_80_IN_DECIDEGREES 800
-
-static void applyMultirotorAltHold(void)
-{
-    static uint8_t isAltHoldChanged = 0;
-    // multirotor alt hold
-    if (rcControlsConfig()->alt_hold_fast_change) {
-        // rapid alt changes
-        if (ABS(rcData[THROTTLE] - initialThrottleHold) > rcControlsConfig()->alt_hold_deadband) {
-            errorVelocityI = 0;
-            isAltHoldChanged = 1;
-            rcCommand[THROTTLE] += (rcData[THROTTLE] > initialThrottleHold) ? -rcControlsConfig()->alt_hold_deadband : rcControlsConfig()->alt_hold_deadband;
-        } else {
-            if (isAltHoldChanged) {
-                AltHold = estimatedAltitude;
-                isAltHoldChanged = 0;
-            }
-            rcCommand[THROTTLE] = constrain(initialThrottleHold + altHoldThrottleAdjustment, PWM_RANGE_MIN, PWM_RANGE_MAX);
-        }
-    } else {
-        // slow alt changes, mostly used for aerial photography
-        if (ABS(rcData[THROTTLE] - initialThrottleHold) > rcControlsConfig()->alt_hold_deadband) {
-            // set velocity proportional to stick movement +100 throttle gives ~ +50 cm/s
-            setVelocity = (rcData[THROTTLE] - initialThrottleHold) / 2;
-            velocityControl = 1;
-            isAltHoldChanged = 1;
-        } else if (isAltHoldChanged) {
-            AltHold = estimatedAltitude;
-            velocityControl = 0;
-            isAltHoldChanged = 0;
-        }
-        rcCommand[THROTTLE] = constrain(initialThrottleHold + altHoldThrottleAdjustment, PWM_RANGE_MIN, PWM_RANGE_MAX);
-    }
-}
-
-static void applyFixedWingAltHold(void)
-{
-    // handle fixedwing-related althold. UNTESTED! and probably wrong
-    // most likely need to check changes on pitch channel and 'reset' althold similar to
-    // how throttle does it on multirotor
-
-    rcCommand[PITCH] += altHoldThrottleAdjustment * GET_DIRECTION(airplaneConfig()->fixedwing_althold_reversed);
-}
-
-void applyAltHold(void)
-{
-    if (STATE(FIXED_WING)) {
-        applyFixedWingAltHold();
-    } else {
-        applyMultirotorAltHold();
-    }
-}
-
-void updateAltHoldState(void)
-{
-    // Baro alt hold activate
-    if (!IS_RC_MODE_ACTIVE(BOXBARO)) {
-        DISABLE_FLIGHT_MODE(BARO_MODE);
-        return;
-    }
-
-    if (!FLIGHT_MODE(BARO_MODE)) {
-        ENABLE_FLIGHT_MODE(BARO_MODE);
-        AltHold = estimatedAltitude;
-        initialThrottleHold = rcData[THROTTLE];
-        errorVelocityI = 0;
-        altHoldThrottleAdjustment = 0;
-    }
-}
-
-void updateRangefinderAltHoldState(void)
-{
-    // Sonar alt hold activate
-    if (!IS_RC_MODE_ACTIVE(BOXRANGEFINDER)) {
-        DISABLE_FLIGHT_MODE(RANGEFINDER_MODE);
-        return;
-    }
-
-    if (!FLIGHT_MODE(RANGEFINDER_MODE)) {
-        ENABLE_FLIGHT_MODE(RANGEFINDER_MODE);
-        AltHold = estimatedAltitude;
-        initialThrottleHold = rcData[THROTTLE];
-        errorVelocityI = 0;
-        altHoldThrottleAdjustment = 0;
-    }
-}
-
-bool isThrustFacingDownwards(attitudeEulerAngles_t *attitude)
-{
-    return ABS(attitude->values.roll) < DEGREES_80_IN_DECIDEGREES && ABS(attitude->values.pitch) < DEGREES_80_IN_DECIDEGREES;
-}
-
-int32_t calculateAltHoldThrottleAdjustment(int32_t vel_tmp, float accZ_tmp, float accZ_old)
-{
-    int32_t result = 0;
-    int32_t error;
-    int32_t setVel;
-
-    if (!isThrustFacingDownwards(&attitude)) {
-        return result;
-    }
-
-    // Altitude P-Controller
-
-    if (!velocityControl) {
-        error = constrain(AltHold - estimatedAltitude, -500, 500);
-        error = applyDeadband(error, 10); // remove small P parameter to reduce noise near zero position
-        setVel = constrain((currentPidProfile->pid[PID_ALT].P * error / 128), -300, +300); // limit velocity to +/- 3 m/s
-    } else {
-        setVel = setVelocity;
-    }
-    // Velocity PID-Controller
-
-    // P
-    error = setVel - vel_tmp;
-    result = constrain((currentPidProfile->pid[PID_VEL].P * error / 32), -300, +300);
-
-    // I
-    errorVelocityI += (currentPidProfile->pid[PID_VEL].I * error);
-    errorVelocityI = constrain(errorVelocityI, -(8192 * 200), (8192 * 200));
-    result += errorVelocityI / 8192;     // I in range +/-200
-
-    // D
-    result -= constrain(currentPidProfile->pid[PID_VEL].D * (accZ_tmp + accZ_old) / 512, -150, 150);
-
-    return result;
-}
-#endif // USE_ALT_HOLD
-
-#if defined(USE_BARO) || defined(USE_RANGEFINDER)
-void calculateEstimatedAltitude(timeUs_t currentTimeUs)
-{
-    static timeUs_t previousTimeUs = 0;
-    const uint32_t dTime = currentTimeUs - previousTimeUs;
-    if (dTime < BARO_UPDATE_FREQUENCY_40HZ) {
-        return;
-    }
-    previousTimeUs = currentTimeUs;
-
-    static float vel = 0.0f;
-    static float accAlt = 0.0f;
-
-    int32_t baroAlt = 0;
-#ifdef USE_BARO
-    if (sensors(SENSOR_BARO)) {
-        if (!isBaroCalibrationComplete()) {
-            performBaroCalibrationCycle();
-            vel = 0;
-            accAlt = 0;
-        } else {
-            baroAlt = baroCalculateAltitude();
-            estimatedAltitude = baroAlt;
-        }
-    }
-#endif
-
-#ifdef USE_RANGEFINDER
-    if (sensors(SENSOR_RANGEFINDER) && rangefinderProcess(getCosTiltAngle())) {
-        int32_t rangefinderAlt = rangefinderGetLatestAltitude();
-        if (rangefinderAlt > 0 && rangefinderAlt >= rangefinderCfAltCm && rangefinderAlt <= rangefinderMaxAltWithTiltCm) {
-            // RANGEFINDER in range, so use complementary filter
-            float rangefinderTransition = (float)(rangefinderMaxAltWithTiltCm - rangefinderAlt) / (rangefinderMaxAltWithTiltCm - rangefinderCfAltCm);
-            rangefinderAlt = (float)rangefinderAlt * rangefinderTransition + baroAlt * (1.0f - rangefinderTransition);
-            estimatedAltitude = rangefinderAlt;
-        }
-    }
-#endif
-
-    float accZ_tmp = 0;
-#ifdef USE_ACC
-    if (sensors(SENSOR_ACC)) {
-        const float dt = accTimeSum * 1e-6f; // delta acc reading time in seconds
-
-        // Integrator - velocity, cm/sec
-        if (accSumCount) {
-            accZ_tmp = (float)accSum[2] / accSumCount;
-        }
-        const float vel_acc = accZ_tmp * accVelScale * (float)accTimeSum;
-
-        // Integrator - Altitude in cm
-        accAlt += (vel_acc * 0.5f) * dt + vel * dt;  // integrate velocity to get distance (x= a/2 * t^2)
-        accAlt = accAlt * CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_cf_alt) + (float)baro.BaroAlt * (1.0f - CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_cf_alt));    // complementary filter for altitude estimation (baro & acc)
-        vel += vel_acc;
-        estimatedAltitude = accAlt;
-    }
-#endif
-
-    DEBUG_SET(DEBUG_ALTITUDE, DEBUG_ALTITUDE_ACC, accSum[2] / accSumCount);
-    DEBUG_SET(DEBUG_ALTITUDE, DEBUG_ALTITUDE_VEL, vel);
-    DEBUG_SET(DEBUG_ALTITUDE, DEBUG_ALTITUDE_HEIGHT, accAlt);
-
-    imuResetAccelerationSum();
-
-    int32_t baroVel = 0;
-#ifdef USE_BARO
-    if (sensors(SENSOR_BARO)) {
-        if (!isBaroCalibrationComplete()) {
-            return;
-        }
-
-        static int32_t lastBaroAlt = 0;
-        baroVel = (baroAlt - lastBaroAlt) * 1000000.0f / dTime;
-        lastBaroAlt = baroAlt;
-
-        baroVel = constrain(baroVel, -1500, 1500);  // constrain baro velocity +/- 1500cm/s
-        baroVel = applyDeadband(baroVel, 10);       // to reduce noise near zero
-    }
-#endif // USE_BARO
-
-    // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity).
-    // By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay
-    vel = vel * CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_cf_vel) + baroVel * (1.0f - CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_cf_vel));
-    int32_t vel_tmp = lrintf(vel);
-
-    // set vario
-    estimatedVario = applyDeadband(vel_tmp, 5);
-
-#ifdef USE_ALT_HOLD
-    static float accZ_old = 0.0f;
-    altHoldThrottleAdjustment = calculateAltHoldThrottleAdjustment(vel_tmp, accZ_tmp, accZ_old);
-    accZ_old = accZ_tmp;
-#else
-    UNUSED(accZ_tmp);
-#endif
-}
-#endif // USE_BARO || USE_RANGEFINDER
-
-int32_t getEstimatedAltitude(void)
-{
-    return estimatedAltitude;
-}
-
-int32_t getEstimatedVario(void)
-{
-    return estimatedVario;
-}
diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c
index 66e62ebd10..8a579ec45d 100644
--- a/src/main/flight/imu.c
+++ b/src/main/flight/imu.c
@@ -193,58 +193,6 @@ void imuResetAccelerationSum(void)
     accTimeSum = 0;
 }
 
-#if defined(USE_ALT_HOLD)
-static void imuTransformVectorBodyToEarth(t_fp_vector * v)
-{
-    // From body frame to earth frame
-    const float x = rMat[0][0] * v->V.X + rMat[0][1] * v->V.Y + rMat[0][2] * v->V.Z;
-    const float y = rMat[1][0] * v->V.X + rMat[1][1] * v->V.Y + rMat[1][2] * v->V.Z;
-    const float z = rMat[2][0] * v->V.X + rMat[2][1] * v->V.Y + rMat[2][2] * v->V.Z;
-
-    v->V.X = x;
-    v->V.Y = -y;
-    v->V.Z = z;
-}
-
-// rotate acc into Earth frame and calculate acceleration in it
-static void imuCalculateAcceleration(timeDelta_t deltaT)
-{
-    static int32_t accZoffset = 0;
-    static float accz_smooth = 0;
-
-    // deltaT is measured in us ticks
-    const float dT = (float)deltaT * 1e-6f;
-
-    t_fp_vector accel_ned;
-    accel_ned.V.X = acc.accADC[X];
-    accel_ned.V.Y = acc.accADC[Y];
-    accel_ned.V.Z = acc.accADC[Z];
-
-    imuTransformVectorBodyToEarth(&accel_ned);
-
-    if (imuRuntimeConfig.acc_unarmedcal == 1) {
-        if (!ARMING_FLAG(ARMED)) {
-            accZoffset -= accZoffset / 64;
-            accZoffset += accel_ned.V.Z;
-        }
-        accel_ned.V.Z -= accZoffset / 64;  // compensate for gravitation on z-axis
-    } else {
-        accel_ned.V.Z -= acc.dev.acc_1G;
-    }
-
-    accz_smooth = accz_smooth + (dT / (fc_acc + dT)) * (accel_ned.V.Z - accz_smooth); // low pass filter
-
-    // apply Deadband to reduce integration drift and vibration influence
-    accSum[X] += applyDeadband(lrintf(accel_ned.V.X), imuRuntimeConfig.accDeadband.xy);
-    accSum[Y] += applyDeadband(lrintf(accel_ned.V.Y), imuRuntimeConfig.accDeadband.xy);
-    accSum[Z] += applyDeadband(lrintf(accz_smooth), imuRuntimeConfig.accDeadband.z);
-
-    // sum up Values for later integration to get velocity and distance
-    accTimeSum += deltaT;
-    accSumCount++;
-}
-#endif // USE_ALT_HOLD
-
 static float invSqrt(float x)
 {
     return 1.0f / sqrtf(x);
@@ -531,6 +479,7 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
     UNUSED(useCOG);
     UNUSED(canUseGPSHeading);
     UNUSED(courseOverGround);
+    UNUSED(deltaT);
 #else
 
 #if defined(SIMULATOR_BUILD) && defined(SIMULATOR_IMU_SYNC)
@@ -552,9 +501,6 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs)
 
     imuUpdateEulerAngles();
 #endif
-#if defined(USE_ALT_HOLD)
-    imuCalculateAcceleration(deltaT); // rotate acc vector into earth frame
-#endif
 }
 
 void imuUpdateAttitude(timeUs_t currentTimeUs)
diff --git a/src/main/flight/position.c b/src/main/flight/position.c
new file mode 100644
index 0000000000..fb1963adf5
--- /dev/null
+++ b/src/main/flight/position.c
@@ -0,0 +1,117 @@
+/*
+ * This file is part of Cleanflight and Betaflight.
+ *
+ * Cleanflight and Betaflight are free software: you can redistribute 
+ * this software and/or modify this software under the terms of the 
+ * GNU General Public License as published by the Free Software 
+ * Foundation, either version 3 of the License, or (at your option) 
+ * any later version.
+ *
+ * Cleanflight and Betaflight are distributed in the hope that they
+ * will be useful, but WITHOUT ANY WARRANTY; without even the implied 
+ * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this software.  
+ * 
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <stdbool.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <math.h>
+
+#include "build/debug.h"
+
+#include "common/maths.h"
+
+#include "fc/runtime_config.h"
+
+#include "flight/position.h"
+#include "flight/imu.h"
+#include "flight/pid.h"
+
+#include "io/gps.h"
+
+#include "sensors/sensors.h"
+#include "sensors/barometer.h"
+
+static int32_t estimatedAltitude = 0;                // in cm
+
+#define BARO_UPDATE_FREQUENCY_40HZ (1000 * 25)
+
+
+#if defined(USE_BARO) || defined(USE_GPS)
+void calculateEstimatedAltitude(timeUs_t currentTimeUs)
+{
+    static timeUs_t previousTimeUs = 0;
+    const uint32_t dTime = currentTimeUs - previousTimeUs;
+    if (dTime < BARO_UPDATE_FREQUENCY_40HZ) {
+        return;
+    }
+    previousTimeUs = currentTimeUs;
+
+    int32_t baroAlt = 0;
+    static int32_t baroAltOffset = 0;
+    static int32_t gpsAltOffset = 0;
+
+    int32_t gpsAlt = 0;
+    float gpsTrust = 0.3; //conservative default
+    bool haveBaroAlt = false;
+    bool haveGPSAlt = false;
+#ifdef USE_BARO
+    if (sensors(SENSOR_BARO)) {
+        if (!isBaroCalibrationComplete()) {
+            performBaroCalibrationCycle();
+        } else {
+            baroAlt = baroCalculateAltitude();
+            haveBaroAlt = true;
+        }
+    }
+#endif
+
+#ifdef USE_GPS
+if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) {
+    gpsAlt = gpsSol.llh.alt;
+    haveGPSAlt = true;
+
+    if (gpsSol.hdop != 0) {
+        gpsTrust = 100.0 / gpsSol.hdop;
+    }
+    // always use at least 10% of other sources besides gps if available
+    gpsTrust = MIN(gpsTrust, 0.9f);
+}
+#endif
+
+    if (!ARMING_FLAG(ARMED)) {
+        baroAltOffset = baroAlt;
+        gpsAltOffset = gpsAlt;
+    }
+    baroAlt -= baroAltOffset;
+    gpsAlt -= gpsAltOffset;
+    
+    if (haveGPSAlt && haveBaroAlt) {
+        estimatedAltitude = gpsAlt * gpsTrust + baroAlt * (1-gpsTrust);
+    } else if (haveGPSAlt && !haveBaroAlt) {
+        estimatedAltitude = gpsAlt;
+    }
+    
+    DEBUG_SET(DEBUG_ALTITUDE, 0, (int32_t)(100 * gpsTrust));
+    DEBUG_SET(DEBUG_ALTITUDE, 1, baroAlt);
+    DEBUG_SET(DEBUG_ALTITUDE, 2, gpsAlt);
+}
+#endif
+
+
+int32_t getEstimatedAltitude(void)
+{
+    return estimatedAltitude;
+}
+
+// This should be removed or fixed, but it would require changing a lot of other things to get rid of.
+int16_t getEstimatedVario(void)
+{
+    return 0;
+}
diff --git a/src/main/flight/altitude.h b/src/main/flight/position.h
similarity index 69%
rename from src/main/flight/altitude.h
rename to src/main/flight/position.h
index 0534f4c446..af7a3b3eca 100644
--- a/src/main/flight/altitude.h
+++ b/src/main/flight/position.h
@@ -22,18 +22,6 @@
 
 #include "common/time.h"
 
-extern int32_t AltHold;
-
-typedef struct airplaneConfig_s {
-    bool fixedwing_althold_reversed;           // false for negative pitch/althold gain. later check if need more than just sign
-} airplaneConfig_t;
-
-PG_DECLARE(airplaneConfig_t, airplaneConfig);
-
 void calculateEstimatedAltitude(timeUs_t currentTimeUs);
 int32_t getEstimatedAltitude(void);
-int32_t getEstimatedVario(void);
-
-void applyAltHold(void);
-void updateAltHoldState(void);
-void updateRangefinderAltHoldState(void);
+int16_t getEstimatedVario(void);
\ No newline at end of file
diff --git a/src/main/interface/cli.c b/src/main/interface/cli.c
index b2728bac89..4cea40eca4 100644
--- a/src/main/interface/cli.c
+++ b/src/main/interface/cli.c
@@ -91,7 +91,7 @@ extern uint8_t __config_end;
 #include "fc/rc_controls.h"
 #include "fc/runtime_config.h"
 
-#include "flight/altitude.h"
+#include "flight/position.h"
 #include "flight/failsafe.h"
 #include "flight/imu.h"
 #include "flight/mixer.h"
diff --git a/src/main/interface/msp.c b/src/main/interface/msp.c
index bcee60565c..4ebee9bbc9 100644
--- a/src/main/interface/msp.c
+++ b/src/main/interface/msp.c
@@ -69,7 +69,7 @@
 #include "fc/rc_modes.h"
 #include "fc/runtime_config.h"
 
-#include "flight/altitude.h"
+#include "flight/position.h"
 #include "flight/failsafe.h"
 #include "flight/imu.h"
 #include "flight/mixer.h"
@@ -1324,29 +1324,6 @@ static mspResult_e mspFcProcessOutCommandWithArg(uint8_t cmdMSP, sbuf_t *arg, sb
 }
 #endif // USE_OSD_SLAVE
 
-#ifdef USE_NAV
-static void mspFcWpCommand(sbuf_t *dst, sbuf_t *src)
-{
-    uint8_t wp_no;
-    int32_t lat = 0, lon = 0;
-    wp_no = sbufReadU8(src);    // get the wp number
-    if (wp_no == 0) {
-        lat = GPS_home[LAT];
-        lon = GPS_home[LON];
-    } else if (wp_no == 16) {
-        lat = GPS_hold[LAT];
-        lon = GPS_hold[LON];
-    }
-    sbufWriteU8(dst, wp_no);
-    sbufWriteU32(dst, lat);
-    sbufWriteU32(dst, lon);
-    sbufWriteU32(dst, AltHold);           // altitude (cm) will come here -- temporary implementation to test feature with apps
-    sbufWriteU16(dst, 0);                 // heading  will come here (deg)
-    sbufWriteU16(dst, 0);                 // time to stay (ms) will come here
-    sbufWriteU8(dst, 0);                  // nav flag will come here
-}
-#endif
-
 #ifdef USE_FLASHFS
 static void mspFcDataFlashReadCommand(sbuf_t *dst, sbuf_t *src)
 {
@@ -1399,10 +1376,6 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
     uint32_t i;
     uint8_t value;
     const unsigned int dataSize = sbufBytesRemaining(src);
-#ifdef USE_NAV
-    uint8_t wp_no;
-    int32_t lat = 0, lon = 0, alt = 0;
-#endif
     switch (cmdMSP) {
     case MSP_SELECT_SETTING:
         value = sbufReadU8(src);
@@ -1869,33 +1842,6 @@ static mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
         GPS_update |= 2;        // New data signalisation to GPS functions // FIXME Magic Numbers
         break;
 #endif // USE_GPS
-#ifdef USE_NAV
-    case MSP_SET_WP:
-        wp_no = sbufReadU8(src);    //get the wp number
-        lat = sbufReadU32(src);
-        lon = sbufReadU32(src);
-        alt = sbufReadU32(src);     // to set altitude (cm)
-        sbufReadU16(src);           // future: to set heading (deg)
-        sbufReadU16(src);           // future: to set time to stay (ms)
-        sbufReadU8(src);            // future: to set nav flag
-        if (wp_no == 0) {
-            GPS_home[LAT] = lat;
-            GPS_home[LON] = lon;
-            DISABLE_FLIGHT_MODE(GPS_HOME_MODE);        // with this flag, GPS_set_next_wp will be called in the next loop -- OK with SERIAL GPS / OK with I2C GPS
-            ENABLE_STATE(GPS_FIX_HOME);
-            if (alt != 0)
-                AltHold = alt;          // temporary implementation to test feature with apps
-        } else if (wp_no == 16) {       // OK with SERIAL GPS  --  NOK for I2C GPS / needs more code dev in order to inject GPS coord inside I2C GPS
-            GPS_hold[LAT] = lat;
-            GPS_hold[LON] = lon;
-            if (alt != 0)
-                AltHold = alt;          // temporary implementation to test feature with apps
-            nav_mode = NAV_MODE_WP;
-            GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]);
-        }
-        break;
-#endif // USE_NAV
-
     case MSP_SET_FEATURE_CONFIG:
         featureClearAll();
         featureSet(sbufReadU32(src)); // features bitmap
@@ -2307,11 +2253,6 @@ mspResult_e mspFcProcessCommand(mspPacket_t *cmd, mspPacket_t *reply, mspPostPro
         mspFc4waySerialCommand(dst, src, mspPostProcessFn);
         ret = MSP_RESULT_ACK;
 #endif
-#ifdef USE_NAV
-    } else if (cmdMSP == MSP_WP) {
-        mspFcWpCommand(dst, src);
-        ret = MSP_RESULT_ACK;
-#endif
 #ifdef USE_FLASHFS
     } else if (cmdMSP == MSP_DATAFLASH_READ) {
         mspFcDataFlashReadCommand(dst, src);
diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c
index 8f7d920818..154ee7cb86 100644
--- a/src/main/interface/settings.c
+++ b/src/main/interface/settings.c
@@ -45,7 +45,7 @@
 #include "fc/rc_adjustments.h"
 #include "fc/rc_controls.h"
 
-#include "flight/altitude.h"
+#include "flight/position.h"
 #include "flight/failsafe.h"
 #include "flight/imu.h"
 #include "flight/mixer.h"
@@ -691,17 +691,7 @@ const clivalue_t valueTable[] = {
     { "nav_speed_max",              VAR_UINT16 | MASTER_VALUE, .config.minmax = { 10, 2000 }, PG_NAVIGATION_CONFIG, offsetof(navigationConfig_t, nav_speed_max) },
     { "nav_slew_rate",              VAR_UINT8  | MASTER_VALUE, .config.minmax = { 0, 100 }, PG_NAVIGATION_CONFIG, offsetof(navigationConfig_t, nav_slew_rate) },
 #endif
-
-// PG_AIRPLANE_CONFIG
-#if defined(USE_ALT_HOLD)
-    { "fixedwing_althold_reversed", VAR_INT8   | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_AIRPLANE_CONFIG, offsetof(airplaneConfig_t, fixedwing_althold_reversed) },
-#endif
-
-// PG_RC_CONTROLS_CONFIG
-#if defined(USE_ALT_HOLD)
-    { "alt_hold_deadband",          VAR_UINT8  | MASTER_VALUE, .config.minmax = { 1, 250 }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, alt_hold_deadband) },
-    { "alt_hold_fast_change",       VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, alt_hold_fast_change) },
-#endif
+    
     { "deadband",                   VAR_UINT8  | MASTER_VALUE, .config.minmax = { 0, 32 }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, deadband) },
     { "yaw_deadband",               VAR_UINT8  | MASTER_VALUE, .config.minmax = { 0, 100 }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, yaw_deadband) },
     { "yaw_control_reversed",       VAR_INT8   | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_RC_CONTROLS_CONFIG, offsetof(rcControlsConfig_t, yaw_control_reversed) },
@@ -758,15 +748,6 @@ const clivalue_t valueTable[] = {
     { "i_yaw",                      VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_YAW].I) },
     { "d_yaw",                      VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_YAW].D) },
 
-#ifdef USE_ALT_HOLD
-    { "p_alt",                      VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_ALT].P) },
-    { "i_alt",                      VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_ALT].I) },
-    { "d_alt",                      VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_ALT].D) },
-    { "p_vel",                      VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_VEL].P) },
-    { "i_vel",                      VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_VEL].I) },
-    { "d_vel",                      VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_VEL].D) },
-#endif
-
     { "p_level",                    VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].P) },
     { "i_level",                    VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].I) },
     { "d_level",                    VAR_UINT8  | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_LEVEL].D) },
diff --git a/src/main/io/gps.c b/src/main/io/gps.c
index c06e399038..667c46a71d 100644
--- a/src/main/io/gps.c
+++ b/src/main/io/gps.c
@@ -636,6 +636,7 @@ typedef struct gpsDataNmea_s {
     uint8_t numSat;
     uint16_t altitude;
     uint16_t speed;
+    uint16_t hdop;
     uint16_t ground_course;
     uint32_t time;
     uint32_t date;
@@ -700,6 +701,9 @@ static bool gpsNewFrameNMEA(char c)
                         case 7:
                             gps_Msg.numSat = grab_fields(string, 0);
                             break;
+                        case 8:
+                            gps_Msg.hdop = grab_fields(string, 1) * 100;          // hdop
+                            break;
                         case 9:
                             gps_Msg.altitude = grab_fields(string, 0);     // altitude in meters added by Mis
                             break;
@@ -793,6 +797,7 @@ static bool gpsNewFrameNMEA(char c)
                             gpsSol.llh.lon = gps_Msg.longitude;
                             gpsSol.numSat = gps_Msg.numSat;
                             gpsSol.llh.alt = gps_Msg.altitude;
+                            gpsSol.hdop = gps_Msg.hdop;
                         }
                         break;
                     case FRAME_RMC:
@@ -1015,7 +1020,7 @@ static bool UBLOX_parse_gps(void)
         //i2c_dataset.time                = _buffer.posllh.time;
         gpsSol.llh.lon = _buffer.posllh.longitude;
         gpsSol.llh.lat = _buffer.posllh.latitude;
-        gpsSol.llh.alt = _buffer.posllh.altitude_msl / 10 / 100;  //alt in m
+        gpsSol.llh.alt = _buffer.posllh.altitude_msl / 10;  //alt in cm
         if (next_fix) {
             ENABLE_STATE(GPS_FIX);
         } else {
diff --git a/src/main/io/gps.h b/src/main/io/gps.h
index 4b8cf045fb..f7c8a226b5 100644
--- a/src/main/io/gps.h
+++ b/src/main/io/gps.h
@@ -85,7 +85,7 @@ typedef struct gpsCoordinateDDDMMmmmm_s {
 typedef struct gpsLocation_s {
     int32_t lat;                    // latitude * 1e+7
     int32_t lon;                    // longitude * 1e+7
-    uint16_t alt;                   // altitude in 0.1m
+    int32_t alt;                   // altitude in 0.1m
 } gpsLocation_t;
 
 typedef struct gpsSolutionData_s {
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index bed34cc9f3..c007a80d73 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -65,7 +65,7 @@
 #include "fc/rc_controls.h"
 #include "fc/runtime_config.h"
 
-#include "flight/altitude.h"
+#include "flight/position.h"
 #include "flight/imu.h"
 #ifdef USE_ESC_SENSOR
 #include "flight/mixer.h"
diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h
index 274192b3a0..e55cfae51a 100644
--- a/src/main/scheduler/scheduler.h
+++ b/src/main/scheduler/scheduler.h
@@ -82,7 +82,7 @@ typedef enum {
 #ifdef USE_RANGEFINDER
     TASK_RANGEFINDER,
 #endif
-#if defined(USE_BARO) || defined(USE_RANGEFINDER)
+#if defined(USE_BARO) || defined(USE_GPS)
     TASK_ALTITUDE,
 #endif
 #ifdef USE_DASHBOARD
diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c
index ca6540b869..8d9d4a331e 100644
--- a/src/main/target/COLIBRI_RACE/i2c_bst.c
+++ b/src/main/target/COLIBRI_RACE/i2c_bst.c
@@ -57,7 +57,7 @@
 
 #include "telemetry/telemetry.h"
 
-#include "flight/altitude.h"
+#include "flight/position.h"
 #include "flight/failsafe.h"
 #include "flight/imu.h"
 #include "flight/mixer.h"
diff --git a/src/main/target/common_fc_post.h b/src/main/target/common_fc_post.h
index 4b35a610d1..3c550aa1f8 100644
--- a/src/main/target/common_fc_post.h
+++ b/src/main/target/common_fc_post.h
@@ -74,11 +74,6 @@
 #undef USE_SPEKTRUM_CMS_TELEMETRY
 #endif
 
-// undefine USE_ALT_HOLD if there is no baro or rangefinder to support it
-#if defined(USE_ALT_HOLD) && !defined(USE_BARO) && !defined(USE_RANGEFINDER)
-#undef USE_ALT_HOLD
-#endif
-
 /* If either VTX_CONTROL or VTX_COMMON is undefined then remove common code and device drivers */
 #if !defined(USE_VTX_COMMON) || !defined(USE_VTX_CONTROL)
 #undef USE_VTX_COMMON
diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h
index 432c1752af..6e8c8694bd 100644
--- a/src/main/target/common_fc_pre.h
+++ b/src/main/target/common_fc_pre.h
@@ -198,7 +198,6 @@
 #endif
 
 #if (FLASH_SIZE > 256)
-#define USE_ALT_HOLD
 #define USE_DASHBOARD
 #define USE_GPS
 #define USE_GPS_NMEA
diff --git a/src/main/telemetry/frsky_hub.c b/src/main/telemetry/frsky_hub.c
index 04cd3c2190..79c8f8f3a5 100644
--- a/src/main/telemetry/frsky_hub.c
+++ b/src/main/telemetry/frsky_hub.c
@@ -60,7 +60,7 @@
 #include "flight/mixer.h"
 #include "flight/pid.h"
 #include "flight/imu.h"
-#include "flight/altitude.h"
+#include "flight/position.h"
 
 #include "rx/rx.h"
 
diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c
index 4e4516a190..f756d3462b 100644
--- a/src/main/telemetry/hott.c
+++ b/src/main/telemetry/hott.c
@@ -75,7 +75,7 @@
 
 #include "fc/runtime_config.h"
 
-#include "flight/altitude.h"
+#include "flight/position.h"
 #include "flight/pid.h"
 
 #include "io/gps.h"
diff --git a/src/main/telemetry/ibus_shared.c b/src/main/telemetry/ibus_shared.c
index 8ccb3461ea..420bc2384c 100644
--- a/src/main/telemetry/ibus_shared.c
+++ b/src/main/telemetry/ibus_shared.c
@@ -54,7 +54,7 @@ static uint16_t calculateChecksum(const uint8_t *ibusPacket);
 #include "sensors/sensors.h"
 #include "sensors/barometer.h"
 #include "flight/imu.h"
-#include "flight/altitude.h"
+#include "flight/position.h"
 #include "flight/navigation.h"
 #include "io/gps.h"
 
diff --git a/src/main/telemetry/jetiexbus.c b/src/main/telemetry/jetiexbus.c
index 0924154fed..731fe6fe19 100644
--- a/src/main/telemetry/jetiexbus.c
+++ b/src/main/telemetry/jetiexbus.c
@@ -38,7 +38,7 @@
 #include "drivers/serial_uart.h"
 #include "drivers/time.h"
 
-#include "flight/altitude.h"
+#include "flight/position.h"
 #include "flight/imu.h"
 
 #include "io/serial.h"
diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c
index c9f193689a..ee4920ebc0 100644
--- a/src/main/telemetry/ltm.c
+++ b/src/main/telemetry/ltm.c
@@ -72,7 +72,7 @@
 #include "flight/pid.h"
 #include "flight/imu.h"
 #include "flight/failsafe.h"
-#include "flight/altitude.h"
+#include "flight/position.h"
 
 #include "telemetry/telemetry.h"
 #include "telemetry/ltm.h"
diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c
index 84f8cf2bc5..b07ff2a4a0 100644
--- a/src/main/telemetry/mavlink.c
+++ b/src/main/telemetry/mavlink.c
@@ -52,7 +52,7 @@
 #include "flight/imu.h"
 #include "flight/failsafe.h"
 #include "flight/navigation.h"
-#include "flight/altitude.h"
+#include "flight/position.h"
 
 #include "io/serial.h"
 #include "io/gimbal.h"
diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c
index f8b286a9d6..690cd4bea2 100644
--- a/src/main/telemetry/smartport.c
+++ b/src/main/telemetry/smartport.c
@@ -51,7 +51,7 @@
 #include "fc/rc_controls.h"
 #include "fc/runtime_config.h"
 
-#include "flight/altitude.h"
+#include "flight/position.h"
 #include "flight/failsafe.h"
 #include "flight/imu.h"
 #include "flight/mixer.h"
diff --git a/src/test/Makefile b/src/test/Makefile
index c132f170d5..a1c34c0188 100644
--- a/src/test/Makefile
+++ b/src/test/Makefile
@@ -26,11 +26,6 @@ alignsensor_unittest_SRC := \
 		$(USER_DIR)/sensors/boardalignment.c \
 		$(USER_DIR)/common/maths.c
 
-
-altitude_hold_unittest_SRC :=  \
-		$(USER_DIR)/flight/altitude.c
-
-
 arming_prevention_unittest_SRC := \
 		$(USER_DIR)/fc/fc_core.c \
 		$(USER_DIR)/fc/rc_controls.c \
@@ -120,7 +115,7 @@ flight_imu_unittest_SRC := \
 		$(USER_DIR)/common/maths.c \
 		$(USER_DIR)/config/feature.c \
 		$(USER_DIR)/fc/rc_modes.c \
-		$(USER_DIR)/flight/altitude.c \
+		$(USER_DIR)/flight/position.c \
 		$(USER_DIR)/flight/imu.c
 
 
diff --git a/src/test/unit/altitude_hold_unittest.cc.txt b/src/test/unit/altitude_hold_unittest.cc.txt
index 089d5c7a78..6bfd6c7388 100644
--- a/src/test/unit/altitude_hold_unittest.cc.txt
+++ b/src/test/unit/altitude_hold_unittest.cc.txt
@@ -45,7 +45,7 @@ extern "C" {
     #include "flight/mixer.h"
     #include "flight/pid.h"
     #include "flight/imu.h"
-    #include "flight/altitude.h"
+    #include "flight/position.h"
 
     #include "config/runtime_config.h"