diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c
index 9221c3d7fb..176fa6b225 100644
--- a/src/main/blackbox/blackbox.c
+++ b/src/main/blackbox/blackbox.c
@@ -1448,11 +1448,12 @@ static bool blackboxWriteSysinfo(void)
 #endif
 #ifdef USE_BARO
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_BARO_HARDWARE, "%d",           barometerConfig()->baro_hardware);
-        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_BARO_NOISE_LPF, "%d",          barometerConfig()->baro_noise_lpf);
-        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_BARO_VARIO_LPF, "%d",          barometerConfig()->baro_vario_lpf);
 #endif
-        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_ALT_SOURCE, "%d",      positionConfig()->altSource);
-        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_ALT_PREFER_BARO, "%d", positionConfig()->altPreferBaro);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_ALTITUDE_SOURCE, "%d",      positionConfig()->altitude_source);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_ALTITUDE_PREFER_BARO, "%d", positionConfig()->altitude_prefer_baro);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_ALTITUDE_LPF, "%d",         positionConfig()->altitude_lpf);
+        BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_POSITION_ALTITUDE_D_LPF, "%d",       positionConfig()->altitude_d_lpf);
+
 #ifdef USE_MAG
         BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_MAG_HARDWARE, "%d",           compassConfig()->mag_hardware);
 #endif
diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c
index 3f2de5f154..95430d39df 100644
--- a/src/main/cli/settings.c
+++ b/src/main/cli/settings.c
@@ -467,7 +467,7 @@ static const char * const lookupTableGyroFilterDebug[] = {
     "ROLL", "PITCH", "YAW"
 };
 
-static const char * const lookupTablePositionAltSource[] = {
+static const char * const lookupTablePositionAltitudeSource[] = {
     "DEFAULT", "BARO_ONLY", "GPS_ONLY"
 };
 
@@ -630,7 +630,7 @@ const lookupTableEntry_t lookupTables[] = {
 
     LOOKUP_TABLE_ENTRY(lookupTableGyroFilterDebug),
 
-    LOOKUP_TABLE_ENTRY(lookupTablePositionAltSource),
+    LOOKUP_TABLE_ENTRY(lookupTablePositionAltitudeSource),
     LOOKUP_TABLE_ENTRY(lookupTableOffOnAuto),
     LOOKUP_TABLE_ENTRY(lookupTableFeedforwardAveraging),
     LOOKUP_TABLE_ENTRY(lookupTableDshotBitbangedTimer),
@@ -733,8 +733,6 @@ const clivalue_t valueTable[] = {
     { "baro_i2c_device",            VAR_UINT8  | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, 5 }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_i2c_device) },
     { "baro_i2c_address",           VAR_UINT8  | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, I2C_ADDR7_MAX }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_i2c_address) },
     { PARAM_NAME_BARO_HARDWARE,     VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BARO_HARDWARE }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_hardware) },
-    { "baro_noise_lpf",             VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 10, 1000 }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_noise_lpf) },
-    { "baro_vario_lpf",             VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 10, 1000 }, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_vario_lpf) },
 #endif
 
 // PG_RX_CONFIG
@@ -1669,8 +1667,11 @@ const clivalue_t valueTable[] = {
 #endif
 
 // PG_POSITION
-    { "position_alt_source",                    VAR_INT8   | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_POSITION_ALT_SOURCE }, PG_POSITION, offsetof(positionConfig_t, altSource) },
-    { "position_alt_prefer_baro",               VAR_INT8   | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_POSITION, offsetof(positionConfig_t, altPreferBaro) },
+    { "altitude_source",       VAR_INT8   | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_POSITION_ALT_SOURCE }, PG_POSITION, offsetof(positionConfig_t, altitude_source) },
+    { "altitude_prefer_baro",  VAR_INT8   | MASTER_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_POSITION, offsetof(positionConfig_t, altitude_prefer_baro) },
+    { "altitude_lpf",          VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 10, 1000 }, PG_POSITION, offsetof(positionConfig_t, altitude_lpf) },
+    { "altitude_d_lpf",        VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 10, 1000 }, PG_POSITION, offsetof(positionConfig_t, altitude_d_lpf) },
+
 // PG_MODE_ACTIVATION_CONFIG
 #if defined(USE_CUSTOM_BOX_NAMES)
     { "box_user_1_name", VAR_UINT8 | HARDWARE_VALUE | MODE_STRING, .config.string = { 1, MAX_BOX_USER_NAME_LENGTH, STRING_FLAGS_NONE }, PG_MODE_ACTIVATION_CONFIG, offsetof(modeActivationConfig_t, box_user_1_name) },
diff --git a/src/main/fc/parameter_names.h b/src/main/fc/parameter_names.h
index 9249f4ca16..d8818627f9 100644
--- a/src/main/fc/parameter_names.h
+++ b/src/main/fc/parameter_names.h
@@ -34,8 +34,6 @@
 #define PARAM_NAME_ACC_LPF_HZ "acc_lpf_hz"
 #define PARAM_NAME_MAG_HARDWARE "mag_hardware"
 #define PARAM_NAME_BARO_HARDWARE "baro_hardware"
-#define PARAM_NAME_BARO_NOISE_LPF "baro_noise_lpf"
-#define PARAM_NAME_BARO_VARIO_LPF "baro_vario_lpf"
 #define PARAM_NAME_RC_SMOOTHING "rc_smoothing"
 #define PARAM_NAME_RC_SMOOTHING_AUTO_FACTOR "rc_smoothing_auto_factor"
 #define PARAM_NAME_RC_SMOOTHING_AUTO_FACTOR_THROTTLE "rc_smoothing_auto_factor_throttle"
@@ -119,8 +117,10 @@
 #define PARAM_NAME_RPM_FILTER_MIN_HZ "rpm_filter_min_hz"
 #define PARAM_NAME_RPM_FILTER_FADE_RANGE_HZ "rpm_filter_fade_range_hz"
 #define PARAM_NAME_RPM_FILTER_LPF_HZ "rpm_filter_lpf_hz"
-#define PARAM_NAME_POSITION_ALT_SOURCE "position_alt_source"
-#define PARAM_NAME_POSITION_ALT_PREFER_BARO "position_alt_prefer_baro"
+#define PARAM_NAME_POSITION_ALTITUDE_SOURCE "altitude_source"
+#define PARAM_NAME_POSITION_ALTITUDE_PREFER_BARO "altitude_prefer_baro"
+#define PARAM_NAME_POSITION_ALTITUDE_LPF "altitude_lpf"
+#define PARAM_NAME_POSITION_ALTITUDE_D_LPF "altitude_d_lpf"
 
 #ifdef USE_GPS
 #define PARAM_NAME_GPS_PROVIDER "gps_provider"
