From eefc41fe300c7409d1583582a8ef657c1abbebee Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sun, 12 Jun 2022 18:38:10 +1000
Subject: [PATCH] Filter: added a way to plot attenuation and phase lag for
 complex filters

---
 libraries/Filter/HarmonicNotchFilter.cpp    |  2 +
 libraries/Filter/tests/plot_harmonics.gnu   | 11 +++
 libraries/Filter/tests/test_notchfilter.cpp | 89 ++++++++++++++++++++-
 3 files changed, 101 insertions(+), 1 deletion(-)
 create mode 100755 libraries/Filter/tests/plot_harmonics.gnu

diff --git a/libraries/Filter/HarmonicNotchFilter.cpp b/libraries/Filter/HarmonicNotchFilter.cpp
index 20733e58ec..9617b3af49 100644
--- a/libraries/Filter/HarmonicNotchFilter.cpp
+++ b/libraries/Filter/HarmonicNotchFilter.cpp
@@ -311,3 +311,5 @@ void HarmonicNotchFilterParams::save_params()
   instantiate template classes
  */
 template class HarmonicNotchFilter<Vector3f>;
+template class HarmonicNotchFilter<float>;
+
diff --git a/libraries/Filter/tests/plot_harmonics.gnu b/libraries/Filter/tests/plot_harmonics.gnu
new file mode 100755
index 0000000000..a534c4994d
--- /dev/null
+++ b/libraries/Filter/tests/plot_harmonics.gnu
@@ -0,0 +1,11 @@
+#!/usr/bin/gnuplot -persist
+set y2tics 0,10
+set ytics nomirror
+set style data linespoints
+set key autotitle
+set datafile separator ","
+set key autotitle columnhead
+set xlabel "Freq(Hz)"
+set ylabel "Attenuation"
+#set ylabel2 "PhaseLag(deg)"
+plot "harmonicnotch_test.csv" using 1:2 axis x1y1, "harmonicnotch_test.csv" using 1:3 axis x1y2
diff --git a/libraries/Filter/tests/test_notchfilter.cpp b/libraries/Filter/tests/test_notchfilter.cpp
index 1c44fddf49..4d1b90a149 100644
--- a/libraries/Filter/tests/test_notchfilter.cpp
+++ b/libraries/Filter/tests/test_notchfilter.cpp
@@ -2,6 +2,7 @@
 
 #include <Filter/Filter.h>
 #include <Filter/NotchFilter.h>
+#include <Filter/HarmonicNotchFilter.h>
 
 const AP_HAL::HAL& hal = AP_HAL::get_HAL();
 
@@ -30,7 +31,7 @@ TEST(NotchFilterTest, SineTest)
     const float test_freq = 47;
     const float attenuation_dB = 43;
     const float rate_hz = 2000;
-    double dt = 1.0 / rate_hz;
+    const double dt = 1.0 / rate_hz;
     const uint32_t period_samples = rate_hz / test_freq;
     const uint32_t periods = 1000;
     const float test_amplitude = 0.7;
@@ -65,4 +66,90 @@ TEST(NotchFilterTest, SineTest)
     EXPECT_LE(err_pct, 1);
 }
 
+/*
+  test attentuation versus frequency
+  This is a way to get a graph of the attenuation and phase lag for a complex filter setup
+ */
+TEST(NotchFilterTest, HarmonicNotchTest)
+{
+    const uint8_t num_test_freq = 150;
+    const uint8_t harmonics = 15;
+    const uint8_t num_harmonics = __builtin_popcount(harmonics);
+    const float base_freq = 46;
+    const float bandwidth = base_freq/2;
+    const float attenuation_dB = 60;
+    // number of identical filters chained together, simulating
+    // usage of per-motor notch filtering
+    const uint8_t chained_filters = 8;
+    const uint16_t rate_hz = 2000;
+    const uint32_t samples = 50000;
+    const float test_amplitude = 0.7;
+    const double dt = 1.0 / rate_hz;
+
+    bool double_notch = false;
+    HarmonicNotchFilter<float> filters[num_test_freq][chained_filters] {};
+    struct {
+        double in;
+        double out;
+        double last_in;
+        double last_out;
+        uint32_t last_crossing;
+        uint32_t total_lag_samples;
+        uint32_t lag_count;
+        float get_lag_degrees(const float freq) const {
+            const float lag_avg = total_lag_samples/float(lag_count);
+            return (360.0 * lag_avg * freq) / rate_hz;
+        }
+    } integrals[num_test_freq] {};
+
+    for (uint8_t i=0; i<num_test_freq; i++) {
+        for (uint8_t c=0; c<chained_filters; c++) {
+            auto &f = filters[i][c];
+            f.allocate_filters(num_harmonics, harmonics, double_notch);
+            f.init(rate_hz, base_freq, bandwidth, attenuation_dB);
+        }
+    }
+
+    for (uint32_t s=0; s<samples; s++) {
+        const double t = s * dt;
+
+        for (uint8_t i=0; i<num_test_freq; i++) {
+            const float freq = i+1;
+            const double sample = sin(freq * t * 2 * M_PI) * test_amplitude;
+            float v = sample;
+            for (uint8_t c=0; c<chained_filters; c++) {
+                auto &f = filters[i][c];
+                v = f.apply(v);
+            }
+            if (s >= s/10) {
+                integrals[i].in += fabsf(sample) * dt;
+                integrals[i].out += fabsf(v) * dt;
+            }
+            if (sample >= 0 && integrals[i].last_in < 0) {
+                integrals[i].last_crossing = s;
+            }
+            if (v >= 0 && integrals[i].last_out < 0 && integrals[i].last_crossing != 0) {
+                integrals[i].total_lag_samples += s - integrals[i].last_crossing;
+                integrals[i].lag_count++;
+            }
+            integrals[i].last_in = sample;
+            integrals[i].last_out = v;
+        }
+    }
+    const char *csv_file = "harmonicnotch_test.csv";
+    FILE *f = fopen(csv_file, "w");
+    fprintf(f, "Freq(Hz),Ratio,Lag(deg)\n");
+    for (uint8_t i=0; i<num_test_freq; i++) {
+        const float freq = i+1;
+        const float lag_degrees = integrals[i].get_lag_degrees(freq);
+        fprintf(f, "%.1f,%f,%f\n", freq, integrals[i].out/integrals[i].in, lag_degrees);
+    }
+    fclose(f);
+    printf("Wrote %s\n", csv_file);
+    ::printf("Lag at 1Hz %.2f degrees\n", integrals[0].get_lag_degrees(1));
+    ::printf("Lag at 2Hz %.2f degrees\n", integrals[1].get_lag_degrees(2));
+    ::printf("Lag at 5Hz %.2f degrees\n", integrals[4].get_lag_degrees(5));
+    ::printf("Lag at 10Hz %.2f degrees\n", integrals[9].get_lag_degrees(10));
+}
+
 AP_GTEST_MAIN()