diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.cpp b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
index b2a76a1f26..bf62d4cdfc 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.cpp
@@ -1850,7 +1850,9 @@ void AP_InertialSensor::HarmonicNotch::update_params(uint8_t instance, bool conv
         last_center_freq_hz[instance] = params.center_freq_hz();
         last_bandwidth_hz[instance] = params.bandwidth_hz();
         last_attenuation_dB[instance] = params.attenuation_dB();
-    } else if (params.tracking_mode() != HarmonicNotchDynamicMode::Fixed) {
+    }
+
+    if (params.tracking_mode() != HarmonicNotchDynamicMode::Fixed) {
         if (num_calculated_notch_frequencies > 1) {
             filter[instance].update(num_calculated_notch_frequencies, calculated_notch_freq_hz);
         } else {
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor.h b/libraries/AP_InertialSensor/AP_InertialSensor.h
index 9bce50d4ac..aa103bb608 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor.h
@@ -449,12 +449,6 @@ public:
         float calculated_notch_freq_hz[INS_MAX_NOTCHES];
         uint8_t num_calculated_notch_frequencies;
 
-        // Update the harmonic notch frequency
-        void update_notch_freq_hz(float scaled_freq);
-
-        // Update the harmonic notch frequencies
-        void update_notch_frequencies_hz(uint8_t num_freqs, const float scaled_freq[]);
-
         // runtime update of notch parameters
         void update_params(uint8_t instance, bool converging, float gyro_rate);