diff --git a/src/main/common/filter.c b/src/main/common/filter.c
index c0b7a7f049..d5a7007a69 100644
--- a/src/main/common/filter.c
+++ b/src/main/common/filter.c
@@ -24,16 +24,75 @@
 #include "common/filter.h"
 #include "common/maths.h"
 
-// PT1 Low Pass filter (when no dT specified it will be calculated from the cycleTime)
-float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float dT) {
+#include "drivers/gyro_sync.h"
 
+#define BIQUAD_BANDWIDTH 1.9f     /* bandwidth in octaves */
+
+/* sets up a biquad Filter */
+void filterInitBiQuad(uint8_t filterCutFreq, biquad_t *newState, int16_t samplingRate)
+{
+    float omega, sn, cs, alpha;
+    float a0, a1, a2, b0, b1, b2;
+
+    /* If sampling rate == 0 - use main loop target rate */
+    if (!samplingRate) {
+        samplingRate = 1000000 / targetLooptime;
+    }
+
+    /* setup variables */
+    omega = 2 * M_PIf * (float)filterCutFreq / (float)samplingRate;
+    sn = sin_approx(omega);
+    cs = cos_approx(omega);
+    alpha = sn * sin_approx(M_LN2f / 2 * BIQUAD_BANDWIDTH * omega / sn);
+
+    b0 = (1 - cs) / 2;
+    b1 = 1 - cs;
+    b2 = (1 - cs) / 2;
+    a0 = 1 + alpha;
+    a1 = -2 * cs;
+    a2 = 1 - alpha;
+
+    /* precompute the coefficients */
+    newState->a0 = b0 / a0;
+    newState->a1 = b1 / a0;
+    newState->a2 = b2 / a0;
+    newState->a3 = a1 / a0;
+    newState->a4 = a2 / a0;
+
+    /* zero initial samples */
+    newState->x1 = newState->x2 = 0;
+    newState->y1 = newState->y2 = 0;
+}
+
+/* Computes a biquad_t filter on a sample */
+float filterApplyBiQuad(float sample, biquad_t *state)
+{
+    float result;
+
+    /* compute result */
+    result = state->a0 * sample + state->a1 * state->x1 + state->a2 * state->x2 -
+        state->a3 * state->y1 - state->a4 * state->y2;
+
+    /* shift x1 to x2, sample to x1 */
+    state->x2 = state->x1;
+    state->x1 = sample;
+
+    /* shift y1 to y2, result to y1 */
+    state->y2 = state->y1;
+    state->y1 = result;
+
+    return result;
+}
+
+// PT1 Low Pass filter (when no dT specified it will be calculated from the cycleTime)
+float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float dT)
+{
 	// Pre calculate and store RC
 	if (!filter->RC) {
 		filter->RC = 1.0f / ( 2.0f * (float)M_PI * f_cut );
 	}
 
     filter->state = filter->state + dT / (filter->RC + dT) * (input - filter->state);
-
     return filter->state;
 }
 
@@ -41,59 +100,3 @@ void filterResetPt1(filterStatePt1_t *filter, float input)
 {
     filter->state = input;
 }
