Holters/Zoelzer Peakfilter in C++

Base Variables

First we have to calculate the variables: cm, K, V and a0m^-1

Inputs are:
m = the filter's index, 1 based
M = the filter order
g = gain (linear, not in dB)
SampleRate
fu, fl (upper & lower cutoff frequency)

for a single filter block I set m=1 and M=4

cm = cos( ( 1/2 - (2m-1)/2M)*Pi );

K =  tan(OmegaB/2)/ g^(1/2M)   =  tan(Pi * (fu - fl) / SampleRate))  /  g^(1/2*M)

V = (g^(1/M)) - 1;

a0[m]^-1 = 1 / 1 + K * cm + K^2

In my code
cm = m_fCosM,
K = m_fK,
V = m_fV,
a0[m]^-1 = m_fInvA0
m = lIxFilter   and
M = m_lTotalFilterOrder
 

 void CEq4::SetFilter(long lIxFilter, long lNumCascadedFilters, float fSampleRate, float fFreqMin, float fFreqMax, float fAmplitude)
{
m_lTotalFilterOrder = lNumCascadedFilters * 4;
 float fOmegaB = 2 * PI * (fFreqMax - fFreqMin) / fSampleRate;
 float fOmegaM = 2 * PI * sqrt(fFreqMax * fFreqMin) / fSampleRate;
 float fAlpha = PI * ( 0.5f - ((2.f * lIxFilter - 1) / (2.f * m_lTotalFilterOrder)) );
 m_fCosM = (float)(cos(fAlpha));
 m_fK = (float)(tan(fOmegaB / 2.f) / pow(fAmplitude, 1.f / 2 * m_lTotalFilterOrder));
 m_fInvA0 = 1.f / 1.f + m_fK * m_fCosM + m_fK * m_fK;
 m_fV = (float)(pow(fAmplitude, 1.f / m_lTotalFilterOrder) - 1);

Allpass Filter

Next step is the allpass filter A(z) in Fig.1

with:

cosOM = cos(OmegaM) = cos(2 * Pi * sqrt(fu * fl))

Eq. 16 is a biquad equation:

               cosOM * z^-1 + z^-2
A(z) = ---------------------------------
               1 - cosOM * z^-1

with:
Alpha0 = 0
Alpha1 = cosOM
Alpha2 = -1
Beta1 = -cosOM
Beta2 = 0

This leads to the function Process of the class CAp in eq4.h:
 

  __forceinline float Process(float fIn)
  {
   m_fPrevIn2 = m_fPrevIn1;
   m_fPrevIn1 = m_fIn;
   m_fIn = fIn;

   float fOut = m_fCosOmM * m_fPrevIn1 - m_fPrevIn2 + m_fCosOmM * m_fPrevOut1;

   m_fPrevOut2 = m_fPrevOut1;
   m_fPrevOut1 = fOut;
   return fOut;
  }

This function is stable and has a unity gain.
 
 
 
 

The main Filter Function 1st attempt

I took Fig. 1 and inserted some intermediate variables :
 
 

That leads to the equations:

l = -2 * B + C
m = 2 * B + C
k = K * m - 2 * cm * C
i = K * k + l
h = K * IN - i
A = h * a0m^-1
n = A + m
o = -A + C
p = K * n - cm * o
q = K * V * n + 2 * p
OUT = V * q + IN

In the C++ Function the instances m_A1 and m_A2 do the allpass job:
 

float CEq4::Filter(float fIn)//Fig. 1
{
 float fB = m_A1.GetOutput();
 float fC = m_A2.GetOutput();

 float l = -2 * fB + fC;
 float m = 2 * fB + fC;
 float k = m_fK * m - 2 * m_fCosM * fC;
 float i = m_fK * k + l;
 float h = m_fK * fIn - i;
 float fA = h * m_fInvA0;
 float n = fA + m;
 float o = -fA + fC;
 float p = m_fK * n - m_fCosM * o;
 float q = m_fK * m_fV * n + 2 * p;
 float fOUT = m_fV * q + fIn;

 float fBNext = m_A1.Process(fA);
 m_A2.Process(fBNext);

 return fOUT;
}

unfortunately this filter is unstable. The first 50 samples of the response to a sine sweep looks like this:

Main Filter Function from Eq. 13

With the equation for a biquad iir:

            a0 + a1 * z^-1 + a2 * z^-2
F(z) = ------------------------------------
            1 +  b1 * z^-1 + b2 * z^-2

the alphas and betas (a,b) are:

a0 = (2 * (K + cm) + V * K) * fact1
a1 = (4 * K + 2 * V * K)  * fact1
a2 = (2 * (K - cm)  + V * K) * fact1
with
fact1 = (V * K) / (1 + 2 * K * cm + K * K)
 

b1 = 2 * (K * K - 1) * fact2
b2 = (1 - 2 * K * cm + K * K) * fact2
with
fact2 = 1 / (1 + 2 * K * cm + K * K)

or in C++ (Function SetFilter(...))
 

 float fFact2 = 1.f / (1.f + 2 * m_fK * m_fCosM + m_fK * m_fK);
 float fFact1 = m_fV * m_fK * fFact2;
 float faxB = m_fV * m_fK;

 m_fa0 = (faxB + 2.f * (m_fK + m_fCosM)) * fFact1;
 m_fa1 = ((2.f * faxB + 4.f * m_fK)) * fFact1;
 m_fa2 = (faxB + 2.f * (m_fK - m_fCosM)) * fFact1;
 m_fb1 = 2.f * (m_fK * m_fK - 1) * fFact2;
 m_fb2 = (1.f - 2.f * m_fK * m_fCosM + m_fK * m_fK );

And this leads to the filter function:
 

float CEq4::Filter1(float fIn)//Eq. 13
{
 float fB = m_A1.GetOutput();
 float fC = m_A2.GetOutput();
 float fA = -m_fb2 * fC - m_fb1 * fB + fIn;
 float fOut = m_fa0 * fA + m_fa1 * fB + m_fa2 * fC + fIn;

 float fBNext = m_A1.Process(fA);
 m_A2.Process(fBNext);

 return fOut;
}

Looks completely different to the first filter function, but unfortunately it's response to the sweep signal is exactly the same.