matlab buttord函数 C实现

buttord

/************************************************************************
 * w是频率范围数组，由ws和wp组成
 * nw为w数组长度,低通或高通长度为2，带通或带阻长度为4
 * rp为通带波纹
 * rs为阻带衰减
 * order为所求最小阶数
 * wn为所求最小阶数对应的频率数组
*/
void buttord(double* w,int nw,double rp,double rs,int* order,double* wn)
{
	int type=0;
	double* wp=(double*)malloc(sizeof(double)*nw/2);
	double* ws=(double*)malloc(sizeof(double)*nw/2);
	double* WA=(double*)malloc(sizeof(double)*nw/2);
	double* WN=(double*)malloc(sizeof(double)*nw/2);
	double minWA, W0;

	for(int i=0;i<nw/2;i++)
	{
		wp[i]=w[i];
		ws[i]=w[nw/2+i];
	}

	if(nw==2)
	{
		type=0;
	}
	else if(nw==4)
	{
		type=2;
	}
	if(wp[0]<ws[0])
	{
		type+=1;
	}
	else
	{
		type+=2;
	}


	for(int i=0;i<nw/2;i++)
	{
		wp[i]=tan(PI*wp[i]/2.0);
		ws[i]=tan(PI*ws[i]/2.0);
	}

	if (type == 1)
	{
		for (int i = 0; i < nw / 2; i++)
		{
			WA[i] = ws[i] / wp[i];
		}
	}
	else if(type==2)
	{
		for (int i = 0; i < nw / 2; i++)
		{
			WA[i] = wp[i] / ws[i];
		}
	}
	else if (type == 3)
	{
		wp[0] = lclfminbnd(wp[0], ws[0] - 1e-12,0, wp, ws, rs, rp, 0);
		wp[1] = lclfminbnd(ws[1] + 1e-12, wp[1],1, wp, ws, rs, rp, 0);		
	}
	else if(type==4)
	{
		for (int i = 0; i < nw / 2; i++)
		{
			WA[i] = (ws[i] * ws[i] - wp[0] * wp[1]) / (ws[i] * (wp[0] - wp[1]));
		}
	}

	minWA=INFINITY;
	for (int i = 0; i < nw / 2; i++)
	{
		double absValue = fabs(WA[i]);
		minWA = fmin(minWA, absValue);
	}

	*order =  ceil(log10((pow(10, 0.1 * fabs(rs)) - 1.0)/(pow(10, 0.1 * fabs(rp)) - 1.0)) / (2.0 * log10(minWA)));

	W0 = minWA / (pow((pow(10, .1 * fabs(rs)) - 1), (1 / (2 * fabs(*order)))));

	if (type == 1) {  // low
        WN[0] = W0 * wp[0];
    } else if (type == 2) {  // high
        WN[0] = wp[0] / W0;
    } else if (type == 3) {  // stop
        WN[0] = fabs(((wp[1] - wp[0]) + sqrt(pow((wp[1] - wp[0]), 2) + 4 * pow(W0, 2) * wp[0] * wp[1])) / (2 * W0));
        WN[1] = fabs(((wp[1] - wp[0]) - sqrt(pow((wp[1] - wp[0]), 2) + 4 * pow(W0, 2) * wp[0] * wp[1])) / (2 * W0));
        // Sorting WN in ascending order
        if (WN[0] > WN[1]) {
            double temp = WN[0];
            WN[0] = WN[1];
            WN[1] = temp;
        }
    } else if (type == 4) {  // pass
        double W0Array[2] = {-W0, W0};
        WN[0] = -W0Array[0] * (wp[1] - wp[0]) / 2 + sqrt(pow(W0Array[0], 2) / 4 * pow((wp[1] - wp[0]), 2) + wp[0] * wp[1]);
        WN[1] = -W0Array[1] * (wp[1] - wp[0]) / 2 + sqrt(pow(W0Array[1], 2) / 4 * pow((wp[1] - wp[0]), 2) + wp[0] * wp[1]);
        // Sorting WN in ascending order
        if (WN[0] > WN[1]) {
            double temp = WN[0];
            WN[0] = WN[1];
            WN[1] = temp;
        }
    }
	for (int i = 0; i < nw / 2; i++) {
        wn[i]=(2/PI)*atan(WN[i]);
    }

	free(wp);
	free(ws);
	free(WA);
	free(WN);
}

其中lclfminbnd代码如下：