-
-/**
- * Typical quadcopter motor noise frequency (at 50% throttle):
- *  450-sized, 920kv, 9.4x4.3 props, 3S : 4622rpm = 77Hz
- *  250-sized, 2300kv, 5x4.5 props, 4S : 14139rpm = 235Hz
- */
-static int8_t gyroFIRCoeff_1000[3][9] = { { 0, 0, 12, 23, 40, 51, 52, 40, 38 },    // looptime=1000; group delay 2.5ms; -0.5db = 32Hz ; -1db = 45Hz; -5db = 97Hz; -10db = 132Hz
-                                          { 19, 31, 43, 48, 41, 35, 23, 8, 8},     // looptime=1000; group delay 3ms;   -0.5db = 18Hz ; -1db = 33Hz; -5db = 81Hz; -10db = 113Hz
-                                          { 18, 12, 28, 40, 44, 40, 32, 22, 20} }; // looptime=1000; group delay 4ms;   -0.5db = 23Hz ; -1db = 35Hz; -5db = 75Hz; -10db = 103Hz
-static int8_t gyroFIRCoeff_2000[3][9] = { { 0, 0, 0, 6, 24, 58, 83, 65, 20 },      // looptime=2000, group delay 4ms;   -0.5db = 21Hz ; -1db = 31Hz; -5db = 71Hz; -10db = 99Hz
-                                          { 0, 0, 14, 21, 45, 59, 55, 39, 23},     // looptime=2000, group delay 5ms;   -0.5db = 20Hz ; -1db = 26Hz; -5db = 52Hz; -10db = 71Hz
-                                          { 14, 12, 26, 38, 45, 43, 34, 24, 20} }; // looptime=2000, group delay 7ms;   -0.5db = 11Hz ; -1db = 18Hz; -5db = 38Hz; -10db = 52Hz
-static int8_t gyroFIRCoeff_3000[3][9] = { { 0, 0, 0, 0, 4, 35, 87, 87, 43 },       // looptime=3000, group delay 4.5ms; -0.5db = 18Hz ; -1db = 26Hz; -5db = 57Hz; -10db = 78Hz
-                                          { 0, 0, 0, 14, 31, 62, 70, 52, 27},      // looptime=3000, group delay 6.5ms; -0.5db = 16Hz ; -1db = 21Hz; -5db = 42Hz; -10db = 57Hz
-                                          { 0, 6, 10, 28, 44, 54, 54, 38, 22} };   // looptime=3000, group delay 9ms;   -0.5db = 10Hz ; -1db = 13Hz; -5db = 32Hz; -10db = 45Hz
-
-int8_t * filterGetFIRCoefficientsTable(uint8_t filter_level, uint16_t targetLooptime)
-{
-    if (filter_level == 0) {
-        return NULL;
-    }
-
-    int firIndex = constrain(filter_level, 1, 3) - 1;
-
-    // For looptimes faster than 1499 (and looptime=0) use filter for 1kHz looptime
-    if (targetLooptime < 1500) {
-        return gyroFIRCoeff_1000[firIndex];
-    }
-    // 1500 ... 2499
-    else if (targetLooptime < 2500) {
-        return gyroFIRCoeff_2000[firIndex];
-    }
-    // > 2500
-    else {
-        return gyroFIRCoeff_3000[firIndex];
-    }
-}
-
-// 9 Tap FIR filter as described here:
-// Thanks to Qcopter & BorisB & DigitalEntity
-void filterApply9TapFIR(int16_t data[3], int16_t state[3][9], int8_t coeff[9])
-{
-    int32_t FIRsum;
-    int axis, i;
-
-    for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-        FIRsum = 0;
-        for (i = 0; i <= 7; i++) {
-            state[axis][i] = state[axis][i + 1];
-            FIRsum += state[axis][i] * (int16_t)coeff[i];
-        }
-        state[axis][8] = data[axis];
-        FIRsum += state[axis][8] * coeff[8];
-        data[axis] = FIRsum / 256;
-    }
-}
diff --git a/src/main/common/filter.h b/src/main/common/filter.h
index 6df3d01f51..dcfec491db 100644
--- a/src/main/common/filter.h
+++ b/src/main/common/filter.h
@@ -23,7 +23,14 @@ typedef struct filterStatePt1_s {
 	float constdT;
 } filterStatePt1_t;
 
+/* this holds the data required to update samples thru a filter */
+typedef struct biquad_s {
+    float a0, a1, a2, a3, a4;
+    float x1, x2, y1, y2;
+} biquad_t;
+
 float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float dt);
 void filterResetPt1(filterStatePt1_t *filter, float input);
-int8_t * filterGetFIRCoefficientsTable(uint8_t filter_level, uint16_t targetLooptime);
-void filterApply9TapFIR(int16_t data[3], int16_t state[3][9], int8_t coeff[9]);
+
+void filterInitBiQuad(uint8_t filterCutFreq, biquad_t *newState, int16_t samplingRate);
+float filterApplyBiQuad(float sample, biquad_t *state);
diff --git a/src/main/common/maths.h b/src/main/common/maths.h
index a92cc3ff39..cd4feb3db8 100644
--- a/src/main/common/maths.h
+++ b/src/main/common/maths.h
@@ -27,6 +27,7 @@
 
 // Use floating point M_PI instead explicitly.
 #define M_PIf       3.14159265358979323846f
+#define M_LN2f      0.69314718055994530942f
 
 #define RAD    (M_PIf / 180.0f)
 
diff --git a/src/main/config/config.c b/src/main/config/config.c
index 93cd486ffa..460dfe62bf 100755
--- a/src/main/config/config.c
+++ b/src/main/config/config.c
@@ -180,12 +180,11 @@ void resetPidProfile(pidProfile_t *pidProfile)
     pidProfile->I8[PIDVEL] = 5;     // NAV_VEL_Z_I * 100
     pidProfile->D8[PIDVEL] = 100;   // NAV_VEL_Z_D * 1000
 
