matlab cheby1函数 C实现

继上一篇文章matlab butter函数 C实现

这一篇实现cheby1(切比雪夫Ⅰ型)

void cheby1(int n, double r, double Wn[], int type, int analog, double *ab, double *bb)
{
    double fs = 2;
    double u[2] = {0.0, 0.0};
    // step 1: get analog, pre-warped frequencies
    if (!analog)
    {
        if (type == 1 || type == 2)
        {
            fs = 2;
            u[0] = 2 * fs * tan(PI * Wn[0] / fs);
        }
        else
        {
            fs = 2;
            u[0] = 2 * fs * tan(PI * Wn[0] / fs);
            u[1] = 2 * fs * tan(PI * Wn[1] / fs);
        }
    }
    else if (type == 3 || type == 4)
    {
        if (type == 1 || type == 2)
        {
            u[0] = Wn[0];
        }
        else
        {
            u[1] = Wn[1];
        }
    }

    // step 2: convert to low-pass prototype estimate
    double Bw = 0.0;
    if (type == 1 || type == 2)
    {
        Wn = u;
    }
    else if (type == 3 || type == 4)
    {
        Bw = u[1] - u[0];
        Wn[0] = sqrt(u[0] * u[1]); // center frequency
        Wn[1] = 0.0;
    }

    // step 3: Get N-th order Butterworth analog lowpass prototype
    Complex *z = (Complex *)malloc(0 * sizeof(Complex));
    Complex *p = (Complex *)malloc(n * sizeof(Complex));
    double k = 0;
    cheb1ap(n, r, p, &k);

    // Transform to state-space
    int a_size = n;
    double *a = (double *)malloc(sizeof(double) * n * n);
    double *b = (double *)malloc(sizeof(double) * n);
    double *c = (double *)malloc(sizeof(double) * n);
    double d;

    zp2ss(z, p, 0, n, k, a, b, c, &d);

    switch (type)
    {
    default:
    case 1: // Lowpass
        lp2lp(n, a, b, Wn[0]);
        break;
    case 2: // highpass
        lp2hp(n, a, b, c, &d, Wn[0]);
        break;
    case 3: // Bandpass
        lp2bp(n, &a, &b, &c, &d, Wn[0], Bw);
        a_size = 2 * n;
        break;
    case 4: // Bandstop
        lp2bs(n, &a, &b, &c, &d, Wn[0], Bw);
        a_size = 2 * n;
        break;
    }


    if (!analog)
    {
        bilinear(a_size, a, b, c, &d, fs);
    }

    
    for (size_t i = 0; i < n; i++)
    {
        for (size_t j = 0; j < n; j++)
        {
            printf("%.20f ",a[i*n+j]);
        }
        printf("\n");
    }

    hhbg(a, a_size);
    double *u_real = (double *)malloc(sizeof(double) * a_size);
    double *v_imag = (double *)malloc(sizeof(double) * a_size);
	double eps = 0.000000000000000000000000000001;
	int jt = 60;
	hhqr(a, a_size, eps, jt, u_real, v_imag);

    Complex *p1 = (Complex *)malloc(sizeof(Complex) * a_size);
    Complex *z1 = (Complex *)malloc(sizeof(Complex) * a_size);
    Complex *ctemp = (Complex *)malloc(sizeof(Complex) * (a_size + 1));

	printf("p:\n");
    
	for (int i = 0; i < a_size; i++)
	{
		p1[i].real = u_real[i];
		p1[i].imaginary = v_imag[i];
        printf("%.20f(%.20f)\n",p1[i].real,p1[i].imaginary);
	}

    cheb1zeros(p1,a_size,type,n,Wn[0],r,z1,&k);

    Complex *tempa = (Complex *)malloc(sizeof(Complex) * (a_size + 1));
    Complex *tempb = (Complex *)malloc(sizeof(Complex) * (a_size + 1));

    complex_poly(p1, a_size, tempa);
    complex_poly(z1, a_size, tempb);

    for (size_t i = 0; i < a_size + 1; i++)
    {
        ab[i] = tempa[i].real;
        bb[i] = tempb[i].real * k;
    }

    free(z);
    free(p);
    free(a);
    free(b);
    free(ctemp);
    free(u_real);
    free(v_imag);   
    free(p1);
    free(z1);
    free(tempa);
    free(tempb);
}

函数中出现比较多的数学计算后面会文章会一一罗列，这里主要介绍切比雪夫Ⅰ型相关的函数

其中cheb1ap为Chebyshev Type I analog lowpass filter prototype,
创建N阶切比雪夫Ⅰ型滤波器的零极点模型，代码如下：

