继上一篇实现matlab cheby2函数 C实现
这篇实现ellip(椭圆)
特点
椭圆滤波器提供比巴特沃斯或切比雪夫滤波器更陡峭的滚降特性，但在通带和阻带都是等纹的。
椭圆滤波器满足给定的性能规格，具有任何滤波器类型中最低的阶数。
C代码
void ellip(int n, double rp, double rs, 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;
ZPK zpk = ellipap(n, rp, rs);
double k = zpk.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(zpk.z, zpk.p, zpk.nz, zpk.np, 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));
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(zpk.p);
free(zpk.z);
free(z1);
free(p1);
free(tempa);
free(tempb);
}
其中ellipap
ZPK ellipap(int n, double rp, double rs)
{
ZPK temp;
double Gp = pow(10, -1.0 * rp / 20.0);
double ep = sqrt(pow(10, rp / 10.0) - 1.0);
double es = sqrt(pow(10, rs / 10.0) - 1.0);
double k1 = ep / es;
double k0 = ellipdeg(n, k1);
int L = floor(n / 2);
double r = n % 2;
double *ui = (double *)malloc(sizeof(double) * L);
for (size_t i = 0; i < L; i++)
{
ui[i] = (2.0 * (i + 1.0) - 1.0) / n;
}
double *zeta_i = cde(ui, L, k0, EPS);
Complex *zt = (Complex *)malloc(sizeof(Complex) * L);
Complex *pt = (Complex *)malloc(sizeof(Complex) * L);
Complex c = create(0, 1.0);
for (size_t i = 0; i < L; i++)
{
zt[i] = complex_divide_double(c, k0 * zeta_i[i]);
}
Complex v0 = asne(complex_divide_double(c, ep), k1, EPS);
v0 = complex_multiply(create(0, -1), v0);
v0 = complex_divide_double(v0, n);
// p = j*cde(ui-1i*v0, k);
for (size_t i = 0; i < L; i++)
{
pt[i] = complex_subtract(create(ui[i], 0), complex_multiply(create(0, 1), v0));
}
pt = complex_cde(pt, L, k0, EPS);
// pt = complex_cde(pt, L, k0, EPS);
for (size_t i = 0; i < L; i++)
{
pt[i] = complex_multiply(c, pt[i]);
}
Complex p0 = complex_multiply(create(0, 1), v0);
p0 = complex_sne(p0, k0, EPS);
p0 = complex_multiply(c, p0);
//(L+1)*3的矩阵 A,B其实可以省略
double *A = (double *)malloc(sizeof(double) * (L + 1) * 3);
double *B = (double *)malloc(sizeof(double) * (L + 1) * 3);
for (int i = 3; i < 3 * (L + 1); i++)
{
if ((i + 1) % 3 == 0)
{
A[i] = pow(get_modulus(complex_divide(create(1.0, 0.0), pt[i - L])), 2);
B[i] = pow(get_modulus(complex_divide(create(1.0, 0.0), zt[i - L])), 2);
}
else if ((i + 1) % 3 % 2 == 0)
{
A[i] = -2 * complex_divide(create(1.0, 0.0), pt[i - L]).real;
B[i] = -2 * complex_divide(create(1.0, 0.0), zt[i - L]).real;
}
else
{
A[i] = 1;
B[i] = 1;
}
}
if (r == 0)
{
A[0] = 1;
A[1] = 0;
A[2] = 0;
B[0] = Gp;
B[1] = 0;
B[2] = 0;
}
else
{
A[0] = 1;
A[1] = -complex_divide(create(1.0, 0.0), p0).real;
A[2] = 0;
B[0] = 1;
B[1] = 0;
B[2] = 0;
}
Complex *z = (Complex *)malloc(sizeof(Complex) * L * 2);
Complex *p = (Complex *)malloc(sizeof(Complex) * L * 2);
for (size_t i = 0; i < L; i++)
{
z[i] = zt[i];
z[L + i] = complex_conj(zt[i]);
p[i] = pt[i];
p[L + i] = complex_conj(pt[i]);
}
cplxpair(z, 2 * L);
cplxpair(p, 2 * L);
temp.nz = 2 * L;
temp.z = (Complex *)malloc(sizeof(Complex) * 2 * L);
for (size_t i = 0; i < temp.nz; i++)
{
temp.z[i].real = z[i].real;
temp.z[i].imaginary = z[i].imaginary;
}
if (r == 1)
{
temp.np = 2 * L + 1;
temp.p = (Complex *)malloc(sizeof(Complex) * (2 * L + 1));
for (size_t i = 0; i < 2 * L; i++)
{
temp.p[i] = p[i];
}
temp.p[2 * L].real = p0.real;
temp.p[2 * L].imaginary = p0.imaginary;
}
else
{
temp.np = 2 * L;
temp.p = (Complex *)malloc(sizeof(Complex) * 2 * L);
for (size_t i = 0; i < 2 * L; i++)
{
temp.p[i] = p[i];
}
}
double H0 = pow(Gp, 1 - r);
Complex zprod=create(1.0,0.0);
if(temp.nz>0)
{
zprod=complex_array_prod(temp.z, temp.nz);
}
temp.k = fabs(complex_multiply_double(complex_divide(complex_array_prod(temp.p, temp.np), zprod), H0).real);
free(ui);
free(zeta_i);
free(zt);
free(pt);
free(z);
free(p);
return temp;
}
ellipdeg：
double ellipdeg(int n, double k1)
{
if (k1 < KMIN)
{
return ellipdeg2(n, k1);
}
else
{
double k;
int L = floor(n / 2);
double kc = sqrt(1 - pow(k1, 2));
double *ui = (double *)malloc(sizeof(double) * L);
for (size_t i = 0; i < L; i++)
{
ui[i] = (2.0 * (i + 1.0) - 1.0) / n;
}
double *w = sne(ui, L, kc, EPS);
double kp = pow(kc, n) * pow(double_array_prod(w, L), 4);
k = sqrt(1 - pow(kp, 2));
free(ui);
return k;
}
}
ellipdeg2:
double ellipdeg2(int n, double k1)
{
double k;
double M = 7; // 默认值
double K, Kprime;
ellipk(k1, EPS, &K, &Kprime);
double q = exp(-PI * Kprime / K);
double q1 = pow(q, n);
// k = 4 * sqrt(q1) * ((1 + sum(q1.^(m.*(m+1))))/(1 + 2*sum(q1.^(m.*m))))^2;
double *temp1 = (double *)malloc(sizeof(double) * M);
double *temp2 = (double *)malloc(sizeof(double) * M);
double sum1 = 0;
double sum2 = 0;
for (size_t i = 0; i < M; i++)
{
sum1 += pow(q1, (i + 1) * (i + 2));
sum2 += pow(q1, (i + 1) * (i + 1));
}
k = 4 * sqrt(q1) * (1 + sum1) / pow(1 + 2 * sum2, 2);
free(temp1);
free(temp2);
return k;
}
ellipk:
void ellipk(double k, double tol, double *K, double *Kprime)
{
if (k == 0)
{
*K = INFINITY;
}
else if (k > KMAX)
{
double kp = sqrt(1 - pow(k, 2));
double L = -log(kp / 4.0);
*K = L + (L - 1) * pow(kp, 2) / 4.0;
}
else
{
int nv = 0;
double *v = landen(k, tol, &nv);
for (size_t i = 0; i < nv; i++)
{
v[i] += 1;
}
*K = double_array_prod(v, nv) * PI / 2.0;
free(v);
}
if (k == 0)
{
*Kprime = INFINITY;
}
else if (k < KMIN)
{
double L = -log(k / 4.0);
*Kprime = L + (L - 1) * pow(k, 2) / 4.0;
}
else
{
double kp = sqrt(1 - pow(k, 2));
int nvp = 0;
double *vp = landen(kp, tol, &nvp);
for (size_t i = 0; i < nvp; i++)
{
vp[i] += 1.0;
}
*Kprime = double_array_prod(vp, nvp) * PI / 2.0;
free(vp);
}
}
cde为带复参数的归一化Jacobi椭圆函数，代码如下:
double *cde(double *u, int nu, double k1, double tol)
{
int nv = 0;
double *v = landen(k1, tol, &nv);
double *w = (double *)malloc(sizeof(double) * nu);
for (size_t i = 0; i < nu; i++)
{
w[i] = u[i] * PI / 2.0;
w[i] = cos(w[i]);
}
for (int i = nv - 1; i >= 0; i--)
{
for (size_t j = 0; j < nu; j++)
{
w[j] = (1 + v[i]) * w[j] / (1 + v[i] * pow(w[j], 2));
}
}
free(v);
return w;
}
复数形式代码：
Complex *complex_cde(Complex *u, int nu, double k1, double tol)
{
int nv = 0;
double *v = landen(k1, tol, &nv);
Complex *w = (Complex *)malloc(sizeof(Complex) * nu);
for (size_t i = 0; i < nu; i++)
{
w[i] = complex_multiply_double(u[i], PI / 2.0);
w[i] = complex_cosine(w[i]);
}
for (int i = nv - 1; i >= 0; i--)
{
for (size_t j = 0; j < nu; j++)
{
Complex t1 = complex_multiply_double(w[j], 1 + v[i]);
Complex t2 = complex_multiply_double(complex_power(w[j], 2), v[i]);
t2 = complex_add_double(t2, 1.0);
w[j] = complex_divide(t1, t2);
}
}
free(v);
return w;
}
landen:
double *landen(double k, double tol, int *n)
{
double tempv[1000];
*n = 0;
while (k > tol)
{
k = pow(k / (1 + sqrt(1 - pow(k, 2))), 2);
tempv[*n] = k;
*n = *n + 1;
}
int length=*n;
double *temp = (double *)malloc( length* sizeof(double));
for (size_t i = 0; i < *n; i++)
{
temp[i] = tempv[i];
}
return temp;
}
sne为正弦振幅的Jacobi椭圆函数，代码如下：
double *sne(double *u, int nu, double k, double tol)
{
int nv = 0;
double *v = landen(k, tol, &nv);
double *w = (double *)malloc(sizeof(double) * nu);
for (size_t i = 0; i < nu; i++)
{
w[i] = sin(u[i] * PI / 2.0);
}
for (int i = nv - 1; i >= 0; i--)
{
for (size_t j = 0; j < nu; j++)
{
w[j] = ((1.0 + v[i]) * w[j]) / (1.0 + v[i] * pow(w[j], 2.0));
}
}
free(v);
return w;
}
asne为正弦振幅的Jacobi椭圆函数的逆，代码如下：
Complex asne(Complex w, double k1, double tol)
{
Complex rst = acde(w, k1, tol);
rst = complex_subtract(create(1.0, 0.0), rst);
return rst;
}
Complex acde(Complex w, double k1, double tol)
{
int nv = 0;
double *v = landen(k1, tol, &nv);
Complex wt;
wt.real = w.real;
wt.imaginary = w.imaginary;
for (size_t i = 0; i < nv; i++)
{
double v1 = i == 0 ? k1 : v[i - 1];
// w = w./(1 + sqrt(1 - w.^2 * v1^2)) * 2/(1+v(n))
//(1 + sqrt(1 - w.^2 * v1^2))
Complex temp = complex_power(w, 2);
temp = complex_multiply_double(temp, pow(v1, 2));
temp = complex_subtract(create(1.0, 0.0), temp);
temp = complex_square_root(temp);
temp = complex_add_double(temp, 1.0);
temp = complex_divide(w, temp);
w = complex_multiply_double(temp, 2.0 / (1.0 + v[i]));
}
Complex u = complex_arccosine(w);
u = complex_multiply_double(u, 2.0 / PI);
double K, Kprime;
ellipk(k1, tol, &K, &Kprime);
double R = Kprime / K;
u = complex_add_double(complex_multiply_double(create(0, 1.0), srem(u.imaginary, 2 * R)), srem(u.real, 4));
free(v);
return u;
}
acde:
Complex acde(Complex w, double k1, double tol)
{
int nv = 0;
double *v = landen(k1, tol, &nv);
Complex wt;
wt.real = w.real;
wt.imaginary = w.imaginary;
for (size_t i = 0; i < nv; i++)
{
double v1 = i == 0 ? k1 : v[i - 1];
// w = w./(1 + sqrt(1 - w.^2 * v1^2)) * 2/(1+v(n))
//(1 + sqrt(1 - w.^2 * v1^2))
Complex temp = complex_power(w, 2);
temp = complex_multiply_double(temp, pow(v1, 2));
temp = complex_subtract(create(1.0, 0.0), temp);
temp = complex_square_root(temp);
temp = complex_add_double(temp, 1.0);
temp = complex_divide(w, temp);
w = complex_multiply_double(temp, 2.0 / (1.0 + v[i]));
}
Complex u = complex_arccosine(w);
u = complex_multiply_double(u, 2.0 / PI);
double K, Kprime;
ellipk(k1, tol, &K, &Kprime);
double R = Kprime / K;
u = complex_add_double(complex_multiply_double(create(0, 1.0), srem(u.imaginary, 2 * R)), srem(u.real, 4));
free(v);
return u;
}
srem:
double srem(double X, double Y)
{
double Z;
Z = fmod(X, Y);
// Bring into interval [-Y/2, Y/2]
Z -= Y * ((Z > Y / 2) - (Z < -Y / 2));
return Z;
}
测试
与matlab测试结果对比，matlab中最高阶49阶，C代码可确保47阶，且误差小于小数点后10位小数
下一篇： matlab besself函数 C实现
1077
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