diff --git a/src/main/flight/gps_rescue.c b/src/main/flight/gps_rescue.c
index ff0f91127e..9b43f09d40 100644
--- a/src/main/flight/gps_rescue.c
+++ b/src/main/flight/gps_rescue.c
@@ -49,6 +49,7 @@
 #include "rx/rx.h"
 
 #include "sensors/acceleration.h"
+#include "sensors/barometer.h" // remove this later, only here temporarily for the cutoff for D
 
 #include "gps_rescue.h"
 
@@ -91,11 +92,12 @@ typedef struct {
     bool updateYaw;
     float descentDistanceM;
     int8_t secondsFailing;
+    float altitudeStep;
 } rescueIntent_s;
 
 typedef struct {
     int32_t maxAltitudeCm;
-    int32_t currentAltitudeCm;
+    float currentAltitudeCm;
     float distanceToHomeCm;
     float distanceToHomeM;
     uint16_t groundSpeedCmS;  //cm/s
@@ -104,9 +106,9 @@ typedef struct {
     bool healthy;
     float errorAngle;
     float gpsDataIntervalSeconds;
+    float altitudeDataIntervalSeconds;
     float velocityToHomeCmS;
-    float ascendStepCm;
-    float descendStepCm;
+    float alitutudeStepCm;
     float maxPitchStep;
     float filterK;
     float absErrorAngle;
@@ -141,16 +143,16 @@ typedef enum {
 PG_REGISTER_WITH_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 2);
 
 PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig,
-    .angle = 32,
+    .angle = 40,
     .initialAltitudeM = 30,
     .descentDistanceM = 20,
     .rescueGroundspeed = 500,
-    .throttleP = 20,
-    .throttleI = 20,
-    .throttleD = 10,
+    .throttleP = 15,
+    .throttleI = 15,
+    .throttleD = 15,
     .velP = 6,
     .velI = 20,
-    .velD = 70,
+    .velD = 50,
     .yawP = 25,
     .throttleMin = 1100,
     .throttleMax = 1600,
@@ -162,7 +164,7 @@ PG_RESET_TEMPLATE(gpsRescueConfig_t, gpsRescueConfig,
     .targetLandingAltitudeM = 5,
     .altitudeMode = MAX_ALT,
     .ascendRate = 500,          // cm/s, for altitude corrections on ascent
-    .descendRate = 125,         // cm/s, for descent and landing phase, or negative ascent
+    .descendRate = 100,         // cm/s, minimum for descent and landing phase, or for descending if starting high ascent
     .rescueAltitudeBufferM = 10,
     .rollMix = 100
 );
@@ -172,9 +174,20 @@ static float rescueYaw;
 float       gpsRescueAngle[ANGLE_INDEX_COUNT] = { 0, 0 };
 bool        magForceDisable = false;
 static bool newGPSData = false;
+static pt2Filter_t throttleDLpf;
 
 rescueState_s rescueState;
 
+void gpsRescueInit(void)
+{
+    const float sampleTimeS = HZ_TO_INTERVAL(TASK_ALTITUDE_RATE_HZ);
+
+    const float throttleDCutoffHz = positionConfig()->altitude_d_lpf / 100.0f;
+    const float throttleDCutoffGain = pt2FilterGain(throttleDCutoffHz, sampleTimeS);
+    pt2FilterInit(&throttleDLpf, throttleDCutoffGain);
+}
+
+
 /*
  If we have new GPS data, update home heading if possible and applicable.
 */
@@ -239,11 +252,8 @@ static void rescueAttainPosition()
     static float velocityI = 0.0f;
     static float previousVelocityD = 0.0f;      // for smoothing
     static float previousPitchAdjustment = 0.0f;
-    static float previousAltitudeError = 0.0f;
     static float throttleI = 0.0f;
-    static float previousThrottleD = 0.0f;      // for jerk calc from raw Derivative
-    static float previousThrottleDVal = 0.0f;   // for moving average of D and jerk
-    static float previousThrottleD2 = 0.0f;     // for additional D first order smoothing
+    static float previousAltitudeError = 0.0f;
     static int16_t throttleAdjustment = 0;
 
     switch (rescueState.phase) {
@@ -260,22 +270,60 @@ static void rescueAttainPosition()
         velocityI = 0.0f;
         previousVelocityD = 0.0f;
         previousPitchAdjustment = 0.0f;
-        previousAltitudeError = 0.0f;
         throttleI = 0.0f;
-        previousThrottleD = 0.0f;
-        previousThrottleDVal = 0.0f;
-        previousThrottleD2 = 0.0f;
+        previousAltitudeError = 0.0f;
         throttleAdjustment = 0;
         return;
     case RESCUE_DO_NOTHING:
         gpsRescueAngle[AI_PITCH] = 0.0f;
         gpsRescueAngle[AI_ROLL] = 0.0f;
-        rescueThrottle = gpsRescueConfig()->throttleHover;
+        rescueThrottle = gpsRescueConfig()->throttleHover - 50;
         return;
     default:
         break;
     }
 
+    /**
+        Altitude (throttle) controller
+    */
+    // currentAltitudeCm is updated at TASK_GPS_RATE since GPS initiates updateGPSRescueState()
+    const float altitudeError = (rescueState.intent.targetAltitudeCm - rescueState.sensor.currentAltitudeCm) * 0.01f;
+    // height above target in metres (negative means too low)
+    // at the start, the target starts at current altitude plus one step.  Increases stepwise to intended value.
+
+    // P component
+    const float throttleP = gpsRescueConfig()->throttleP * altitudeError;
+
+    // I component
+    throttleI += 0.1f * gpsRescueConfig()->throttleI * altitudeError * rescueState.sensor.altitudeDataIntervalSeconds;
+    throttleI = constrainf(throttleI, -1.0f * GPS_RESCUE_MAX_ITERM_THROTTLE, 1.0f * GPS_RESCUE_MAX_ITERM_THROTTLE);
+    // up to 20% increase in throttle from I alone
+
+    // D component from position.c
+//    float throttleD = rescueState.sensor.currentAltitudeDerivative;
+
+// is error based, so includes positive boost when climbing and negative boost on descent
+    const float throttleDRaw = ((altitudeError - previousAltitudeError) / rescueState.sensor.altitudeDataIntervalSeconds);
+    previousAltitudeError = altitudeError;
+    float throttleD = pt2FilterApply(&throttleDLpf, throttleDRaw);
+    throttleD = gpsRescueConfig()->throttleD * throttleD;
+
+    // acceleration component not implemented - was needed previously due to GPS lag, maybe not needed now.
+
+    float tiltAdjustment = 1.0f - getCosTiltAngle(); // 0 = flat, gets to 0.2 correcting on a windy day
+    tiltAdjustment *= (gpsRescueConfig()->throttleHover - 1000);
+    // if hover is 1300, and adjustment .2, this gives us 0.2*300 or 60 of extra throttle, not much, but useful
+    // too much and landings with lots of pitch adjustment, eg windy days, can be a problem
+
+    throttleAdjustment = throttleP + throttleI + throttleD + tiltAdjustment;
+
+    rescueThrottle = gpsRescueConfig()->throttleHover + throttleAdjustment;
+    rescueThrottle = constrainf(rescueThrottle, gpsRescueConfig()->throttleMin, gpsRescueConfig()->throttleMax);
+    DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 0, throttleP);
+    DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 1, throttleD);
+
+
+    // *** other updates occur only with new GPS data ***
     if (!newGPSData) {
         return;
     }
