//不會用的請參考我的部落格:
#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;
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;
}
}
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];
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);
}
/*************************************************************************
* *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;
else
eye_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 frequencies
if (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 estimate
double 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-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;
myzp2ss(p, n, k, a, b, c, &d);
if (type == 1) // Lowpass
mylp2lp(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 1
std::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;
#endif
system("pause");
return 0;
}
轉載于:https://www.cnblogs.com/hjj-fighting/p/10773722.html