void cheb1ap(int n, double r, Complex *p, double *k)
{
    int iseven = n % 2 == 0;
    double epsilon = sqrt(pow(10, 0.1 * r) - 1); // sqrt(10^(.1*rp)-1)
    double mu = asinh(1.0 / epsilon) / n;

    double *realp = (double *)malloc(sizeof(double) * n);
    double *imagp = (double *)malloc(sizeof(double) * n);
    double *tempreal = (double *)malloc(sizeof(double) * n);
    double *tempimag = (double *)malloc(sizeof(double) * n);
    for (int i = 0; i < n; i++)
    {
        double angle = PI * (1.0 + 2.0 * i) / (2.0 * n) + PI / 2.0;
        p[i] = create(cos(angle), sin(angle));
        realp[i] = get_real(p[i]);
        imagp[i] = get_imaginary(p[i]);
    }
    for (size_t i = 0; i < n; i++)
    {
        tempreal[i] = realp[i];
        tempimag[i] = imagp[i];
    }
    flipud(tempreal, n);
    flipud(tempimag, n);
    double smu = sinh(mu);
    double cmu = cosh(mu);
    for (size_t i = 0; i < n; i++)
    {
        realp[i] = (realp[i] + tempreal[i]) / 2.0;
        imagp[i] = (imagp[i] - tempimag[i]) / 2.0;
        p[i] = create(smu * realp[i], cmu * imagp[i]);
    }

    Complex sump = complex_array_prod(p, n);
    *k = get_real(sump);
    if (iseven)
    {
        *k = *k / sqrt(1 + pow(epsilon, 2));
    }
    free(realp);
    free(imagp);
    free(tempreal);
    free(tempimag);
}

其中cheb1zeros 与buttzeros作用相似，用于计算零点（zeros）和增益(gain)，原代码如下：

function [z,k] = cheb1zeros(btype,n,Wn,analog,p,Rp)
% This internal function returns the exact zeros and gain of the ZPK form.
if iseven(n)
   g = 10^(-Rp/20);  % -Rp decibels
else
   g = 1;  % 0 dB
end
if analog
   % for lowpass and bandpass, don't include zeros at +Inf or -Inf
   switch btype
      case 1  % lowpass: H(0) = g
         z = zeros(0,1);
         k = g * real(prod(-p));
      case 2  % bandpass: H(1i*Wn) = 1
         z = zeros(n,1);
         k = g * real(prod(1i*Wn-p)/(1i*Wn)^n);
      case 3  % highpass: H(inf) = g
         z = zeros(n,1);
         k = g;
      case 4  % bandstop: H(0) = g
         z = 1i*Wn*((-1).^(0:2*n-1)');
         k = g;
   end
else
   Wn = 2*atan2(Wn,4);
   switch btype
      case 1  % lowpass: H(1) = g
         z = -ones(n,1);
         k = g * real(prod(1-p))/2^n;
      case 2  % bandpass: H(z) = g for z=exp(1i*sqrt(Wn(1)*Wn(2)))
         z = [ones(n,1); -ones(n,1)];
         zWn = exp(1i*Wn);
         k = g * real(prod(zWn-p)/prod(zWn-z));
      case 3  % highpass: H(-1) = g
         z = ones(n,1);
         k = g * real(prod(1+p))/2^n;
      case 4  % bandstop: H(1) = g
         z = exp(1i*Wn*( (-1).^(0:2*n-1)' ));  
         k = g * real(prod(1-p)/prod(1-z));
   end
end

C实现如下：

void cheb1zeros(Complex *p, int np, int type, int n, double wn, double r, Complex *z, double *k)
{
    Complex *zt = (Complex *)malloc(sizeof(Complex) * np);
    Complex *pt=(Complex*)malloc(sizeof(Complex)*np);
	Complex zwn;
    wn = 2 * atan2(wn, 4);

    double g=1.0;
    if(n%2==0)
    {
        g=pow(10,-r/20.0);
    }

    switch (type)
    {
    case 1:
       for (size_t i = 0; i < np; i++)
		{
			z[i]=create(-1.0,0);
			pt[i] = complex_subtract(create(1.0,0.0),p[i]);
		}

        *k = get_real(complex_array_prod(pt, np)) / pow(2, n);
        break;
    case 2:
        for (size_t i = 0; i < np; i++)
        {
            z[i] = create(1.0,0.0);
            pt[i] = complex_add(create(1.0, 0.0), p[i]);
        }

        *k = get_real(complex_array_prod(pt, np)) / pow(2, n);
        break;
    case 3:
        zwn = complex_exponent(complex_multiply_double(create(0, 1.0), wn));
        for (size_t i = 0; i < np; i++)
        {
            if (i < n)
            {
                z[i] = create(1.0,0.0);
            }
            else
            {
                z[i] = create(-1.0,0.0);
            }

            zt[i] = complex_subtract(zwn, z[i]);

            pt[i] = complex_subtract(zwn, p[i]);
        }

        *k = get_real(complex_divide(complex_array_prod(pt, np), complex_array_prod(zt, np)));
        break;
    case 4:
        for (size_t i = 0; i < 2*n; i++)
        {
            z[i] = complex_exponent(complex_multiply_double(create(1.0, 0.0), wn * pow(-1, i)));

            zt[i] = complex_subtract(create(1.0,0.0), z[i]);

            pt[i] = complex_subtract(create(1.0,0.0), p[i]);
        }

        *k = get_real(complex_divide(complex_array_prod(pt, np), complex_array_prod(zt, np)));
        break;

    default:
        break;
    }

    *k = *k * g;
    free(zt);
    free(pt);
}

测试

与matlab结果对比测试，当n>34阶时，过程中求解矩阵本征矢量达到病态条件，出现错误，需要重新整理矩阵的本征矢量求解方法；对于1-34阶计算结果基本吻合。

下一篇： matlab cheby2函数 C实现