@@ -372,56 +420,6 @@ static void rescueAttainPosition()
     DEBUG_SET(DEBUG_GPS_RESCUE_VELOCITY, 1, velocityD);
     DEBUG_SET(DEBUG_GPS_RESCUE_VELOCITY, 3, rescueState.intent.targetVelocityCmS);
     DEBUG_SET(DEBUG_GPS_RESCUE_TRACKING, 1, rescueState.intent.targetVelocityCmS);
-
-    /**
-        Altitude (throttle) controller
-    */
-    // note that currentAltitudeCm can be updated more frequently than GPS rate from Baro, but this code requires GPS data to update
-    // ToDo: use a delta time for changes in currentAltitudeCm, and run more frequently than GPS rate
-    const float altitudeError = (rescueState.intent.targetAltitudeCm - rescueState.sensor.currentAltitudeCm) * 0.01f;
-    // height above target in metres (negative means too low)
-    // at the start, the target starts at current altitude plus one step.  Increases stepwise to intended value.
-
-    // P component
-    const float throttleP = gpsRescueConfig()->throttleP * altitudeError;
-
-    // I component
-    throttleI += 0.01f * gpsRescueConfig()->throttleI * altitudeError * sampleIntervalNormaliseFactor;
-    throttleI = constrainf(throttleI, -1.0f * GPS_RESCUE_MAX_ITERM_THROTTLE, 1.0f * GPS_RESCUE_MAX_ITERM_THROTTLE);
-    // up to 20% increase in throttle from I alone
-
-    // D component
-    // is error based, so includes positive boost when climbing and negative boost on descent
-    float throttleD = ((altitudeError - previousAltitudeError) / sampleIntervalNormaliseFactor);
-    previousAltitudeError = altitudeError;
-
-    // Acceleration (Jerk) component
-    const float throttleDJerk = 2.0f * (throttleD - previousThrottleD);
-    previousThrottleD = throttleD;
-    throttleD += throttleDJerk;
-
-    // D Smoothing
-    const float movingAvgAltitudeD = 0.5f * (previousThrottleDVal + throttleD);
-     // moving average seems to work best here, a lot of sequential up and down in altitude data
-    previousThrottleDVal = throttleD;
-    throttleD = movingAvgAltitudeD;
-    throttleD = previousThrottleD2 + rescueState.sensor.filterK * (throttleD - previousThrottleD2);
-    // additional final first order D throttle smoothing
-    previousThrottleD2 = throttleD;
-
-    throttleD = 10.0f * gpsRescueConfig()->throttleD * throttleD;
-
-    float tiltAdjustment = 1.0f - getCosTiltAngle(); // 0 = flat, gets to 0.2 correcting on a windy day
-    tiltAdjustment *= (gpsRescueConfig()->throttleHover - 1000);
-    // if hover is 1300, and adjustment .2, this gives us 0.2*300 or 60 of extra throttle, not much, but useful
-    // too much and landings with lots of pitch adjustment, eg windy days, can be a problem
-
-    throttleAdjustment = throttleP + throttleI + throttleD + tiltAdjustment;
-
-    rescueThrottle = gpsRescueConfig()->throttleHover + throttleAdjustment;
-    rescueThrottle = constrainf(rescueThrottle, gpsRescueConfig()->throttleMin, gpsRescueConfig()->throttleMax);
-    DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 0, throttleP);
-    DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 1, throttleD);
 }
 
 static void performSanityChecks()
@@ -456,9 +454,11 @@ static void performSanityChecks()
         }
     }
 
-    // Check if crash recovery mode is active
+    // If crash recovery is triggered during a rescue, immediately disarm, ignoring the crash flip recovery settings.
     if (crashRecoveryModeActive()) {
-        rescueState.failure = RESCUE_CRASH_FLIP_DETECTED;
+        setArmingDisabled(ARMING_DISABLED_ARM_SWITCH);
+        disarm(DISARM_REASON_CRASH_PROTECTION);
+        rescueStop();
     }
 
     // Check if GPS comms are healthy
@@ -533,12 +533,17 @@ static void performSanityChecks()
 
 static void sensorUpdate()
 {
-    static timeUs_t previousDataTimeUs = 0;
     static float prevDistanceToHomeCm = 0.0f;
+    const timeUs_t currentTimeUs = micros();
+
+    static timeUs_t previousAltitudeDataTimeUs = 0;
+    const timeDelta_t altitudeDataIntervalUs = cmpTimeUs(currentTimeUs, previousAltitudeDataTimeUs);
+    rescueState.sensor.altitudeDataIntervalSeconds = altitudeDataIntervalUs * 0.000001f;
+    previousAltitudeDataTimeUs = currentTimeUs;
+
+    rescueState.sensor.currentAltitudeCm = getAltitude();
 
-    rescueState.sensor.currentAltitudeCm = getEstimatedAltitudeCm();
     DEBUG_SET(DEBUG_GPS_RESCUE_TRACKING, 2, rescueState.sensor.currentAltitudeCm);
-    DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 2, rescueState.sensor.currentAltitudeCm);
     // may be updated more frequently than GPS data
 
     rescueState.sensor.healthy = gpsIsHealthy();
@@ -565,11 +570,11 @@ static void sensorUpdate()
     }
     rescueState.sensor.absErrorAngle = fabsf(rescueState.sensor.errorAngle);
 
-    const timeUs_t currentTimeUs = micros();
-    const timeDelta_t gpsDataIntervalUs = cmpTimeUs(currentTimeUs, previousDataTimeUs);
+    static timeUs_t previousGPSDataTimeUs = 0;
+    const timeDelta_t gpsDataIntervalUs = cmpTimeUs(currentTimeUs, previousGPSDataTimeUs);
     rescueState.sensor.gpsDataIntervalSeconds = constrainf(gpsDataIntervalUs * 0.000001f, 0.01f, 1.0f);
     // Range from 10ms (100hz) to 1000ms (1Hz). Intended to cover common GPS data rates and exclude unusual values.
-    previousDataTimeUs = currentTimeUs;
+    previousGPSDataTimeUs = currentTimeUs;
 
     rescueState.sensor.filterK = pt1FilterGain(0.8, rescueState.sensor.gpsDataIntervalSeconds);
     // 0.8341 for 1hz, 0.5013 for 5hz, 0.3345 for 10hz, 0.1674 for 25Hz, etc
@@ -578,8 +583,6 @@ static void sensorUpdate()
     // positive = towards home.  First value is useless since prevDistanceToHomeCm was zero.
     prevDistanceToHomeCm = rescueState.sensor.distanceToHomeCm;
 
