/************************************************************************
* 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对应函数实现,验证结果一致。