diff --git a/libraries/Filter/NotchFilter.cpp b/libraries/Filter/NotchFilter.cpp
index 5f5fa6e1f3..db9e2e234b 100644
--- a/libraries/Filter/NotchFilter.cpp
+++ b/libraries/Filter/NotchFilter.cpp
@@ -47,7 +47,7 @@ void NotchFilter<T>::init(float sample_freq_hz, float center_freq_hz, float band
     // check center frequency is in the allowable range
     if ((center_freq_hz > 0.5 * bandwidth_hz) && (center_freq_hz < 0.5 * sample_freq_hz)) {
         float A, Q;
-        _center_freq_hz = center_freq_hz;
+        initialised = false;    // force center frequency change
         calculate_A_and_Q(center_freq_hz, bandwidth_hz, attenuation_dB, A, Q);
         init_with_A_and_Q(sample_freq_hz, center_freq_hz, A, Q);
     } else {
@@ -58,6 +58,11 @@ void NotchFilter<T>::init(float sample_freq_hz, float center_freq_hz, float band
 template <class T>
 void NotchFilter<T>::init_with_A_and_Q(float sample_freq_hz, float center_freq_hz, float A, float Q)
 {
+    // don't update if no updates required
+    if (initialised && is_equal(center_freq_hz, _center_freq_hz) && is_equal(sample_freq_hz, _sample_freq_hz)) {
+        return;
+    }
+
     float new_center_freq = center_freq_hz;
 
     // constrain the new center frequency by a percentage of the old frequency
@@ -76,6 +81,7 @@ void NotchFilter<T>::init_with_A_and_Q(float sample_freq_hz, float center_freq_h
         a1 = b1;
         a2 =  1.0 - alpha;
         _center_freq_hz = new_center_freq;
+        _sample_freq_hz = sample_freq_hz;
         initialised = true;
     } else {
         // leave center_freq_hz at last value
diff --git a/libraries/Filter/NotchFilter.h b/libraries/Filter/NotchFilter.h
index 1eb7d56059..53bacc8e74 100644
--- a/libraries/Filter/NotchFilter.h
+++ b/libraries/Filter/NotchFilter.h
@@ -42,7 +42,7 @@ private:
 
     bool initialised, need_reset;
     float b0, b1, b2, a1, a2, a0_inv;
-    float _center_freq_hz;
+    float _center_freq_hz, _sample_freq_hz;
     T ntchsig, ntchsig1, ntchsig2, signal2, signal1;
 };