diff --git a/libraries/Filter/HarmonicNotchFilter.cpp b/libraries/Filter/HarmonicNotchFilter.cpp
new file mode 100644
index 0000000000..623061afbe
--- /dev/null
+++ b/libraries/Filter/HarmonicNotchFilter.cpp
@@ -0,0 +1,179 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "HarmonicNotchFilter.h"
+
+#define HNF_MAX_FILTERS 3
+#define HNF_MAX_HARMONICS 8
+
+// table of user settable parameters
+const AP_Param::GroupInfo HarmonicNotchFilterParams::var_info[] = {
+
+    // @Param: ENABLE
+    // @DisplayName: Enable
+    // @Description: Enable harmonic notch filter
+    // @Values: 0:Disabled,1:Enabled
+    // @User: Advanced
+    AP_GROUPINFO_FLAGS("ENABLE", 1, HarmonicNotchFilterParams, _enable, 0, AP_PARAM_FLAG_ENABLE),
+
+    // @Param: FREQ
+    // @DisplayName: Base Frequency
+    // @Description: Notch base center frequency in Hz
+    // @Range: 10 400
+    // @Units: Hz
+    // @User: Advanced
+    AP_GROUPINFO("FREQ", 2, HarmonicNotchFilterParams, _center_freq_hz, 80),
+
+    // @Param: BW
+    // @DisplayName: Bandwidth
+    // @Description: Harmonic notch bandwidth in Hz
+    // @Range: 5 100
+    // @Units: Hz
+    // @User: Advanced
+    AP_GROUPINFO("BW", 3, HarmonicNotchFilterParams, _bandwidth_hz, 20),
+
+    // @Param: ATT
+    // @DisplayName: Attenuation
+    // @Description: Harmonic notch attenuation in dB
+    // @Range: 5 30
+    // @Units: dB
+    // @User: Advanced
+    AP_GROUPINFO("ATT", 4, HarmonicNotchFilterParams, _attenuation_dB, 15),
+
+    // @Param: HMNCS
+    // @DisplayName: Harmonics
+    // @Description: Bitmask of harmonic frequencies to apply notches to. This option takes effect on the next reboot. A maximum of 3 harmonics can be used at any one time
+    // @Bitmask: 0:1st harmonic,1:2nd harmonic,2:3rd harmonic,3:4th hamronic,4:5th harmonic,5:6th harmonic,6:7th harmonic,7:8th harmonic
+    // @User: Advanced
+    // @RebootRequired: True
+    AP_GROUPINFO("HMNCS", 5, HarmonicNotchFilterParams, _harmonics, 3),
+
+    // @Param: REF
+    // @DisplayName: Reference value
+    // @Description: Reference value associated with the specified frequency to facilitate frequency scaling
+    // @User: Advanced
+    // @Range: 0.1 0.9
+    // @RebootRequired: True
+    AP_GROUPINFO("REF", 6, HarmonicNotchFilterParams, _reference, 0.1f),
+
+    AP_GROUPEND
+};
+
+
+/*
+  initialise filter
+ */
+template <class T>
+void HarmonicNotchFilter<T>::init(float sample_freq_hz, float center_freq_hz, float bandwidth_hz, float attenuation_dB)
+{
+    if (_filters == nullptr) {
+        return;
+    }
+
+    _sample_freq_hz = sample_freq_hz;
+
+    // adjust the center frequency to be in the allowable range
+    center_freq_hz = constrain_float(center_freq_hz, bandwidth_hz * 0.52f, sample_freq_hz * 0.48f);
+
+    NotchFilter<T>::calculate_A_and_Q(center_freq_hz, bandwidth_hz, attenuation_dB, _A, _Q);
+
+    for (uint8_t i = 0, filt = 0; i < HNF_MAX_HARMONICS && filt < _num_filters; i++) {
+        if ((1U<<i) & _harmonics) {
+            _filters[filt++].init_with_A_and_Q(sample_freq_hz, center_freq_hz * (i+1), _A, _Q);
+        }
+    }
+    _initialised = true;
+}
+
+/*
+  initialise filter
+ */
+template <class T>
+void HarmonicNotchFilter<T>::create(uint8_t harmonics)
+{
+    for (uint8_t i = 0; i < HNF_MAX_HARMONICS && _num_filters < HNF_MAX_FILTERS; i++) {
+        if ((1U<<i) & harmonics) {
+            _num_filters++;
+        }
+    }
+    if (_num_filters > 0) {
+        _filters = new NotchFilter<T>[_num_filters];
+    }
+    _harmonics = harmonics;
+}
+
+template <class T>
+HarmonicNotchFilter<T>::~HarmonicNotchFilter() {
+    if (_num_filters > 0) {
+        delete[] _filters;
+    }
+}
+
+template <class T>
+void HarmonicNotchFilter<T>::update(float center_freq_hz)
+{
+    if (!_initialised) {
+        return;
+    }
+
+    // adjust the center frequency to be in the allowable range
+    center_freq_hz = constrain_float(center_freq_hz, 1.0f, _sample_freq_hz * 0.48f);
+
+    for (uint8_t i = 0, filt = 0; i < HNF_MAX_HARMONICS && filt < _num_filters; i++) {
+        if ((1U<<i) & _harmonics) {
+            _filters[filt++].init_with_A_and_Q(_sample_freq_hz, center_freq_hz * (i+1), _A, _Q);
+        }
+    }
+}
+
+template <class T>
+T HarmonicNotchFilter<T>::apply(const T &sample)
+{
+    if (!_initialised) {
+        return sample;
+    }
+
+    T output = sample;
+    for (uint8_t i = 0; i < _num_filters; i++) {
+        output = _filters[i].apply(output);
+    }
+    return output;
+}
+
+template <class T>
+void HarmonicNotchFilter<T>::reset()
+{
+    if (!_initialised) {
+        return;
+    }
+
+    for (uint8_t i = 0; i < _num_filters; i++) {
+        _filters[i].reset();
+    }
+}
+
+
+/*
+  a notch filter with enable and filter parameters - constructor
+ */
+HarmonicNotchFilterParams::HarmonicNotchFilterParams(void)
+{
+    AP_Param::setup_object_defaults(this, var_info);
+}
+
+/* 
+   instantiate template classes
+ */
+template class HarmonicNotchFilter<Vector3f>;
diff --git a/libraries/Filter/HarmonicNotchFilter.h b/libraries/Filter/HarmonicNotchFilter.h
new file mode 100644
index 0000000000..e70003d4a0
--- /dev/null
+++ b/libraries/Filter/HarmonicNotchFilter.h
@@ -0,0 +1,61 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+#include <AP_Math/AP_Math.h>
+#include <cmath>
+#include <inttypes.h>
+#include <AP_Param/AP_Param.h>
+#include "NotchFilter.h"
+
+template <class T>
+class HarmonicNotchFilter {
+public:
+    // set parameters
+    void create(uint8_t harmonics);
+    void init(float sample_freq_hz, float center_freq_hz, float bandwidth_hz, float attenuation_dB);
+    T apply(const T &sample);
+    void reset();
+    void update(float center_freq_hz);
+    ~HarmonicNotchFilter();
+
+private:
+    NotchFilter<T>*  _filters;
+    float _sample_freq_hz;
+    float _A;
+    float _Q;
+    uint8_t _harmonics;
+    uint8_t _num_filters;
+    bool _initialised;
+};
+
+/*
+  notch filter enable and filter parameters
+ */
+class HarmonicNotchFilterParams : public NotchFilterParams {
+public:
+    HarmonicNotchFilterParams(void);
+    void set_center_freq_hz(float center_freq) { _center_freq_hz.set(center_freq); }
+    uint8_t harmonics(void) const { return _harmonics; }
+    float reference(void) const { return _reference; }
+    static const struct AP_Param::GroupInfo var_info[];
+
+private:
+    AP_Int8 _harmonics;
+    AP_Float _reference;
+};
+
+typedef HarmonicNotchFilter<Vector3f> HarmonicNotchFilterVector3f;
+
diff --git a/libraries/Filter/NotchFilter.cpp b/libraries/Filter/NotchFilter.cpp
index 7962cd6815..8b024989fa 100644
--- a/libraries/Filter/NotchFilter.cpp
+++ b/libraries/Filter/NotchFilter.cpp
@@ -15,16 +15,34 @@
 
 #include "NotchFilter.h"
 
