diff --git a/docs/Settings.md b/docs/Settings.md
index 27e354dc9ce..0de2f3f70d8 100644
--- a/docs/Settings.md
+++ b/docs/Settings.md
@@ -5946,6 +5946,7 @@ Selection of pitot hardware. VIRTUAL only works if a GPS is enabled.
 | FAKE |  |
 | MSP |  |
 | DLVR-L10D |  |
+| MS5525 |  |
 
 ---
 
diff --git a/src/main/common/filter.c b/src/main/common/filter.c
index 2e9e4574e71..602c78d249d 100644
--- a/src/main/common/filter.c
+++ b/src/main/common/filter.c
@@ -34,72 +34,54 @@ float nullFilterApply(void *filter, float input)
     return input;
 }
 
-float nullFilterApply4(void *filter, float input, float f_cut, float dt)
+float nullFilterApply3(void *filter, float input, float dt)
 {
     UNUSED(filter);
-    UNUSED(f_cut);
     UNUSED(dt);
     return input;
 }
 
-// PT1 Low Pass filter
+/* PT1 Low Pass filter
+ * f_cut = cutoff frequency. Use pt1FilterSetTimeConstant to directly set RC time constant tau if required.
+ */
 
-static float FAST_CODE pt1ComputeRC(const float f_cut)
+static float pt1ComputeRC(const float f_cut)
 {
     return 1.0f / (2.0f * M_PIf * f_cut);
 }
 
-// f_cut = cutoff frequency
-void pt1FilterInitRC(pt1Filter_t *filter, float tau, float dT)
+void pt1FilterInit(pt1Filter_t *filter, float f_cut, float dT)
 {
-    filter->state = 0.0f;
-    filter->RC = tau;
+    filter->RC = pt1ComputeRC(f_cut);
     filter->dT = dT;
     filter->alpha = filter->dT / (filter->RC + filter->dT);
+    filter->state = 0.0f;
 }
 
-void pt1FilterInit(pt1Filter_t *filter, float f_cut, float dT)
+float FAST_CODE NOINLINE pt1FilterApply(pt1Filter_t *filter, float input)  // use with pt1FilterInit if dT and f_cut are constants
 {
-    pt1FilterInitRC(filter, pt1ComputeRC(f_cut), dT);
-}
-
-void pt1FilterSetTimeConstant(pt1Filter_t *filter, float tau) {
-    filter->RC = tau;
-}
-
-float pt1FilterGetLastOutput(pt1Filter_t *filter) {
-    return filter->state;
+    return filter->state = filter->state + filter->alpha * (input - filter->state);
 }
 
-void pt1FilterUpdateCutoff(pt1Filter_t *filter, float f_cut)
+float FAST_CODE NOINLINE pt1FilterApply3(pt1Filter_t *filter, float input, float dT)
 {
-    filter->RC = pt1ComputeRC(f_cut);
+    filter->dT = dT;    // cache latest dT for possible use in pt1FilterApply
     filter->alpha = filter->dT / (filter->RC + filter->dT);
-}
 
-float FAST_CODE NOINLINE pt1FilterApply(pt1Filter_t *filter, float input)
-{
-    filter->state = filter->state + filter->alpha * (input - filter->state);
-    return filter->state;
+    return filter->state = filter->state + filter->alpha * (input - filter->state);
 }
 
-float pt1FilterApply3(pt1Filter_t *filter, float input, float dT)
-{
-    filter->dT = dT;
-    filter->state = filter->state + dT / (filter->RC + dT) * (input - filter->state);
-    return filter->state;
+void pt1FilterSetTimeConstant(pt1Filter_t *filter, float tau) {
+    filter->RC = tau;
 }
 
-float FAST_CODE NOINLINE pt1FilterApply4(pt1Filter_t *filter, float input, float f_cut, float dT)
+void pt1FilterSetCutoff(pt1Filter_t *filter, float f_cut)
 {
-    // Pre calculate and store RC
-    if (!filter->RC) {
-        filter->RC = pt1ComputeRC(f_cut);
-    }
-
-    filter->dT = dT;    // cache latest dT for possible use in pt1FilterApply
+    filter->RC = pt1ComputeRC(f_cut);
     filter->alpha = filter->dT / (filter->RC + filter->dT);
-    filter->state = filter->state + filter->alpha * (input - filter->state);
+}
+
+float pt1FilterGetLastOutput(pt1Filter_t *filter) {
     return filter->state;
 }
 
diff --git a/src/main/common/filter.h b/src/main/common/filter.h
index 9be86b495ce..ded4307be99 100644
--- a/src/main/common/filter.h
+++ b/src/main/common/filter.h
@@ -47,8 +47,8 @@ typedef struct biquadFilter_s {
     float x1, x2, y1, y2;
 } biquadFilter_t;
 
-typedef union { 
-    biquadFilter_t biquad; 
+typedef union {
+    biquadFilter_t biquad;
     pt1Filter_t pt1;
     pt2Filter_t pt2;
     pt3Filter_t pt3;
@@ -88,22 +88,19 @@ typedef struct alphaBetaGammaFilter_s {
 } alphaBetaGammaFilter_t;
 
 typedef float (*filterApplyFnPtr)(void *filter, float input);
-typedef float (*filterApply4FnPtr)(void *filter, float input, float f_cut, float dt);
+typedef float (*filterApply3FnPtr)(void *filter, float input, float dt);
 
 #define BIQUAD_BANDWIDTH 1.9f     /* bandwidth in octaves */
 #define BIQUAD_Q 1.0f / sqrtf(2.0f)     /* quality factor - butterworth*/
 
 float nullFilterApply(void *filter, float input);
-float nullFilterApply4(void *filter, float input, float f_cut, float dt);
-
+float nullFilterApply3(void *filter, float input, float dt);
 void pt1FilterInit(pt1Filter_t *filter, float f_cut, float dT);
-void pt1FilterInitRC(pt1Filter_t *filter, float tau, float dT);
-void pt1FilterSetTimeConstant(pt1Filter_t *filter, float tau);
-void pt1FilterUpdateCutoff(pt1Filter_t *filter, float f_cut);
-float pt1FilterGetLastOutput(pt1Filter_t *filter);
 float pt1FilterApply(pt1Filter_t *filter, float input);
 float pt1FilterApply3(pt1Filter_t *filter, float input, float dT);
-float pt1FilterApply4(pt1Filter_t *filter, float input, float f_cut, float dt);
+void pt1FilterSetTimeConstant(pt1Filter_t *filter, float tau);
+void pt1FilterSetCutoff(pt1Filter_t *filter, float f_cut);
+float pt1FilterGetLastOutput(pt1Filter_t *filter);
 void pt1FilterReset(pt1Filter_t *filter, float input);
 
 /*
diff --git a/src/main/common/fp_pid.c b/src/main/common/fp_pid.c
index df8ccaad6b7..58a2fec57ed 100644
--- a/src/main/common/fp_pid.c
+++ b/src/main/common/fp_pid.c
@@ -46,9 +46,9 @@ float navPidApply3(
 ) {
     float newProportional, newDerivative, newFeedForward;
     float error = 0.0f;
-    
+
     if (pid->errorLpfHz > 0.0f) {
-        error = pt1FilterApply4(&pid->error_filter_state, setpoint - measurement, pid->errorLpfHz, dt);
+        error = pt1FilterApply3(&pid->error_filter_state, setpoint - measurement, dt);
     } else {
         error = setpoint - measurement;
     }
@@ -73,7 +73,7 @@ float navPidApply3(
     }
 
     if (pid->dTermLpfHz > 0.0f) {
-        newDerivative = pid->param.kD * pt1FilterApply4(&pid->dterm_filter_state, newDerivative, pid->dTermLpfHz, dt);
+        newDerivative = pid->param.kD * pt1FilterApply3(&pid->dterm_filter_state, newDerivative, dt);
     } else {
         newDerivative = pid->param.kD * newDerivative;
     }
@@ -105,10 +105,7 @@ float navPidApply3(
     pid->output_constrained = outValConstrained;
 
     /* Update I-term */
-    if (
-        !(pidFlags & PID_ZERO_INTEGRATOR) &&
-        !(pidFlags & PID_FREEZE_INTEGRATOR) 
-    ) {
+    if (!(pidFlags & PID_ZERO_INTEGRATOR) && !(pidFlags & PID_FREEZE_INTEGRATOR)) {
         const float newIntegrator = pid->integrator + (error * pid->param.kI * gainScaler * dt) + (backCalc * pid->param.kT * dt);
 
         if (pidFlags & PID_SHRINK_INTEGRATOR) {
@@ -121,10 +118,10 @@ float navPidApply3(
             pid->integrator = newIntegrator;
         }
     }
-    
+
     if (pidFlags & PID_LIMIT_INTEGRATOR) {
         pid->integrator = constrainf(pid->integrator, outMin, outMax);
-    } 
+    }
 
     return outValConstrained;
 }
@@ -143,8 +140,9 @@ void navPidReset(pidController_t *pid)
     pid->integrator = 0.0f;
     pid->last_input = 0.0f;
     pid->feedForward = 0.0f;
-    pt1FilterReset(&pid->dterm_filter_state, 0.0f);
     pid->output_constrained = 0.0f;
+
+    pt1FilterReset(&pid->dterm_filter_state, 0.0f);
 }
 
 void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kFF, float _dTermLpfHz, float _errorLpfHz)
@@ -169,5 +167,9 @@ void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kF
     }
     pid->dTermLpfHz = _dTermLpfHz;
     pid->errorLpfHz = _errorLpfHz;
