前面几篇是IIR常用滤波器和三大模型转换部分函数的实现。
在IIR滤波器实现中存在将低通滤波器(LPF)的设计转换成其他类型的滤波器的函数,即lp2lp、lp2hp、lp2bp和lp2bs函数组,下面是对应的C实现:
lp2lp
void lp2lp(int n, double *a, double *b, double wo)
{
for (int i = 0; i < n * n; i++)
{
a[i] = wo * a[i];
}
for (int i = 0; i < n; i++)
{
b[i] = wo * b[i];
}
}
lp2hp
void lp2hp(int n, double *a, double *b, double *c, double *d, double wo)
{
double *at = (double *)malloc(sizeof(double) * n * n);
double *bt = (double *)malloc(sizeof(double) * n);
double *ct = (double *)malloc(sizeof(double) * n);
double *dt = (double *)malloc(sizeof(double) * n);
for (size_t i = 0; i < n * n; i++)
{
at[i] = a[i];
}
rinv(at, n);
trmul(at, b, n, n, 1, bt);
trmul(c, at, n, n, 1, ct);
trmul(ct, at, n, n, 1, dt);
for (size_t i = 0; i < n * n; i++)
{
at[i] = wo * at[i];
}
for (size_t i = 0; i < n; i++)
{
bt[i] = -wo * bt[i];
dt[i] = *d - dt[i];
}
for (int i = 0; i < n * n; i++)
{
a[i] = at[i];
}
for (int i = 0; i < n; i++)
{
b[i] = bt[i];
b[i] = ct[i];
}
*d = dt[0];
free(at);
free(bt);
free(ct);
free(dt);
}
lp2bp
void lp2bp(int n, double **a, double **b, double **c, double *d, double wo, double bw)
{
double q = wo / bw;
double *at = (double *)malloc(sizeof(double) * (2 * n) * (2 * n));
double *bt = (double *)malloc(sizeof(double) * (2 * n));
double *ct = (double *)malloc(sizeof(double) * (2 * n));
double dt = *d;
for (int i = 0; i < 2 * n; i++)
{
for (int j = 0; j < 2 * n; j++)
{
if (i < n && j < n)
at[i * 2 * n + j] = (*a)[+i * n + j] / q * wo;
else if (i < n && j >= n)
{
if (i == j - n)
at[i * 2 * n + j] = 1 * wo;
else
at[i * 2 * n + j] = 0;
}
else if (i >= n && j < n)
{
if (i - n == j)
at[i * 2 * n + j] = -1 * wo;
else
at[i * 2 * n + j] = 0;
}
else if (i >= n && j >= n)
at[i * 2 * n + j] = 0;
}
}
for (int i = 0; i < n; i++)
{
bt[i] = (*b)[i] * wo / q;
}
for (int i = n; i < 2 * n; i++)
{
bt[i] = 0;
}
for (int i = 0; i < 2 * n; i++)
{
if (i < n)
ct[i] = (*c)[i];
else
ct[i] = 0;
}
*a = (double *)realloc(*a, (2 * n) * (2 * n) * sizeof(double));
*b = (double *)realloc(*b, (2 * n) * sizeof(double));
*c = (double *)realloc(*c, (2 * n) * sizeof(double));
for (int i = 0; i < 2 * n * 2 * n; i++)
{
(*a)[i] = at[i];
}
for (int i = 0; i < 2 * n; i++)
{
(*b)[i] = bt[i];
(*c)[i] = ct[i];
}
free(at);
free(bt);
free(ct);
}
lp2bs
void lp2bs(int n, double **a, double **b, double **c, double *d, double wo, double bw)
{
double q = wo / bw;
double *ai = (double *)malloc(sizeof(double) * n * n);
double *bb = (double *)malloc(sizeof(double) * n);
double *cc = (double *)malloc(sizeof(double) * n);
double *t1 = (double *)malloc(sizeof(double) * n);
double *t2 = (double *)malloc(sizeof(double) * n);
double *at = (double *)malloc(sizeof(double) * (2 * n) * (2 * n));
double *bt = (double *)malloc(sizeof(double) * (2 * n));
double *ct = (double *)malloc(sizeof(double) * (2 * n));
double *dt = (double *)malloc(sizeof(double) * n);
for (size_t i = 0; i < n; i++)
{
for (size_t j = 0; j < n; j++)
{
ai[i * n + j] = (*a)[i * n + j];
}
}
for (size_t i = 0; i < n; i++)
{
bb[i] = (*b)[i];
cc[i] = (*c)[i];
}
rinv(ai, n); // a的逆矩阵
trmul(ai, bb, n, n, 1, t1); // a\b=t1
trmul(cc, ai, 1, n, n, t2); // c/a=t2
trmul(t2, bb, 1, n, 1, dt); // c/a*b
// at = [w1/q*inv(as), w1*eye(ma); -w1*eye(ma), zeros(ma)];
for (int i = 0; i < 2 * n; i++)
{
for (int j = 0; j < 2 * n; j++)
{
if (i < n && j < n)
at[i * 2 * n + j] = ai[i * n + j] * wo / q;
else if (i < n && j >= n)
{
if (i == j - n)
at[i * 2 * n + j] = 1 * wo;
else
at[i * 2 * n + j] = 0;
}
else if (i >= n && j < n)
{
if (i - n == j)
at[i * 2 * n + j] = -1 * wo;
else
at[i * 2 * n + j] = 0;
}
else if (i >= n && j >= n)
at[i * 2 * n + j] = 0;
}
}
for (size_t i = 0; i < 2 * n; i++)
{
if (i < n)
{
bt[i] = -1.0 * t1[i] * wo / q;
ct[i] = t2[i];
dt[i] = *d - dt[i];
}
else
{
bt[i] = 0;
ct[i] = 0;
}
}
*d = dt[0];
*a = (double *)realloc(*a, sizeof(double) * 2 * n * 2 * n);
*b = (double *)realloc(*b, sizeof(double) * 2 * n);
*c = (double *)realloc(*c, sizeof(double) * 2 * n);
for (int i = 0; i < 2 * n * 2 * n; i++)
(*a)[i] = at[i];
for (int i = 0; i < 2 * n; i++)
{
(*b)[i] = bt[i];
(*c)[i] = ct[i];
}
free(ai);
free(t1);
free(t2);
free(at);
free(bt);
free(ct);
free(dt);
}
测试
经测试对比matlba结果,基本无误,可以放心食用。