diff --git a/libraries/Filter/NotchFilter.cpp b/libraries/Filter/NotchFilter.cpp
index b4c834f0cd..590dafcd71 100644
--- a/libraries/Filter/NotchFilter.cpp
+++ b/libraries/Filter/NotchFilter.cpp
@@ -55,15 +55,22 @@ T NotchFilter<T>::apply(const T &sample)
     return output;
 }
 
+template <class T>
+void NotchFilter<T>::reset()
+{
+    ntchsig2 = ntchsig1 = T();
+    signal2 = signal1 = T();
+}
+
 // table of user settable parameters
-const AP_Param::GroupInfo NotchFilterVector3fParam::var_info[] = {
+const AP_Param::GroupInfo NotchFilterParams::var_info[] = {
 
     // @Param: ENABLE
     // @DisplayName: Enable
     // @Description: Enable notch filter
     // @Values: 0:Disabled,1:Enabled
     // @User: Advanced
-    AP_GROUPINFO_FLAGS("ENABLE", 1, NotchFilterVector3fParam, enable, 0, AP_PARAM_FLAG_ENABLE),
+    AP_GROUPINFO_FLAGS("ENABLE", 1, NotchFilterParams, _enable, 0, AP_PARAM_FLAG_ENABLE),
 
     // @Param: FREQ
     // @DisplayName: Frequency
@@ -71,7 +78,7 @@ const AP_Param::GroupInfo NotchFilterVector3fParam::var_info[] = {
     // @Range: 10 200
     // @Units: Hz
     // @User: Advanced
-    AP_GROUPINFO("FREQ", 2, NotchFilterVector3fParam, center_freq_hz, 80),
+    AP_GROUPINFO("FREQ", 2, NotchFilterParams, _center_freq_hz, 80),
 
     // @Param: BW
     // @DisplayName: Bandwidth
@@ -79,7 +86,7 @@ const AP_Param::GroupInfo NotchFilterVector3fParam::var_info[] = {
     // @Range: 5 50
     // @Units: Hz
     // @User: Advanced
-    AP_GROUPINFO("BW", 3, NotchFilterVector3fParam, bandwidth_hz, 20),
+    AP_GROUPINFO("BW", 3, NotchFilterParams, _bandwidth_hz, 20),
 
     // @Param: ATT
     // @DisplayName: Attenuation
@@ -87,7 +94,7 @@ const AP_Param::GroupInfo NotchFilterVector3fParam::var_info[] = {
     // @Range: 5 30
     // @Units: dB
     // @User: Advanced
-    AP_GROUPINFO("ATT", 4, NotchFilterVector3fParam, attenuation_dB, 15),
+    AP_GROUPINFO("ATT", 4, NotchFilterParams, _attenuation_dB, 15),
     
     AP_GROUPEND
 };
@@ -95,46 +102,11 @@ const AP_Param::GroupInfo NotchFilterVector3fParam::var_info[] = {
 /*
   a notch filter with enable and filter parameters - constructor
  */
-NotchFilterVector3fParam::NotchFilterVector3fParam(void)
+NotchFilterParams::NotchFilterParams(void)
 {
     AP_Param::setup_object_defaults(this, var_info);    
 }
 
-/*
-  initialise filter
- */
-void NotchFilterVector3fParam::init(float _sample_freq_hz)
-{
-    filter.init(_sample_freq_hz, center_freq_hz, bandwidth_hz, attenuation_dB);
-
-    sample_freq_hz = _sample_freq_hz;
-    last_center_freq = center_freq_hz;
-    last_bandwidth = bandwidth_hz;
-    last_attenuation = attenuation_dB;
-}
-
-/*
-  apply a filter sample
- */
-Vector3f NotchFilterVector3fParam::apply(const Vector3f &sample)
-{
-    if (!enable) {
-        // when not enabled it is a simple pass-through
-        return sample;
-    }
-
-    // check for changed parameters
-    if (!is_equal(center_freq_hz.get(), last_center_freq) ||
-        !is_equal(bandwidth_hz.get(), last_bandwidth) ||
-        !is_equal(attenuation_dB.get(), last_attenuation)) {
-        if (!is_zero(sample_freq_hz)) {
-            init(sample_freq_hz);
-        }
-    }
-    
-    return filter.apply(sample);
-}
-
 /* 
    instantiate template classes
  */
diff --git a/libraries/Filter/NotchFilter.h b/libraries/Filter/NotchFilter.h
index fb61ef98f0..435a2a9b4a 100644
--- a/libraries/Filter/NotchFilter.h
+++ b/libraries/Filter/NotchFilter.h
@@ -32,6 +32,7 @@ public:
     // set parameters
     void init(float sample_freq_hz, float center_freq_hz, float bandwidth_hz, float attenuation_dB);
     T apply(const T &sample);
+    void reset();
 
 private:
     bool initialised;
@@ -40,29 +41,23 @@ private:
 };
 
 /*
-  a notch filter with enable and filter parameters
+  notch filter enable and filter parameters
  */
-class NotchFilterVector3fParam {
+class NotchFilterParams {
 public:
-    NotchFilterVector3fParam(void);
-    void init(float sample_freq_hz);
-    Vector3f apply(const Vector3f &sample);
-
+    NotchFilterParams(void);
     static const struct AP_Param::GroupInfo var_info[];
+
+    uint16_t center_freq_hz(void) const { return _center_freq_hz; }
+    uint8_t bandwidth_hz(void) const { return _bandwidth_hz; }
+    float attenuation_dB(void) const { return _attenuation_dB; }
+    uint8_t enabled(void) const { return _enable; }
     
 private:
-    AP_Int8 enable;
-    AP_Float center_freq_hz;
-    AP_Float bandwidth_hz;
-    AP_Float attenuation_dB;
-
-    float sample_freq_hz;
-
-    float last_center_freq;
-    float last_bandwidth;
-    float last_attenuation;
-    
-    NotchFilter<Vector3f> filter;
+    AP_Int8 _enable;
+    AP_Int16 _center_freq_hz;
+    AP_Int8 _bandwidth_hz;
+    AP_Float _attenuation_dB;
 };
 
 typedef NotchFilter<float> NotchFilterFloat;