-    pidProfile->acc_soft_lpf = 2;    // MEDIUM filtering by default
-    pidProfile->gyro_soft_lpf = 1;   // LOW filtering by default
+    pidProfile->acc_soft_lpf_hz = 15;
+    pidProfile->gyro_soft_lpf_hz = 60;
 
     pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX;
-    pidProfile->yaw_pterm_cut_hz = 0;
-    pidProfile->dterm_cut_hz = 30;
+    pidProfile->dterm_lpf_hz = 30;
 
     pidProfile->P_f[ROLL] = 1.4f;     // new PID with preliminary defaults test carefully
     pidProfile->I_f[ROLL] = 0.3f;
@@ -764,7 +763,7 @@ void activateConfig(void)
         &currentProfile->pidProfile
     );
 
-    useGyroConfig(&masterConfig.gyroConfig, filterGetFIRCoefficientsTable(currentProfile->pidProfile.gyro_soft_lpf, masterConfig.looptime));
+    useGyroConfig(&masterConfig.gyroConfig, currentProfile->pidProfile.gyro_soft_lpf_hz);
 
 #ifdef TELEMETRY
     telemetryUseConfig(&masterConfig.telemetryConfig);
@@ -777,7 +776,7 @@ void activateConfig(void)
 
     setAccelerationZero(&masterConfig.accZero);
     setAccelerationGain(&masterConfig.accGain);
-    setAccelerationFilter(filterGetFIRCoefficientsTable(currentProfile->pidProfile.acc_soft_lpf, masterConfig.looptime));
+    setAccelerationFilter(currentProfile->pidProfile.acc_soft_lpf_hz);
 
     mixerUseConfigs(
 #ifdef USE_SERVOS
diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c
index 28a1cfcb95..2a20eac6f4 100644
--- a/src/main/flight/imu.c
+++ b/src/main/flight/imu.c
@@ -495,17 +495,14 @@ static void imuUpdateMeasuredRotationRate(void)
 }
 
 /* Calculate measured acceleration in body frame cm/s/s */
-#define ACCELERATION_LPF_HZ 10
-static void imuUpdateMeasuredAcceleration(float dT)
+static void imuUpdateMeasuredAcceleration(void)
 {
-    t_fp_vector measuredAccelerationBF;
-    static filterStatePt1_t accFilter[XYZ_AXIS_COUNT];
     int axis;
 
     /* Convert acceleration to cm/s/s */
     for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-        measuredAccelerationBF.A[axis] = accADC[axis] * (GRAVITY_CMSS / acc_1G);
-        imuMeasuredGravityBF.A[axis] = measuredAccelerationBF.A[axis];
+        imuAccelInBodyFrame.A[axis] = accADC[axis] * (GRAVITY_CMSS / acc_1G);
+        imuMeasuredGravityBF.A[axis] = imuAccelInBodyFrame.A[axis];
     }
 
 #ifdef GPS
@@ -519,11 +516,6 @@ static void imuUpdateMeasuredAcceleration(float dT)
         imuMeasuredGravityBF.A[Z] += gpsSol.groundSpeed * imuMeasuredRotationBF.A[Y];
     }
 #endif
-
-    /* Apply LPF to acceleration readings and publish imuAccelInBodyFrame */
-    for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
-        imuAccelInBodyFrame.A[axis] = filterApplyPt1(measuredAccelerationBF.A[axis], &accFilter[axis], ACCELERATION_LPF_HZ, dT);
-    }
 }
 
 #ifdef HIL
@@ -574,16 +566,16 @@ void imuUpdateGyroAndAttitude(void)
 #ifdef HIL
         if (!hilActive) {
             imuUpdateMeasuredRotationRate();    // Calculate gyro rate in body frame in rad/s
-            imuUpdateMeasuredAcceleration(dT);  // Calculate accel in body frame in cm/s/s
+            imuUpdateMeasuredAcceleration();  // Calculate accel in body frame in cm/s/s
             imuCalculateEstimatedAttitude(dT);  // Update attitude estimate
         }
         else {
             imuHILUpdate();
-            imuUpdateMeasuredAcceleration(dT);
+            imuUpdateMeasuredAcceleration();
         }
 #else
             imuUpdateMeasuredRotationRate();    // Calculate gyro rate in body frame in rad/s