-    rescueState.sensor.ascendStepCm = rescueState.sensor.gpsDataIntervalSeconds * gpsRescueConfig()->ascendRate;
-    rescueState.sensor.descendStepCm = rescueState.sensor.gpsDataIntervalSeconds * gpsRescueConfig()->descendRate;
     rescueState.sensor.maxPitchStep = rescueState.sensor.gpsDataIntervalSeconds * GPS_RESCUE_MAX_PITCH_RATE;
 
     DEBUG_SET(DEBUG_GPS_RESCUE_HEADING, 2, attitude.values.yaw);  // degrees * 10
@@ -638,6 +641,22 @@ static bool checkGPSRescueIsAvailable(void)
     return result;
 }
 
+void disarmOnImpact(void)
+{
+    if (rescueState.sensor.accMagnitude > 2.0f) {
+        setArmingDisabled(ARMING_DISABLED_ARM_SWITCH);
+        disarm(DISARM_REASON_GPS_RESCUE);
+        rescueStop();
+    }
+}
+
+void altitudeAchieved(void)
+{
+    rescueState.intent.targetAltitudeCm = rescueState.intent.returnAltitudeCm;
+    rescueState.intent.altitudeStep = 0;
+    rescueState.phase = RESCUE_ROTATE;
+}
+
 void updateGPSRescueState(void)
 // this runs a lot faster than the GPS Data update rate, and runs whether or not rescue is active
 {
@@ -651,12 +670,15 @@ void updateGPSRescueState(void)
 
     // Will now be in RESCUE_INITIALIZE mode, if just entered Rescue while IDLE, otherwise stays IDLE
 
-    sensorUpdate(); // always get latest GPS / Altitude data
+    sensorUpdate(); // always get latest GPS / Altitude data; update ascend and descend rates
 
     uint8_t halfAngle = gpsRescueConfig()->angle / 2;
+    const int32_t targetLandingAltitudeCm = 100.0f * gpsRescueConfig()->targetLandingAltitudeM;
+    float proximityToLandingArea = 0.0f;
     bool startedLow = true;
     rescueState.isAvailable = checkGPSRescueIsAvailable();
 
+
     switch (rescueState.phase) {
     case RESCUE_IDLE:
         // in Idle phase = NOT in GPS Rescue
@@ -677,7 +699,8 @@ void updateGPSRescueState(void)
             // alternative is to prevent the rescue by returning to IDLE, but this could cause flyaways
         } else if (rescueState.sensor.distanceToHomeM < gpsRescueConfig()->minRescueDth) {
             // Attempt to initiate inside minimum activation distance -> landing mode
-            rescueState.intent.targetAltitudeCm = rescueState.sensor.currentAltitudeCm - rescueState.sensor.descendStepCm;
+            rescueState.intent.altitudeStep = -rescueState.sensor.altitudeDataIntervalSeconds * gpsRescueConfig()->descendRate;
+            rescueState.intent.targetAltitudeCm = rescueState.sensor.currentAltitudeCm + rescueState.intent.altitudeStep;
             rescueState.phase = RESCUE_LANDING;
             // start landing from current altitude
         } else {
@@ -688,6 +711,7 @@ void updateGPSRescueState(void)
             rescueState.intent.targetVelocityCmS = 0; // zero forward velocity while climbing
             rescueState.intent.pitchAngleLimitDeg = halfAngle; // only half pitch authority
             rescueState.intent.rollAngleLimitDeg = 0; // don't roll yet
+            rescueState.intent.altitudeStep = 0;
         }
         break;
 
@@ -696,21 +720,19 @@ void updateGPSRescueState(void)
         // also require that the final target altitude has been achieved before moving on
         // sanity check will abort if altitude gain is blocked for a cumulative period
         // TO DO: if overshoots are a problem after craft achieves target altitude changes, adjust termination threshold with current vertical velocity 
-        if (newGPSData) {
-            if (startedLow) {
-                if (rescueState.intent.targetAltitudeCm < rescueState.intent.returnAltitudeCm) {
-                    rescueState.intent.targetAltitudeCm += rescueState.sensor.ascendStepCm;
-                } else if (rescueState.sensor.currentAltitudeCm > rescueState.intent.returnAltitudeCm) {
-                    rescueState.intent.targetAltitudeCm = rescueState.intent.returnAltitudeCm;
-                    rescueState.phase = RESCUE_ROTATE;
-                } 
-            } else {
-                if (rescueState.intent.targetAltitudeCm > rescueState.intent.returnAltitudeCm) {
-                    rescueState.intent.targetAltitudeCm -= rescueState.sensor.descendStepCm;
-                } else if (rescueState.sensor.currentAltitudeCm < rescueState.intent.returnAltitudeCm) {
-                    rescueState.intent.targetAltitudeCm = rescueState.intent.returnAltitudeCm;
-                    rescueState.phase = RESCUE_ROTATE;
-                }
+        if (startedLow) {
+            if (rescueState.intent.targetAltitudeCm < rescueState.intent.returnAltitudeCm) {
+                rescueState.intent.altitudeStep = rescueState.sensor.altitudeDataIntervalSeconds * gpsRescueConfig()->ascendRate;
+                rescueState.intent.targetAltitudeCm += rescueState.intent.altitudeStep;
+            } else if (rescueState.sensor.currentAltitudeCm > rescueState.intent.returnAltitudeCm) {
+                altitudeAchieved();
+            } 
+        } else {
+            if (rescueState.intent.targetAltitudeCm > rescueState.intent.returnAltitudeCm) {
+                rescueState.intent.altitudeStep = -rescueState.sensor.altitudeDataIntervalSeconds * gpsRescueConfig()->descendRate;
+                rescueState.intent.targetAltitudeCm += rescueState.intent.altitudeStep;
+            } else if (rescueState.sensor.currentAltitudeCm < rescueState.intent.returnAltitudeCm) {
+                altitudeAchieved();
             }
         }
         break;
@@ -739,8 +761,9 @@ void updateGPSRescueState(void)
         // fly home with full control on all axes, pitching forward to gain speed
         if (newGPSData) {
             if (rescueState.sensor.distanceToHomeM <= rescueState.intent.descentDistanceM) {
-            rescueState.phase = RESCUE_DESCENT;
-            rescueState.intent.secondsFailing = 0; // reset sanity timer for descent
+                rescueState.phase = RESCUE_DESCENT;
+                rescueState.intent.altitudeStep = -1.0f * rescueState.sensor.altitudeDataIntervalSeconds * gpsRescueConfig()->descendRate; // negative step dsescending
+                rescueState.intent.secondsFailing = 0; // reset sanity timer for descent
             }
         }
         break;
@@ -748,42 +771,35 @@ void updateGPSRescueState(void)
     case RESCUE_DESCENT:
         // attenuate velocity and altitude targets while updating the heading to home
         // once inside the landing box, stop rotating, just descend
-        if (newGPSData) {
-            const int32_t targetLandingAltitudeCm = 100.0f * gpsRescueConfig()->targetLandingAltitudeM;
-            if (rescueState.sensor.currentAltitudeCm < targetLandingAltitudeCm) {
-                // enter landing mode once below landing altitude
-                rescueState.phase = RESCUE_LANDING;
-                rescueState.intent.targetAltitudeCm -= rescueState.sensor.descendStepCm;
-                rescueState.intent.secondsFailing = 0; // reset sanity timer for landing
-                rescueState.intent.targetVelocityCmS = 0; // zero velocity to home
-                rescueState.intent.pitchAngleLimitDeg = halfAngle; // reduced pitch angles
-                rescueState.intent.rollAngleLimitDeg = 0; // no roll while landing
-            } else {
-                const float distanceToLandingAreaM = MAX(rescueState.sensor.distanceToHomeM - 2.0f, 0.0f);
-                // considers home to be within a 2m circle of home to avoid searching around when crossing home
-                const float proximityToLandingArea = constrainf(distanceToLandingAreaM / rescueState.intent.descentDistanceM, 0.0f, 1.0f);
-                rescueState.intent.targetAltitudeCm -= rescueState.sensor.descendStepCm * (1.0f + proximityToLandingArea);
-                // reduce current altitude inexorably, by not less than descendStepCm and not more than 2*descendStepCm
-                rescueState.intent.targetVelocityCmS = gpsRescueConfig()->rescueGroundspeed * proximityToLandingArea;
-                // reduce target velocity as we get closer to home. Zero within 2m of home, reducing risk of overshooting.
-                // if quad drifts further than 2m away from home, should by then have rotated towards home, and pitch is allowed
-                rescueState.intent.rollAngleLimitDeg = gpsRescueConfig()->angle * proximityToLandingArea;
-                // reduce roll capability when closer to home, none within final 2m
-            }
+        if (rescueState.sensor.currentAltitudeCm < targetLandingAltitudeCm) {
+            // enter landing mode once below landing altitude
+            rescueState.phase = RESCUE_LANDING;
+            rescueState.intent.altitudeStep = -1.0f * rescueState.sensor.altitudeDataIntervalSeconds * gpsRescueConfig()->descendRate;
+            rescueState.intent.secondsFailing = 0; // reset sanity timer for landing
+            rescueState.intent.targetVelocityCmS = 0; // zero velocity to home
+            rescueState.intent.pitchAngleLimitDeg = halfAngle; // reduced pitch angles
+            rescueState.intent.rollAngleLimitDeg = 0; // no roll while landing
         }
+        if (newGPSData) {
+            const float distanceToLandingAreaM = rescueState.sensor.distanceToHomeM - 2.0f;
+            // considers home to be a 2m circle around home to avoid overshooting home point
+            proximityToLandingArea = constrainf(distanceToLandingAreaM / rescueState.intent.descentDistanceM, 0.0f, 1.0f);
+            rescueState.intent.targetVelocityCmS = gpsRescueConfig()->rescueGroundspeed * proximityToLandingArea;
+            // reduce target velocity as we get closer to home. Zero within 2m of home, reducing risk of overshooting.
+            // if quad drifts further than 2m away from home, should by then have rotated towards home, and pitch is allowed
+            rescueState.intent.rollAngleLimitDeg = gpsRescueConfig()->angle * proximityToLandingArea;
+            // reduce roll capability when closer to home, none within final 2m
+        }
+        const float proximityToGround = constrainf(rescueState.intent.targetAltitudeCm / 3000.0f, 0.0f, 2.0f);
+        rescueState.intent.targetAltitudeCm += rescueState.intent.altitudeStep * (1.0f + proximityToGround);
+        // reduce current altitude inexorably, by not less than descendStepCm and not more than 5 * altitudeStep
         break;
 
     case RESCUE_LANDING:
         // keep reducing target altitude, keep nose to home, zero velocity target with limited pitch control, no roll
-        if (newGPSData) {
-            rescueState.intent.targetAltitudeCm -= rescueState.sensor.descendStepCm;
-            // take one step off target altitude every time we get new GPS data
-        }
-        if (rescueState.sensor.accMagnitude > 2.0f) {
-            setArmingDisabled(ARMING_DISABLED_ARM_SWITCH);
-            disarm(DISARM_REASON_GPS_RESCUE);
-            rescueState.phase = RESCUE_COMPLETE;
-        }
+        rescueState.intent.targetAltitudeCm += rescueState.intent.altitudeStep;
+        // take one step off target altitude every time we get new GPS data
+        disarmOnImpact();
         break;
 
     case RESCUE_COMPLETE:
@@ -797,6 +813,7 @@ void updateGPSRescueState(void)
         break;
 
     case RESCUE_DO_NOTHING:
+        disarmOnImpact();
         break;
 
     default:
@@ -804,6 +821,7 @@ void updateGPSRescueState(void)
     }
 
     DEBUG_SET(DEBUG_GPS_RESCUE_TRACKING, 3, rescueState.intent.targetAltitudeCm);
+    DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 2, rescueState.sensor.currentAltitudeCm);
     DEBUG_SET(DEBUG_GPS_RESCUE_THROTTLE_PID, 3, rescueState.intent.targetAltitudeCm);
     DEBUG_SET(DEBUG_RTH, 1, rescueState.phase);
 
diff --git a/src/main/flight/gps_rescue.h b/src/main/flight/gps_rescue.h
index 5d514bf0fd..3ed4ffee33 100644
--- a/src/main/flight/gps_rescue.h
+++ b/src/main/flight/gps_rescue.h
@@ -48,6 +48,7 @@ PG_DECLARE(gpsRescueConfig_t, gpsRescueConfig);
 
 extern float gpsRescueAngle[ANGLE_INDEX_COUNT]; //NOTE: ANGLES ARE IN CENTIDEGREES
 
+void gpsRescueInit(void);
 void updateGPSRescueState(void);
 void rescueNewGpsData(void);
 
diff --git a/src/main/flight/position.c b/src/main/flight/position.c
index 3cbc7cb47e..59022f2a25 100644
--- a/src/main/flight/position.c
+++ b/src/main/flight/position.c
@@ -47,34 +47,44 @@
 #include "pg/pg.h"
 #include "pg/pg_ids.h"
 
-static int32_t estimatedAltitudeCm = 0;                // in cm
-#ifdef USE_BARO
-    static pt2Filter_t baroDerivativeLpf;
+static float estimatedAltitudeCm = 0;
+static float estimatedAltitudeDerivative = 0;
+
+#ifdef USE_VARIO
+static int16_t estimatedVario = 0;                   // in cm/s
 #endif
 
+static pt2Filter_t altitudeLpf;
+static pt2Filter_t altitudeDerivativeLpf;
+
+void positionInit(void)
+{
+    const float sampleTimeS = HZ_TO_INTERVAL(TASK_ALTITUDE_RATE_HZ);
+
+    const float altitudeCutoffHz = positionConfig()->altitude_lpf / 100.0f;
+    const float altitudeGain = pt2FilterGain(altitudeCutoffHz, sampleTimeS);
+    pt2FilterInit(&altitudeLpf, altitudeGain);
+
+    const float altitudeDerivativeCutoffHz = positionConfig()->altitude_d_lpf / 100.0f;
+    const float altitudeDerivativeGain = pt2FilterGain(altitudeDerivativeCutoffHz, sampleTimeS);
+    pt2FilterInit(&altitudeDerivativeLpf, altitudeDerivativeGain);
+}
+
 typedef enum {
     DEFAULT = 0,
     BARO_ONLY,
     GPS_ONLY
-} altSource_e;
+} altitude_source_e;
 
-PG_REGISTER_WITH_RESET_TEMPLATE(positionConfig_t, positionConfig, PG_POSITION, 3);
+PG_REGISTER_WITH_RESET_TEMPLATE(positionConfig_t, positionConfig, PG_POSITION, 4);
 
 PG_RESET_TEMPLATE(positionConfig_t, positionConfig,
-    .altSource = DEFAULT,
-    .altPreferBaro = 100,
+    .altitude_source = DEFAULT,
+    .altitude_prefer_baro = 100,
+    .altitude_lpf = 400,
+    .altitude_d_lpf = 100,
 );
 
-#ifdef USE_VARIO
-static int16_t estimatedVario = 0;                   // in cm/s
-
-int16_t calculateEstimatedVario(float baroAltVelocity)
-{
-    baroAltVelocity = applyDeadband(baroAltVelocity, 10.0f); // cm/s, so ignore climb rates less than 0.1 m/s
-    return constrain(lrintf(baroAltVelocity), -1500, 1500);
-}
-#endif
-
 #if defined(USE_BARO) || defined(USE_GPS)
 static bool altitudeOffsetSetBaro = false;
 static bool altitudeOffsetSetGPS = false;
@@ -83,34 +93,34 @@ void calculateEstimatedAltitude()
 {
     float baroAltCm = 0;
     float gpsAltCm = 0;
-    float baroAltVelocity = 0;
+
     float gpsTrust = 0.3; //conservative default
     bool haveBaroAlt = false;
     bool haveGpsAlt = false;
-#if defined(USE_GPS) && defined(USE_VARIO)
-    float gpsVertSpeed = 0;
-#endif
 
 // *** Set baroAlt once its calibration is complete (cannot arm until it is)
 #ifdef USE_BARO
     if (sensors(SENSOR_BARO)) {
-        static float lastBaroAltCm = 0;
-        static bool initBaroFilter = false;
-        if (!initBaroFilter) {
-            const float cutoffHz = barometerConfig()->baro_vario_lpf / 100.0f;
-            const float sampleTimeS = HZ_TO_INTERVAL(TASK_ALTITUDE_RATE_HZ);
-            const float gain = pt2FilterGain(cutoffHz, sampleTimeS);
-            pt2FilterInit(&baroDerivativeLpf, gain);
-            initBaroFilter = true;
-        }
-        baroAltCm = baroUpsampleAltitude();
-        const float baroAltVelocityRaw = (baroAltCm - lastBaroAltCm) * TASK_ALTITUDE_RATE_HZ; // cm/s
-        baroAltVelocity = pt2FilterApply(&baroDerivativeLpf, baroAltVelocityRaw);
-        lastBaroAltCm = baroAltCm;
+        baroAltCm = getBaroAltitude();
         if (baroIsCalibrated()) {
             haveBaroAlt = true;
         }
     }
+
+    // *** Zero Baro Altitude on arming (every time we re-arm, also)
+    // note that arming is blocked until baro 'calibration' (baro ground zeroing) is complete, no need to check status of haveBaroAlt
+    // this code adds a secondary zeroing to whatever baro altitude value exists on arming
+    // since props spin on arming, we want the last value before arming
+    // provided that this function is called before significant motor spin-up has occured, this may not be a big problem
+
+    static float baroAltOffsetCm = 0;
+    if (ARMING_FLAG(ARMED)) {
+        baroAltCm -= baroAltOffsetCm; // use the last offset value from the disarm period
+        altitudeOffsetSetBaro = true; // inevitable, but needed if no GPS
+    } else {
+        baroAltOffsetCm = baroAltCm; // while disarmed, keep capturing current altitude to zero any offset once armed
+    }
+
 #endif
 
     // *** Check GPS for 3D fix, set haveGpsAlt if we have a 3D fix, and GPS Trust based on hdop, or leave at default of 0.3
@@ -119,39 +129,18 @@ void calculateEstimatedAltitude()
         // Need a 3D fix, which requires min 4 sats
         // if not, gpsAltCm remains zero, haveGpsAlt stays false, and gpsTrust is zero.
         gpsAltCm = gpsSol.llh.altCm;
-#ifdef USE_VARIO
-        gpsVertSpeed = GPS_verticalSpeedInCmS;
-#endif
         haveGpsAlt = true; // remains false if no 3D fix
 
-#ifdef USE_GPS
         if (gpsSol.hdop != 0) {
             gpsTrust = 100.0 / gpsSol.hdop;
             // *** TO DO - investigate if we should use vDOP or vACC with UBlox units; this hDOP value is actually pDOP in UBLox code !!!
         }
-#endif
         // always use at least 10% of other sources besides gps if available; limit effect of HDOP
         gpsTrust = MIN(gpsTrust, 0.9f);
         // With a 3D fix, GPS trust starts at 0.3
     } else {
         gpsTrust = 0.0f; // don't trust GPS if no sensor or 3D fix
     }
-#endif
-
-    // *** Zero Baro Altitude on arming (every time we re-arm, also)
-    // note that arming is blocked until baro 'calibration' (baro ground zeroing) is complete, no need to check status of haveBaroAlt
-    // this code adds a secondary zeroing to whatever baro altitude value exists on arming
-    // since props spin on arming, we want the last value before arming
-    // provided that this function is called before significant motor spin-up has occured, this may not be a big problem
-#ifdef USE_BARO
-    static float baroAltOffsetCm = 0;
-    if (ARMING_FLAG(ARMED)) {
-        baroAltCm -= baroAltOffsetCm; // use the last offset value from the disarm period
-        altitudeOffsetSetBaro = true; // inevitable, but needed if no GPS
-    } else {
-        baroAltOffsetCm = baroAltCm; // while disarmed, keep capturing current altitude to zero any offset once armed
-    }
-#endif
 
     // *** Zero GPS Altitude on every arm or re-arm using most recent disarmed values
     // but do not use GPS if there are not the required satellites
@@ -168,7 +157,7 @@ void calculateEstimatedAltitude()
     // If we don't, we wait until we get a 3D fix, and then correct the offset using the baro value
     // This won't be very accurate, but all we need is a 3D fix.
     // Note that without the 3D fix, GPS trust will be zero, and on getting a 3D fix, will depend on hDOP
-#ifdef USE_GPS
+
     static float gpsAltOffsetCm = 0;
     if (ARMING_FLAG(ARMED)) {
         gpsAltCm -= gpsAltOffsetCm; // while armed, use the last offset value from the disarm period
@@ -187,14 +176,14 @@ void calculateEstimatedAltitude()
     // *** adjust gpsTrust, favouring Baro increasingly when there is a discrepancy of more than a meter
     // favour GPS if Baro reads negative, this happens due to ground effects
     float gpsTrustModifier = gpsTrust;
-    const float absDifferenceM = fabsf(gpsAltCm - baroAltCm) * positionConfig()->altPreferBaro / 10000.0f;
+    const float absDifferenceM = fabsf(gpsAltCm - baroAltCm) * positionConfig()->altitude_prefer_baro / 10000.0f;
     if (absDifferenceM > 1.0f && baroAltCm > -100.0f) { // significant difference, and baro altitude not negative
         gpsTrustModifier /=  absDifferenceM;
     }
     // eg if discrepancy is 3m and GPS trust was 0.9, it would now be 0.3
 
     // *** If we have  a GPS with 3D fix and a Baro signal, blend them
-    if (haveGpsAlt && haveBaroAlt && positionConfig()->altSource == DEFAULT) {
+    if (haveGpsAlt && haveBaroAlt && positionConfig()->altitude_source == DEFAULT) {
         if (ARMING_FLAG(ARMED)) {
             estimatedAltitudeCm = gpsAltCm * gpsTrustModifier + baroAltCm * (1 - gpsTrustModifier);
         } else {
@@ -202,34 +191,32 @@ void calculateEstimatedAltitude()
             //absolute GPS altitude is shown before arming, ignoring baro (I have no clue why this is)
         }
 
-#ifdef USE_VARIO
-        // baro is a better source for vario, so ignore gpsVertSpeed
-        estimatedVario = calculateEstimatedVario(baroAltVelocity);
-#endif
-
     // *** If we have a GPS but no baro, and are in Default or GPS_ONLY modes, use GPS values
-    } else if (haveGpsAlt && (positionConfig()->altSource == GPS_ONLY || positionConfig()->altSource == DEFAULT )) {
+    } else if (haveGpsAlt && (positionConfig()->altitude_source == GPS_ONLY || positionConfig()->altitude_source == DEFAULT )) {
         estimatedAltitudeCm = gpsAltCm;
-#if defined(USE_VARIO) && defined(USE_GPS)
-        estimatedVario = gpsVertSpeed;
-#endif
 
     // *** If we have a Baro, and can work with it in Default or Baro Only modes
-    } else if (haveBaroAlt && (positionConfig()->altSource == BARO_ONLY || positionConfig()->altSource == DEFAULT)) {
+    } else if (haveBaroAlt && (positionConfig()->altitude_source == BARO_ONLY || positionConfig()->altitude_source == DEFAULT)) {
         estimatedAltitudeCm = baroAltCm;
-
-#ifdef USE_VARIO
-        estimatedVario = calculateEstimatedVario(baroAltVelocity); // cm/s
-#endif
     }
 
-    // Note that if we have no GPS but user chooses GPS Only, or no Baro but user chooses Baro only, then the reported altitude will be zero
-    // The latter may cause GPS rescue to fail, but the user should notice an absence of altitude values.
+    estimatedAltitudeCm = pt2FilterApply(&altitudeLpf, estimatedAltitudeCm);
+
+    static float previousEstimatedAltitudeCm = 0;
+    const float altitudeDerivativeRaw = (estimatedAltitudeCm - previousEstimatedAltitudeCm) * TASK_ALTITUDE_RATE_HZ; // cm/s
+    previousEstimatedAltitudeCm = estimatedAltitudeCm;
+    estimatedAltitudeDerivative = pt2FilterApply(&altitudeDerivativeLpf, altitudeDerivativeRaw);
+
+#ifdef USE_VARIO
+    estimatedVario = lrintf(estimatedAltitudeDerivative);
+    estimatedVario = applyDeadband(estimatedVario, 10); // ignore climb rates less than 0.1 m/s
+    estimatedVario = constrain(estimatedVario, -1500, 1500);
+#endif
 
     DEBUG_SET(DEBUG_ALTITUDE, 0, (int32_t)(100 * gpsTrust));
     DEBUG_SET(DEBUG_ALTITUDE, 1, baroAltCm);
-    DEBUG_SET(DEBUG_ALTITUDE, 2, gpsAltCm);
-    DEBUG_SET(DEBUG_ALTITUDE, 3, estimatedAltitudeCm);
+    DEBUG_SET(DEBUG_ALTITUDE, 2, estimatedAltitudeCm);
+    DEBUG_SET(DEBUG_ALTITUDE, 3, estimatedAltitudeDerivative); // cm/s
 }
 
 bool isAltitudeOffset(void)
@@ -239,6 +226,11 @@ bool isAltitudeOffset(void)
 #endif
 
 int32_t getEstimatedAltitudeCm(void)
+{
+    return lrintf(estimatedAltitudeCm);
+}
+
+float getAltitude(void)
 {
     return estimatedAltitudeCm;
 }
diff --git a/src/main/flight/position.h b/src/main/flight/position.h
index e53f7dc939..a9733192fe 100644
--- a/src/main/flight/position.h
+++ b/src/main/flight/position.h
@@ -24,13 +24,17 @@
 
 #define TASK_ALTITUDE_RATE_HZ 120
 typedef struct positionConfig_s {
-    uint8_t altSource;
-    uint8_t altPreferBaro;
+    uint8_t altitude_source;
+    uint8_t altitude_prefer_baro;
+    uint16_t altitude_lpf;                // lowpass cutoff (value / 100) Hz for baro smoothing
+    uint16_t altitude_d_lpf;                // lowpass for (value / 100) Hz baro derivative smoothing
 } positionConfig_t;
 
 PG_DECLARE(positionConfig_t, positionConfig);
 
 bool isAltitudeOffset(void);
 void calculateEstimatedAltitude();
+void positionInit(void);
 int32_t getEstimatedAltitudeCm(void);
+float getAltitude(void);
 int16_t getEstimatedVario(void);
diff --git a/src/main/io/gps.h b/src/main/io/gps.h
index 5da3236aa6..4a1c7d7219 100644
--- a/src/main/io/gps.h
+++ b/src/main/io/gps.h
@@ -199,7 +199,7 @@ extern uint8_t GPS_svinfo_cno[GPS_SV_MAXSATS_M8N];      // Carrier to Noise Rati
 #define GPS_DBHZ_MIN 0
 #define GPS_DBHZ_MAX 55
 
-#define TASK_GPS_RATE       100
+#define TASK_GPS_RATE       120
 #define TASK_GPS_RATE_FAST  1000
 
 void gpsInit(void);
diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c
index ac71b6b230..fefe1b6c40 100644
--- a/src/main/sensors/barometer.c
+++ b/src/main/sensors/barometer.c
@@ -59,12 +59,10 @@
 
 baro_t baro;                        // barometer access functions
 
-PG_REGISTER_WITH_RESET_FN(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 2);
+PG_REGISTER_WITH_RESET_FN(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 3);
 
 void pgResetFn_barometerConfig(barometerConfig_t *barometerConfig)
 {
-    barometerConfig->baro_noise_lpf = 50;
-    barometerConfig->baro_vario_lpf = 10;
     barometerConfig->baro_hardware = BARO_DEFAULT;
 
     // For backward compatibility; ceate a valid default value for bus parameters
@@ -145,7 +143,6 @@ static int32_t baroPressure = 0;
 static int32_t baroTemperature = 0;
 static float baroGroundAltitude = 0.0f;
 static float baroAltitudeRaw = 0.0f;
-static pt2Filter_t baroUpsampleLpf;
 
 
 #define CALIBRATING_BARO_CYCLES 100 // 10 seconds init_delay + 200 * 25 ms = 15 seconds before ground pressure settles
@@ -163,20 +160,6 @@ void baroPreInit(void)
 #endif
 }
 
-static bool baroDetect(baroDev_t *baroDev, baroSensor_e baroHardwareToUse);
-
-void baroInit(void)
-{
-    baroDetect(&baro.dev, barometerConfig()->baro_hardware);
-
-    const float cutoffHz = barometerConfig()->baro_noise_lpf / 100.0f;
-    const float sampleTimeS = HZ_TO_INTERVAL(TASK_ALTITUDE_RATE_HZ);
-    const float gain = pt2FilterGain(cutoffHz, sampleTimeS);
-    pt2FilterInit(&baroUpsampleLpf, gain);
-
-    baroReady = true;
-}
-
 static bool baroDetect(baroDev_t *baroDev, baroSensor_e baroHardwareToUse)
 {
     extDevice_t *dev = &baroDev->dev;
@@ -311,6 +294,12 @@ static bool baroDetect(baroDev_t *baroDev, baroSensor_e baroHardwareToUse)
     return true;
 }
 
+void baroInit(void)
+{
+    baroDetect(&baro.dev, barometerConfig()->baro_hardware);
+    baroReady = true;
+}
+
 bool baroIsCalibrated(void)
 {
     return baroCalibrated;
@@ -432,6 +421,12 @@ uint32_t baroUpdate(timeUs_t currentTimeUs)
 
             performBaroCalibrationCycle();
 
+            if (baroIsCalibrated()) {
+                baro.BaroAlt = baroAltitudeRaw - baroGroundAltitude;
+            } else {
+                baro.BaroAlt = 0.0f;
+            }
+
             DEBUG_SET(DEBUG_BARO, 1, baro.baroTemperature);
             DEBUG_SET(DEBUG_BARO, 2, baroAltitudeRaw - baroGroundAltitude);
 
@@ -461,15 +456,9 @@ uint32_t baroUpdate(timeUs_t currentTimeUs)
     return sleepTime;
 }
 
-// baroAltitudeRaw will get updated in the BARO task while baroUpsampleAltitude() will run in the ALTITUDE task.
-float baroUpsampleAltitude(void)
+// baroAltitude samples baro.BaroAlt the ALTITUDE task rate
+float getBaroAltitude(void)
 {
-    const float baroAltitudeRawFiltered = pt2FilterApply(&baroUpsampleLpf, baroAltitudeRaw);
-    if (baroIsCalibrated()) {
-        baro.BaroAlt = baroAltitudeRawFiltered - baroGroundAltitude;
-    } else {
-        baro.BaroAlt = 0.0f;
-    }
     DEBUG_SET(DEBUG_BARO, 3, baro.BaroAlt); // cm
     return baro.BaroAlt;
 }
diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h
index 4cd72ae990..c6c067151e 100644
--- a/src/main/sensors/barometer.h
+++ b/src/main/sensors/barometer.h
@@ -43,8 +43,6 @@ typedef struct barometerConfig_s {
     uint8_t baro_i2c_device;
     uint8_t baro_i2c_address;
     uint8_t baro_hardware;                  // Barometer hardware to use
-    uint16_t baro_noise_lpf;                // lowpass cutoff (value / 100) Hz for baro smoothing
-    uint16_t baro_vario_lpf;                // lowpass for (value / 100) Hz baro derivative smoothing
     ioTag_t baro_eoc_tag;
     ioTag_t baro_xclr_tag;
 } barometerConfig_t;
@@ -72,5 +70,5 @@ void baroSetGroundLevel(void);
 uint32_t baroUpdate(timeUs_t currentTimeUs);
 bool isBaroReady(void);
 bool isBaroSampleReady(void);
-float baroUpsampleAltitude(void);
+float getBaroAltitude(void);
 void performBaroCalibrationCycle(void);
diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c
index c4a97e0efa..88840e8881 100644
--- a/src/main/sensors/initialisation.c
+++ b/src/main/sensors/initialisation.c
@@ -32,6 +32,8 @@
 #include "fc/runtime_config.h"
 
 #include "flight/pid.h"
+#include "flight/position.h"
+#include "flight/gps_rescue.h"
 
 #include "pg/pg.h"
 #include "pg/pg_ids.h"
@@ -46,6 +48,7 @@
 #include "sensors/rangefinder.h"
 #include "sensors/sensors.h"
 
+
 // requestedSensors is not actually used
 uint8_t requestedSensors[SENSOR_INDEX_COUNT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE, RANGEFINDER_NONE };
 uint8_t detectedSensors[SENSOR_INDEX_COUNT] = { GYRO_NONE, ACC_NONE, BARO_NONE, MAG_NONE, RANGEFINDER_NONE };
@@ -84,6 +87,9 @@ bool sensorsAutodetect(void)
     baroInit();
 #endif
 
+positionInit();
+gpsRescueInit();
+
 #ifdef USE_RANGEFINDER
     rangefinderInit();
 #endif
diff --git a/src/test/unit/telemetry_ibus_unittest.cc b/src/test/unit/telemetry_ibus_unittest.cc
index 5c4aa2f248..a499b4b2b4 100644
--- a/src/test/unit/telemetry_ibus_unittest.cc
+++ b/src/test/unit/telemetry_ibus_unittest.cc
@@ -73,7 +73,7 @@ uint16_t getVbat(void)
 
 extern "C" {
 static int32_t amperage = 100;
-static int32_t estimatedVario = 0;
+static int16_t estimatedVario = 0;
 static uint8_t batteryRemaining = 0;
 static throttleStatus_e throttleStatus = THROTTLE_HIGH;
 static uint32_t definedFeatures = 0;