Optimize runtime configurable version

mike919192 · Jul 25, 2024 · 14b5541 · 14b5541
1 parent 6271ede
commit 14b5541
Showing 1 changed file with 24 additions and 15 deletions.
diff --git a/include/sdsp/casc2orderIIR.h b/include/sdsp/casc2orderIIR.h
@@ -62,7 +62,7 @@ class casc2orderIIR {
             uint j;
 
             for (j = 0; j < M; j++) {
-                y.at(j + 1).at(p) = y.at(j).at(p) * b.at(j).at(0);
+                y.at(j + 1).at(p) = y.at(j).at(p);
 
                 y.at(j + 1).at(p) += y.at(j).at(d1) * b.at(j).at(j1) - y.at(j + 1).at(d1) * a.at(j).at(j1);
                 y.at(j + 1).at(p) += y.at(j).at(d2) * b.at(j).at(j2) - y.at(j + 1).at(d2) * a.at(j).at(j2);
@@ -119,12 +119,14 @@ class casc2orderIIR {
             double alpha1{ (0.5 - beta1) * t / 2.0 };
             double alpha2{ (0.5 - beta2) * t / 2.0 };
 
-            bCoeff.at(2 * k).at(0) = 2 * alpha1;
-            bCoeff.at(2 * k + 1).at(0) = 2 * alpha2;
+            gain *= 4 * alpha1 * alpha2;
+
+            bCoeff.at(2 * k).at(0) = 1.0;
+            bCoeff.at(2 * k + 1).at(0) = 1.0;
             bCoeff.at(2 * k).at(1) = 0;
             bCoeff.at(2 * k + 1).at(1) = 0;
-            bCoeff.at(2 * k).at(2) = -bCoeff.at(2 * k).at(0);
-            bCoeff.at(2 * k + 1).at(2) = -bCoeff.at(2 * k + 1).at(0);
+            bCoeff.at(2 * k).at(2) = -1.0;
+            bCoeff.at(2 * k + 1).at(2) = -1.0;
 
             aCoeff.at(2 * k).at(0) = 1;
             aCoeff.at(2 * k + 1).at(0) = 1;
@@ -149,11 +151,13 @@ class casc2orderIIR {
 
             double beta1{ (1 - t) / dnm / 2 };
             double gamma1{ (0.5 + beta1) * std::cos(e0) };
-            double alpha1{ (0.5 + beta1 + gamma1) / 4 }; //-
+            double alpha1{ (0.5 + beta1 + gamma1) / 4 };
 
-            bCoeff.at(k).at(0) = 2 * alpha1;
-            bCoeff.at(k).at(1) = -2 * bCoeff.at(k).at(0); //-
-            bCoeff.at(k).at(2) = bCoeff.at(k).at(0);
+            gain *= 2 * alpha1;
+
+            bCoeff.at(k).at(0) = 1.0;
+            bCoeff.at(k).at(1) = -2.0;
+            bCoeff.at(k).at(2) = 1.0;
 
             aCoeff.at(k).at(0) = 1;
             aCoeff.at(k).at(1) = -2 * gamma1;
@@ -177,9 +181,11 @@ class casc2orderIIR {
             double gamma1{ (0.5 + beta1) * std::cos(e0) };
             double alpha1{ (0.5 + beta1 - gamma1) / 4 };
 
-            bCoeff.at(k).at(0) = 2 * alpha1;
-            bCoeff.at(k).at(1) = 2 * bCoeff.at(k).at(0);
-            bCoeff.at(k).at(2) = bCoeff.at(k).at(0);
+            gain *= 2 * alpha1;
+
+            bCoeff.at(k).at(0) = 1.0;
+            bCoeff.at(k).at(1) = 2.0;
+            bCoeff.at(k).at(2) = 1.0;
 
             aCoeff.at(k).at(0) = 1;
             aCoeff.at(k).at(1) = -2 * gamma1;
@@ -190,14 +196,17 @@ class casc2orderIIR {
     // preload the filter memory for steady state input equal to value parameter
     void PreloadFilter(double value)
     {
+        double preload_value = value * gain;
         std::array<std::array<double, 3>, M + 1> memVals{ 0 };
         for (int i = 0; i < 3; i++) {
-            memVals.at(0).at(i) = value;
+            memVals.at(0).at(i) = preload_value;
         }
         if (fType == FilterType::LowPass) {
             for (uint j = 1; j < M + 1; j++) {
+                preload_value /= 1 + aCoeff.at(j - 1).at(1) + aCoeff.at(j - 1).at(2);
+                preload_value *= bCoeff.at(j - 1).at(0) + bCoeff.at(j - 1).at(1) + bCoeff.at(j - 1).at(2);
                 for (uint i = 0; i < 3; i++) {
-                    memVals.at(j).at(i) = value;
+                    memVals.at(j).at(i) = preload_value;
                 }
             }
         }
@@ -457,4 +466,4 @@ class casc_2o_IIR_bp : casc_2o_IIR<M> {
         }
     }
 };
-}
+}