+
+    pt1FilterSetCutoff(&pid->error_filter_state, pid->errorLpfHz);
+    pt1FilterSetCutoff(&pid->dterm_filter_state, pid->dTermLpfHz);
+
     navPidReset(pid);
 }
\ No newline at end of file
diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c
index 379a023ffd8..70db2b114a9 100644
--- a/src/main/flight/imu.c
+++ b/src/main/flight/imu.c
@@ -99,21 +99,16 @@ FASTRAM float rMat[3][3];
 
 STATIC_FASTRAM imuRuntimeConfig_t imuRuntimeConfig;
 
-STATIC_FASTRAM pt1Filter_t rotRateFilterX;
-STATIC_FASTRAM pt1Filter_t rotRateFilterY;
-STATIC_FASTRAM pt1Filter_t rotRateFilterZ;
+STATIC_FASTRAM pt1Filter_t rotRateFilter[XYZ_AXIS_COUNT];
 FASTRAM fpVector3_t imuMeasuredRotationBFFiltered = {.v = {0.0f, 0.0f, 0.0f}};
 
-STATIC_FASTRAM pt1Filter_t accelFilterX;
-STATIC_FASTRAM pt1Filter_t accelFilterY;
-STATIC_FASTRAM pt1Filter_t accelFilterZ;
+STATIC_FASTRAM pt1Filter_t accelFilter[XYZ_AXIS_COUNT];
 FASTRAM fpVector3_t imuMeasuredAccelBFFiltered = {.v = {0.0f, 0.0f, 0.0f}};
 
-STATIC_FASTRAM pt1Filter_t HeadVecEFFilterX;
-STATIC_FASTRAM pt1Filter_t HeadVecEFFilterY;
-STATIC_FASTRAM pt1Filter_t HeadVecEFFilterZ;
+STATIC_FASTRAM pt1Filter_t HeadVecEFFilter[XYZ_AXIS_COUNT];
 FASTRAM fpVector3_t HeadVecEFFiltered = {.v = {0.0f, 0.0f, 0.0f}};
 
+static pt1Filter_t GPS3DspeedFilter;
 
 FASTRAM bool gpsHeadingInitialized;
 
@@ -202,18 +197,19 @@ void imuInit(void)
     quaternionInitUnit(&orientation);
     imuComputeRotationMatrix();
 
-    // Initialize rotation rate filter
-    pt1FilterReset(&rotRateFilterX, 0);
-    pt1FilterReset(&rotRateFilterY, 0);
-    pt1FilterReset(&rotRateFilterZ, 0);
-    // Initialize accel filter
-    pt1FilterReset(&accelFilterX, 0);
-    pt1FilterReset(&accelFilterY, 0);
-    pt1FilterReset(&accelFilterZ, 0);
-    // Initialize Heading vector filter
-    pt1FilterReset(&HeadVecEFFilterX, 0);
-    pt1FilterReset(&HeadVecEFFilterY, 0);
-    pt1FilterReset(&HeadVecEFFilterZ, 0);
+    // Initialize rotation rate, accel and Heading vector filters
+    for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+        pt1FilterSetCutoff(&rotRateFilter[axis], IMU_ROTATION_LPF);
+        pt1FilterReset(&rotRateFilter[axis], 0.0f);
+
+        pt1FilterSetCutoff(&accelFilter[axis], IMU_ROTATION_LPF);
+        pt1FilterReset(&accelFilter[axis], 0.0f);
+
+        pt1FilterSetCutoff(&HeadVecEFFilter[axis], IMU_ROTATION_LPF);
+        pt1FilterReset(&HeadVecEFFilter[axis], 0.0f);
+    }
+
+    pt1FilterSetCutoff(&GPS3DspeedFilter, IMU_ROTATION_LPF);
 }
 
 void imuSetMagneticDeclination(float declinationDeg)
@@ -684,19 +680,11 @@ static float imuCalculateAccelerometerWeightRateIgnore(const float acc_ignore_sl
 
 static void imuCalculateFilters(float dT)
 {
-    //flitering
-    imuMeasuredRotationBFFiltered.x = pt1FilterApply4(&rotRateFilterX, imuMeasuredRotationBF.x, IMU_ROTATION_LPF, dT);
-    imuMeasuredRotationBFFiltered.y = pt1FilterApply4(&rotRateFilterY, imuMeasuredRotationBF.y, IMU_ROTATION_LPF, dT);
-    imuMeasuredRotationBFFiltered.z = pt1FilterApply4(&rotRateFilterZ, imuMeasuredRotationBF.z, IMU_ROTATION_LPF, dT);
-
-    imuMeasuredAccelBFFiltered.x = pt1FilterApply4(&accelFilterX, imuMeasuredAccelBF.x, IMU_ROTATION_LPF, dT);
-    imuMeasuredAccelBFFiltered.y = pt1FilterApply4(&accelFilterY, imuMeasuredAccelBF.y, IMU_ROTATION_LPF, dT);
-    imuMeasuredAccelBFFiltered.z = pt1FilterApply4(&accelFilterZ, imuMeasuredAccelBF.z, IMU_ROTATION_LPF, dT);
-
-    HeadVecEFFiltered.x = pt1FilterApply4(&HeadVecEFFilterX, rMat[0][0], IMU_ROTATION_LPF, dT);
-    HeadVecEFFiltered.y = pt1FilterApply4(&HeadVecEFFilterY, rMat[1][0], IMU_ROTATION_LPF, dT);
-    HeadVecEFFiltered.z = pt1FilterApply4(&HeadVecEFFilterZ, rMat[2][0], IMU_ROTATION_LPF, dT);
-
+    for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+        imuMeasuredRotationBFFiltered.v[axis] = pt1FilterApply3(&rotRateFilter[axis], imuMeasuredRotationBF.v[axis], dT);
+        imuMeasuredAccelBFFiltered.v[axis] = pt1FilterApply3(&accelFilter[axis], imuMeasuredAccelBF.v[axis], dT);
+        HeadVecEFFiltered.v[axis] = pt1FilterApply3(&HeadVecEFFilter[axis], rMat[axis][0], dT);
+    }
 }
 
 static void imuCalculateGPSacceleration(fpVector3_t *vEstAccelEF,fpVector3_t *vEstcentrifugalAccelBF, float *acc_ignore_slope_multipiler)
