Filter: added test for attenuation adjustment

libraries/Filter/tests/plot_harmonictest2.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(dB)"
+#set ylabel2 "PhaseLag(deg)"
+plot "harmonicnotch_test2.csv" using 1:2 axis x1y1, "harmonicnotch_test2.csv" using 1:3 axis x1y2

libraries/Filter/tests/plot_harmonictest3.gnu

@@ -0,0 +1,12 @@
+#!/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(dB)"
+#set ylabel2 "PhaseLag(deg)"
+plot "harmonicnotch_test3.csv" using 1:2 axis x1y1, "harmonicnotch_test3.csv" using 1:3 axis x1y2, "harmonicnotch_test3.csv" using 1:4 axis x1y1, "harmonicnotch_test3.csv" using 1:5 axis x1y2
+

libraries/Filter/tests/plot_harmonictest4.gnu

@@ -0,0 +1,12 @@
+#!/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(dB)"
+set key left bottom
+plot "harmonicnotch_test4.csv" using 1:2, "harmonicnotch_test4.csv" using 1:3, "harmonicnotch_test4.csv" using 1:4, "harmonicnotch_test4.csv" using 1:5, "harmonicnotch_test4.csv" using 1:6, "harmonicnotch_test4.csv" using 1:7, "harmonicnotch_test4.csv" using 1:8
+

libraries/Filter/tests/test_notchfilter.cpp

@@ -6,6 +6,11 @@
 
 const AP_HAL::HAL& hal = AP_HAL::get_HAL();
 