-            imuUpdateMeasuredAcceleration(dT);  // Calculate accel in body frame in cm/s/s
+            imuUpdateMeasuredAcceleration();  // Calculate accel in body frame in cm/s/s
             imuCalculateEstimatedAttitude(dT);  // Update attitude estimate
 #endif
     } else {
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 8393cd5eb1..70c05838c9 100644
--- a/src/main/flight/pid.c
+++ b/src/main/flight/pid.c
@@ -29,6 +29,7 @@
 
 #include "drivers/sensor.h"
 #include "drivers/accgyro.h"
+#include "drivers/gyro_sync.h"
 
 #include "sensors/sensors.h"
 #include "sensors/gyro.h"
@@ -83,7 +84,8 @@ void pidResetErrorGyro(void)
 
 const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
 
-static filterStatePt1_t yawPTermState, DTermState[3];
+static biquad_t deltaBiQuadState[3];
+static bool deltaStateIsSet = false;
 
 static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
         uint16_t max_angle_inclination, rxConfig_t *rxConfig)
@@ -92,11 +94,16 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
     float ITerm,PTerm,DTerm;
     int32_t stickPosAil, stickPosEle, mostDeflectedPos;
     static float lastError[3];
-    static float delta1[3], delta2[3];
-    float delta, deltaSum;
+    float delta;
     int axis;
     float horizonLevelStrength = 1;
 
+    if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) {
+        for (axis = 0; axis < 3; axis++)
+            filterInitBiQuad(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], 0);
+        deltaStateIsSet = true;
+    }
+
     if (FLIGHT_MODE(HORIZON_MODE)) {
 
         // Figure out the raw stick positions
@@ -156,11 +163,6 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
         // -----calculate P component
         PTerm = RateError * pidProfile->P_f[axis] * PIDweight[axis] / 100;
 
-        // Yaw Pterm low pass
-        if (axis == YAW && pidProfile->yaw_pterm_cut_hz) {
-            PTerm = filterApplyPt1(PTerm, &yawPTermState, pidProfile->yaw_pterm_cut_hz, dT);
-        }
-
         // -----calculate I component.
         errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * 10, -250.0f, 250.0f);
 
@@ -176,23 +178,11 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
         // would be scaled by different dt each time. Division by dT fixes that.
         delta *= (1.0f / dT);
 
-        // In case of soft gyro lpf combined with dterm_cut_hz we don't need the old 3 point averaging
-        if (pidProfile->gyro_soft_lpf && pidProfile->dterm_cut_hz) {
-            deltaSum = delta;
-        } else {
-            // add moving average here to reduce noise
-            deltaSum = delta1[axis] + delta2[axis] + delta;
-            delta2[axis] = delta1[axis];
-            delta1[axis] = delta;
-            deltaSum /= 3.0f;
+        if (deltaStateIsSet) {
+            delta = filterApplyBiQuad(delta, &deltaBiQuadState[axis]);
         }
 
-        // Dterm low pass
-        if (pidProfile->dterm_cut_hz) {
-            deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
-        }
-
-        DTerm = constrainf(deltaSum * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f);
+        DTerm = constrainf(delta * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f);
 
         // -----calculate total PID output
         axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000);
@@ -216,36 +206,30 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
 {
     UNUSED(rxConfig);
 
-    int32_t errorAngle;
     int axis;
-    int32_t delta, deltaSum;
-    static int32_t delta1[3], delta2[3];
+    int32_t delta;
     int32_t PTerm, ITerm, DTerm;
     static int32_t lastError[3] = { 0, 0, 0 };
-    int32_t AngleRateTmp, RateError;
+    int32_t AngleRateTmp, RateError, gyroRate;
 
     int8_t horizonLevelStrength = 100;
-    int32_t stickPosAil, stickPosEle, mostDeflectedPos;
+
+    if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) {
+        for (axis = 0; axis < 3; axis++)
+            filterInitBiQuad(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], 0);
+        deltaStateIsSet = true;
+    }
 
     if (FLIGHT_MODE(HORIZON_MODE)) {
-
         // Figure out the raw stick positions
-        stickPosAil = getRcStickDeflection(FD_ROLL, rxConfig->midrc);
-        stickPosEle = getRcStickDeflection(FD_PITCH, rxConfig->midrc);
-
-        if(ABS(stickPosAil) > ABS(stickPosEle)){
-            mostDeflectedPos = ABS(stickPosAil);
-        }
-        else {
-            mostDeflectedPos = ABS(stickPosEle);
-        }
-
+        const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc));
+        const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc));
+        const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle);
         // Progressively turn off the horizon self level strength as the stick is banged over
         horizonLevelStrength = (500 - mostDeflectedPos) / 5;  // 100 at centre stick, 0 = max stick deflection
