matlab cheby2函数 C实现

继上篇matlab cheby1函数 C实现

这篇实现cheby2(切比雪夫而型)

void cheby2(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
    int nz = n % 2 == 0 ? n : n - 1;
    Complex *z = (Complex *)malloc(nz * sizeof(Complex));
    Complex *p = (Complex *)malloc(n * sizeof(Complex));
    double k = 0;
    cheb2ap(n, r, z, 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, nz, 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);
    }

    Complex *p1 = (Complex *)malloc(sizeof(Complex) * a_size);
    Complex *z1 = (Complex *)malloc(sizeof(Complex) * a_size);
    ss2zp(a, b, c, &d, a_size, z1, p1, &k); // 需要完善

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

    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);
     for (size_t i = 0; i < n+1; i++)
    {
        p1[i].real=u_real[i];
        p1[i].imaginary=v_imag[i];
    }

    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(a);
    free(b);
    free(p);
    // free(z1);
    free(p1);
    free(u_real);
    free(v_imag);
    free(tempa);
    free(tempb);
}

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

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

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

    int m = iseven ? n : n - 1;
    Complex *tempz = (Complex *)malloc(sizeof(Complex) * m);
    double *realz = (double *)malloc(sizeof(double) * m);
    if (iseven)
    {
        for (size_t i = 0; i < m; i++)
        {
            z[i].real = i * 2.0 + 1.0;
        }
    }
    else
    {
        for (int i = 0; i < m; i++)
        {
            if (i < n / 2)
            {
                z[i].real = i * 2.0 + 1.0;
            }
            else
            {
                int j = i - n / 2;
                z[i].real = (n + 2.0) + j * 2.0;
            }
        }
    }

    if (m != 0)
    {
        for (size_t i = 0; i < m; i++)
        {
            z[i].real = cos(z[i].real * PI / (2.0 * n));
            realz[i] = z[i].real;
        }
        flipud(realz, m);
        for (size_t i = 0; i < m; i++)
        {
            tempz[i].real = (z[i].real - realz[i]) / 2.0;
            z[i] = complex_divide(create(0, 1), tempz[i]);
        }
        for (size_t i = 0; i < m; i++)
        {
            tempz[i] = z[i];
        }

        int *index = (int *)malloc(sizeof(int) * n);

        for (size_t i = 0; i < m; i++)
        {
            if (i < m / 2)
            {
                index[i] = i + 1;
            }
            else
            {
                index[i] = m - (i - m / 2);
            }
        }

        for (size_t i = 0; i < m / 2; i++)
        {
            for (size_t j = 0; j < 2; j++)
            {
                z[i * 2 + j] = tempz[index[j * (m / 2) + i] - 1];
            }
        }

        free(tempz);
        free(realz);
        free(index);
    }

    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] = complex_divide(create(1.0, 0.0), create(smu * realp[i], cmu * imagp[i]));
    }


    if(m==0)
    {
        *k = get_real(complex_divide(complex_array_prod(complex_array_multiply_double(p, -1.0, n), n), create(1.0,0.0)));
    }
    else
    {
        *k = get_real(complex_divide(complex_array_prod(complex_array_multiply_double(p, -1.0, n), n), complex_array_prod(complex_array_multiply_double(z, -1.0, m), m)));
    }
    

    free(realp);
    free(imagp);
    free(tempreal);
    free(tempimag);
}

这里没有cheb2zeros，原代码如下：

  % Transform to zero-pole-gain and polynomial forms:
    [z,k] = ltipack.sszeroCG(a,b,c,d,[]);

这里是直接调用ltipack中状态空间模型求得零极点模型的，由于多方查询无果，只能借鉴python中SCIPY库的ss2zp函数

而这个ss2zp函数是由ss2tf函数（状态空间模型转传递函数模型）和tf2zp函数（传递函数模型转零极点模型）曲线救国而来。

通过matlab中相同函数对比，是存在过程精度丢失的。

测试

与matlab测试对比，和切比雪夫Ⅰ型类似，n>30阶出现过程数据达到病态条件，矩阵求解出现问题，1-30阶基本无差别。

后面会专门研究这个三个模型的相互转换，以及对于的实现代码，这里不过多解释。

下一篇： matlab ellip函数 C实现