+/*
+   calculate the attenuation and quality factors of the filter
+ */
+template <class T>
+void NotchFilter<T>::calculate_A_and_Q(float center_freq_hz, float bandwidth_hz, float attenuation_dB, float& A, float& Q) {
+    const float octaves = log2f(center_freq_hz / (center_freq_hz - bandwidth_hz / 2.0f)) * 2.0f;
+    A = powf(10, -attenuation_dB / 40.0f);
+    Q = sqrtf(powf(2, octaves)) / (powf(2, octaves) - 1.0f);
+}
+
 /*
   initialise filter
  */
 template <class T>
 void NotchFilter<T>::init(float sample_freq_hz, float center_freq_hz, float bandwidth_hz, float attenuation_dB)
+{
+    // adjust the center frequency to be in the allowable range
+    center_freq_hz = constrain_float(center_freq_hz, bandwidth_hz * 0.52f, sample_freq_hz * 0.48f);
+
+    float A, Q;
+    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);
+}
+
+template <class T>
+void NotchFilter<T>::init_with_A_and_Q(float sample_freq_hz, float center_freq_hz, float A, float Q)
 {
     float omega = 2.0 * M_PI * center_freq_hz / sample_freq_hz;
-    float octaves = log2f(center_freq_hz  / (center_freq_hz - bandwidth_hz/2)) * 2;
-    float A = powf(10, -attenuation_dB/40);
-    float Q = sqrtf(powf(2, octaves)) / (powf(2,octaves) - 1);
     float alpha = sinf(omega) / (2 * Q/A);
     b0 =  1.0 + alpha*A;
     b1 = -2.0 * cosf(omega);
@@ -72,13 +90,21 @@ const AP_Param::GroupInfo NotchFilterParams::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO_FLAGS("ENABLE", 1, NotchFilterParams, _enable, 0, AP_PARAM_FLAG_ENABLE),
 
+    // @Param: ATT
+    // @DisplayName: Attenuation
+    // @Description: Notch attenuation in dB
+    // @Range: 5 30
+    // @Units: dB
+    // @User: Advanced
+    AP_GROUPINFO("ATT", 4, NotchFilterParams, _attenuation_dB, 15),
+
     // @Param: FREQ
     // @DisplayName: Frequency
     // @Description: Notch center frequency in Hz
     // @Range: 10 400
     // @Units: Hz
     // @User: Advanced
-    AP_GROUPINFO("FREQ", 2, NotchFilterParams, _center_freq_hz, 80),
+    AP_GROUPINFO("FREQ", 5, NotchFilterParams, _center_freq_hz, 80),
 
     // @Param: BW
     // @DisplayName: Bandwidth
@@ -86,16 +112,8 @@ const AP_Param::GroupInfo NotchFilterParams::var_info[] = {
     // @Range: 5 100
     // @Units: Hz
     // @User: Advanced
-    AP_GROUPINFO("BW", 3, NotchFilterParams, _bandwidth_hz, 20),
+    AP_GROUPINFO("BW", 6, NotchFilterParams, _bandwidth_hz, 20),
 
-    // @Param: ATT
-    // @DisplayName: Attenuation
-    // @Description: Notch attenuation in dB
-    // @Range: 5 30
-    // @Units: dB
-    // @User: Advanced
-    AP_GROUPINFO("ATT", 4, NotchFilterParams, _attenuation_dB, 15),
-    
     AP_GROUPEND
 };
 
diff --git a/libraries/Filter/NotchFilter.h b/libraries/Filter/NotchFilter.h
index 9fd258c2ed..78fea08d92 100644
--- a/libraries/Filter/NotchFilter.h
+++ b/libraries/Filter/NotchFilter.h
@@ -31,10 +31,15 @@ class NotchFilter {
 public:
     // set parameters
     void init(float sample_freq_hz, float center_freq_hz, float bandwidth_hz, float attenuation_dB);
+    void init_with_A_and_Q(float sample_freq_hz, float center_freq_hz, float A, float Q);
     T apply(const T &sample);
     void reset();
 
+    // calculate attenuation and quality from provided center frequency and bandwidth
+    static void calculate_A_and_Q(float center_freq_hz, float bandwidth_hz, float attenuation_dB, float& A, float& Q); 
+
 private:
+
     bool initialised;
     float b0, b1, b2, a1, a2, a0_inv;
     T ntchsig, ntchsig1, ntchsig2, signal2, signal1;
@@ -48,15 +53,15 @@ public:
     NotchFilterParams(void);
     static const struct AP_Param::GroupInfo var_info[];
 
-    uint16_t center_freq_hz(void) const { return _center_freq_hz; }
-    uint16_t bandwidth_hz(void) const { return _bandwidth_hz; }
+    float center_freq_hz(void) const { return _center_freq_hz; }
+    float bandwidth_hz(void) const { return _bandwidth_hz; }
     float attenuation_dB(void) const { return _attenuation_dB; }
     uint8_t enabled(void) const { return _enable; }
     
-private:
+protected:
     AP_Int8 _enable;
-    AP_Int16 _center_freq_hz;
-    AP_Int16 _bandwidth_hz;
+    AP_Float _center_freq_hz;
+    AP_Float _bandwidth_hz;
     AP_Float _attenuation_dB;
 };