Matlab IIR滤波器 C实现 已测试

#include "MatrixUtils.h" #include <stdio.h> #include <stdlib.h> #include <string.h> #include <stdbool.h> #include <math.h> #include <assert.h> #include "complex.h" #include "complexarray.h" #include "constants.h" #include "commonutils.h" /************************************************************************* * @brief : Characteristic polynomial or polynomial with specified roots, matlab函数 * @inparam : p 复矩阵 * np 复矩阵的大小 * @outparam: d 返回复矩阵 *************************************************************************/ void complex_poly(Complex *p, int np, Complex *d) { Complex *c = (Complex *)malloc((np + 1) * sizeof(Complex)); c[0].real = 1.0; c[0].imaginary = 0.0; d[0] = c[0]; for (int i = 1; i < np + 1; i++) { c[i].real = 0.0; c[i].imaginary = 0.0; d[i]=c[i]; } Complex temp; for (int i = 0; i < np; i++) { for (int j = 1; j <= i + 1; j++) { temp = complex_multiply(p[i], d[j - 1]); c[j].real = d[j].real - temp.real; c[j].imaginary = d[j].imaginary - temp.imaginary; } for (int j = 1; j <= i + 1; j++) { d[j].real = c[j].real; d[j].imaginary = c[j].imaginary; } } free(c); } /************************************************************************* * @brief : Characteristic polynomial or polynomial with specified roots, matlab函数 * @inparam : p 实数矩阵 * np 实数矩阵的大小 * @outparam: d 返回复数矩阵 *************************************************************************/ void poly(double *p, int np, double *d) { double *temp = (double *)malloc(sizeof(double) * np * np); for (size_t i = 0; i < np; i++) { for (size_t j = 0; j < np; j++) { temp[i * np + j] = p[i * np + j]; } } // eig(A) double *u_real = (double *)malloc(sizeof(double) * np); double *v_imag = (double *)malloc(sizeof(double) * np); int jt = 60; hhbg(temp, np); hhqr(temp, np, EPS, jt, u_real, v_imag); Complex *dt = (Complex *)malloc(sizeof(Complex) * (np + 1)); Complex *dtt = (Complex *)malloc(sizeof(Complex) * (np + 1)); dt[0].real = 1; dt[0].imaginary = 0; for (size_t i = 1; i < np + 1; i++) { dt[i].real = 0; dt[i].imaginary = 0; } for (size_t i = 0; i < np; i++) { for (size_t j = 1; j <= i + 1; j++) { dtt[j] = complex_subtract(dt[j], complex_multiply(create(u_real[i], v_imag[i]), dt[j - 1])); } for (size_t j = 1; j <= i + 1; j++) { dt[j].real = dtt[j].real; dt[j].imaginary = dtt[j].imaginary; } } for (size_t i = 0; i < np + 1; i++) { d[i] = dt[i].real; } free(temp); free(u_real); free(v_imag); free(dt); free(dtt); } /// @brief 根据列向量p（多项式的根）获取多项式系数 /// @param p /// @param np /// @param d void vector_poly(double *p, int np, double *d) { // 初始化多项式系数 double *dt = (double *)malloc(sizeof(double) * (np + 1)); double *dtt = (double *)malloc(sizeof(double) * (np + 1)); for (int i = 0; i <= np; ++i) { dt[i] = 0.0; } dt[0] = 1.0; for (size_t i = 0; i < np; i++) { for (size_t j = 1; j <= i + 1; j++) { dtt[j] = dt[j]-p[i]*dt[j - 1]; } for (size_t j = 1; j <= i + 1; j++) { dt[j] = dtt[j]; } } for (size_t i = 0; i < np + 1; i++) { d[i] = dt[i]; } free(dt); free(dtt); } Complex *poly_complex_array(Complex *p, int np) { Complex *c = (Complex *)malloc((np + 1) * sizeof(Complex)); Complex *d = (Complex *)malloc((np + 1) * sizeof(Complex)); c[0].real = 1.0; c[0].imaginary = 0.0; d[0].real = 1.0; d[0].imaginary = 0.0; for (int i = 1; i < np + 1; i++) { c[i].real = 0.0; c[i].imaginary = 0.0; d[i].real = 0.0; d[i].imaginary = 0.0; } Complex temp; for (int i = 0; i < np; i++) { for (int j = 1; j <= i + 1; j++) { temp = complex_multiply(p[i], d[j - 1]); c[j].real = d[j].real - temp.real; c[j].imaginary = d[j].imaginary - temp.imaginary; } for (int j = 1; j <= i + 1; j++) { d[j].real = c[j].real; d[j].imaginary = c[j].imaginary; } } free(c); return d; } Complex *poly_complex(Complex p) { Complex *c = (Complex *)malloc(2 * sizeof(Complex)); Complex *d = (Complex *)malloc(2 * sizeof(Complex)); c[0].real = 1.0; c[0].imaginary = 0.0; d[0].real = 1.0; d[0].imaginary = 0.0; for (int i = 1; i < 2; i++) { c[i].real = 0.0; c[i].imaginary = 0.0; d[i].real = 0.0; d[i].imaginary = 0.0; } Complex temp; for (int i = 0; i < 1; i++) { for (int j = 1; j <= i + 1; j++) { temp = complex_multiply(p, d[j - 1]); c[j].real = d[j].real - temp.real; c[j].imaginary = d[j].imaginary - temp.imaginary; } for (int j = 1; j <= i + 1; j++) { d[j].real = c[j].real; d[j].imaginary = c[j].imaginary; } } free(c); return d; } /************************************************************************* * @brief : 实数矩阵相乘 * @inparam : a 矩阵A * b 矩阵B * m 矩阵A与乘积矩阵C的行数 * n 矩阵A的行数,矩阵B的列数 * k 矩阵B与乘积矩阵C的列数 * @outparam: c 乘积矩阵 C=AB *************************************************************************/ void trmul(double a[], double b[], int m, int n, int k, double c[]) { int i, j, l, u; for (i = 0; i <= m - 1; i++) for (j = 0; j <= k - 1; j++) { u = i * k + j; c[u] = 0.0; for (l = 0; l <= n - 1; l++) c[u] = c[u] + a[i * n + l] * b[l * k + j]; } } //计算逆的主函数 double *inver(double *arr,int n) { double* res = (double *)malloc(sizeof(double) * n * n); double W[n*n], L[n*n], U[n*n], L_n[n*n], U_n[n*n]; int i, j, k, d; float s; // 赋初值 for(i=0;i<n;i++){ for(j=0;j<n;j++){ W[i*n+j] = arr[i*n+j]; L[i*n+j] = 0.0; U[i*n+j] = 0.0; L_n[i*n+j] = 0.0; U_n[i*n+j] = 0.0; res[i*n+j] = 0.0; } } for(i=0;i<n;i++) // L对角置1 { L[i*n+i] = 1.0; } for(j=0;j<n;j++) { U[j] = W[j]; } for(i=1;i<n;i++) { L[i*n] = W[i*n] / U[0]; } for(i=1;i<n;i++) { for(j=i;j<n;j++) // 求U { s = 0.0; for(k=0;k<i;k++) { s += L[i*n+k] * U[k*n+j]; } U[i*n+j] = W[i*n+j] - s; } for(d=i;d<n;d++) // 求L { s = 0; for(k=0;k<i;k++) { s += L[d*n+k] * U[k*n+i]; } L[d*n+i] = (W[d*n+i] - s) / U[i*n+i]; } } for(j=0;j<n;j++) //求L的逆 { for(i=j;i<n;i++) { if(i==j) L_n[i*n+j] = 1.0 / L[i*n+j]; else if(i<j) L_n[i*n+j] = 0.0; else { s = 0.0; for(k=j;k<i;k++) { s += L[i*n+k] * L_n[k*n+j]; } L_n[i * n + j] = -1.0 * L_n[j * n + j] * s; } } } for(i=0;i<n;i++) //求U的逆 { for(j=i;j>=0;j--) { if(i==j) U_n[j*n+i] = 1.0 / U[j*n+i]; else if(j>i) U_n[j*n+i] = 0.0; else { s = 0.; for(k=j+1;k<=i;k++) { s += U[j*n+k] * U_n[k*n+i]; } U_n[j*n+i] = -1.0 / U[j*n+j] * s; } } } for(i=0;i<n;i++) { f