diff --git a/libraries/Filter/NotchFilter.cpp b/libraries/Filter/NotchFilter.cpp
index ea818ec68b..510f085094 100644
--- a/libraries/Filter/NotchFilter.cpp
+++ b/libraries/Filter/NotchFilter.cpp
@@ -74,7 +74,7 @@ void NotchFilter<T>::init_with_A_and_Q(float sample_freq_hz, float center_freq_h
 template <class T>
 T NotchFilter<T>::apply(const T &sample)
 {
-    if (!initialised) {
+    if (!initialised || need_reset) {
         // if we have not been initialised when return the input
         // sample as output and update delayed samples
         ntchsig2 = ntchsig1;
@@ -82,6 +82,7 @@ T NotchFilter<T>::apply(const T &sample)
         ntchsig = sample;
         signal2 = signal1;
         signal1 = sample;
+        need_reset = false;
         return sample;
     }
     ntchsig2 = ntchsig1;
@@ -96,8 +97,7 @@ T NotchFilter<T>::apply(const T &sample)
 template <class T>
 void NotchFilter<T>::reset()
 {
-    ntchsig2 = ntchsig1 = T();
-    signal2 = signal1 = T();
+    need_reset = true;
 }
 
 /*
diff --git a/libraries/Filter/NotchFilter.h b/libraries/Filter/NotchFilter.h
index 62352b60ee..4c74fe7f5d 100644
--- a/libraries/Filter/NotchFilter.h
+++ b/libraries/Filter/NotchFilter.h
@@ -40,7 +40,7 @@ public:
 
 private:
 
-    bool initialised;
+    bool initialised, need_reset;
     float b0, b1, b2, a1, a2, a0_inv;
     T ntchsig, ntchsig1, ntchsig2, signal2, signal1;
 };