Line data    Source code

       1             : #pragma once
       2             : 
       3             : #include <AH/STL/cmath>
       4             : #include <Filters/SOSFilter.hpp>
       5             : 
       6             : /// @addtogroup FilterDesign
       7             : /// @{
       8             : 
       9             : /** 
      10             :  * @brief   Compute Butterworth filter coefficients.
      11             :  * @tparam  N
      12             :  *          Order of the filter.
      13             :  * @param   f_n
      14             :  *          Normalized cut-off frequency in half-cycles per sample.  
      15             :  *          @f$ f_n = \frac{2 f_c}{f_s} \in \left[0, 1\right] @f$, where
      16             :  *          @f$ f_s @f$ is the sample frequency in @f$ \text{Hz} @f$, and 
      17             :  *          @f$ f_c @f$ is the cut-off frequency in @f$ \text{Hz} @f$.
      18             :  * @param   normalize
      19             :  *          If true, normalize all coefficients such that @f$ a_0 = 1 @f$.
      20             :  * @tparam  T
      21             :  *          The type of the coefficients.
      22             :  * 
      23             :  * @see <https://tttapa.github.io/Pages/Mathematics/Systems-and-Control-Theory/Digital-filters/Discretization/Discretization-of-a-fourth-order-Butterworth-filter.html#discretization-using-second-order-sections-sos>
      24             :  */
      25             : template <uint8_t N, class T = float>
      26          12 : SOSCoefficients<T, (N + 1) / 2> butter_coeff(double f_n,
      27             :                                              bool normalize = true) {
      28          12 :     const double gamma = 1 / std::tan(M_PI * f_n / 2); // pre-warp factor
      29             : 
      30          24 :     auto make_sos = [=](uint8_t k) {
      31           6 :         const double gamma2 = gamma * gamma;
      32           6 :         const double alpha = 2 * std::cos(2 * M_PI * (2 * k + N + 1) / (4 * N));
      33             :         return BiQuadCoefficients<T>{
      34             :             {{T(1.), T(2.), T(1.)}}, // b0, b1, b2
      35             :             {{
      36           6 :                 T(gamma2 - alpha * gamma + 1), // a0
      37           6 :                 T(2 * (1 - gamma2)),           // a1
      38           6 :                 T(gamma2 + alpha * gamma + 1), // a2
      39             :             }},
      40          12 :         };
      41             :     };
      42          16 :     auto make_fos = [=]() {
      43             :         return BiQuadCoefficients<T>{
      44             :             {{T(1.), T(1.)}}, // b0, b1
      45             :             {{
      46           1 :                 T(gamma + 1), // a0
      47           1 :                 T(1 - gamma), // a1
      48             :             }},
      49           1 :         };
      50             :     };
      51             : 
      52         113 :     auto make_sos_norm = [=](uint8_t k) {
      53          71 :         const double gamma2 = gamma * gamma;
      54          71 :         const double alpha = 2 * std::cos(2 * M_PI * (2 * k + N + 1) / (4 * N));
      55          71 :         const double a0 = gamma2 - alpha * gamma + 1;
      56             :         return BiQuadCoefficients<T>{
      57          41 :             {{T(1. / a0), T(2. / a0), T(1. / a0)}}, // b0, b1, b2
      58             :             {{
      59             :                 T(1.),                                // a0
      60          71 :                 T(2 * (1 - gamma2) / a0),             // a1
      61          71 :                 T((gamma2 + alpha * gamma + 1) / a0), // a2
      62             :             }},
      63         101 :         };
      64             :     };
      65          14 :     auto make_fos_norm = [=]() {
      66           2 :         const double a0 = gamma + 1;
      67             :         return BiQuadCoefficients<T>{
      68           2 :             {{T(1. / a0), T(1. / a0)}}, // b0, b1
      69             :             {{
      70             :                 T(1.),               // a0
      71           2 :                 T((1 - gamma) / a0), // a1
      72             :             }},
      73           2 :         };
      74             :     };
      75             : 
      76          12 :     SOSCoefficients<T, (N + 1) / 2> sections;
      77             : 
      78             :     if (N % 2 == 0) {
      79          77 :         for (uint8_t i = 0; i < sections.length; ++i)
      80          68 :             sections[i] = normalize ? make_sos_norm(i) : make_sos(i);
      81             :     } else {
      82          12 :         for (uint8_t i = 0; i < sections.length - 1; ++i)
      83           9 :             sections[i] = normalize ? make_sos_norm(i) : make_sos(i);
      84           3 :         sections.end()[-1] = normalize ? make_fos_norm() : make_fos();
      85             :     }
      86          24 :     return sections;
      87             : }
      88             : 
      89             : /** 
      90             :  * @brief   Create a Butterworth filter, implemented as Second Order Sections
      91             :  *          (SOS) filter.
      92             :  * @tparam  N
      93             :  *          Order of the filter.
      94             :  * @param   f_n
      95             :  *          Normalized cut-off frequency in half-cycles per sample.  
      96             :  *          @f$ f_n = \frac{2 f_c}{f_s} \in \left[0, 1\right] @f$, where
      97             :  *          @f$ f_s @f$ is the sample frequency in @f$ \text{Hz} @f$, and 
      98             :  *          @f$ f_c @f$ is the cut-off frequency in @f$ \text{Hz} @f$.
      99             :  * @tparam  T
     100             :  *          The type of filter values and coefficients.
     101             :  * @param   normalize
     102             :  *          If true, normalize all coefficients such that @f$ a_0 = 1 @f$.
     103             :  * @tparam  Implementation
     104             :  *          The BiQuad implementation to use.
     105             :  * 
     106             :  * @see <https://tttapa.github.io/Pages/Mathematics/Systems-and-Control-Theory/Digital-filters/Discretization/Discretization-of-a-fourth-order-Butterworth-filter.html#discretization-using-second-order-sections-sos>
     107             :  */
     108             : template <uint8_t N, class T = float, class Implementation = BiQuadFilterDF1<T>>
     109          11 : SOSFilter<T, (N + 1) / 2, Implementation> butter(double f_n,
     110             :                                                  bool normalize = true) {
     111          22 :     return butter_coeff<N, T>(f_n, normalize);
     112             : }
     113             : 
     114             : /// @}