-
         // Using Level D as a Sensitivity for Horizon. 0 more level to 255 more rate. Default value of 100 seems to work fine.
         // For more rate mode increase D and slower flips and rolls will be possible
-       	horizonLevelStrength = constrain((10 * (horizonLevelStrength - 100) * (10 * pidProfile->D8[PIDLEVEL] / 80) / 100) + 100, 0, 100);
+        horizonLevelStrength = constrain((10 * (horizonLevelStrength - 100) * (10 * pidProfile->D8[PIDLEVEL] / 80) / 100) + 100, 0, 100);
     }
 
     // ----------PID controller----------
@@ -256,18 +240,21 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
         if (axis == FD_YAW) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand)
             AngleRateTmp = (((int32_t)(rate + 27) * rcCommand[YAW]) >> 5);
         } else {
-            // calculate error and limit the angle to max configured inclination
-            errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination),
-                    +max_angle_inclination) - attitude.raw[axis]; // 16 bits is ok here
+            AngleRateTmp = ((int32_t)(rate + 27) * rcCommand[axis]) >> 4;
 
-            if (!FLIGHT_MODE(ANGLE_MODE)) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
-                AngleRateTmp = ((int32_t)(rate + 27) * rcCommand[axis]) >> 4;
-                if (FLIGHT_MODE(HORIZON_MODE)) {
-                    // mix up angle error to desired AngleRateTmp to add a little auto-level feel. horizonLevelStrength is scaled to the stick input
-                	AngleRateTmp += (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 100) >> 4;
+            if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
+                // calculate error and limit the angle to max configured inclination
+                int32_t errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination),
+                        +max_angle_inclination) - attitude.raw[axis]; // 16 bits is ok here
+
+                if (FLIGHT_MODE(ANGLE_MODE)) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
+                    // it's the ANGLE mode - control is angle based, so control loop is needed
+                    AngleRateTmp = (errorAngle * pidProfile->P8[PIDLEVEL]) >> 4;
+                } else {
+                    // HORIZON mode - mix up angle error to desired AngleRateTmp to add a little auto-level feel,
+                    // horizonLevelStrength is scaled to the stick input
+                    AngleRateTmp += (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 100) >> 4;
                 }
-            } else { // it's the ANGLE mode - control is angle based, so control loop is needed
-                AngleRateTmp = (errorAngle * pidProfile->P8[PIDLEVEL]) >> 4;
             }
         }
 
@@ -275,22 +262,18 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
         // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
         // -----calculate scaled error.AngleRates
         // multiplication of rcCommand corresponds to changing the sticks scaling here
-        RateError = AngleRateTmp - (gyroADC[axis] / 4);
+        gyroRate = gyroADC[axis] / 4;
+        RateError = AngleRateTmp - gyroRate;
 
         // -----calculate P component
         PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7;
 
-        // Yaw Pterm low pass
-        if (axis == YAW && pidProfile->yaw_pterm_cut_hz) {
-            PTerm = filterApplyPt1(PTerm, &yawPTermState, pidProfile->yaw_pterm_cut_hz, dT);
-        }
-
         // -----calculate I component
         // there should be no division before accumulating the error to integrator, because the precision would be reduced.
         // Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used.
         // Time correction (to avoid different I scaling for different builds based on average cycle time)
         // is normalized to cycle time = 2048.
-        errorGyroI[axis] = errorGyroI[axis] + ((RateError * cycleTime) >> 11) * pidProfile->I8[axis];
+        errorGyroI[axis] = errorGyroI[axis] + ((RateError * (uint16_t)cycleTime) >> 11) * pidProfile->I8[axis];
 
         // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
         // I coefficient (I8) moved before integration to make limiting independent from PID settings
@@ -303,24 +286,14 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
 
         // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
         // would be scaled by different dt each time. Division by dT fixes that.
-        delta = (delta * ((uint16_t) 0xFFFF / (cycleTime >> 4))) >> 6;
+        delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)cycleTime >> 4))) >> 6;
 