+static double ratio_to_dB(double ratio)
+{
+    return 10*log(ratio);
+}
+
 /*
   test if a reset of a notch filter results in no glitch in the following samples
   with constant input
@@ -74,7 +79,6 @@ 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;
@@ -86,7 +90,6 @@ TEST(NotchFilterTest, HarmonicNotchTest)
     const float test_amplitude = 0.7;
     const double dt = 1.0 / rate_hz;
 
-    bool double_notch = true;
     HarmonicNotchFilter<float> filters[num_test_freq][chained_filters] {};
     struct {
         double in;
@@ -105,13 +108,14 @@ TEST(NotchFilterTest, HarmonicNotchTest)
     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?2:1);
             HarmonicNotchFilterParams notch_params {};
+            notch_params.set_options(uint16_t(HarmonicNotchFilterParams::Options::TreatLowAsMin) |
+                                     uint16_t(HarmonicNotchFilterParams::Options::DoubleNotch));
             notch_params.set_attenuation(attenuation_dB);
             notch_params.set_bandwidth_hz(bandwidth);
             notch_params.set_center_freq_hz(base_freq);
             notch_params.set_freq_min_ratio(1.0);
-            notch_params.set_options(uint16_t(HarmonicNotchFilterParams::Options::TreatLowAsMin));
+            f.allocate_filters(1, harmonics, notch_params.num_composite_notches());
             f.init(rate_hz, notch_params);
             f.update(base_freq);
         }
@@ -149,7 +153,7 @@ TEST(NotchFilterTest, HarmonicNotchTest)
     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);
+        fprintf(f, "%.3f,%f,%f\n", freq, integrals[i].out/integrals[i].in, lag_degrees);
     }
     fclose(f);
     printf("Wrote %s\n", csv_file);
@@ -163,4 +167,162 @@ TEST(NotchFilterTest, HarmonicNotchTest)
     EXPECT_NEAR(integrals[9].get_lag_degrees(10), 112.23, 0.5);
 }
 
+
+/*
+  calculate attenuation and phase lag for a single harmonic notch filter
+ */
+static void test_one_filter(float base_freq, float attenuation_dB,
+                            float bandwidth, float test_freq, float source_freq,
+                            uint16_t harmonics, uint16_t options,
+                            float &phase_lag, float &out_attenuation_dB)
+{
+    const uint16_t rate_hz = 2000;
+    const uint32_t samples = 50000;
+    const float test_amplitude = 1.0;
+    const double dt = 1.0 / rate_hz;
+
+    HarmonicNotchFilter<float> filter {};
+    struct {
+        double last_in;
+        double last_out;
+        double v_max;
+        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;
+        }
+    } integral {};
+
+    auto &f = filter;
+
+
+    HarmonicNotchFilterParams notch_params {};
+    notch_params.set_options(options);
+    notch_params.set_attenuation(attenuation_dB);
+    notch_params.set_bandwidth_hz(bandwidth);
+    notch_params.set_center_freq_hz(base_freq);
+    notch_params.set_freq_min_ratio(1.0);
+    f.allocate_filters(1, harmonics, notch_params.num_composite_notches());
+    f.init(rate_hz, notch_params);
+    f.update(source_freq);
+
+    for (uint32_t s=0; s<samples; s++) {
+        const double t = s * dt;
+
+        const double sample = sin(test_freq * t * 2 * M_PI) * test_amplitude;
+        float v = sample;
+        v = f.apply(v);
+        if (s >= samples/10) {
+            integral.v_max = MAX(integral.v_max, v);
+        }
+        if (sample >= 0 && integral.last_in < 0) {
+            integral.last_crossing = s;
+        }
+        if (v >= 0 && integral.last_out < 0 && integral.last_crossing != 0) {
+            integral.total_lag_samples += s - integral.last_crossing;
+            integral.lag_count++;
+        }
+        integral.last_in = sample;
+        integral.last_out = v;
+        f.update(source_freq);
+    }
+    phase_lag = integral.get_lag_degrees(test_freq);
+    out_attenuation_dB = ratio_to_dB(integral.v_max/test_amplitude);
+}
+
+/*
+  test the test_one_filter function
+ */
+TEST(NotchFilterTest, HarmonicNotchTest2)
+{
+    const float min_freq = 1.0;
+    const float max_freq = 200;
+    const uint16_t steps = 2000;
+
+    const char *csv_file = "harmonicnotch_test2.csv";
+    FILE *f = fopen(csv_file, "w");
+    fprintf(f, "Freq(Hz),Attenuation(dB),Lag(deg)\n");
+
+    for (uint16_t i=0; i<steps; i++) {
+        float attenuation_dB, phase_lag;
+        const float test_freq = min_freq + i*(max_freq-min_freq)/steps;
+        test_one_filter(50, 30, 25, test_freq, 50, 3, uint16_t(HarmonicNotchFilterParams::Options::TripleNotch), phase_lag, attenuation_dB);
+        fprintf(f, "%.3f,%f,%f\n", test_freq, attenuation_dB, phase_lag);
+    }
+    fclose(f);
+}
+
+/*
+  test behaviour with TreatLowAsMin, we expect attenuation to decrease
+  as we get close to zero source frequency and phase lag to approach
+  zero
+ */
+TEST(NotchFilterTest, HarmonicNotchTest3)
+{
+    const float min_freq = 1.0;
+    const float max_freq = 200;
+    const uint16_t steps = 1000;
+
+    const char *csv_file = "harmonicnotch_test3.csv";
+    FILE *f = fopen(csv_file, "w");
+    fprintf(f, "Freq(Hz),Attenuation1(dB),Lag1(deg),Attenuation2(dB),Lag2(deg)\n");
+
+    const float source_freq = 35;
+    for (uint16_t i=0; i<steps; i++) {
+        float attenuation_dB1, phase_lag1;
+        float attenuation_dB2, phase_lag2;
+        const float test_freq = min_freq + i*(max_freq-min_freq)/steps;
+        // first with TreatLowAsMin
+        test_one_filter(50, 30, 25, test_freq, source_freq, 1, uint16_t(HarmonicNotchFilterParams::Options::TreatLowAsMin),
+                        phase_lag1,
+                        attenuation_dB1);
+        // and without
+        test_one_filter(50, 30, 25, test_freq, source_freq, 1, 0,
+                        phase_lag2,
+                        attenuation_dB2);
+        fprintf(f, "%.3f,%f,%f,%f,%f\n", test_freq, attenuation_dB1, phase_lag1, attenuation_dB2, phase_lag2);
+
+        // the phase lag with the attenuation adjustment should not be
+        // more than without for frequencies below min freq
+        if (test_freq < 50) {
+            EXPECT_LE(phase_lag2, phase_lag1);
+        }
+    }
+    fclose(f);
+}
+
+/*
+  show the progress of a multi-harmonic notch as the source frequency drops below the min frequency
+ */
+TEST(NotchFilterTest, HarmonicNotchTest4)
+{
+    const float min_freq = 1.0;
+    const float max_freq = 250;
+    const uint16_t steps = 1000;
+
+    const char *csv_file = "harmonicnotch_test4.csv";
+    FILE *f = fopen(csv_file, "w");
+    fprintf(f, "Freq(Hz),60Hz(dB),50Hz(dB),40Hz(dB),30Hz(dB),20Hz(dB),10Hz(dB),0Hz(dB)\n");
+
+    for (uint16_t i=0; i<steps; i++) {
+        float phase_lag;
+        const float source_freq[7] { 60, 50, 40, 30, 20, 10, 0 };
+        float attenuations[7];
+        const float test_freq = min_freq + i*(max_freq-min_freq)/steps;
+        for (uint8_t F = 0; F < ARRAY_SIZE(source_freq); F++) {
+            test_one_filter(50, 30, 25, test_freq, source_freq[F], 15, 0,
+                            phase_lag,
+                            attenuations[F]);
+        }
+        fprintf(f, "%.3f,%f,%f,%f,%f,%f,%f,%f\n",
+                test_freq,
+                attenuations[0], attenuations[1], attenuations[2],
+                attenuations[3], attenuations[4], attenuations[5],
+                attenuations[6]);
+    }
+    fclose(f);
+}
+
 AP_GTEST_MAIN()