Flter: notchfilter: remove unneeded value and pre-multiply for speed

ArduPilot · Jul 8, 2023 · 1cae820 · 1cae820
1 parent d986668
commit 1cae820
Show file tree

Hide file tree

Showing 3 changed files with 18 additions and 8 deletions.
diff --git a/libraries/Filter/NotchFilter.cpp b/libraries/Filter/NotchFilter.cpp
@@ -77,9 +77,18 @@ void NotchFilter<T>::init_with_A_and_Q(float sample_freq_hz, float center_freq_h
         b0 =  1.0 + alpha*sq(A);
         b1 = -2.0 * cosf(omega);
         b2 =  1.0 - alpha*sq(A);
-        a0_inv =  1.0/(1.0 + alpha);
         a1 = b1;
         a2 =  1.0 - alpha;
+
+        const float a0_inv =  1.0/(1.0 + alpha);
+
+        // Pre-multiply to save runtime calc
+        b0 *= a0_inv;
+        b1 *= a0_inv;
+        b2 *= a0_inv;
+        a1 *= a0_inv;
+        a2 *= a0_inv;
+
         _center_freq_hz = new_center_freq;
         _sample_freq_hz = sample_freq_hz;
         initialised = true;
@@ -100,16 +109,17 @@ T NotchFilter<T>::apply(const T &sample)
         // sample as output and update delayed samples
         signal1 = sample;
         signal2 = sample;
-        ntchsig = sample;
         ntchsig1 = sample;
         ntchsig2 = sample;
         need_reset = false;
         return sample;
     }
+
+    T output = sample*b0 + ntchsig1*b1 + ntchsig2*b2 - signal1*a1 - signal2*a2;
+
     ntchsig2 = ntchsig1;
-    ntchsig1 = ntchsig;
-    ntchsig = sample;
-    T output = (ntchsig*b0 + ntchsig1*b1 + ntchsig2*b2 - signal1*a1 - signal2*a2) * a0_inv;
+    ntchsig1 = sample;
+
     signal2 = signal1;
     signal1 = output;
     return output;

diff --git a/libraries/Filter/NotchFilter.h b/libraries/Filter/NotchFilter.h
@@ -41,9 +41,9 @@ class NotchFilter {
 protected:
 
     bool initialised, need_reset;
-    float b0, b1, b2, a1, a2, a0_inv;
+    float b0, b1, b2, a1, a2;
     float _center_freq_hz, _sample_freq_hz;
-    T ntchsig, ntchsig1, ntchsig2, signal2, signal1;
+    T ntchsig1, ntchsig2, signal2, signal1;
 };
 
 /*

diff --git a/libraries/Filter/examples/TransferFunctionCheck/TransferFunctionCheck.cpp b/libraries/Filter/examples/TransferFunctionCheck/TransferFunctionCheck.cpp
@@ -61,7 +61,7 @@ class NotchHelper : public NotchFilterFloat {
         hal.console->printf("NotchFilterFloat\n");
         hal.console->printf("Sample rate: %.9f Hz, Center: %.9f Hz\n", _sample_freq_hz, _center_freq_hz);
         hal.console->printf("Notch filter in the form: H(z) = (b0 + b1*z^-1 + b2*z^-2)/(a0 + a1*z^-1 + a2*z^-2)\n");
-        hal.console->printf("a0: %.9f, a1: %.9f, a2: %.9f, b0: %.9f, b1: %.9f, b2: %.9f\n", 1.0/a0_inv, a1, a2, b0, b1, b2);
+        hal.console->printf("a0: %.9f, a1: %.9f, a2: %.9f, b0: %.9f, b1: %.9f, b2: %.9f\n", 1.0, a1, a2, b0, b1, b2);
     }
 };