-        // In case of soft gyro lpf combined with dterm_cut_hz we don't need the old 3 point averaging
-        if (pidProfile->gyro_soft_lpf && pidProfile->dterm_cut_hz) {
-            deltaSum = delta * 3;  // Scaling to approximately match the old D values
-        } else {
-            // add moving average here to reduce noise
-            deltaSum = delta1[axis] + delta2[axis] + delta;
-            delta2[axis] = delta1[axis];
-            delta1[axis] = delta;
+        if (deltaStateIsSet) {
+            // Upscale x3 before filtering to avoid rounding errors
+            delta = lrintf(filterApplyBiQuad(3.0f * delta, &deltaBiQuadState[axis]));
         }
 
-        // Dterm delta low pass
-        if (pidProfile->dterm_cut_hz) {
-            deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
-        }
-
-        DTerm = (deltaSum * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;
+        DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;
 
         // -----calculate total PID output
         axisPID[axis] = PTerm + ITerm + DTerm;
@@ -341,6 +314,9 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
 
 void pidSetController(pidControllerType_e type)
 {
+    // Force filter re-init
+    deltaStateIsSet = false;
+
     switch (type) {
         default:
         case PID_CONTROLLER_MWREWRITE:
diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
index d6740c098b..d4ea811332 100644
--- a/src/main/flight/pid.h
+++ b/src/main/flight/pid.h
@@ -58,11 +58,10 @@ typedef struct pidProfile_s {
     uint8_t H_sensitivity;
 
     uint16_t yaw_p_limit;                   // set P term limit (fixed value was 300)
-    uint8_t dterm_cut_hz;                   // (default 17Hz, Range 1-50Hz) Used for PT1 element in PID1, PID2 and PID5
-    uint8_t yaw_pterm_cut_hz;               // Used for filering Pterm noise on noisy frames
+    uint8_t dterm_lpf_hz;                   // (default 17Hz, Range 1-50Hz) Used for PT1 element in PID1, PID2 and PID5
 
-    uint8_t gyro_soft_lpf;                  // Gyro FIR filtering
-    uint8_t acc_soft_lpf;                   // Set the Low Pass Filter factor for ACC. Reducing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time. Zero = no filter
+    uint8_t gyro_soft_lpf_hz;               // Gyro FIR filtering
+    uint8_t acc_soft_lpf_hz;                // Set the Low Pass Filter factor for ACC. Reducing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time. Zero = no filter
 
 #ifdef GTUNE
     uint8_t  gtune_lolimP[3];               // [0..200] Lower limit of P during G tune
diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c
index e8a81ff178..b1d57ad939 100644
--- a/src/main/io/serial_cli.c
+++ b/src/main/io/serial_cli.c
@@ -374,10 +374,6 @@ static const char * const lookupTableGyroLpf[] = {
     "10HZ"
 };
 
-static const char * const lookupTableSoftFilter[] = {
-    "OFF", "LOW", "MEDIUM", "HIGH"
-};
-
 static const char * const lookupTableFailsafeProcedure[] = {
     "SET-THR", "DROP", "RTH"
 };
@@ -413,7 +409,6 @@ typedef enum {
     TABLE_GIMBAL_MODE,
     TABLE_PID_CONTROLLER,
     TABLE_SERIAL_RX,
-    TABLE_SOFT_FILTER,
     TABLE_GYRO_LPF,
     TABLE_FAILSAFE_PROCEDURE,
 #ifdef NAV
@@ -438,7 +433,6 @@ static const lookupTableEntry_t lookupTables[] = {
     { lookupTableGimbalMode, sizeof(lookupTableGimbalMode) / sizeof(char *) },
     { lookupTablePidController, sizeof(lookupTablePidController) / sizeof(char *) },
     { lookupTableSerialRX, sizeof(lookupTableSerialRX) / sizeof(char *) },
-    { lookupTableSoftFilter, sizeof(lookupTableSoftFilter) / sizeof(char *) },
     { lookupTableGyroLpf, sizeof(lookupTableGyroLpf) / sizeof(char *) },
     { lookupTableFailsafeProcedure, sizeof(lookupTableFailsafeProcedure) / sizeof(char *) },
 #ifdef NAV
@@ -732,10 +726,9 @@ const clivalue_t valueTable[] = {
     { "d_level",                    VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDLEVEL], .config.minmax = { 0,  200 }, 0 },
 
     { "yaw_p_limit",                VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_p_limit, .config.minmax = { YAW_P_LIMIT_MIN, YAW_P_LIMIT_MAX } },
-	{ "gyro_soft_lpf",              VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.gyro_soft_lpf, .config.lookup = { TABLE_SOFT_FILTER }, 0 },
-    { "acc_soft_lpf",               VAR_UINT8  | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.acc_soft_lpf, .config.lookup = { TABLE_SOFT_FILTER }, 0 },
-    { "dterm_cut_hz",               VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_cut_hz, .config.minmax = {0, 200 } },
-    { "yaw_pterm_cut_hz",           VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_pterm_cut_hz, .config.minmax = {0, 200 } },
+	{ "gyro_soft_lpf_hz",           VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gyro_soft_lpf_hz, .config.minmax = {0, 200 } },
+    { "acc_soft_lpf_hz",            VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.acc_soft_lpf_hz, .config.minmax = {0, 200 } },
+    { "dterm_lpf_hz",               VAR_UINT8  | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 200 } },
 
 #ifdef GTUNE
     { "gtune_loP_rll",              VAR_UINT8  | PROFILE_VALUE,  &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_ROLL], .config.minmax = { 10,  200 }, 0 },
diff --git a/src/main/mw.c b/src/main/mw.c
index a30394f62b..850dc52bd0 100755
--- a/src/main/mw.c
+++ b/src/main/mw.c
@@ -113,8 +113,6 @@ extern uint32_t currentTime;
 extern uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3];
 
 static bool isRXDataNew;
-static filterStatePt1_t filteredCycleTimeState;
-uint16_t filteredCycleTime;
 
 typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
         uint16_t max_angle_inclination, rxConfig_t *rxConfig);            // pid controller function prototype
@@ -538,15 +536,27 @@ void processRx(void)
 
 }
 
-void filterRc(bool isRXDataNew){
+void filterRc(bool isRXDataNew)
+{
     static int16_t lastCommand[4] = { 0, 0, 0, 0 };
     static int16_t deltaRC[4] = { 0, 0, 0, 0 };
     static int16_t factor, rcInterpolationFactor;
+    static biquad_t filteredCycleTimeState;
+    static bool filterInitialised;
+    uint16_t filteredCycleTime;
     uint16_t rxRefreshRate;
 
     // Set RC refresh rate for sampling and channels to filter
     initRxRefreshRate(&rxRefreshRate);
 
+    // Calculate average cycle time (1Hz LPF on cycle time)
+    if (!filterInitialised) {
+        filterInitBiQuad(1, &filteredCycleTimeState, 0);
+        filterInitialised = true;
+    }
+
+    filteredCycleTime = filterApplyBiQuad((float) cycleTime, &filteredCycleTimeState);
+
     rcInterpolationFactor = rxRefreshRate / filteredCycleTime + 1;
 
     if (isRXDataNew) {
@@ -575,9 +585,6 @@ void taskMainPidLoop(void)
     cycleTime = getTaskDeltaTime(TASK_SELF);
     dT = (float)cycleTime * 0.000001f;
 
-    // Calculate average cycle time and average jitter
-    filteredCycleTime = filterApplyPt1(cycleTime, &filteredCycleTimeState, 1, dT);
-    
     imuUpdateAccelerometer();
     imuUpdateGyroAndAttitude();
 
diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c
index 6067ae6a44..09ac393def 100644
--- a/src/main/sensors/acceleration.c
+++ b/src/main/sensors/acceleration.c
@@ -27,6 +27,7 @@
 
 #include "drivers/sensor.h"
 #include "drivers/accgyro.h"
+#include "drivers/gyro_sync.h"
 
 #include "sensors/battery.h"
 #include "sensors/sensors.h"
@@ -47,8 +48,10 @@ uint16_t calibratingA = 0;      // the calibration is done is the main loop. Cal
 
 static flightDynamicsTrims_t * accZero;
 static flightDynamicsTrims_t * accGain;
-static int8_t * accFIRTable = 0L;
-static int16_t accFIRState[3][9];
+
+static int8_t accLpfCutHz = 0;
+static biquad_t accFilterState[XYZ_AXIS_COUNT];
+static bool accFilterInitialised = false;
 
 void accSetCalibrationCycles(uint16_t calibrationCyclesRequired)
 {
@@ -173,12 +176,28 @@ void applyAccelerationZero(flightDynamicsTrims_t * accZero, flightDynamicsTrims_
 
 void updateAccelerationReadings(void)
 {
+    int axis;
+
     if (!acc.read(accADC)) {
         return;
     }
 
-    if (accFIRTable) {
-        filterApply9TapFIR(accADC, accFIRState, accFIRTable);
+    if (accLpfCutHz) {
+        if (!accFilterInitialised) {
+            if (targetLooptime) {  /* Initialisation needs to happen once sample rate is known */
+                for (axis = 0; axis < 3; axis++) {
+                    filterInitBiQuad(accLpfCutHz, &accFilterState[axis], 0);
+                }
+
+                accFilterInitialised = true;
+            }
+        }
+
+        if (accFilterInitialised) {
+            for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+                accADC[axis] = lrintf(filterApplyBiQuad((float) accADC[axis], &accFilterState[axis]));
+            }
+        }
     }
 
     if (!isAccelerationCalibrationComplete()) {
@@ -200,7 +219,7 @@ void setAccelerationGain(flightDynamicsTrims_t * accGainToUse)
     accGain = accGainToUse;
 }
 
-void setAccelerationFilter(int8_t * filterTableToUse)
+void setAccelerationFilter(int8_t initialAccLpfCutHz)
 {
-    accFIRTable = filterTableToUse;
+    accLpfCutHz = initialAccLpfCutHz;
 }
diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h
index ebf64b276d..c3527b8791 100644
--- a/src/main/sensors/acceleration.h
+++ b/src/main/sensors/acceleration.h
@@ -44,4 +44,4 @@ void accSetCalibrationCycles(uint16_t calibrationCyclesRequired);
 void updateAccelerationReadings(void);
 void setAccelerationZero(flightDynamicsTrims_t * accZeroToUse);
 void setAccelerationGain(flightDynamicsTrims_t * accGainToUse);
-void setAccelerationFilter(int8_t * filterTableToUse);
+void setAccelerationFilter(int8_t initialAccLpfCutHz);
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
index fcf1bdc0a6..60ededde6a 100644
--- a/src/main/sensors/gyro.c
+++ b/src/main/sensors/gyro.c
@@ -17,6 +17,7 @@
 
 #include <stdbool.h>
 #include <stdint.h>
+#include <math.h>
 
 #include "platform.h"
 
@@ -26,11 +27,13 @@
 
 #include "drivers/sensor.h"
 #include "drivers/accgyro.h"
-#include "sensors/sensors.h"
+#include "drivers/gyro_sync.h"
+
 #include "io/beeper.h"
 #include "io/statusindicator.h"
-#include "sensors/boardalignment.h"
 
+#include "sensors/sensors.h"
+#include "sensors/boardalignment.h"
 #include "sensors/gyro.h"
 
 uint16_t calibratingG = 0;
@@ -38,16 +41,18 @@ int16_t gyroADC[XYZ_AXIS_COUNT];
 int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
 
 static gyroConfig_t *gyroConfig;
-static int8_t * gyroFIRTable = 0L;
-static int16_t gyroFIRState[3][9];
+
+static int8_t gyroLpfCutHz = 0;
+static biquad_t gyroFilterState[XYZ_AXIS_COUNT];
+static bool gyroFilterInitialised = false;
 
 gyro_t gyro;                      // gyro access functions
 sensor_align_e gyroAlign = 0;
 
-void useGyroConfig(gyroConfig_t *gyroConfigToUse, int8_t * filterTableToUse)
+void useGyroConfig(gyroConfig_t *gyroConfigToUse, int8_t initialGyroLpfCutHz)
 {
     gyroConfig = gyroConfigToUse;
-    gyroFIRTable = filterTableToUse;
+    gyroLpfCutHz = initialGyroLpfCutHz;
 }
 
 void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired)
@@ -120,13 +125,29 @@ static void applyGyroZero(void)
 
 void gyroUpdate(void)
 {
+    int8_t axis;
+
     // range: +/- 8192; +/- 2000 deg/sec
     if (!gyro.read(gyroADC)) {
         return;
     }
 
-    if (gyroFIRTable) {
-        filterApply9TapFIR(gyroADC, gyroFIRState, gyroFIRTable);
+    if (gyroLpfCutHz) {
+        if (!gyroFilterInitialised) {
+            if (targetLooptime) {  /* Initialisation needs to happen once sample rate is known */
+                for (axis = 0; axis < 3; axis++) {
+                    filterInitBiQuad(gyroLpfCutHz, &gyroFilterState[axis], 0);
+                }
+
+                gyroFilterInitialised = true;
+            }
+        }
+
+        if (gyroFilterInitialised) {
+            for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
+                gyroADC[axis] = lrintf(filterApplyBiQuad((float) gyroADC[axis], &gyroFilterState[axis]));
+            }
+        }
     }
 
     alignSensors(gyroADC, gyroADC, gyroAlign);
diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h
index ca8b738b01..e5697ce9d9 100644
--- a/src/main/sensors/gyro.h
+++ b/src/main/sensors/gyro.h
@@ -39,7 +39,7 @@ typedef struct gyroConfig_s {
     uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
 } gyroConfig_t;
 
-void useGyroConfig(gyroConfig_t *gyroConfigToUse, int8_t * filterTableToUse);
+void useGyroConfig(gyroConfig_t *gyroConfigToUse, int8_t initialGyroLpfCutHz);
 void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
 void gyroUpdate(void);
 bool isGyroCalibrationComplete(void);