double lclfminbnd(double ax, double bx,int ind,double* wp,double* ws,double rs,double rp,int type) 
{
	// Initialization
    double seps = sqrt(EPS);
    double c = 0.5 * (3.0 - sqrt(5.0));
    double a = ax, b = bx;
    double v = a + c * (b - a);
    double w = v, xf = v;
    double d = 0.0, e = 0.0;
    double x = xf;
    double fx = bscost(x,ind,wp,ws,2,rs,rp,type);
    int num = 1;
    double fv = fx, fw = fx;
    double xm = 0.5 * (a + b);
	double tol=1.0e-4;
    double tol1 = seps * fabs(xf) + tol / 3.0;
    double tol2 = 2.0 * tol1;

	// Main loop
	while (fabs(xf - xm) > (tol2 - 0.5 * (b - a)))
	{
		int gs = 1;
		if (fabs(e) > tol1)
		{
			gs = 0;
			// Parabolic interpolation
			double r = (xf - w) * (fx - fv);
			double q = (xf - v) * (fx - fw);
			double p = (xf - v) * q - (xf - w) * r;
			q = 2.0 * (q - r);
			if (q > 0.0)
			{
				p = -p;
			}
			q = fabs(q);
			double temp = e;
			e = d;
			if ((fabs(p) < fabs(0.5 * q * temp)) && (p > q * (a - xf)) && (p < q * (b - xf)))
			{
				d = p / q;
				x = xf + d;
				// Parabolic interpolation step
				if ((x - a) < tol2 || (b - x) < tol2)
				{
					int si = (xm - xf > 0) - (xm - xf < 0);
					d = tol1 * si;
				}
			}
			else
			{
				gs = 1;
			}
		}
		if (gs)
		{
			if (xf >= xm)
			{
				e = a - xf;
			}
			else
			{
				e = b - xf;
			}
			d = c * e;			
		}
		double si = d >= 0?1.0:-1.0;
		x = xf + si * fmax(fabs(d), tol1);
		double fu = bscost(x,ind,wp,ws,2,rs,rp,type);
		num++;
		// Update variables
		if (fu <= fx)
		{
			if (x >= xf)
			{
				a = xf;
			}
			else
			{
				b = xf;
			}
			v = w;
			fv = fw;
			w = xf;
			fw = fx;
			xf = x;
			fx = fu;
		}
		else
		{ // fu > fx
			if (x < xf)
			{
				a = x;
			}
			else
			{
				b = x;
			}
			if ((fu <= fw) || (w == xf))
			{
				v = w;
				fv = fw;
				w = x;
				fw = fu;
			}
			else if ((fu <= fv) || (v == xf) || (v == w))
			{
				v = x;
				fv = fu;
			}
		}
		xm = 0.5 * (a + b);
		tol1 = seps * fabs(xf) + tol / 3.0;
		tol2 = 2.0 * tol1;
	}

	return xf;
}

又涉及到bscost函数实现：

double bscost(double wp,int ind,double* WP,double* WS,int n,double rs,double rp,int type)
{
    double result;
    double* WA=(double*)malloc(sizeof(double)*n);
    double* tempwp=(double*)malloc(sizeof(double)*n);
    double minWA=INFINITY;
    for (int i = 0; i < n; i++)
    {
        tempwp[i]=WP[i];
    }
    

    tempwp[ind]=wp;
    for (int i = 0; i < n; i++)
    {
       WA[i]=(WS[i]*(tempwp[0]-tempwp[1]))/(WS[i]*WS[i]  - tempwp[0]*tempwp[1]);
    }
    
    for (int i = 0; i < n; i++)
	{
		double absValue = fabs(WA[i]);
		minWA = fmin(minWA, absValue);
	}

    switch (type)
    {
        case 0: // Butter
        {

            result = log10((pow(10, 0.1 * fabs(rs)) - 1.0) / pow(10, 0.1 * fabs(rp)) - 1.0) / (2 * log10(minWA));
            break;
        }
        case 1: // cheby
        {
            result = acosh(sqrt((pow(10, 0.1 * fabs(rs)) - 1.0) / (pow(10, 0.1 * fabs(rp)) - 1.0))) / acosh(minWA);
            break;
        }
        case 2: // ellip
        {
            double epsilon;
            double k1;
            double k;
            // Assuming ellipke function is already defined
            double *ks = (double *)malloc(sizeof(double) * 2);
            double *ks1 = (double *)malloc(sizeof(double) * 2);
            epsilon = sqrt(pow(10, 0.1 * rp) - 1.0);
            k1 = epsilon / sqrt(pow(10, 0.1 * rs) - 1.0);
            k = 1.0 / minWA;
            ks[0] = k * k;
            ks[1] = 1 - k * k;
            ks1[0] = k1 * k1;
            ks1[1] = 1 - k1 * k1;
            double *capk = ellipke(ks, 2);
            double *capk1 = ellipke(ks1, 2);
            result = (capk[0] * capk1[1]) / (capk[1] * capk1[0]);
            free(ks);
            free(ks1);
            free(capk);
            free(capk1);
            break;
        }
        default:
            break;
    }

    free(tempwp);
    free(WA);
    return result;
    
}

以上函数都经过和matlab对应函数实现，验证结果一致。