Line data    Source code

       1             : #pragma once
       2             : 
       3             : #include <AH/Containers/ArrayHelpers.hpp>
       4             : #include <Filters/BiQuad.hpp>
       5             : 
       6             : /// @addtogroup Filters
       7             : /// @{
       8             : 
       9             : template <class T, size_t N>
      10             : using SOSCoefficients = AH::Array<BiQuadCoefficients<T>, N>;
      11             : 
      12             : /**
      13             :  * @brief   Second Order Sections filter. Higher cost than direct 
      14             :  *          implementation, but better numerical stability.
      15             :  * 
      16             :  * @tparam  T 
      17             :  *          The type of the signals and filter coefficients.
      18             :  * @tparam  N 
      19             :  *          The number of sections.
      20             :  */
      21             : template <class T, size_t N, class Implementation = BiQuadFilterDF1<T>>
      22             : class SOSFilter {
      23             :   public:
      24             :     /// Constructor.
      25          12 :     SOSFilter(const SOSCoefficients<T, N> &sectionCoefficients)
      26          12 :         : sections(AH::copyAs<Implementation>(sectionCoefficients)) {}
      27             : 
      28             :     /**
      29             :      * @brief   Update the internal state with the new input @f$ x[n] @f$ and
      30             :      *          return the new output @f$ y[n] @f$.
      31             :      * 
      32             :      * @param   input 
      33             :      *          The new input @f$ x[n] @f$.
      34             :      * @return  The new output @f$ y[n] @f$.
      35             :      */
      36         600 :     T operator()(T input) {
      37        6656 :         for (auto &section : sections)
      38        6056 :             input = section(input);
      39         600 :         return input;
      40             :     }
      41             : 
      42             :   private:
      43             :     AH::Array<Implementation, N> sections;
      44             : };
      45             : 
      46             : /// @}
      47             : 
      48             : template <class T, size_t M, size_t N>
      49             : TransferFunction<M + N * 2, M + N * 2, T>
      50           2 : sos2tf_helper(const TransferFunction<M, M, T> &tf,
      51             :               AH::ArraySlice<BiQuadCoefficients<T>, N, false, true> sos) {
      52           2 :     auto sub_tf = sos2tf_helper(tf, sos.template slice<0, N - 2>());
      53             :     return TransferFunction<M + N * 2, M + N * 2, T>{
      54           4 :         AH::distribute(sub_tf.b, sos[N - 1].b),
      55           4 :         AH::distribute(sub_tf.a, sos[N - 1].a),
      56           4 :     };
      57             : }
      58             : 
      59             : template <class T, size_t M>
      60             : TransferFunction<M + 2, M + 2, T>
      61           1 : sos2tf_helper(const TransferFunction<M, M, T> &tf,
      62             :               AH::ArraySlice<BiQuadCoefficients<T>, 1, false, true> sos) {
      63             :     return TransferFunction<M + 2, M + 2, T>{
      64           2 :         AH::distribute(tf.b, sos[0].b),
      65           2 :         AH::distribute(tf.a, sos[0].a),
      66           2 :     };
      67             : }
      68             : 
      69             : /**
      70             :  * @brief   Convert Second Order Section (SOS) coefficients to an equivalent 
      71             :  *          tranfer function representation.
      72             :  * @ingroup    FilterDesign
      73             :  */
      74             : template <class T, size_t N>
      75             : TransferFunction<N * 2 + 1, N * 2 + 1, T>
      76           1 : sos2tf(const SOSCoefficients<T, N> &sos) {
      77           1 :     return sos2tf_helper(sos[N - 1], sos.template slice<0, N - 2>());
      78             : }
      79             : 
      80             : template <class T>
      81             : TransferFunction<3, 3, T> sos2tf(const SOSCoefficients<T, 1> &sos) {
      82             :     return sos[0];
      83             : }