低通滤波参数
//不會用的請參考我的博客:
#include "stdafx.h"
#include <stdlib.h>
#include <iostream>
#include <iomanip>//必需包含下面兩個頭文件
#include "complex.h"
#include <math.h>
double pi = 3.1415926535897932384626433832795;
//20181224
typedef struct _C_double_complex{ /* double complex */double _Val[2];} _C_double_complex;/*************************************************************************
* *Function: : Butterworth filter prototype, matlab函數
* @inparam : n 階數
*
* @outparam: p 復矩陣
* k
* @author : HJJ
* @date : 20181228
* @version : ver 1.0
*************************************************************************/
static void mybuttap(int n, _C_double_complex* p, double *k)
{int i = 0, j = 0;int size = (n - 1) / 2;int isodd = n % 2;_C_double_complex temp;for (i = 1, j = 0; i < n; j++){temp._Val[0] = 0;temp._Val[1] = pi * i / (2 * n) + pi / 2;p[j * 2] = cexp(temp);i += 2;}for (int m = 1, i = 0; i < j; i++){p[m] = conj(p[m - 1]);m += 2;}if (isodd){p[size * 2]._Val[0] = -1.0;p[size * 2]._Val[1] = 0.0;}_C_double_complex a = _Cmulcc(_Cmulcr(p[0], -1), _Cmulcr(p[1], -1));for (int m = 2; m < size * 2 + isodd; m++){a = _Cmulcc(a, _Cmulcr(p[m], -1));}*k = creal(a);
}/*************************************************************************
* *Function: : Characteristic polynomial or polynomial with specified roots, matlab函數
* @inparam : p 復矩陣
* np 復矩陣的大小
* @outparam: d 返回復矩陣
* @author : HJJ
* @date : 20181228
* @version : ver 1.0
*************************************************************************/
static void mypoly(_C_double_complex* p, int np, _C_double_complex *d)
{_C_double_complex *c = (_C_double_complex *)malloc((np + 1) * sizeof(_C_double_complex));c[0]._Val[0] = 1.0;c[0]._Val[1] = 0.0;d[0]._Val[0] = 1.0;d[0]._Val[1] = 0.0;for (int i = 1; i<np + 1; i++){c[i]._Val[0] = 0.0;c[i]._Val[1] = 0.0;d[i]._Val[0] = 0.0;d[i]._Val[1] = 0.0;}_C_double_complex temp;for (int i = 0; i < np; i++){for (int j = 1; j <= i + 1; j++){temp = _Cmulcc(p[i], d[j - 1]);c[j]._Val[0] = d[j]._Val[0] - temp._Val[0];c[j]._Val[1] = d[j]._Val[1] - temp._Val[1];}for (int j = 1; j <= i + 1; j++){d[j]._Val[0] = c[j]._Val[0];d[j]._Val[1] = c[j]._Val[1];}}free(c);
}/*************************************************************************
* *Function: : 實數矩陣相乘
* @inparam : a 矩陣A
* b 矩陣B
* m 矩陣A與乘積矩陣C的行數
* n 矩陣A的行數,矩陣B的列數
* k 矩陣B與乘積矩陣C的列數
* @outparam: c 乘積矩陣 C=AB
* @author : HJJ
* @date : 20181228
* @version : ver 1.0
*************************************************************************/
static void mytrmul(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];}
}/*************************************************************************
* *Function: : 矩陣求逆
* @inparam : a 矩陣A
* n 矩陣A的階數
* @outparam: a 逆矩陣
* @author : HJJ
* @date : 20181228
* @version : ver 1.0
*************************************************************************/
static void myrinv(double a[], int n)
{int *is, *js, i, j, k, l, u, v;double d, p;is = (int *)malloc(n * sizeof(int));js = (int *)malloc(n * sizeof(int));for (k = 0; k <= n - 1; k++){d = 0.0;for (i = k; i <= n - 1; i++)for (j = k; j <= n - 1; j++){l = i*n + j; p = fabs(a[l]);if (p>d) { d = p; is[k] = i; js[k] = j; }}if (d + 1.0 == 1.0){free(is); free(js);}if (is[k] != k)for (j = 0; j <= n - 1; j++){u = k*n + j; v = is[k] * n + j;p = a[u]; a[u] = a[v]; a[v] = p;}if (js[k] != k)for (i = 0; i <= n - 1; i++){u = i*n + k; v = i*n + js[k];p = a[u]; a[u] = a[v]; a[v] = p;}l = k*n + k;a[l] = 1.0 / a[l];for (j = 0; j <= n - 1; j++)if (j != k){u = k*n + j; a[u] = a[u] * a[l];}for (i = 0; i <= n - 1; i++)if (i != k)for (j = 0; j <= n - 1; j++)if (j != k){u = i*n + j;a[u] = a[u] - a[i*n + k] * a[k*n + j];}for (i = 0; i <= n - 1; i++)if (i != k){u = i*n + k; a[u] = -a[u] * a[l];}}for (k = n - 1; k >= 0; k--){if (js[k] != k)for (j = 0; j <= n - 1; j++){u = k*n + j; v = js[k] * n + j;p = a[u]; a[u] = a[v]; a[v] = p;}if (is[k] != k)for (i = 0; i <= n - 1; i++){u = i*n + k; v = i*n + is[k];p = a[u]; a[u] = a[v]; a[v] = p;}}free(is); free(js);
}/*************************************************************************
* *Function: : 復矩陣求逆
* @inparam : ar 矩陣A的實部
* ai 矩陣A的虛部
* n 矩陣A的階數
* @outparam: ar 逆矩陣的實部
* ai 逆矩陣的虛部
* @author : HJJ
* @date : 20181228
* @version : ver 1.0
*************************************************************************/
//復矩陣求逆
static int mycinv(double *ar, double *ai, int n)
{ int *is； *js, i, j, k, l, u, v, w;double p, q, s, t, d, b;is = (int*)malloc(n * sizeof(int));js = (int*)malloc(n * sizeof(int));for (k = 0; k <= n - 1; k++){d = 0.0;for (i = k; i <= n - 1; i++)for (j = k; j <= n - 1; j++){u = i*n + j;p = ar[u] * ar[u] + ai[u] * ai[u];if (p>d){ d = p; is[k] = i; js[k] = j; }}if (d + 1.0 == 1.0){free(is);free(js);return(0);}if (is[k] != k)for (j = 0; j <= n - 1; j++){u = k*n + j; v = is[k] * n + j;t = ar[u]; ar[u] = ar[v]; ar[v] = t;t = ai[u]; ai[u] = ai[v]; ai[v] = t;}if (js[k] != k)for (i = 0; i <= n - 1; i++){u = i*n + k; v = i*n + js[k];t = ar[u]; ar[u] = ar[v]; ar[v] = t;t = ai[u]; ai[u] = ai[v]; ai[v] = t;}l = k*n + k;ar[l] = ar[l] / d; ai[l] = -ai[l] / d;for (j = 0; j <= n - 1; j++)if (j != k){u = k*n + j;p = ar[u] * ar[l]; q = ai[u] * ai[l];s = (ar[u] + ai[u])*(ar[l] + ai[l]);ar[u] = p - q; ai[u] = s - p - q;}for (i = 0; i <= n - 1; i++)if (i != k){v = i*n + k;for (j = 0; j <= n - 1; j++)if (j != k){u = k*n + j; w = i*n + j;p = ar[u] * ar[v]; q = ai[u] * ai[v];s = (ar[u] + ai[u])*(ar[v] + ai[v]);t = p - q; b = s - p - q;ar[w] = ar[w] - t;ai[w] = ai[w] - b;}}for (i = 0; i <= n - 1; i++)if (i != k){u = i*n + k;p = ar[u] * ar[l]; q = ai[u] * ai[l];s = (ar[u] + ai[u])*(ar[l] + ai[l]);ar[u] = q - p; ai[u] = p + q - s;}}for (k = n - 1; k >= 0; k--){if (js[k] != k)for (j = 0; j <= n - 1; j++){u = k*n + j;v = js[k] * n + j;t = ar[u]; ar[u] = ar[v]; ar[v] = t;t = ai[u];ai[u] = ai[v]; ai[v] = t;}if (is[k] != k)for (i = 0; i <= n - 1; i++){u = i*n + k; v = i*n + is[k];t = ar[u]; ar[u] = ar[v]; ar[v] = t;t = ai[u]; ai[u] = ai[v]; ai[v] = t;}}free(is); free(js);return(1);
}/*************************************************************************
* *Function: : 對復數排序
* @inparam : p 復矩陣
* n 復矩陣大小
* @outparam: p
* @author : HJJ
* @date : 20181228
* @version : ver 1.0
*************************************************************************/
static void sort_complex(_C_double_complex *p, int n)
{_C_double_complex *pa, *pb, *k, temp;for (pa = p; pa<p + n - 1; pa++){k = pa;for (pb = pa + 1; pb < p + n; pb++){if (creal(*k) > creal(*pb)){k = pb;}}temp = *pa;*pa = *k;*k = temp;}
}/*************************************************************************
* *Function: : Convert zero-pole-gain filter parameters to state-space form,matlab函數
* @inparam : np 復矩陣p的階數
* p,k0
* a,b,c,d
* @outparam: a,b,c,d
* @author : HJJ
* @date : 20181228
* @version : ver 1.0
*************************************************************************/
static void myzp2ss(_C_double_complex* p, int np, double k0, double *a, double *b, double *c, double *d)
{int np1 = np;for (int i = 0; i<np*np; i++){a[i] = 0.0;}for (int i = 0; i<np; i++){b[i] = 0.0;c[i] = 0.0;}*d = 1;//If odd number of poles only, convert the pole at the//end into state-space.//H(s) = 1/(s-p1) = 1/(s + den(2)) if (np % 2){a[0] = -1;b[0] = 1;c[0] = 1;*d = 0;np1 = np - 1;}sort_complex(p, np1);//Take care of any left over unmatched pole pairs.//H(s) = 1/(s^2+den(2)s+den(3))_C_double_complex p_temp[2];_C_double_complex c_temp[3];double den[3], wn;double t[2 * 2] = { 0.0, 0.0, 0.0, 0.0 };double t_rinv[2 * 2] = { 0.0, 0.0, 0.0, 0.0 };double tr_den[2 * 2] = { 0.0, 0.0, 0.0, 0.0 };double a1_temp[2 * 2] = { 0.0, 0.0, 0.0, 0.0 };double a1[2 * 2] = { 0.0, 0.0, 0.0, 0.0 };double b1[2 * 2] = { 0.0, 0.0, 0.0, 0.0 };double c1[2 * 2] = { 0.0, 0.0, 0.0, 0.0 };double d1 = 0;int c_a = np % 2; //a的列數int ma1 = np % 2; //a的行數int na2 = 2; //a1的列數 for (int i = 0; i < np1; i = i + 2){p_temp[0] = p[i];p_temp[1] = p[i + 1];mypoly(p_temp, 2, c_temp);for (int j = 0; j<3; j++){den[j] = creal(c_temp[j]);}wn = sqrt(cabs(p_temp[0]) * cabs(p_temp[1]));
// wn = 1;if (wn == 0)wn = 1;//a1 = t\[-den(2) -den(3); 1 0]*t;t[0] = 1.0; //t[0][0]t[1*2+1] = 1.0 / wn; //t[1][1]t_rinv[0] = 1.0;t_rinv[1 * 2 + 1] = 1.0 / wn;myrinv(t_rinv, 2);tr_den[0] = -den[1];tr_den[0*2+1] = -den[2];tr_den[1*2+0] = 1.0;tr_den[1*2+1] = 0.0;mytrmul(t_rinv, tr_den, 2, 2, 2, a1_temp);mytrmul(a1_temp, t, 2, 2, 2, a1);//b1 = t\[1; 0];double tr_temp1[2 * 1] = { 1.0, 0.0 };mytrmul(t_rinv, tr_temp1, 2, 2, 1, b1);double tr_temp2[2] = { 0.0, 1.0 };mytrmul(tr_temp2, t_rinv, 1, 2, 2, c1);d1 = 0;//[a,b,c,d] = series(a,b,c,d,a1,b1,c1,d1);//Next lines perform series connection if (ma1 != 0){//a = [a zeros(ma1,na2); b1*c a1];for (int k = 0; k<ma1; k++){for (int j = c_a; j<c_a + 2; j++){a[k*np + j] = 0;}} a[ma1*np+(c_a - 1)] = 1;for (int k = ma1, kk = 0; kk < 2; k++,kk++){for (int j = c_a,jj=0; jj < 2; j++,jj++){a[k*np + j] = a1[kk*2 + jj];} }//b = [b; b1*d]; //c = [d1*c c1];for (int k = 0; k<c_a + 2; k++){c[k] = 0;} c[c_a+1] = 1;(*d) = d1*(*d);ma1 += 2;na2 = 2;c_a += 2;}if (ma1 == 0){//a = [a zeros(ma1,na2); b1*c a1];for (int k = 0; k<2; k++){for (int j = 0; j<2; j++){a[k*np+j] = a1[k*2+j];}}//b = [b; b1*d];for (int k = 0; k<2; k++){b[k] = b1[k];}//c = [d1*c c1];for (int k = 0; k<2; k++){c[k] = c1[k];}(*d) = d1*(*d);ma1 += 2;na2 = 2;c_a += 2;}}for (int i = 0; i<np; i++){c[i] *= k0;}(*d) = k0*(*d);
}/*************************************************************************
* *Function: : Change cutoff frequency for lowpass analog filter,matlab函數
* @inparam : n 矩陣A的階數
* a,b
* wo
* @outparam: a,b
* @author : HJJ
* @date : 20181228
* @version : ver 1.0
*************************************************************************/
static void mylp2lp(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];}
}/*************************************************************************
* *Function: : Transform lowpass analog filters to bandpass,matlab函數
* @inparam : n 矩陣A的階數
* a,b,c,d
* wo
* bw
* @outparam: a,b,c,d
* @author : HJJ
* @date : 20181228
* @version : ver 1.0
*************************************************************************/
static void mylp2bp(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;elseat[i * 2 * n + j] = 0;}else if (i >= n && j < n){if (i - n == j)at[i * 2 * n + j] = -1 * wo;elseat[i * 2 * n + j] = 0;}else if (i >= n && j >= n)at[i * 2 * n + j] = 0;}}bt[0] = (*b)[0] * wo;for (int i = 1; i < 2 * n; i++){bt[i] = 0;}for (int i = 0; i < 2 * n; i++){if (i < n)ct[i] = (*c)[i];elsect[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);
}/*************************************************************************
* *Function: : 用于模數轉換的雙線性變換方法,matlab函數
* @inparam : n 矩陣A的階數
* a,b,c,d
* fs
* @outparam: a,b,c,d
* @author : HJJ
* @date : 20181228
* @version : ver 1.0
*************************************************************************/
static void mybilinear(int n, double *a, double *b, double *c, double *d, double fs)
{double t = 1 / fs;double r = sqrt(t);double *eye_a = (double *)malloc(n*n * sizeof(double));double *t1 = (double *)malloc(n*n * sizeof(double));double *t2 = (double *)malloc(n*n * sizeof(double));double *t2_rinv = (double *)malloc(n*n * sizeof(double));double *ad = (double *)malloc(n*n * sizeof(double));double *bd = (double *)malloc(n*n * sizeof(double));double *cd = (double *)malloc(n*n * sizeof(double));double *dd = (double *)malloc(n*n * sizeof(double));for (int i = 0; i < n; i++){for (int j = 0; j < n; j++){if (i == j)eye_a[i*n + j] = 1;elseeye_a[i*n + j] = 0;t1[i*n + j] = eye_a[i*n + j] + a[i*n + j] * t / 2;t2[i*n + j] = eye_a[i*n + j] - a[i*n + j] * t / 2;t2_rinv[i*n + j] = eye_a[i*n + j] - a[i*n + j] * t / 2;}}myrinv(t2_rinv, n);mytrmul(t2_rinv, t1, n, n, n, ad);mytrmul(t2_rinv, b, n, n, 1, bd);mytrmul(c, t2_rinv, 1, n, n, cd);mytrmul(cd, b, 1, n, 1, dd);for (int i = 0; i < n; i++){for (int j = 0; j < n; j++){ bd[i*n + j] = bd[i*n + j] * t / r;cd[i*n + j] = cd[i*n + j] * r;dd[i*n + j] = dd[i*n + j] * t / 2 + *d;a[i*n + j] = ad[i*n + j];}}for (int i = 0; i < n; i++){b[i] = bd[i*n];c[i] = cd[i];}*d = dd[0];free(eye_a);free(t1);free(t2); free(t2_rinv);free(ad);free(bd);free(cd);free(dd);
}/*************************************************************************
* *Function: : 一般實矩陣約化為Hessenberg矩陣
* @inparam : a 存放一般實矩陣A,返回上H矩陣
* n 矩陣的階數
* @outparam: a
* @author : HJJ
* @date : 20181228
* @version : ver 1.0
*************************************************************************/
static void myhhbg(double a[], int n)
{int i, j, k, u, v;double d, t;for (k = 1; k <= n - 2; k++){d = 0.0;for (j = k; j <= n - 1; j++){u = j*n + k - 1; t = a[u];if (fabs(t)>fabs(d)){d = t; i = j;}}if (fabs(d) + 1.0 != 1.0){if (i != k){for (j = k - 1; j <= n - 1; j++){u = i*n + j; v = k*n + j;t = a[u]; a[u] = a[v]; a[v] = t;}for (j = 0; j <= n - 1; j++){u = j*n + i; v = j*n + k;t = a[u]; a[u] = a[v]; a[v] = t;}}for (i = k + 1; i <= n - 1; i++){u = i*n + k - 1; t = a[u] / d; a[u] = 0.0;for (j = k; j <= n - 1; j++){v = i*n + j;a[v] = a[v] - t*a[k*n + j];}for (j = 0; j <= n - 1; j++){v = j*n + k;a[v] = a[v] + t*a[j*n + i];}}}}return;
}/*************************************************************************
**Function: : 用帶原點位移的雙重步QR方法計算實上H矩陣的全部特征值
* @inparam : a 存放上H矩陣A
* n 上H矩陣A的階數
* eps 控制精度要求
* jt 控制最大迭代次數
* @outparam: u 返回n個特征值的實部
* v 返回n個特征值的虛部
* @author : HJJ
* @date : 20181228
* @version : ver 1.0
*************************************************************************/
static int myhhqr(double a[], int n, double eps, int jt, double *u, double *v)
{int m, it, i, j, k, l, ii, jj, kk, ll;double b, c, w, g, xy, p, q, r, x, s, e, f, z, y;it = 0; m = n;while (m != 0){l = m - 1;while ((l>0) && (fabs(a[l*n + l - 1])>eps*(fabs(a[(l - 1)*n + l - 1]) + fabs(a[l*n + l])))){l = l - 1;}ii = (m - 1)*n + m - 1; jj = (m - 1)*n + m - 2;kk = (m - 2)*n + m - 1; ll = (m - 2)*n + m - 2;if (l == m - 1){u[m - 1] = a[(m - 1)*n + m - 1]; v[m - 1] = 0.0;m = m - 1; it = 0;}else if (l == m - 2){b = -(a[ii] + a[ll]);c = a[ii] * a[ll] - a[jj] * a[kk];w = b*b - 4.0*c;y = sqrt(fabs(w));if (w>0.0){xy = 1.0;if (b<0.0) xy = -1.0;u[m - 1] = (-b - xy*y) / 2.0;u[m - 2] = c / u[m - 1];v[m - 1] = 0.0; v[m - 2] = 0.0;}else{u[m - 1] = -b / 2.0; u[m - 2] = u[m - 1];v[m - 1] = y / 2.0; v[m - 2] = -v[m - 1];}m = m - 2; it = 0;}else{if (it >= jt){return(-1);}it = it + 1;for (j = l + 2; j <= m - 1; j++){a[j*n + j - 2] = 0.0;}for (j = l + 3; j <= m - 1; j++){a[j*n + j - 3] = 0.0;}for (k = l; k <= m - 2; k++){if (k != l){p = a[k*n + k - 1]; q = a[(k + 1)*n + k - 1];r = 0.0;if (k != m - 2) {r = a[(k + 2)*n + k - 1];}}else{x = a[ii] + a[ll];y = a[ll] * a[ii] - a[kk] * a[jj];ii = l*n + l; jj = l*n + l + 1;kk = (l + 1)*n + l; ll = (l + 1)*n + l + 1;p = a[ii] * (a[ii] - x) + a[jj] * a[kk] + y;q = a[kk] * (a[ii] + a[ll] - x);r = a[kk] * a[(l + 2)*n + l + 1];}if ((fabs(p) + fabs(q) + fabs(r)) != 0.0){xy = 1.0;if (p<0.0){xy = -1.0; }s = xy*sqrt(p*p + q*q + r*r);if (k != l){ a[k*n + k - 1] = -s;}e = -q / s; f = -r / s; x = -p / s;y = -x - f*r / (p + s);g = e*r / (p + s);z = -x - e*q / (p + s);for (j = k; j <= m - 1; j++){ii = k*n + j; jj = (k + 1)*n + j;p = x*a[ii] + e*a[jj];q = e*a[ii] + y*a[jj];r = f*a[ii] + g*a[jj];if (k != m - 2){kk = (k + 2)*n + j;p = p + f*a[kk];q = q + g*a[kk];r = r + z*a[kk]; a[kk] = r;}a[jj] = q; a[ii] = p;}j = k + 3;if (j >= m - 1) {j = m - 1;}for (i = l; i <= j; i++){ii = i*n + k; jj = i*n + k + 1;p = x*a[ii] + e*a[jj];q = e*a[ii] + y*a[jj];r = f*a[ii] + g*a[jj];if (k != m - 2){kk = i*n + k + 2;p = p + f*a[kk];q = q + g*a[kk];r = r + z*a[kk]; a[kk] = r;}a[jj] = q; a[ii] = p;}}}}}return(1);
}/*************************************************************************
* *Function: : 復數除法
* @inparam : a,b 表示復數a+jb
* c,d 表示復數c+jd
* @outparam: *e,*f 指向返回的復數商 e+jf = (a+jb) / (c+jd)
* @author : HJJ
* @date : 20181228
* @version : ver 1.0
*************************************************************************/
static void mycdiv(double a, double b, double c, double d, double *e, double *f)
{double p, q, s, w;p = a*c; q = -b*d; s = (a + b)*(c - d);w = c*c + d*d;if (w + 1.0 == 1.0){*e = 1.0e+35*a / fabs(a);*f = 1.0e+35*b / fabs(b);}else{*e = (p - q) / w;*f = (s - p - q) / w;}return;
}/*************************************************************************
* *Function: : 求巴特沃斯濾波器系數 matlab對應函數 butter,精度大約在小數點后10~16位
* @inparam : n 濾波器階數
* Wn[2] Wn在[0.0, 1.0]之間,與1/2采樣率對應
* type 1 = "low" 低通濾波器
* 2 = "bandpass" 帶通濾波器
* 3 = "high" 高通濾波器 注:沒寫
* 4 = "stop" 帶阻濾波器 注:沒寫
* analog 0 = digital
* 1 = analog
* @outparam: ab 長度為 n+1
* bb 長度為 n+1 或 2n+1(帶通)
* @author : HJJ
* @date : 20181228
* @version : ver 1.0
*************************************************************************/
static void mybutter(int n, double* Wn, int type, double *ab, double *bb)
{double fs = 2;double u[2] = { 0.0, 0.0 };//step 1: get analog, pre-warped frequenciesif (type == 1 || type == 3){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);}//step 2: convert to low-pass prototype estimatedouble Bw = 0.0;if (type == 1 || type == 3){Wn = u;}else if (type == 2 || type == 4){Bw = u[1] - u[0];Wn[0] = sqrt(u[0] * u[1]); //center Wn[1] = 0.0;}//step 3: Get N-th order Butterworth analog lowpass prototype_C_double_complex* p = (_C_double_complex*)malloc(n * sizeof(_C_double_complex));double k = 0;mybuttap(n, p, &k);//Transform to state-spaceint 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;myzp2ss(p, n, k, a, b, c, &d);if (type == 1) // Lowpassmylp2lp(n, a, b, Wn[0]);else if (type == 2) // Bandpass
{mylp2bp(n, &a, &b, &c, &d, Wn[0], Bw);a_size = 2 * n;}else{return;}mybilinear(a_size, a, b, c, &d, fs);myhhbg(a, a_size);double *u_real = (double *)malloc(sizeof(double) *a_size);double *v_imag = (double *)malloc(sizeof(double) *a_size);double eps = 0.000000000000000000000000000001;int jt = 60;myhhqr(a, a_size, eps, jt, u_real, v_imag);_C_double_complex* p1 = (_C_double_complex*)malloc(a_size * sizeof(_C_double_complex));_C_double_complex* ctemp = (_C_double_complex*)malloc((a_size +1) * sizeof(_C_double_complex));for (int i = 0; i < a_size; i++){p1[i]._Val[0] = u_real[i];p1[i]._Val[1] = v_imag[i];}mypoly(p1, a_size, ctemp);for (int j = 0; j < a_size + 1; j++){ab[j] = creal(ctemp[j]);}int r_lenth = 0;if (type == 1) r_lenth = n;else if (type == 2) r_lenth = n * 2;else if (type == 3) r_lenth = n;else if (type == 4) r_lenth = n; //這里大小不清楚,待定_C_double_complex *r = (_C_double_complex *)malloc(sizeof(_C_double_complex) * r_lenth);double w = 0.0;Wn[0] = 2 * atan2(Wn[0], 4);switch (type){case 1:for (int i = 0; i < r_lenth; i++){r[i]._Val[0] = -1;r[i]._Val[1] = 0;}w = 0;break;case 2:for (int i = 0; i < r_lenth; i++){if (i < n){r[i]._Val[0] = 1;r[i]._Val[1] = 0;}else{r[i]._Val[0] = -1;r[i]._Val[1] = 0;}}w = Wn[0];break;case 3:for (int i = 0; i < r_lenth; i++){r[i]._Val[0] = 1;r[i]._Val[1] = 0;}w = pi;break;default:return;break;}_C_double_complex *r_temp = (_C_double_complex *)malloc(sizeof(_C_double_complex) * (r_lenth+1));_C_double_complex *kern = (_C_double_complex *)malloc(sizeof(_C_double_complex) * (r_lenth + 1));mypoly(r, r_lenth, r_temp);for (int j = 0; j < r_lenth + 1; j++){bb[j] = creal(r_temp[j]);}_C_double_complex temp;for (int i = 0; i < r_lenth + 1; i++){temp._Val[0] = 0;temp._Val[1] = -1 * w * i;kern[i] = cexp(temp);}_C_double_complex c_temp1, c_temp2, c_temp3;c_temp3._Val[0] = 0;c_temp3._Val[1] = 0;for (int i = 0; i < r_lenth + 1; i++){c_temp1 = _Cmulcr(kern[i], ab[i]);c_temp3._Val[0] += c_temp1._Val[0];c_temp3._Val[1] += c_temp1._Val[1];}c_temp2._Val[0] = 0;c_temp2._Val[1] = 0;for (int i = 0; i < r_lenth + 1; i++){r_temp[i] = _Cmulcr(c_temp3, bb[i]);c_temp1 = _Cmulcr(kern[i], bb[i]);c_temp2._Val[0] += c_temp1._Val[0];c_temp2._Val[1] += c_temp1._Val[1];}for (int i = 0; i < r_lenth + 1; i++){mycdiv(r_temp[i]._Val[0], r_temp[i]._Val[1], c_temp2._Val[0], c_temp2._Val[1], &c_temp1._Val[0], &c_temp1._Val[1]);bb[i] = creal(c_temp1);}free(p);free(a);free(b);free(c);free(u_real);free(v_imag);free(p1);free(ctemp);free(r);free(r_temp);free(kern);
}///
int main()
{#if 1//低通double wn[2] = { 8.15214 / (88.658 / 2), 0.0};double ab[4];double bb[4];int n = 4;mybutter(3, wn, 1, 0, ab, bb);
#else//帶通double wn[2] = { 8.15214 / (88.658 / 2), 26.25876 / (88.658 / 2) };double ab[2 * 4 + 1];double bb[2 * 4 + 1];int n = 2 * 4 + 1;mybutter(4, wn, 2, 0, ab, bb);
#endif;#if 1std::cout << "ab:" << std::endl;for (int i = 0; i < n; i++){std::cout << std::left << std::setprecision(20) << ab[i] << std::endl;}std::cout << std::endl;std::cout << "bb:" << std::endl;for (int i = 0; i < n; i++){std::cout << std::left << std::setprecision(20) << bb[i] << std::endl;}std::cout << std::endl;
#endifsystem("pause");return 0;
}
?
轉載于:https://www.cnblogs.com/hjj-fighting/p/10773722.html
總結
- 上一篇： JS操作字符串
- 下一篇： java.nio.file.NoSuch