Цифровые рекурсивные фильтры. Часть 1.

#include <math.h>
    double F0,Fs,Pi,Psi,Psi0,Psi2,Psi3,Psi4,Psi5,a0Psi,z1,z2;
    double b_1,b_2,b_3,b_4,b_5,a0,a_1,a_2,a_3,a_4,a_5,Omega_p,O2,O3,O4,O5;
    double A[11], B[11];
    int i;
    void main(void){
    //!F0- относительная центральная частота
    //!A,B - выходные массивы коэффициентов
    Pi=3.1415926535897932;
    F0=0.3;
    Fs=0.2;   //!Частота среза ФНЧ (относительная) или полоса фильтра
    Omega_p=tan(Fs*0.5*Pi);
    O2=Omega_p*Omega_p;
    O3=O2*Omega_p;
    O4=O3*Omega_p;
    O5=O4*Omega_p;
    //!Эллиптический фильтр
    b_1=0.02974*Omega_p;
    b_2=0.;
    b_3=0.16652*O3;
    b_4=0.;
    b_5=0.2042*O5;
    a_1=0.92546*Omega_p;
    a_2=1.81487*O2;
    a_3=1.09691*O3;
    a_4=0.74668*O4;
    a_5=0.20420*O5;
    // Или    Фильтр Чебышева 2 рода    (коэффициенты из статьи)
    //b_1=0.15819*Omega_p;
    //b_2=0.;
    //b_3=0.63277*O3;
    //b_4=0.;
    //b_5=0.50622*O5;
    //a_1=2.64719*Omega_p;
    //a_2=3.49129*O2;
    //a_3=2.91421*O3;
    //a_4=1.51981*O4;
    //a_5=0.50622*O5;

    Psi0=1./cos(Fs*0.5*Pi);
    Psi=-2.*cos(F0*Pi)*Psi0;
    Psi2=Psi*Psi;
    Psi3=Psi2*Psi;
    Psi4=Psi3*Psi;
    Psi5=Psi4*Psi;
    a0=1./(1+a_1+a_2+a_3+a_4+a_5);
    a0Psi=a0*Psi;
    A[0]=1.;
    A[10]=a0*(1-a_1+a_2-a_3+a_4-a_5);
    z1=a0Psi*(5+3* a_2+a_4);
    z2=-2*a0Psi*(2*a_1+ a_3);
    A[1]=z1-z2;
    A[9]=z1+z2;
    z1=a0*(5+Psi2*(10+3*a_2)+a_2-3*a_4);
    z2=a0*(-(3+6*Psi2)*a_1-(Psi2-1)*a_3+5*a_5);
    A[2]=z1-z2;
    A[8]=z1+z2;
    z1=a0Psi*(20+Psi2*(10+ a_2)-4* a_4);
    z2=a0Psi*(-(8+4*Psi2)*a_1+4*a_3);
    A[3]=z1-z2;
    A[7]=z1+z2;
    z1=a0*(10+30*Psi2+5*Psi4-(2+3*Psi2)*a_2+2*a_4);
    z2=a0*(-(2+6*Psi2+Psi4)*a_1+(2+3*Psi2)*a_3-10*a_5);
    A[4]=z1-z2;
    A[6]=z1+z2;
    A[5]=a0Psi*(30+20*Psi2+Psi4-(6+2*Psi2)*a_2+6* a_4);
    B[0]=a0*(b_1+b_2+b_3+b_4+b_5);
    B[10]=a0*(-b_1+b_2-b_3+b_4-b_5);
    z1=a0Psi*(+3* b_2+ b_4);
    z2=a0Psi*(-4*b_1-2*b_3);
    B[1]=z1-z2;
    B[9]=z1+z2;
    z1=a0*(+(1+3*Psi2)*b_2-3*b_4);
    z2=a0*(-(3+6*Psi2)*b_1-(Psi2-1)*b_3+5*b_5);
    B[2]=z1-z2;
    B[8]=z1+z2;
    z1=a0Psi*(Psi2*b_2-4* b_4);
    z2=a0Psi*(-(8+4*Psi2)*b_1+4* b_3);
    B[3]=z1-z2;
    B[7]=z1+z2;
    z1=a0*(2*b_4);
    z2=a0*(-(2+6*Psi2+Psi4)*b_1+(2+3*Psi2)*b_3-10*b_5);
    B[4]=z1-z2;
    B[6]=z1+z2;
    B[5]=a0Psi*(-(6+2*Psi2)*b_2+6* b_4);
    return;             
    } 

#include <math.h>
float b1,b2,b3,a0,a1,a5,zz,Psi,Psi2,Beta;
float A[7],B[7];
void BP_Filter(float F0){
Psi=cos(F0*PI)*zz;
Psi2=Psi*Psi;
A[1]=Psi*a1;
A[2]=a0*((Psi2+1.)*(3.+b1)-b2-3.*b3);
A[3]=a0*Psi*(Psi2+6.-2*b2);
A[4]=a0*((Psi2+1.)*(3.-b1)-b2+3.*b3);
A[5]=Psi*a5;
return;}

void BP_Filter_Init(float Fs){
float z;
z=Fs*0.5*PI;
Beta=tan(z);
zz=-2./cos(z);
b1=2.*Beta;
b3=Beta*Beta;
b2=2.*b3;
b3=Beta*b3;
a0=1.0/(1.+b1+b2+b3);
A[0]=1.;
a1=a0*(3.+2.*b1+b2);
a5=a0*(3.-2.*b1+b2);
A[6]=a0*(1.-b1+b2-b3);
B[0]=b3*a0;
B[1]=0.;
B[2]=-3.*B[0];
B[3]=0.;
B[4]=-B[2];
B[5]=0.;
B[6]=-B[0]; 
return;
}
void main(void){
float F0,Fs;
int i,N;
N=10;
Fs=0.1;  // полоса фильтра
BP_Filter_Init(Fs);  //
for(i=1;i<N;i++){
F0=0.3+i/20;
BP_Filter(F0);  }
return;             
}