@@ -732,12 +720,11 @@ static void imuCalculateTurnRateacceleration(fpVector3_t *vEstcentrifugalAccelBF
     if (isGPSTrustworthy()) {
         //first speed choice is gps
         static bool lastGPSHeartbeat;
-        static pt1Filter_t GPS3DspeedFilter;
         static float GPS3DspeedFiltered = 0.0f;
         if (gpsSol.flags.gpsHeartbeat != lastGPSHeartbeat) {
             lastGPSHeartbeat = gpsSol.flags.gpsHeartbeat;
             float GPS3Dspeed = calc_length_pythagorean_3D(gpsSol.velNED[X], gpsSol.velNED[Y], gpsSol.velNED[Z]);
-            GPS3DspeedFiltered = pt1FilterApply4(&GPS3DspeedFilter, GPS3Dspeed, IMU_ROTATION_LPF, dT);
+            GPS3DspeedFiltered = pt1FilterApply3(&GPS3DspeedFilter, GPS3Dspeed, dT);
         }
         currentspeed = GPS3DspeedFiltered;
         *acc_ignore_slope_multipiler = 4.0f;
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 5bf08e20dfe..c9684ac16f4 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -110,7 +110,7 @@ typedef struct {
     biquadFilter_t dBoostGyroLpf;
     float dBoostTargetAcceleration;
 #endif
-    filterApply4FnPtr ptermFilterApplyFn;
+    filterApply3FnPtr ptermFilterApplyFn;
     bool itermLimitActive;
     bool itermFreezeActive;
 
@@ -152,7 +152,7 @@ static EXTENDED_FASTRAM float antigravityAccelerator;
 #endif
 
 #define D_BOOST_GYRO_LPF_HZ 80    // Biquad lowpass input cutoff to peak D around propwash frequencies
-#define D_BOOST_LPF_HZ 7          // PT1 lowpass cutoff to smooth the boost effect
+#define D_BOOST_LPF_HZ 7.0f       // PT1 lowpass cutoff to smooth the boost effect
 
 #ifdef USE_D_BOOST
 static EXTENDED_FASTRAM float dBoostMin;
@@ -355,6 +355,11 @@ bool pidInitFilters(void)
     pt1FilterInit(&antigravityThrottleLpf, pidProfile()->antigravityCutoff, US2S(TASK_PERIOD_HZ(TASK_AUX_RATE_HZ)));
 #endif
 
+    pt1FilterSetCutoff(&pidState->ptermLpfState, yawLpfHz);
+    pt1FilterSetCutoff(&pidState->angleFilterState, pidBank()->pid[PID_LEVEL].I);
+    pt1FilterSetCutoff(&pidState->dBoostLpf, D_BOOST_LPF_HZ);
+    pt1FilterSetCutoff(&headingHoldRateFilter, HEADING_HOLD_ERROR_LPF_FREQ);
+
 #ifdef USE_D_BOOST
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
         biquadFilterInitLPF(&pidState[axis].dBoostGyroLpf, pidProfile()->dBoostGyroDeltaLpfHz, getLooptime());
@@ -399,7 +404,8 @@ bool pidInitFilters(void)
 void pidResetTPAFilter(void)
 {
     if (usedPidControllerType == PID_TYPE_PIFF && currentControlProfile->throttle.fixedWingTauMs > 0) {
-        pt1FilterInitRC(&fixedWingTpaFilter, MS2S(currentControlProfile->throttle.fixedWingTauMs), US2S(TASK_PERIOD_HZ(TASK_AUX_RATE_HZ)));
+        pt1FilterInit(&fixedWingTpaFilter, 1.0f, HZ2S(TASK_AUX_RATE_HZ));
+        pt1FilterSetTimeConstant(&fixedWingTpaFilter, MS2S(currentControlProfile->throttle.fixedWingTauMs));
         pt1FilterReset(&fixedWingTpaFilter, getThrottleIdleValue());
     }
 }
@@ -564,7 +570,7 @@ void updatePIDCoefficients(void)
     for (int axis = 0; axis < 3; axis++) {
         pidState[axis].stickPosition = constrain(rxGetChannelValue(axis) - PWM_RANGE_MIDDLE, -500, 500) / 500.0f;
     }
-    
+
     float tpaFactor=1.0f;
     float iTermFactor=1.0f;  // Separate factor for I-term scaling
     if(usedPidControllerType == PID_TYPE_PIFF){ // Fixed wing TPA calculation
@@ -584,13 +590,13 @@ void updatePIDCoefficients(void)
     }
     tpaFactorprev = tpaFactor;
 
-    
+
     // If nothing changed - don't waste time recalculating coefficients
     if (!pidGainsUpdateRequired) {
         return;
     }
 
-    
+
     // PID coefficients can be update only with THROTTLE and TPA or inflight PID adjustments
     //TODO: Next step would be to update those only at THROTTLE or inflight adjustments change
     for (int axis = 0; axis < 3; axis++) {
@@ -729,7 +735,7 @@ static void pidLevel(const float angleTarget, pidState_t *pidState, flight_dynam
     //     response to rapid attitude changes and smoothing out self-leveling reaction
     if (pidBank()->pid[PID_LEVEL].I) {
         // I8[PIDLEVEL] is filter cutoff frequency (Hz). Practical values of filtering frequency is 5-10 Hz
-        angleRateTarget = pt1FilterApply4(&pidState->angleFilterState, angleRateTarget, pidBank()->pid[PID_LEVEL].I, dT);
+        angleRateTarget = pt1FilterApply3(&pidState->angleFilterState, angleRateTarget, dT);
     }
 
     // P[LEVEL] defines self-leveling strength (both for ANGLE and HORIZON modes)
@@ -753,7 +759,7 @@ static void FAST_CODE pidApplySetpointRateLimiting(pidState_t *pidState, flight_
 static float FAST_CODE pTermProcess(pidState_t *pidState, float rateError, float dT) {
     float newPTerm = rateError * pidState->kP;
 
-    return pidState->ptermFilterApplyFn(&pidState->ptermLpfState, newPTerm, yawLpfHz, dT);
+    return pidState->ptermFilterApplyFn(&pidState->ptermLpfState, newPTerm, dT);
 }
 
 #ifdef USE_D_BOOST
@@ -773,7 +779,7 @@ static float FAST_CODE applyDBoost(pidState_t *pidState, float currentRateTarget
         dBoost = scaleRangef(dBoostRateAcceleration, 0.0f, pidState->dBoostTargetAcceleration, 1.0f, dBoostMin);
     }
 
-    dBoost = pt1FilterApply4(&pidState->dBoostLpf, dBoost, D_BOOST_LPF_HZ, dT);
+    dBoost = pt1FilterApply3(&pidState->dBoostLpf, dBoost, dT);
     dBoost = constrainf(dBoost, dBoostMin, dBoostMax);
 
     return dBoost;
@@ -1071,7 +1077,7 @@ float pidHeadingHold(float dT)
 
     headingHoldRate = error * pidBank()->pid[PID_HEADING].P / 30.0f;
     headingHoldRate = constrainf(headingHoldRate, -pidProfile()->heading_hold_rate_limit, pidProfile()->heading_hold_rate_limit);
-    headingHoldRate = pt1FilterApply4(&headingHoldRateFilter, headingHoldRate, HEADING_HOLD_ERROR_LPF_FREQ, dT);
+    headingHoldRate = pt1FilterApply3(&headingHoldRateFilter, headingHoldRate, dT);
 
     return headingHoldRate;
 }
@@ -1398,14 +1404,11 @@ void pidInit(void)
 
         pidState[axis].axis = axis;
         pidState[axis].pidSumLimit = getPidSumLimit(axis);
-        if (axis == FD_YAW) {
-            if (yawLpfHz) {
-                pidState[axis].ptermFilterApplyFn = (filterApply4FnPtr) pt1FilterApply4;
-            } else {
-                pidState[axis].ptermFilterApplyFn = (filterApply4FnPtr) nullFilterApply4;
-            }
+
+        if (axis == FD_YAW && yawLpfHz) {
+            pidState[axis].ptermFilterApplyFn = (filterApply3FnPtr) pt1FilterApply3;
         } else {
-            pidState[axis].ptermFilterApplyFn = (filterApply4FnPtr) nullFilterApply4;
+            pidState[axis].ptermFilterApplyFn = (filterApply3FnPtr) nullFilterApply3;
         }
     }
 
diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
index dbcd0e3e26a..68f09018763 100644
--- a/src/main/flight/pid.h
+++ b/src/main/flight/pid.h
@@ -33,7 +33,7 @@
 
 #define AXIS_ACCEL_MIN_LIMIT        50
 
-#define HEADING_HOLD_ERROR_LPF_FREQ 2
+#define HEADING_HOLD_ERROR_LPF_FREQ 2.0f
 
 /*
 FP-PID has been rescaled to match LuxFloat (and MWRewrite) from Cleanflight 1.13
diff --git a/src/main/flight/power_limits.c b/src/main/flight/power_limits.c
index 1770fd71a7f..a75d0a81206 100644
--- a/src/main/flight/power_limits.c
+++ b/src/main/flight/power_limits.c
@@ -73,6 +73,8 @@ static bool wasLimitingPower = false;
 #endif
 
 void powerLimiterInit(void) {
+    const float attnFilterCutoff = powerLimitsConfig()->attnFilterCutoff;
+
     // Only enforce burst >= continuous if burst is enabled (non-zero)
     // A value of 0 means "disabled/unlimited", not "zero amps allowed"
     if (currentBatteryProfile->powerLimits.burstCurrent > 0 &&
@@ -86,8 +88,8 @@ void powerLimiterInit(void) {
     burstCurrentReserve = burstCurrentReserveMax = currentBurstOverContinuous * currentBatteryProfile->powerLimits.burstCurrentTime * 1e6;
     burstCurrentReserveFalldown = currentBurstOverContinuous * currentBatteryProfile->powerLimits.burstCurrentFalldownTime * 1e6;
 
-    pt1FilterInit(&currentThrAttnFilter, powerLimitsConfig()->attnFilterCutoff, 0);
-    pt1FilterInitRC(&currentThrLimitingBaseFilter, LIMITING_THR_FILTER_TCONST, 0);
+    pt1FilterSetCutoff(&currentThrAttnFilter, attnFilterCutoff);
+    pt1FilterSetTimeConstant(&currentThrLimitingBaseFilter, LIMITING_THR_FILTER_TCONST);
 
 #ifdef USE_ADC
     // Only enforce burst >= continuous if burst is enabled (non-zero)
@@ -102,8 +104,8 @@ void powerLimiterInit(void) {
     burstPowerReserve = burstPowerReserveMax = powerBurstOverContinuous * currentBatteryProfile->powerLimits.burstPowerTime * 1e6;
     burstPowerReserveFalldown = powerBurstOverContinuous * currentBatteryProfile->powerLimits.burstPowerFalldownTime * 1e6;
 
-    pt1FilterInit(&powerThrAttnFilter, powerLimitsConfig()->attnFilterCutoff, 0);
-    pt1FilterInitRC(&powerThrLimitingBaseFilter, LIMITING_THR_FILTER_TCONST, 0);
+    pt1FilterSetCutoff(&powerThrAttnFilter, attnFilterCutoff);
+    pt1FilterSetTimeConstant(&powerThrLimitingBaseFilter, LIMITING_THR_FILTER_TCONST);
 #endif
 }
 
@@ -172,9 +174,8 @@ void powerLimiterApply(int16_t *throttleCommand) {
 
     float currentThrAttnProportional = MAX(0, overCurrent) * powerLimitsConfig()->piP * 1e-3f;
 
-    uint16_t currentThrAttn = lrintf(pt1FilterApply3(&currentThrAttnFilter, currentThrAttnProportional + currentThrAttnIntegrator, callTimeDelta * 1e-6f));
-
-    throttleBase = wasLimitingCurrent ? lrintf(pt1FilterApply3(&currentThrLimitingBaseFilter, *throttleCommand, callTimeDelta * 1e-6f)) : *throttleCommand;
+    uint16_t currentThrAttn = lrintf(pt1FilterApply3(&currentThrAttnFilter, currentThrAttnProportional + currentThrAttnIntegrator, US2S(callTimeDelta)));
+    throttleBase = wasLimitingCurrent ? lrintf(pt1FilterApply3(&currentThrLimitingBaseFilter, *throttleCommand, US2S(callTimeDelta))) : *throttleCommand;
     uint16_t currentThrAttned = MAX(PWM_RANGE_MIN, (int16_t)throttleBase - currentThrAttn);
 
     if (activeCurrentLimit && currentThrAttned < *throttleCommand) {
@@ -201,9 +202,8 @@ void powerLimiterApply(int16_t *throttleCommand) {
 
     float powerThrAttnProportional = MAX(0, overPower) * powerLimitsConfig()->piP / voltage * 1e-1f;
 
-    uint16_t powerThrAttn = lrintf(pt1FilterApply3(&powerThrAttnFilter, powerThrAttnProportional + powerThrAttnIntegrator, callTimeDelta * 1e-6f));
-
-    throttleBase = wasLimitingPower ? lrintf(pt1FilterApply3(&powerThrLimitingBaseFilter, *throttleCommand, callTimeDelta * 1e-6)) : *throttleCommand;
+    uint16_t powerThrAttn = lrintf(pt1FilterApply3(&powerThrAttnFilter, powerThrAttnProportional + powerThrAttnIntegrator, US2S(callTimeDelta)));
+    throttleBase = wasLimitingPower ? lrintf(pt1FilterApply3(&powerThrLimitingBaseFilter, *throttleCommand, US2S(callTimeDelta))) : *throttleCommand;
     uint16_t powerThrAttned = MAX(PWM_RANGE_MIN, (int16_t)throttleBase - powerThrAttn);
 
     if (activePowerLimit && powerThrAttned < *throttleCommand) {
diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c
index 9f6eb4851a7..855eb57e494 100755
--- a/src/main/flight/servos.c
+++ b/src/main/flight/servos.c
@@ -192,6 +192,10 @@ void servosInit(void)
     for (uint8_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
         servoComputeScalingFactors(i);
     }
+
+    // Initialise filters
+    pt1FilterSetCutoff(&rotRateFilter, SERVO_AUTOTRIM_FILTER_CUTOFF);
+    pt1FilterSetCutoff(&targetRateFilter, SERVO_AUTOTRIM_FILTER_CUTOFF);
 }
 
 int getServoCount(void)
@@ -206,7 +210,7 @@ int getServoCount(void)
 
 void loadCustomServoMixer(void)
 {
-    
+
     //move the rate filter to new servo rules
     int movefilterCount = 0;
     static servoMixerSwitch_t servoMixerSwitchHelper[MAX_SERVO_RULES_SWITCH_CARRY]; // helper to keep track of servoSpeedLimitFilter of servo rules
@@ -603,12 +607,6 @@ void processServoAutotrimMode(void)
     }
 }
 
-#define SERVO_AUTOTRIM_FILTER_CUTOFF    1       // LPF cutoff frequency
-#define SERVO_AUTOTRIM_CENTER_MIN       1300
-#define SERVO_AUTOTRIM_CENTER_MAX       1700
-#define SERVO_AUTOTRIM_UPDATE_SIZE      5
-#define SERVO_AUTOTRIM_ATTITUDE_LIMIT   50       // 5 degrees
-
 void processContinuousServoAutotrim(const float dT)
 {
     static timeMs_t lastUpdateTimeMs;
@@ -616,8 +614,8 @@ void processContinuousServoAutotrim(const float dT)
     static uint32_t servoMiddleUpdateCount;
     static float prevAxisIterm[2] = {0};  // Track previous I-term for rate-of-change calculation
 
-    const float rotRateMagnitudeFiltered = pt1FilterApply4(&rotRateFilter, fast_fsqrtf(vectorNormSquared(&imuMeasuredRotationBF)), SERVO_AUTOTRIM_FILTER_CUTOFF, dT);
-    const float targetRateMagnitudeFiltered = pt1FilterApply4(&targetRateFilter, getTotalRateTarget(), SERVO_AUTOTRIM_FILTER_CUTOFF, dT);
+    const float rotRateMagnitudeFiltered = pt1FilterApply3(&rotRateFilter, fast_fsqrtf(vectorNormSquared(&imuMeasuredRotationBF)), dT);
+    const float targetRateMagnitudeFiltered = pt1FilterApply3(&targetRateFilter, getTotalRateTarget(), dT);
 
     if (ARMING_FLAG(ARMED)) {
         trimState = AUTOTRIM_COLLECTING;
diff --git a/src/main/flight/servos.h b/src/main/flight/servos.h
index b16dd7ca915..1699f50903a 100644
--- a/src/main/flight/servos.h
+++ b/src/main/flight/servos.h
@@ -161,6 +161,12 @@ typedef struct servoParam_s {
 
 PG_DECLARE_ARRAY(servoParam_t, MAX_SUPPORTED_SERVOS, servoParams);
 
+#define SERVO_AUTOTRIM_FILTER_CUTOFF    1.0f     // LPF cutoff frequency
+#define SERVO_AUTOTRIM_CENTER_MIN       1300
+#define SERVO_AUTOTRIM_CENTER_MAX       1700
+#define SERVO_AUTOTRIM_UPDATE_SIZE      5
+#define SERVO_AUTOTRIM_ATTITUDE_LIMIT   50       // 5 degrees
+
 typedef struct servoConfig_s {
     // PWM values, in milliseconds, common range is 1000-2000 (1ms to 2ms)
     uint16_t servoCenterPulse;              // This is the value for servos when they should be in the middle. e.g. 1500.
diff --git a/src/main/io/osd.c b/src/main/io/osd.c
index 8ea42ade97e..fc1da5a28eb 100644
--- a/src/main/io/osd.c
+++ b/src/main/io/osd.c
@@ -127,7 +127,7 @@
 #define VIDEO_BUFFER_CHARS_HDZERO 900
 #define VIDEO_BUFFER_CHARS_DJIWTF 1320
 
-#define GFORCE_FILTER_TC 0.2
+#define GFORCE_FILTER_T_CUT_HZ 0.8f
 
 #define OSD_STATS_SINGLE_PAGE_MIN_ROWS 18
 #define IS_HI(X)  (rxGetChannelValue(X) > 1750)
@@ -1238,7 +1238,8 @@ uint16_t osdGetRemainingGlideTime(void) {
     const  timeMs_t curTimeMs = millis();
     static timeMs_t glideTimeUpdatedMs;
 
-    value = pt1FilterApply4(&glideTimeFilterState, isnormal(value) ? value : 0, 0.5, MS2S(curTimeMs - glideTimeUpdatedMs));
+    if (!glideTimeFilterState.RC) pt1FilterSetCutoff(&glideTimeFilterState, 0.5f);
+    value = pt1FilterApply3(&glideTimeFilterState, isnormal(value) ? value : 0, MS2S(curTimeMs - glideTimeUpdatedMs));
     glideTimeUpdatedMs = curTimeMs;
 
     if (value < 0) {
@@ -1786,7 +1787,8 @@ static int32_t osdUpdateEfficiencyFilter(pt1Filter_t *state, timeUs_t *lastUpdat
     timeUs_t currentTimeUs = micros();
     timeDelta_t delta = cmpTimeUs(currentTimeUs, *lastUpdated);
     if (delta >= EFFICIENCY_UPDATE_INTERVAL) {
-        pt1FilterApply4(state, rawValue, 1, US2S(delta));
+        if (!state->RC) pt1FilterSetCutoff(state, 1.0f);
+        pt1FilterApply3(state, rawValue, US2S(delta));
         *lastUpdated = currentTimeUs;
     }
     return (int32_t)state->state;
@@ -1996,7 +1998,9 @@ static bool osdDrawSingleElement(uint8_t item)
             const timeMs_t currentTimeMs = millis();
             static timeMs_t gsUpdatedTimeMs;
             float glideSlope = horizontalSpeed / sinkRate;
-            glideSlope = pt1FilterApply4(&gsFilterState, isnormal(glideSlope) ? glideSlope : 200, 0.5, MS2S(currentTimeMs - gsUpdatedTimeMs));
+
+            if (!gsFilterState.RC) pt1FilterSetCutoff(&gsFilterState, 0.5f);
+            glideSlope = pt1FilterApply3(&gsFilterState, isnormal(glideSlope) ? glideSlope : 200, MS2S(currentTimeMs - gsUpdatedTimeMs));
             gsUpdatedTimeMs = currentTimeMs;
 
             buff[0] = SYM_GLIDESLOPE;
@@ -2216,7 +2220,7 @@ static bool osdDrawSingleElement(uint8_t item)
     case OSD_GPS_EXTRA_STATS:
         {
             gpsSetOsdMonRfWidgetEnabled(true);
-            
+
             // Collect satellite stats grouped by GNSS ID: 0,2,3,6
             uint8_t stats[4][2] = { {0,0}, {0,0}, {0,0}, {0,0} }; // [group][0]=total, [group][1]=good
             for (int i = 0; i < UBLOX_MAX_SIGNALS; ++i) {
@@ -3096,8 +3100,9 @@ static bool osdDrawSingleElement(uint8_t item)
             timeDelta_t vEfficiencyTimeDelta = cmpTimeUs(currentTimeUs, vEfficiencyUpdated);
             if (getEstimatedActualVelocity(Z) > 0) {
                 if (vEfficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) {
-                                                            // Centiamps (kept for osdFormatCentiNumber) / m/s - Will appear as A / m/s in OSD
-                    value = pt1FilterApply4(&veFilterState, (float)getAmperage() / (getEstimatedActualVelocity(Z) / 100.0f), 1, US2S(vEfficiencyTimeDelta));
+                    // getAmperage() Centiamps (kept for osdFormatCentiNumber) / m/s - Will appear as A / m/s in OSD
+                    if (!veFilterState.RC) pt1FilterSetCutoff(&veFilterState, 1.0f);
+                    value = pt1FilterApply3(&veFilterState, (float)getAmperage() / (getEstimatedActualVelocity(Z) / 100.0f), US2S(vEfficiencyTimeDelta));
 
                     vEfficiencyUpdated = currentTimeUs;
                 } else {
@@ -4418,7 +4423,7 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig)
 
     osdLayoutsConfig->item_pos[0][OSD_MISSION] = OSD_POS(0, 10);
     osdLayoutsConfig->item_pos[0][OSD_GPS_SATS] = OSD_POS(0, 11) | OSD_VISIBLE_FLAG;
-    osdLayoutsConfig->item_pos[0][OSD_GPS_HDOP] = OSD_POS(0, 10);    
+    osdLayoutsConfig->item_pos[0][OSD_GPS_HDOP] = OSD_POS(0, 10);
     osdLayoutsConfig->item_pos[0][OSD_GPS_EXTRA_STATS] = OSD_POS(3, 11);
 
     osdLayoutsConfig->item_pos[0][OSD_GPS_LAT] = OSD_POS(0, 12);
@@ -5716,26 +5721,28 @@ static void osdShowArmed(void)
     }
 }
 
-static void osdFilterData(timeUs_t currentTimeUs) {
+static void osdFilterData(timeUs_t currentTimeUs)
+{
     static timeUs_t lastRefresh = 0;
     float refresh_dT = US2S(cmpTimeUs(currentTimeUs, lastRefresh));
 
     GForce = fast_fsqrtf(vectorNormSquared(&imuMeasuredAccelBF)) / GRAVITY_MSS;
-    for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; ++axis) GForceAxis[axis] = imuMeasuredAccelBF.v[axis] / GRAVITY_MSS;
+    for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+        GForceAxis[axis] = imuMeasuredAccelBF.v[axis] / GRAVITY_MSS;
+    }
 
     if (lastRefresh) {
         GForce = pt1FilterApply3(&GForceFilter, GForce, refresh_dT);
-        for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; ++axis) pt1FilterApply3(GForceFilterAxis + axis, GForceAxis[axis], refresh_dT);
+        for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+            GForceAxis[axis] = pt1FilterApply3(&GForceFilterAxis[axis], GForceAxis[axis], refresh_dT);
+        }
     } else {
-        pt1FilterInitRC(&GForceFilter, GFORCE_FILTER_TC, 0);
-        pt1FilterReset(&GForceFilter, GForce);
+        pt1FilterSetCutoff(&GForceFilter, GFORCE_FILTER_T_CUT_HZ);
 
-        for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; ++axis) {
-            pt1FilterInitRC(GForceFilterAxis + axis, GFORCE_FILTER_TC, 0);
-            pt1FilterReset(GForceFilterAxis + axis, GForceAxis[axis]);
+        for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+            pt1FilterSetCutoff(&GForceFilterAxis[axis], GFORCE_FILTER_T_CUT_HZ);
         }
     }
-
     lastRefresh = currentTimeUs;
 }
 
diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c
index a8adf663d57..086c9a6965f 100644
--- a/src/main/io/osd_dji_hd.c
+++ b/src/main/io/osd_dji_hd.c
@@ -785,8 +785,8 @@ static void osdDJIEfficiencyMahPerKM(char *buff)
 #endif
         ) && gpsSol.groundSpeed > 0) {
         if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) {
-            value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f,
-                1, US2S(efficiencyTimeDelta));
+            if (!eFilterState.RC) pt1FilterSetCutoff(&eFilterState, 1.0f);
+            value = pt1FilterApply3(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f, US2S(efficiencyTimeDelta));
 
             efficiencyUpdated = currentTimeUs;
         } else {
diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c
index 2051f64232f..92ebb699f89 100755
--- a/src/main/navigation/navigation_fixedwing.c
+++ b/src/main/navigation/navigation_fixedwing.c
@@ -96,6 +96,9 @@ static float getPitchToThrottleSmoothnessCutoffFreq(float baseFreq)
 /*-----------------------------------------------------------
  * Altitude controller
  *-----------------------------------------------------------*/
+static pt1Filter_t pitchFilterState;
+static pt1Filter_t pitchToThrFilterState;
+
 void setupFixedWingAltitudeController(void)
 {
     // TODO
@@ -107,6 +110,11 @@ void resetFixedWingAltitudeController(void)
     posControl.rcAdjustment[PITCH] = 0;
     isPitchAdjustmentValid = false;
     throttleSpeedAdjustment = 0;
+
+    pt1FilterSetCutoff(&pitchFilterState, getSmoothnessCutoffFreq(NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ));
+    pt1FilterReset(&pitchFilterState, 0.0f);
+    pt1FilterSetCutoff(&pitchToThrFilterState, getPitchToThrottleSmoothnessCutoffFreq(NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ));
+    pt1FilterReset(&pitchToThrFilterState, 0.0f);
 }
 
 bool adjustFixedWingAltitudeFromRCInput(void)
@@ -131,7 +139,6 @@ bool adjustFixedWingAltitudeFromRCInput(void)
 // Position to velocity controller for Z axis
 static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
 {
-    static pt1Filter_t pitchFilterState;
 
     float desiredClimbRate = getDesiredClimbRate(posControl.desiredState.pos.z, deltaMicros);
 
@@ -198,7 +205,7 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
     float targetPitchAngle = navPidApply2(&posControl.pids.fw_alt, targetValue, measuredValue, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, 0);
 
     // Apply low-pass filter to prevent rapid correction
-    targetPitchAngle = pt1FilterApply4(&pitchFilterState, targetPitchAngle, getSmoothnessCutoffFreq(NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ), US2S(deltaMicros));
+    targetPitchAngle = pt1FilterApply3(&pitchFilterState, targetPitchAngle, US2S(deltaMicros));
 
     // Reconstrain pitch angle (> 0 - climb, < 0 - dive)
     targetPitchAngle = constrainf(targetPitchAngle, minDiveDeciDeg, maxClimbDeciDeg);
@@ -250,10 +257,11 @@ bool adjustFixedWingHeadingFromRCInput(void)
 }
 
 /*-----------------------------------------------------------
- * XY-position controller for multicopter aircraft
+ * XY-position controller
  *-----------------------------------------------------------*/
 static fpVector3_t virtualDesiredPosition;
 static pt1Filter_t fwPosControllerCorrectionFilterState;
+static pt1Filter_t fwCrossTrackErrorRateFilterState;
 
 /*
  * TODO Currently this function resets both FixedWing and Rover & Boat position controller
@@ -271,6 +279,9 @@ void resetFixedWingPositionController(void)
     isRollAdjustmentValid = false;
     isYawAdjustmentValid = false;
 
+    pt1FilterSetCutoff(&fwCrossTrackErrorRateFilterState, 3.0f);
+    pt1FilterReset(&fwCrossTrackErrorRateFilterState, 0.0f);
+    pt1FilterSetCutoff(&fwPosControllerCorrectionFilterState, getSmoothnessCutoffFreq(NAV_FW_BASE_ROLL_CUTOFF_FREQUENCY_HZ));
     pt1FilterReset(&fwPosControllerCorrectionFilterState, 0.0f);
 }
 
@@ -469,14 +480,13 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
             static float crossTrackErrorRate;
             static timeUs_t previousCrossTrackErrorUpdateTime;
             static float previousCrossTrackError = 0.0f;
-            static pt1Filter_t fwCrossTrackErrorRateFilterState;
 
             if ((currentTimeUs - previousCrossTrackErrorUpdateTime) >= HZ2US(20) && fabsf(previousCrossTrackError - navCrossTrackError) > 10.0f) {
                 const float crossTrackErrorDtSec =  US2S(currentTimeUs - previousCrossTrackErrorUpdateTime);
                 if (fabsf(previousCrossTrackError - navCrossTrackError) < 500.0f) {
                     crossTrackErrorRate = (previousCrossTrackError - navCrossTrackError) / crossTrackErrorDtSec;
                 }
-                crossTrackErrorRate = pt1FilterApply4(&fwCrossTrackErrorRateFilterState, crossTrackErrorRate, 3.0f, crossTrackErrorDtSec);
+                crossTrackErrorRate = pt1FilterApply3(&fwCrossTrackErrorRateFilterState, crossTrackErrorRate, crossTrackErrorDtSec);
                 previousCrossTrackErrorUpdateTime = currentTimeUs;
                 previousCrossTrackError = navCrossTrackError;
             }
@@ -539,7 +549,7 @@ static void updatePositionHeadingController_FW(timeUs_t currentTimeUs, timeDelta
                                         pidFlags);
 
     // Apply low-pass filter to prevent rapid correction
-    rollAdjustment = pt1FilterApply4(&fwPosControllerCorrectionFilterState, rollAdjustment, getSmoothnessCutoffFreq(NAV_FW_BASE_ROLL_CUTOFF_FREQUENCY_HZ), US2S(deltaMicros));
+    rollAdjustment = pt1FilterApply3(&fwPosControllerCorrectionFilterState, rollAdjustment, US2S(deltaMicros));
 
     // Convert rollAdjustment to decidegrees (rcAdjustment holds decidegrees)
     posControl.rcAdjustment[ROLL] = CENTIDEGREES_TO_DECIDEGREES(rollAdjustment);
@@ -638,10 +648,9 @@ int16_t fixedWingPitchToThrottleCorrection(int16_t pitch, timeUs_t currentTimeUs
     const timeDeltaLarge_t deltaMicrosPitchToThrCorr = currentTimeUs -  previousTimePitchToThrCorr;
     previousTimePitchToThrCorr = currentTimeUs;
 
-    static pt1Filter_t pitchToThrFilterState;
 
     // Apply low-pass filter to pitch angle to smooth throttle correction
-    int16_t filteredPitch = (int16_t)pt1FilterApply4(&pitchToThrFilterState, pitch, getPitchToThrottleSmoothnessCutoffFreq(NAV_FW_BASE_PITCH_CUTOFF_FREQUENCY_HZ), US2S(deltaMicrosPitchToThrCorr));
+    int16_t filteredPitch = pt1FilterApply3(&pitchToThrFilterState, pitch, US2S(deltaMicrosPitchToThrCorr));
 
     int16_t pitchToThrottle = currentBatteryProfile->nav.fw.pitch_to_throttle;
 
diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c
index 5d4b7a5b425..d3099eb2465 100644
--- a/src/main/navigation/navigation_multicopter.c
+++ b/src/main/navigation/navigation_multicopter.c
@@ -112,8 +112,8 @@ static void updateAltitudeThrottleController_MC(timeDelta_t deltaMicros)
     const int16_t thrCorrectionMax = getMaxThrottle() - currentBatteryProfile->nav.mc.hover_throttle;
 
     float velocity_controller = navPidApply2(&posControl.pids.vel[Z], posControl.desiredState.vel.z, navGetCurrentActualPositionAndVelocity()->vel.z, US2S(deltaMicros), thrCorrectionMin, thrCorrectionMax, 0);
-
-    int16_t rcThrottleCorrection = pt1FilterApply4(&altholdThrottleFilterState, velocity_controller, NAV_THROTTLE_CUTOFF_FREQENCY_HZ, US2S(deltaMicros));
+    
+    int16_t rcThrottleCorrection = pt1FilterApply3(&altholdThrottleFilterState, velocity_controller, US2S(deltaMicros));
     rcThrottleCorrection = constrain(rcThrottleCorrection, thrCorrectionMin, thrCorrectionMax);
 
     posControl.rcAdjustment[THROTTLE] = setDesiredThrottle(currentBatteryProfile->nav.mc.hover_throttle + rcThrottleCorrection, false);
@@ -212,6 +212,7 @@ void resetMulticopterAltitudeController(void)
 
     posControl.desiredState.vel.z = posToUse->vel.z;   // Gradually transition from current climb
 
+    pt1FilterSetCutoff(&altholdThrottleFilterState, NAV_THROTTLE_CUTOFF_FREQENCY_HZ);
     pt1FilterReset(&altholdThrottleFilterState, 0.0f);
     pt1FilterReset(&posControl.pids.vel[Z].error_filter_state, 0.0f);
     pt1FilterReset(&posControl.pids.vel[Z].dterm_filter_state, 0.0f);
diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c
index 188ab7d25e3..57a74f74600 100644
--- a/src/main/navigation/navigation_pos_estimator.c
+++ b/src/main/navigation/navigation_pos_estimator.c
@@ -57,6 +57,10 @@
 navigationPosEstimator_t posEstimator;
 static float initialBaroAltitudeOffset = 0.0f;
 
+static pt1Filter_t estVelFilterState_X;
+static pt1Filter_t estVelFilterState_Y;
+static pt1Filter_t estVelFilterState_Z;
+
 PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 8);
 
 PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig,
@@ -887,11 +891,8 @@ static void publishEstimatedTopic(timeUs_t currentTimeUs)
 
         /* Publish position update */
         if (posEstimator.est.eph < positionEstimationConfig()->max_eph_epv) {
-            static pt1Filter_t estVelFilterState_X;
-            static pt1Filter_t estVelFilterState_Y;
-            float filteredVelX = pt1FilterApply4(&estVelFilterState_X, posEstimator.est.vel.x, INAV_EST_VEL_F_CUT_HZ, HZ2S(INAV_POSITION_PUBLISH_RATE_HZ));
-            float filteredVelY = pt1FilterApply4(&estVelFilterState_Y, posEstimator.est.vel.y, INAV_EST_VEL_F_CUT_HZ, HZ2S(INAV_POSITION_PUBLISH_RATE_HZ));
-
+            float filteredVelX = pt1FilterApply3(&estVelFilterState_X, posEstimator.est.vel.x, HZ2S(INAV_POSITION_PUBLISH_RATE_HZ));
+            float filteredVelY = pt1FilterApply3(&estVelFilterState_Y, posEstimator.est.vel.y, HZ2S(INAV_POSITION_PUBLISH_RATE_HZ));
             // FIXME!!!!!
             updateActualHorizontalPositionAndVelocity(true, true, posEstimator.est.pos.x, posEstimator.est.pos.y, filteredVelX, filteredVelY);
         }
@@ -901,8 +902,7 @@ static void publishEstimatedTopic(timeUs_t currentTimeUs)
 
         /* Publish altitude update and set altitude validity */
         if (posEstimator.est.epv < positionEstimationConfig()->max_eph_epv) {
-            static pt1Filter_t estVelFilterState_Z;
-            float filteredVelZ = pt1FilterApply4(&estVelFilterState_Z, posEstimator.est.vel.z, INAV_EST_VEL_F_CUT_HZ, HZ2S(INAV_POSITION_PUBLISH_RATE_HZ));
+            float filteredVelZ = pt1FilterApply3(&estVelFilterState_Z, posEstimator.est.vel.z, HZ2S(INAV_POSITION_PUBLISH_RATE_HZ));
 
             const float gpsCfEstimatedAltitudeError = STATE(GPS_FIX) ? posEstimator.gps.pos.z - posEstimator.est.pos.z : 0;
             navigationEstimateStatus_e aglStatus = (posEstimator.est.aglQual == SURFACE_QUAL_LOW) ? EST_USABLE : EST_TRUSTED;
@@ -973,8 +973,13 @@ void initializePositionEstimator(void)
         posEstimator.est.vel.v[axis] = 0;
     }
 
-    pt1FilterInit(&posEstimator.baro.avgFilter, INAV_BARO_AVERAGE_HZ, 0.0f);
-    pt1FilterInit(&posEstimator.surface.avgFilter, INAV_SURFACE_AVERAGE_HZ, 0.0f);
+    // Filters
+    pt1FilterSetCutoff(&posEstimator.baro.avgFilter, INAV_BARO_AVERAGE_HZ);
+    pt1FilterSetCutoff(&posEstimator.surface.avgFilter, INAV_SURFACE_AVERAGE_HZ);
+
+    pt1FilterSetCutoff(&estVelFilterState_X, INAV_EST_VEL_F_CUT_HZ);
+    pt1FilterSetCutoff(&estVelFilterState_Y, INAV_EST_VEL_F_CUT_HZ);
+    pt1FilterSetCutoff(&estVelFilterState_Z, INAV_EST_VEL_F_CUT_HZ);
 }
 
 /**
diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h
index a1f07e470c0..6f076659caf 100644
--- a/src/main/navigation/navigation_private.h
+++ b/src/main/navigation/navigation_private.h
@@ -28,7 +28,7 @@
 #include "navigation/navigation.h"
 
 #define MIN_POSITION_UPDATE_RATE_HZ         5       // Minimum position update rate at which XYZ controllers would be applied
-#define NAV_THROTTLE_CUTOFF_FREQENCY_HZ     4       // low-pass filter on throttle output
+#define NAV_THROTTLE_CUTOFF_FREQENCY_HZ     4.0f    // low-pass filter on throttle output
 #define NAV_FW_CONTROL_MONITORING_RATE      2
 #define NAV_DTERM_CUT_HZ                    10.0f
 #define NAV_VEL_Z_DERIVATIVE_CUT_HZ         5.0f
diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c
index 699b502645d..da8a6237bd0 100644
--- a/src/main/sensors/battery.c
+++ b/src/main/sensors/battery.c
@@ -103,6 +103,8 @@ static int32_t power = 0;                       // power draw in cW (0.01W resol
 static int32_t mAhDrawn = 0;                    // milliampere hours drawn from the battery since start
 static int32_t mWhDrawn = 0;                    // energy (milliWatt hours) drawn from the battery since start
 
+static pt1Filter_t amperageFilterState;
+
 batteryState_e batteryState;
 const batteryProfile_t *currentBatteryProfile;
 
@@ -211,6 +213,8 @@ void batteryInit(void)
     batteryFullVoltage = 0;
     batteryWarningVoltage = 0;
     batteryCriticalVoltage = 0;
+
+    pt1FilterSetCutoff(&amperageFilterState, AMPERAGE_LPF_FREQ);
 }
 
 #ifdef USE_ADC
@@ -344,9 +348,10 @@ static void updateBatteryVoltage(timeUs_t timeDelta, bool justConnected)
 #endif
 
     if (justConnected) {
+        pt1FilterSetCutoff(&vbatFilterState, VBATT_LPF_FREQ);
         pt1FilterReset(&vbatFilterState, vbat);
     } else {
-        vbat = pt1FilterApply4(&vbatFilterState, vbat, VBATT_LPF_FREQ, US2S(timeDelta));
+        vbat = pt1FilterApply3(&vbatFilterState, vbat, US2S(timeDelta));
     }
 }
 
@@ -600,13 +605,12 @@ int32_t getMWhDrawn(void)
 
 void currentMeterUpdate(timeUs_t timeDelta)
 {
-    static pt1Filter_t amperageFilterState;
     static int64_t mAhdrawnRaw = 0;
 
     switch (batteryMetersConfig()->current.type) {
         case CURRENT_SENSOR_ADC:
             {
-                amperage = pt1FilterApply4(&amperageFilterState, getAmperageSample(), AMPERAGE_LPF_FREQ, US2S(timeDelta));
+                amperage = pt1FilterApply3(&amperageFilterState, getAmperageSample(), US2S(timeDelta));
                 break;
             }
         case CURRENT_SENSOR_VIRTUAL:
@@ -631,7 +635,7 @@ void currentMeterUpdate(timeUs_t timeDelta)
             {
                 escSensorData_t * escSensor = escSensorGetData();
                 if (escSensor && escSensor->dataAge <= ESC_DATA_MAX_AGE) {
-                    amperage = pt1FilterApply4(&amperageFilterState, escSensor->current, AMPERAGE_LPF_FREQ, US2S(timeDelta));
+                    amperage = pt1FilterApply3(&amperageFilterState, escSensor->current, US2S(timeDelta));
                 }
                 else {
                     amperage = 0;
@@ -755,7 +759,7 @@ void sagCompensatedVBatUpdate(timeUs_t currentTime, timeUs_t timeDelta)
             }
 
             if (impedanceFilterState.state) {
-                pt1FilterSetTimeConstant(&impedanceFilterState, impedanceSampleCount > IMPEDANCE_STABLE_SAMPLE_COUNT_THRESH ? 1.2 : 0.5);
+                pt1FilterSetTimeConstant(&impedanceFilterState, impedanceSampleCount > IMPEDANCE_STABLE_SAMPLE_COUNT_THRESH ? 1.2f : 0.5f);
                 pt1FilterApply3(&impedanceFilterState, impedanceSample, US2S(timeDelta));
             } else {
                 pt1FilterReset(&impedanceFilterState, impedanceSample);
@@ -769,7 +773,7 @@ void sagCompensatedVBatUpdate(timeUs_t currentTime, timeUs_t timeDelta)
         }
 
         uint16_t sagCompensatedVBatSample = MIN(batteryFullVoltage, vbat + (int32_t)powerSupplyImpedance * amperage / 1000);
-        pt1FilterSetTimeConstant(&sagCompVBatFilterState, sagCompensatedVBatSample < pt1FilterGetLastOutput(&sagCompVBatFilterState) ? 40 : 500);
+        pt1FilterSetTimeConstant(&sagCompVBatFilterState, sagCompensatedVBatSample < pt1FilterGetLastOutput(&sagCompVBatFilterState) ? 40.0f : 500.0f);
         sagCompensatedVBat = lrintf(pt1FilterApply3(&sagCompVBatFilterState, sagCompensatedVBatSample, US2S(timeDelta)));
     }
 
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index 6507abcce30..f55ba1292e2 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -336,7 +336,7 @@ bool gyroInit(void)
 
     // initFn will initialize sampleRateIntervalUs to actual gyro sampling rate (if driver supports it). Calculate target looptime using that value
     gyro.targetLooptime = gyroDev[0].sampleRateIntervalUs;
- 
+
     gyroInitFilters();
 
 #ifdef USE_DYNAMIC_FILTERS
@@ -515,7 +515,7 @@ void FAST_CODE NOINLINE gyroFilter(void)
         }
 
         /**
-         * Secondary dynamic notch filter. 
+         * Secondary dynamic notch filter.
          * In some cases, noise amplitude is high enough not to be filtered by the primary filter.
          * This happens on the first frequency with the biggest aplitude
          */
@@ -544,7 +544,7 @@ void FAST_CODE NOINLINE gyroFilter(void)
             );
 
             secondaryDynamicGyroNotchFiltersUpdate(
-                &secondaryDynamicGyroNotchState, 
+                &secondaryDynamicGyroNotchState,
                 gyroAnalyseState.filterUpdateAxis,
                 gyroAnalyseState.centerFrequency[gyroAnalyseState.filterUpdateAxis]
             );
@@ -622,7 +622,7 @@ int16_t gyroRateDps(int axis)
 
 void gyroUpdateDynamicLpf(float cutoffFreq) {
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-        pt1FilterUpdateCutoff(&gyroLpf2State[axis].pt1, cutoffFreq);
+        pt1FilterSetCutoff(&gyroLpf2State[axis].pt1, cutoffFreq);
     }
 }
 
diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c
index 3618923614a..edd6a62ae9e 100755
--- a/src/main/sensors/pitotmeter.c
+++ b/src/main/sensors/pitotmeter.c
@@ -241,8 +241,10 @@ STATIC_PROTOTHREAD(pitotThread)
 
     // Init filter
     pitot.lastMeasurementUs = micros();
-    if (pitotmeterConfig()->pitot_lpf_milli_hz > 0) {
-        pt1FilterInit(&pitot.lpfState, pitotmeterConfig()->pitot_lpf_milli_hz / 1000.0f, 0.0f);
+
+    const uint16_t pitot_lpf_milli_hz = pitotmeterConfig()->pitot_lpf_milli_hz;
+    if (pitot_lpf_milli_hz > 0) {
+        pt1FilterSetCutoff(&pitot.lpfState, pitot_lpf_milli_hz);
     }
 
     while(1) {
@@ -296,7 +298,8 @@ STATIC_PROTOTHREAD(pitotThread)
 
             // NOTE ::filter pressure - apply filter when NOT calibrating for zero !!!
             currentTimeUs = micros();
-            if (pitotmeterConfig()->pitot_lpf_milli_hz > 0) {
+
+            if (pitotmeterConfig()->pitot_lpf_milli_hz) {
                 pitot.pressure = pt1FilterApply3(&pitot.lpfState, pitotPressureTmp, US2S(currentTimeUs - pitot.lastMeasurementUs));
             } else {
                 pitot.pressure = pitotPressureTmp;