📄 filterdesign.~cpp
字号:
//! \todo 压缩冗余代码。
//! \todo 处理coe临时系数问题。
//
#include "FilterDesign.h"
#include <math.h>
#include <stdlib.h>
#include <stdexcept>
int max (int value1, int value2);
int max(int value1, int value2)
{
return ( (value1 > value2) ? value1 : value2);
}
#ifdef __cplusplus
int min (int value1, int value2);
int min(int value1, int value2)
{
return ( (value1 < value2) ? value1 : value2);
}
void filter_error(char* pFile, int line, const std::string& what_arg)
{
char chLine[10];
std::string str;
str.append("file: ");
str.append(pFile);
str.append("; ");
str.append("line: ");
str.append(itoa(line, chLine, 10));
str.append("; ");
str.append("description: ");
str.append(what_arg);
throw std::runtime_error(str);
}
FilterDesign::~FilterDesign()
{
}
/*----------------------------------------------------------------------------
* \param passType 通带类型， 高通 低通
* \param fs 采样率
* \param lowFre 过渡带的低频
* \param heightFre 过渡带的高频
* \param aPass 最大通带衰减
* \param aStop 最小止带衰减
*
* ----------------------------------------------------------------------------*/
/*IIRFilterDesign::IIRFilterDesign(PassType passType, double fs,
double aPass, double aStop)
{
Init(passType, fs, aPass, aStop);
}*/
/*!----------------------------------------------------------------------------
* \param passtype 通带类型， 高通 低通
* \param fs 采样率
*
* \warning apass astop 使用默认值
*
* ----------------------------------------------------------------------------*/
IIRFilterDesign::IIRFilterDesign(PassType passType, double fs) //IIRFilterDesign构造函数
{
//Init(passType, fs, 1, 60);
this->passType = passType;
this->fs = fs;
pNum = NULL;
pDen = NULL;
pZ = NULL;
order = 0;
OMEGA= 0;
}
IIRFilterDesign::~IIRFilterDesign()
{
void ReleaseArray();
}
void IIRFilterDesign::Reset()
{
ZeroZ();
}
/*!----------------------------------------------------------------------------
* \brief 对两个多项式做卷积
*
* \param M 系数数组h长度
* \param h 系数
* \param L 系数数组x长度
* \param x 系数
* \param y 卷积结果
*
* \return 新多项式长度
*
* ----------------------------------------------------------------------------*/
int IIRFilterDesign::Conv(int M,double h[3],int L,double x[20],double y[20])
{
int i,j;
int y_Length;
//两组系数相乘的新系数的长度。
y_Length=L+M-1;
for (i=0;i<y_Length;i++)
for (y[i]=0,j=max(0,i-L+1);j<=min(i,M-1);j++)
y[i]+=h[j]*x[i-j];
return y_Length;
}
/*!----------------------------------------------------------------------------
* \brief 切比雪夫多项式
*
* 切比雪夫多项式定义为：
* \f[
* C_{N}(x) = \left\{ \begin{array}{ll} \cos(N\cos^{-1}(x)) &
* \textrm{当} ~|x|\leq 1\\
* \cosh(N\ln(x + \sqrt{x^{2} - 1})) &
* \textrm{当} ~|x| > 1\\
* \end{array}\right.
* \f]
*
* \param N
* 其中N：
* \f[
* N_{exact} =
* \frac{\ln(e + \sqrt{e^{2} - 1})}{\ln(w + \sqrt{w^{2} - 1})}
* \f]
*
* \param x 变量
*
* ----------------------------------------------------------------------------*/
double IIRFilterDesign::CN(int N,double x)
{
if (x<=1)
return cos(N*acos(x));
else
return cosh(N*log(x+sqrt(x*x-1)));
}
void IIRFilterDesign::GenerateCoe()
{
}
double IIRFilterDesign::MagResponse(double freq)
{
return 0.0;
}
void IIRFilterDesign::AllocateArray()
{
int length = 2*order + 1;
if(order > 0)
{
pNum = new double[length];
pDen = new double[length];
pZ = new double[length * MAXFILTERLEAD * 2];
ZeroZ();
if(pDen == NULL || pNum == NULL || pZ == NULL)
{
FILTER_ERROR("Array allocate error!");
}
//事实上可以不要, 因为紧跟着就赋值了。
//加上，因为一阶的时候会使用到3个数值，最后一个是0 @i 2005-08-22 10:48
memset(pNum, 0, length * sizeof(*pNum));
memset(pDen, 0, length * sizeof(*pDen));
}
else
{
FILTER_ERROR("invaild order");
}
}
void IIRFilterDesign::ReleaseArray()
{
delete[] pNum;
pNum = NULL;
delete[] pDen;
pDen = NULL;
delete[] pZ;
pZ = NULL;
}
double* IIRFilterDesign::GetNum()
{
return pNum;
}
double* IIRFilterDesign::GetDen()
{
return pDen;
}
double* IIRFilterDesign::GetZ()
{
return pZ;
}
void IIRFilterDesign::ZeroZ()
{
int len = 2 * sizeof(*pZ) * MAXFILTERLEAD * (2*order + 1);
memset(pZ, 0, len);
}
int IIRFilterDesign::GetFs()
{
return fs;
}
void IIRFilterDesign::SetFs(int fs)
{
if(fs > 0)
{
this->fs = fs;
}
else
{
FILTER_ERROR("invaild fs");
}
}
int IIRFilterDesign::GetOrder()
{
return order;
}
/*!----------------------------------------------------------------------------
* \param passType 通带类型， 高通 低通
* \param fs 采样率
* \param lowFre 过渡带的低频
* \param heightFre 过渡带的高频
* \param aPass 最大通带衰减
* \param aStop 最小止带衰减
*
* ----------------------------------------------------------------------------*/
Butter::Butter(PassType passType, double fs, double lowFre, double heightFre,
double aPass, double aStop)
: IIRFilterDesign(passType, fs)
{
Init(lowFre, heightFre, aPass, aStop);
GenerateCoe();
}
/*!----------------------------------------------------------------------------
* \param passtype 通带类型， 高通 低通
* \param fs 采样率
* \param lowfre 过渡带的低频
* \param heightfre 过渡带的高频
* \param apass 使用默认 1 db
* \param astop 使用默认 60 db
*
* \warning apass astop 使用默认值
*
* ----------------------------------------------------------------------------*/
Butter::Butter(PassType passType, double fs, double lowFre, double heightFre)
: IIRFilterDesign(passType, fs)
{
Init(lowFre, heightFre, 1, 60);
GenerateCoe();
}
Butter::~Butter()
{
ReleaseArray();
}
void Butter::Init(double lowFre, double heightFre, double aPass, double aStop)
{
this->lowFre = lowFre;
this->heightFre = heightFre;
Ap = aPass;
As = aStop;
}
/*!----------------------------------------------------------------------------
* \brief 生成系数
*
* 根据通带类型生成分子分母的系数
*
* ----------------------------------------------------------------------------*/
void Butter::GenerateCoe()
{
switch(passType)
{
case LOW_PASS:
GenerateLowCoe();
break;
case HEIGHT_PASS:
GenerateHeightCoe();
break;
default:
break;
}
}
void Butter::GenerateLowCoe()
{
//计算阶数
Butter_Lowpass_N();
//根据阶数分配分子分母数组
AllocateArray();
Den_Butter_Lowpass();
Num_Butter_Lowpass();
}
void Butter::GenerateHeightCoe()
{
Butter_Highpass_N();
AllocateArray();
Den_Butter_Highpass();
Num_Butter_Highpass();
}
/*!----------------------------------------------------------------------------
* \brief
* 幅频函数
*
* butterworth 的幅频响应函数
* \f[
* |H(\Omega)|^{2} = \frac{1}{1+(\frac{\Omega}{\Omega_{0}})^{2N}}
* \f]
*
* \param freq 要求的频率点
*
* \return 幅频
*
* ----------------------------------------------------------------------------*/
double Butter::MagResponse(double freq)
{
double range = 0.0;
double freqW = 0.0;
double freqw = (2*PI*freq)/fs;
if(passType == LOW_PASS)
{
freqW = tan(freqw/2);
}
else
{
freqW = cos(freqw/2)/sin(freqw/2);
}
//!
//range = 1.0/(1 + pow(freqW/OMEGA, 2*order));
range = 1.0/sqrt(1 + pow(freqW/OMEGA, 2*order));
return range;
}
/*!----------------------------------------------------------------------------
* \brief 生成 Butterworth Lowpass 的阶数
*
* 先做频率转换：
* \f[
* \omega_{pass} = \frac{2\pi f_{pass}}{f_{s}} \qquad,\qquad
* \omega_{stop} = \frac{2\pi f_{stop}}{f_{s}}
* \f]
*
* 再做预畸：
* \f[
* \Omega_{pass} = \tan(\frac{\omega_{pass}}{2}) \qquad,\qquad
* \Omega_{stop} = \tan(\frac{\omega_{stop}}{2})
* \f]
*
* 求出阶数：
* \f[
* \omega_{pass} = \sqrt{10^{\frac{A_{pass}}{10}} - 1} \qquad, \qquad
* \omega_{stop} = \sqrt{10^{\frac{A_{stop}}{10}} - 1}
* \f]
*
* \f[
* N_{exact} =
* \frac{\ln(\varepsilon_{stop}/\varepsilon_{pass})}{\ln(\Omega_{stop}/\Omega_{pass})}=
* \frac{\ln(e)}{\ln(w)}
* \f]
*
* 确定归一化频率\f$ \Omega_{0} \f$：
* \f[
* \Omega_{0} =
* \frac{\Omega_{pass}}{\varepsilon_{pass}^{\phantom{pass}{1/N}}}
* \f]
*
* ----------------------------------------------------------------------------*/
void Butter::Butter_Lowpass_N() ///计算低通的阶数
{
double wpass;
double wstop;
double Wpass;
double Wstop;
double e,w;
wpass=2*PI*lowFre/fs;
wstop=2*PI*heightFre/fs;
Wpass=tan(wpass/2);
Wstop=tan(wstop/2);
e=sqrt(pow(10,As/10)-1)/sqrt(pow(10,Ap/10)-1);
//e=sqrt(pow(double(10),Asb/10)-1)/sqrt(pow(double(10,Ap/10)-1);
w=Wstop/Wpass;
order=int(log(e)/log(w))+1;
if(order > 16)
{
throw std::logic_error("设计的滤波器不稳定！请重试！");
}
OMEGA=Wpass/pow(pow(10,Ap/10)-1,1.0/(2*order));
}
/*!----------------------------------------------------------------------------
* \brief 求Butter Lowpass的分母
*
* 先根据阶数的奇偶进行初始化。
*
* 如果阶数为奇数有：
* \f[
* G_{0} = \frac{\Omega_{0}}{\Omega_{0} + 1} \qquad, \qquad
* a_{01} = \frac{\Omega_{0} - 1}{\Omega_{0} + 1}
* \f]
* \f[
* H_{0}(z) = \left\{ \begin{array}{ll}
* $ 1 $& \textrm{当 ~N = 2K}\\
* \frac{G_{0}(1 + z^{-1})}{1 + a_{01}z^{-1}} & \textrm{当~N = 2K + 1}\\
* \end{array} \right.
* \f]
*
* 其中角度Angle为 \f$ \theta_{i} \f$：
* \f[
* \theta_{i} = \frac{\pi}{2N}(N-1+2i)
* \f]
*
* 求多项式系数其中：
*
* H[1]：
* \f[
* a_{i1} =
* \frac{2(\Omega_{0}^{\phantom{0}{2}} - 1)}{1 -
* 2\Omega_{0}\cos{\theta_{i}} + \Omega_{0}^{\phantom{0}{2}}}
* \f]
* H[2]:
* \f[
* a_{i2} = \frac{1 + 2\Omega_{0}\cos\theta_{i} +
* \Omega_{0}^{\phantom{0}{2}}}{1 - 2\Omega_{0}\cos{\theta_{i}} +
* \Omega_{0}^{\phantom{0}{2}}}
* \f]
*
* 上面只能计算出每个二阶节的分子或分母然后根据每个二阶节的分母做卷积，如下式：
* \f[
* H_{i}(z) =
* \frac{G_{i}(1+b_{i1}z^{-1}+z^{-2})}{1+a_{i1}z^{-1}+a_{i2}z^{-2}}
* \quad,\quad i = 1,2,\ldots,K
* \f]
* 完全展开为：
* \f[
* \mathop{\mathrm{H}}(z)=
* \frac{\sum_{j=0}^{M}b_{j}z^{-j}}{1+\sum_{i=1}^{N}a_{i}z^{-i}}
* \f]
*
*----------------------------------------------------------------------------*/
void Butter::Den_Butter_Lowpass()
{
int K=order/2;
double X[20]; //存放系数。 z^{0} 的系数在X[0]，z^{-1}的系数在X[1]……
// double X[40];// test
double H[3];
double Angle;
int X_Length,H_Length; //做卷积的两个数列的长度。
//根据阶数的奇偶进行不同的初始化。
if (order%2==0)
{
X[0]=1;
X_Length=1;
}
else
{
X[0]=1;
X[1]=(OMEGA-1)/(OMEGA+1);
X_Length=2;
pDen[0] = X[0];
pDen[1] = X[1];
}
for (int i=1;i<=K;i++)
{
//算每个多项式的系数，放在数组H中。
Angle=PI*(order-1+2*i)/(2*order);
H[0]=1;
H[1]=2*(pow(OMEGA,2)-1)/(1-2*OMEGA*cos(Angle)+pow(OMEGA,2));
H[2]=(1+2*OMEGA*cos(Angle)+pow(OMEGA,2))/(1-2*OMEGA*cos(Angle)+pow(OMEGA,2));
H_Length=3;
//X和H做卷积，新的系数长度放在X_Length。
X_Length=Conv(H_Length,H,X_Length,X,pDen);
//把卷积结果放在X中，等待下一次卷积。
for (int j=0;j<X_Length;j++)
X[j]=pDen[j];
}
/* X[0]=1;
X_Length=1;
if(order % 2 == 0)
{
for (int i=1;i<=K;i++)
{
//算每个多项式的系数，放在数组H中。
Angle=PI*(order-1+2*i)/(2*order);
H[0]=1;
H[1]=2*(pow(OMEGA,2)-1)/(1-2*OMEGA*cos(Angle)+pow(OMEGA,2));
H[2]=(1+2*OMEGA*cos(Angle)+pow(OMEGA,2))/(1-2*OMEGA*cos(Angle)+pow(OMEGA,2));
H_Length=3;
//X和H做卷积，新的系数长度放在X_Length。
X_Length=Conv(H_Length,H,X_Length,X,pDen);
//把卷积结果放在X中，等待下一次卷积。
for (int j=0;j<X_Length;j++)
X[j]=pDen[j];
}
}
else
{
H[0]=1;
H[1]=(OMEGA-1)/(OMEGA+1);
H_Length=2;
for(int i=1; i<=K; i++)
{
X_Length=Conv(H_Length,H,X_Length,X,pDen);
//把卷积结果放在X中，等待下一次卷积。
for (int j=0;j<X_Length;j++)
X[j]=pDen[j];
}
}*/
}
/*!----------------------------------------------------------------------------
* \brief 求Butter_Lowpass的分子
*
* 参考 Butter::Den_Butter_Lowpass
*
* 其中\f$ H[0] = G_{i} \f$：
* \f[
* G_{i} =
* \frac{\Omega_{0}^{\phantom{0}{2}}}{1 - 2\Omega_{0}\cos{\theta_{i}} + \Omega_{0}^{\phantom{0}{2}}}
* \f]
*
* 把\f$ H_{i}(z) \f$ 的分子的平方部分展开可以得到系数H[0]~H[3]。 然后对H数组做卷积
*
* ----------------------------------------------------------------------------*/
void Butter::Num_Butter_Lowpass()
{
int K=order/2;
double X[20];
// double X[40]; //test cai
double H[3];
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -