ButterWorthFIlter(巴特沃斯滤波器)

ButterWorthFIlter(巴特沃斯滤波器)

一、背景

这种滤波器最先由英国工程师斯蒂芬·巴特沃斯(Stephen Butterworth)在1930年发表在英国《无线电工程》期刊的一篇论文中提出的。

二、特点

巴特沃斯滤波器的特点是通频带内的频率响应曲线最大限度平坦,没有起伏,而在阻频带则逐渐下降为零。 在振幅的对数对角频率的波特图上,从某一边界角频率开始,振幅随着角频率的增加而逐步减少,趋向负无穷大。一阶巴特沃斯滤波器的衰减率为每倍频6分贝,每十倍频20分贝。二阶巴特沃斯滤波器的衰减率为每倍频12分贝、三阶巴特沃斯滤波器的衰减率为每倍频18分贝、如此类推。巴特沃斯滤波器的振幅对角频率单调下降,并且也是唯一的无论阶数,振幅对角频率曲线都保持同样的形状的滤波器。只不过滤波器阶数越高,在阻频带振幅衰减速度越快。其他滤波器高阶的振幅对角频率图和低阶数的振幅对角频率有不同的形状。

ButterWorthFIlter(巴特沃斯滤波器)_第1张图片

由图可见,当N趋于无穷时,将会得到一个理想的低通滤波器

三、原理

1. 幅值平方

∣ H a ( j w ) ∣ 2 = 1 1 + ( w w c ) 2 N |H_{a}(jw)|^{2}= \frac{1}{1+ (\frac{w}{wc})^{2N}} Ha(jw)2=1+(wcw)2N1

2. 拉普拉斯变换


s = σ + j ω s=\sigma+j\omega s=σ+jω
其中,
σ = 0 \sigma=0 σ=0
所以有,
w = s j w=\frac{s}{j} w=js
重新带入幅值平方
∣ H ( s ) ∣ 2 = 1 1 + ( − s 2 w c 2 ) N |H(s)|^{2}= \frac{1}{1+ (\frac{-s^2}{wc^2})^{N}} H(s)2=1+(wc2s2)N1

H ( s ) = ω c N a 0 ω c N + a 1 ω c N − 1 s + . . . a n − 1 ω c 1 s n − 1 + s N H(s)= \frac{\omega_{c}^{N}} {a_{0}\omega_{c}^{N}+ a_{1}\omega_{c}^{N-1}s+... a_{n-1}\omega_{c}^{1}s^{n-1}+ s^{N}} H(s)=a0ωcN+a1ωcN1s+...an1ωc1sn1+sNωcN
其中,
a 0 = 1 a_{0}=1 a0=1
设数组sb的各个元素为上式分母中关于s的幂次方项的系数
s b = { s b [ 0 ] , s b [ 1 ] , . . . s b [ N ] } = { a 0 ω c N , a 1 ω c N − 1 , . . . a n − 1 ω c 1 , 1 } sb=\{sb[0],sb[1],...sb[N]\} = \{a_{0}\omega_{c}^{N},a_{1}\omega_{c}^{N-1},...a_{n-1}\omega_{c}^{1},1\} sb={sb[0],sb[1],...sb[N]}={a0ωcN,a1ωcN1,...an1ωc1,1}

3. Z域变换(离散化)


z = e s T = e s T 2 e − s T 2 z=e^{sT}=\frac{e^{\frac{sT}{2}}}{e^{\frac{-sT}{2}}} z=esT=e2sTe2sT
得到
s = 1 T l n z s=\frac{1}{T}ln{z} s=T1lnz

3.1 线性化

试图使用泰勒公式对s进行展开,但是lnz在z=0处没有定义,于是令
z ′ = z − 1 z + 1 z^{'}=\frac{z-1}{z+1} z=z+1z1
于是
z = 1 + z ′ 1 − z ′ z=\frac{1+z^{'}}{1-z^{'}} z=1z1+z
对上式在z‘=0处泰勒展开,有
ln ⁡ z = ln ⁡ 1 + z ′ 1 − z ′ = 2 ( z ′ + 1 3 z ′ 3 + 1 5 z ′ 5 . . . ) \ln z=\ln {\frac{1+z^{'}}{1-z^{'}}}=2(z^{'}+\frac{1}{3}z^{'3}+\frac{1}{5}z^{'5}...) lnz=ln1z1+z=2(z+31z3+51z5...)
带入s的定义,整理,得到
s = 1 T l n z = 2 T ( z − 1 z + 1 + 1 3 z − 1 z + 1 3 + 1 5 z − 1 z + 1 5 . . . ) s=\frac{1}{T}ln{z}=\frac{2}{T}(\frac{z-1}{z+1}+\frac{1}{3}\frac{z-1}{z+1}^{3}+\frac{1}{5}\frac{z-1}{z+1}^{5}...) s=T1lnz=T2(z+1z1+31z+1z13+51z+1z15...)
取一阶近似,最终得到
s = 1 T l n z = 2 T ( z − 1 z + 1 ) = 2 T ( 1 − z − 1 1 + z − 1 ) s=\frac{1}{T}ln{z}=\frac{2}{T}(\frac{z-1}{z+1})=\frac{2}{T}(\frac{1-z^{-1}}{1+z^{-1}}) s=T1lnz=T2(z+1z1)=T2(1+z11z1)

3.2 双线性变换

双线性变换就是使用这种一阶估计法,将连续时间传递函数H(s)中的s替换成离散域中的z变量
H d ( z ) = H a ( s ) ∣ s = 2 T ( z − 1 z + 1 ) = H a ( 2 T z − 1 z + 1 ) H_{d}(z)=H_{a}(s)|_{s=\frac{2}{T}(\frac{z-1}{z+1})}=H_{a}(\frac{2}{T}\frac{z-1}{z+1}) Hd(z)=Ha(s)s=T2(z+1z1)=Ha(T2z+1z1)

进行整理后,最终得到
H ( z ) = ω c N ( 1 + z − 1 ) N a 0 ω c N ( 2 T ) N ( 1 − z − 1 ) N + a 1 ω c N − 1 ( 2 T ) N − 1 ( 1 − z − 1 ) N − 1 ( 1 + z − 1 ) 1 + . . . a N − m ω c N − m ( 2 T ) N − m ( 1 − z − 1 ) N − m ( 1 + z − 1 ) m + . . . a N ( 1 + z − 1 ) N H(z)=\frac{\omega_{c}^{N}(1+z^{-1})^{N}} {a_{0}\omega_{c}^{N}(\frac{2}{T})^N(1-z^{-1})^{N}+ a_{1}\omega_{c}^{N-1}(\frac{2}{T})^{N-1}(1-z^{-1})^{N-1}(1+z^{-1})^{1}+ ... a_{N-m}\omega_{c}^{N-m}(\frac{2}{T})^{N-m}(1-z^{-1})^{N-m}(1+z^{-1})^{m}+... a_{N}(1+z^{-1})^{N} } H(z)=a0ωcN(T2)N(1z1)N+a1ωcN1(T2)N1(1z1)N1(1+z1)1+...aNmωcNm(T2)Nm(1z1)Nm(1+z1)m+...aN(1+z1)NωcN(1+z1)N

最终将上式写成如下:
H ( z ) = n u m [ 0 ] ( z − 1 ) 0 + n u m [ 1 ] ( z − 1 ) 1 + . . . n u m [ N ] ( z − 1 ) N d e n [ 0 ] ( z − 1 ) 0 + d e n [ 1 ] ( z − 1 ) 1 + . . . d e n [ N ] ( z − 1 ) N H(z)=\frac { num[0](z^{-1})^{0}+ num[1](z^{-1})^{1}+... num[N](z^{-1})^{N} } { den[0](z^{-1})^{0}+ den[1](z^{-1})^{1}+... den[N](z^{-1})^{N} } H(z)=den[0](z1)0+den[1](z1)1+...den[N](z1)Nnum[0](z1)0+num[1](z1)1+...num[N](z1)N
上式中的序列num、den就是离散化的目标。

四、巴特沃斯滤波器的设计(数字滤波器实现)

1.确定参数

需要确定参数如下:

  • passF:通带截止频率
  • stopF:阻带截止频率
  • fs:采样频率
  • rp:通带最大衰减
  • rs:阻带最小衰减

2.计算预畸参数

双线性变换具有从根本上避免脉冲响应不变法中的频率混叠现象,缺点是引入了频率失真,因此,引入预畸变的概念,即在双线性变换之前,进行预畸来校正频率失真的问题,使得进行双线性变换之后的频率与预期一致。

2.1 预畸原理


s = 2 T ( z − 1 z + 1 ) = σ + j Ω s=\frac{2}{T}(\frac{z-1}{z+1})=\sigma+j\Omega s=T2(z+1z1)=σ+jΩ
使用下式进行变量代换
z = e j w z=e^{jw} z=ejw
得到
s = 2 T ( z − 1 z + 1 ) = 2 T ( 1 − e − j w 1 + e − j w ) = 2 T ( 1 − ( c o s ω − j sin ⁡ ω ) 1 + ( c o s ω − j sin ⁡ ω ) ) = 2 T ( 2 j sin ⁡ ω 2 + 2 cos ⁡ ω ) = 2 T ( j 2 sin ⁡ ω 2 cos ⁡ ω 2 4 cos ⁡ 2 ω 2 ) = 2 T j tan ⁡ ω 2 = σ + j Ω \begin{aligned} s=& \frac{2}{T}(\frac{z-1}{z+1}) \\ =& \frac{2}{T}(\frac{1-e^{-jw}}{1+e^{-jw}}) \\ =& \frac{2}{T}(\frac{1-(cos \omega-j\sin \omega)}{1+(cos \omega-j\sin \omega)}) \\ =& \frac{2}{T}(\frac{2j\sin \omega}{2+2 \cos \omega}) \\ =& \frac{2}{T}(\frac{j2 \sin \frac{\omega}{2}\cos \frac{\omega}{2}}{4 \cos^{2} \frac{\omega}{2}}) \\ =& \frac{2}{T}j\tan {\frac{\omega}{2}}=\sigma+j\Omega \end{aligned} s======T2(z+1z1)T2(1+ejw1ejw)T2(1+(cosωjsinω)1(cosωjsinω))T2(2+2cosω2jsinω)T2(4cos22ωj2sin2ωcos2ω)T2jtan2ω=σ+jΩ
所以有
Ω = 2 T tan ⁡ ω 2 = 2 T tan ⁡ π f \begin{aligned} \Omega=& \frac{2}{T}\tan{\frac{\omega}{2}} \\ =& \frac{2}{T}\tan{\pi f} \\ \end{aligned} Ω==T2tan2ωT2tanπf

2.2 计算第一步确定的参数预畸值

p a s s Ω = tan ⁡ π ∗ p a s s F f s pass\Omega=\tan {\frac{\pi*passF}{fs}} passΩ=tanfsπpassF
s t o p Ω = tan ⁡ π ∗ s t o p F f s stop\Omega=\tan {\frac{\pi*stopF}{fs}} stopΩ=tanfsπstopF

3. 计算低通滤波器阶数


r p = 10 lg ⁡ ( 1 + ( p a s s Ω Ω c ) 2 N ) r s = 10 lg ⁡ ( 1 + ( s t o p Ω Ω c ) 2 N ) \begin{aligned} rp=10\lg (1+(\frac{pass\Omega}{\Omega_c})^{2N}) \\ rs=10\lg (1+(\frac{stop\Omega}{\Omega_c})^{2N}) \end{aligned} rp=10lg(1+(ΩcpassΩ)2N)rs=10lg(1+(ΩcstopΩ)2N)
可得,阶数N
N = 1 2 = lg ⁡ 1 0 0.1 r s − 1 1 0 0.1 r p − 1 lg ⁡ s t o p Ω p a s s Ω \begin{aligned} N=\frac{1}{2}=\frac{\lg \frac{10^{0.1rs-1}}{10^{0.1rp-1}}}{\lg \frac{stop\Omega}{pass\Omega}} \end{aligned} N=21=lgpassΩstopΩlg100.1rp1100.1rs1

4. 计算截止频率\Omega_c


r s = 10 lg ⁡ ( 1 + ( s t o p Ω Ω c ) 2 N ) \begin{aligned} rs=10\lg (1+(\frac{stop\Omega}{\Omega_c})^{2N}) \end{aligned} rs=10lg(1+(ΩcstopΩ)2N)

Ω c = s t o p Ω ( 1 0 0.1 r s − 1 ) 1 2 N \begin{aligned} \Omega_c=\frac{stop\Omega}{(10^{0.1rs-1})^{\frac{1}{2N}}} \end{aligned} Ωc=(100.1rs1)2N1stopΩ

5. 确定分子分母系数数组长度length

l e n g t h = N + 1 length=N+1 length=N+1

6. 查表,获取拉氏变换下传递函数的分母系数

H ( s ) = Ω c N a 0 Ω c N + a 1 Ω c N − 1 s + . . . a n − 1 Ω c 1 s n − 1 + s N H(s)= \frac{\Omega_{c}^{N}} {a_{0}\Omega_{c}^{N}+ a_{1}\Omega_{c}^{N-1}s+... a_{n-1}\Omega_{c}^{1}s^{n-1}+ s^{N}} H(s)=a0ΩcN+a1ΩcN1s+...an1Ωc1sn1+sNΩcN

即计算:
a 0 Ω c N + a 1 Ω c N − 1 s + . . . a n − 1 Ω c 1 s n − 1 + s N a_{0}\Omega_{c}^{N}+ a_{1}\Omega_{c}^{N-1}s+... a_{n-1}\Omega_{c}^{1}s^{n-1}+ s^{N} a0ΩcN+a1ΩcN1s+...an1Ωc1sn1+sN

其中,关于a可查表获得
ButterWorthFIlter(巴特沃斯滤波器)_第2张图片

最终返回数组sb数组sb的各个元素为上式分母中关于s的幂次方项的系数
s b = { s b [ 0 ] , s b [ 1 ] , . . . s b [ N ] } = { a 0 ω c N , a 1 ω c N − 1 , . . . a n − 1 ω c 1 , 1 } sb=\{sb[0],sb[1],...sb[N]\} = \{a_{0}\omega_{c}^{N},a_{1}\omega_{c}^{N-1},...a_{n-1}\omega_{c}^{1},1\} sb={sb[0],sb[1],...sb[N]}={a0ωcN,a1ωcN1,...an1ωc1,1}

代码如下:

    for(int i = 0; i < length; i++)
    {
        //a0*Wc^N+a1*Wc^(N-1)*S+....+an-1*Wc^0*S^(n-1)
        returnSb[i] = g_butterPb[length - 1][i] * pow(Wc, length-i); //计算系数
    }
    
    returnSb[length] = 1.0; //最高次幂的系数为1

7. 双线性变换

根据前面得出来的公式:
H ( z ) = ω c N ( 1 + z − 1 ) N a 0 ω c N ( 2 T ) N ( 1 − z − 1 ) N + a 1 ω c N − 1 ( 2 T ) N − 1 ( 1 − z − 1 ) N − 1 ( 1 + z − 1 ) 1 + . . . a N − m ω c N − m ( 2 T ) N − m ( 1 − z − 1 ) N − m ( 1 + z − 1 ) m + . . . a N ( 1 + z − 1 ) N H(z)=\frac{\omega_{c}^{N}(1+z^{-1})^{N}} {a_{0}\omega_{c}^{N}(\frac{2}{T})^N(1-z^{-1})^{N}+ a_{1}\omega_{c}^{N-1}(\frac{2}{T})^{N-1}(1-z^{-1})^{N-1}(1+z^{-1})^{1}+ ... a_{N-m}\omega_{c}^{N-m}(\frac{2}{T})^{N-m}(1-z^{-1})^{N-m}(1+z^{-1})^{m}+... a_{N}(1+z^{-1})^{N} } H(z)=a0ωcN(T2)N(1z1)N+a1ωcN1(T2)N1(1z1)N1(1+z1)1+...aNmωcNm(T2)Nm(1z1)Nm(1+z1)m+...aN(1+z1)NωcN(1+z1)N

7.1 利用杨辉三角计算多项式(1-z^-1)、(1+z^-1)次方项内各自关于(z^-1)的系数

ButterWorthFIlter(巴特沃斯滤波器)_第3张图片
图片来源:https://blog.csdn.net/NDHuaErFeiFei/article/details/88379163

  • 对于 (1+z^-1),
    ( z − 1 + 1 ) 1 = 1 ∗ z − 1 + 1 ∗ 1 ( z − 1 + 1 ) 2 = 1 ∗ ( z − 1 ) 2 + 2 ∗ z − 1 + 1 ∗ 1 ( z − 1 + 1 ) 3 = 1 ∗ ( z − 1 ) 3 + 3 ∗ ( z − 1 ) 2 + 3 ∗ z − 1 + 1 ∗ 1 \begin{aligned} (z^{-1}+1)^{1}=& 1*z^{-1}+1*1 \\ (z^{-1}+1)^{2}=& 1*(z^{-1})^{2}+2*z^{-1}+1*1 \\ (z^{-1}+1)^{3}=& 1*(z^{-1})^{3}+3*(z^{-1})^{2}+3*z^{-1}+1*1 \end{aligned} (z1+1)1=(z1+1)2=(z1+1)3=1z1+111(z1)2+2z1+111(z1)3+3(z1)2+3z1+11

以3次方为例,杨辉三角函数输出[1,3,3,1]

  • 对于 (1-z^-1)=(-z^-1+1)
    ( − z − 1 + 1 ) 1 = − 1 ∗ z − 1 + 1 ∗ 1 ( − z − 1 + 1 ) 2 = 1 ∗ ( z − 1 ) 2 − 2 ∗ z − 1 + 1 ∗ 1 ( − z − 1 + 1 ) 3 = − 1 ∗ ( z − 1 ) 3 + 3 ∗ ( z − 1 ) 2 − 3 ∗ z − 1 + 1 ∗ 1 \begin{aligned} (-z^{-1}+1)^{1}=& -1*z^{-1}+1*1 \\ (-z^{-1}+1)^{2}=& 1*(z^{-1})^{2}-2*z^{-1}+1*1 \\ (-z^{-1}+1)^{3}=& -1*(z^{-1})^{3}+3*(z^{-1})^{2}-3*z^{-1}+1*1 \end{aligned} (z1+1)1=(z1+1)2=(z1+1)3=1z1+111(z1)22z1+111(z1)3+3(z1)23z1+11

以3次方为例,杨辉三角函数输出[-1,3,-3,1]

杨辉三角函数代码:

/*======================================================================
 * 函数名:  pascalTriangle
 * 函数功能:计算杨辉三角的第N行的值(数组),该系列值为(x+1)^N的系数,
 *         加改进(x-1)^N的系数
 *
 * 变量名称:
 *          N      - 杨辉三角第N行,N=0,1,...,N
 *          symbol - 运算符号,0——(x+1)^N,1——(x-1)^N
 *          vector - 返回数组,杨辉三角的第N行的值
 *
 * 返回值:  void
 *=====================================================================*/
void pascalTriangle(
		int		N,
		int		symbol,
		int		*vector)
{
    vector[0] = 1;
    if(N == 0)
    {
        return;
    }
    else if (N == 1)
    {
        if(symbol == SYMBOL_ADD)
        {
            vector[1] = 1;
        }
        else
        {
            vector[0] = -1; //如果是减号,则第二项系数是-1
            vector[1] = 1;
        }
        return;
    }
    int length = N + 1; //数组长度

	int *temp = (int *)malloc(sizeof(int) * length);
	/*
    int temp[length];   //定义中间变量
    */
    
    temp[0] = 1;
    temp[1] = 1;
    
    for(int i = 2; i <= N; i++)
    {
        vector[i] = 1;
        for(int j = 1; j < i; j++)
        {
            vector[j] = temp[j - 1] + temp[j]; //x[m] = x[m-1][n-1] + x[m-1]
        }
        if(i == N) //最后一次不需要给中间变量赋值
        {
            if(symbol == SYMBOL_SUB) //运算符为减号
            {
                for(int k = 0; k < length; k++)
                {
                    vector[k] = vector[k] * pow((float)-1, length - 1 - k);
                }
            }
            return;
        }
        for(int j = 1; j <= i; j++)
        {
            temp[j] = vector[j];
        }
    }

	if (temp != NULL)
		free(temp);
}

具体实现:

for(int i = 0; i <= length; i++)
{
    for(int j = 0; j<= length; j++)
    {
        tempCoef1[j] = 0;     //tempCoef1和tempCoef2进行初始化
        tempCoef2[j] = 0;
    }
    
    //i :1 - z^(-1)幂次数
    //otherN : 1 + z^(-1)幂次数
    otherN = length - i;

    pascalTriangle(3, SYMBOL_SUB, tempCoef1);      //利用杨辉三角计算1 - z^(-1)幂的系数
    pascalTriangle(otherN, SYMBOL_ADD, tempCoef2); //利用杨辉三角计算1 + z^(-1)幂的系数

    coefficientEquation(tempCoef1, i+1, tempCoef2, otherN+1); //两个多项式相乘,求其系数
}

7.2 利用杨辉三角计算(1-z^-1)^{N-m}*(1+z^-1)^{m}多项式关于z^-1项的系数

最终的系数数组长度:L=N-m+1+m+1-1=N+1

计算的目标多项式:
( 1 − z − 1 ) N − m ( 1 + z − 1 ) m (1-z^{-1})^{N-m}(1+z^{-1})^{m} (1z1)Nm(1+z1)m

最终得到如下形式:
C 0 ( z − 1 ) N + C 1 ( z − 1 ) N − 1 + . . . + C N ( z − 1 ) 0 C_{0}(z^{-1})^{N}+C_{1}(z^{-1})^{N-1}+...+C_{N}(z^{-1})^{0} C0(z1)N+C1(z1)N1+...+CN(z1)0

返回:
return [C0,C1,C2…CN]

核心函数coefficientEquation()实现两个系数数组相乘后得到的多项式数组

/*======================================================================
 * 函数名:  coefficientEquation(整数)和coefficientEquation2(浮点数)
 * 函数功能:计算多项式相乘的系数
 *
 * 变量名称:
 *          originalCoef - 原来的系数数组,计算后的系数也存储在该数组内
 *          N            - 上面系数对应的多项式系数个数
 *          nextCoef     - 与原数组相乘的数组的系数
 *          nextN        - 上面系数对应的多项式系数个数
 *
 * 返回值:  void
 *=====================================================================*/
void coefficientEquation(
		int		*originalCoef,
		int		N,
		int		*nextCoef,
		int		nextN)
{
    int *tempCoef = (int *)malloc(sizeof(int) * (N+nextN-1));
	/*
    double tempCoef[N + nextN - 1];    //中间变量
    */
    for(int i = 0; i < N + nextN - 1; i++)
    {
        tempCoef[i] = originalCoef[i]; //中间变量初始化
        originalCoef[i] = 0;
    }
    
    //乘完之后有多少项= N + nextN - 1
    //设 (1,2,3) ===>N=3
    //   (3,4,5) ===>nextN=3
    // original[0]=1*3              | (z^-1)^4
    // original[1]=2*3+1*4          | (z^-1)^3
    // original[2]=3*3+2*4+1*5      | (z^-1)^2
    // original[3]=2*5+3*4          | (z^-1)^1
    // original[4]=3*5              | (z^-1)^0
    for(int j = 0; j < nextN; j++)
    {
        for(int i = j; i < N + nextN - 1; i++)
        {
            //printf("%d %d ",tempCoef[i-j],nextCoef[j]);
            originalCoef[i] += tempCoef[i-j] * nextCoef[j];
            //printf("%d %d \n",originalCoef[i],i);
        }
    }
	if (tempCoef != NULL)
		free(tempCoef);
}

7.3 计算H(z)分母系数

分母形式:
d e n [ 0 ] ( z − 1 ) 0 + d e n [ 1 ] ( z − 1 ) 1 + . . . d e n [ N ] ( z − 1 ) N den[0](z^{-1})^{0}+ den[1](z^{-1})^{1}+... den[N](z^{-1})^{N} den[0](z1)0+den[1](z1)1+...den[N](z1)N

其中,
d e n [ 0 ] = ( 2 T ) 0 ∗ C N ∗ s b [ 0 ] + ( 2 T ) 1 ∗ C N ∗ s b [ 1 ] + . . . + ( 2 T ) N ∗ C N ∗ s b [ N ] \begin{aligned} den[0]=& (\frac{2}{T})^{0}*C_{N}*sb[0]+ (\frac{2}{T})^{1}*C_{N}*sb[1]+ ... +(\frac{2}{T})^{N}*C_{N}*sb[N] \end{aligned} den[0]=(T2)0CNsb[0]+(T2)1CNsb[1]+...+(T2)NCNsb[N]

d e n [ 1 ] = ( 2 T ) 0 ∗ C N − 1 ∗ s b [ 0 ] + ( 2 T ) 1 ∗ C N − 1 ∗ s b [ 1 ] + . . . + ( 2 T ) N ∗ C N − 1 ∗ s b [ N ] \begin{aligned} den[1]=& (\frac{2}{T})^{0}*C_{N-1}*sb[0]+ (\frac{2}{T})^{1}*C_{N-1}*sb[1]+ ... +(\frac{2}{T})^{N}*C_{N-1}*sb[N] \end{aligned} den[1]=(T2)0CN1sb[0]+(T2)1CN1sb[1]+...+(T2)NCN1sb[N]

. . . . . . ...... ......

d e n [ N ] = ( 2 T ) 0 ∗ C 0 ∗ s b [ 0 ] + ( 2 T ) 1 ∗ C 0 ∗ s b [ 1 ] + . . . + ( 2 T ) N ∗ C 0 ∗ s b [ N ] \begin{aligned} den[N]=& (\frac{2}{T})^{0}*C_{0}*sb[0]+ (\frac{2}{T})^{1}*C_{0}*sb[1]+ ... +(\frac{2}{T})^{N}*C_{0}*sb[N] \end{aligned} den[N]=(T2)0C0sb[0]+(T2)1C0sb[1]+...+(T2)NC0sb[N]

最终计算结果:
return [den[0],den[1], … den[N]]

实现代码:

//分母
length=N;
for(int i = 0; i <= length; i++)
{
    for(int j = 0; j <= length; j++)
    {
        denominator[j] += pow(Fsx2, i) * (float)tempCoef1[length - j] * sb[i];
    }
}

7.4 计算H(z)分子系数

(1)如7.2一样,由杨辉三角计算得到(1+z^-1)^N的系数[tmpC0,tmpC1, … tmpCN]
(2)分子形式:

根据双线性变换得到的分子:

ω c N ( 1 + z − 1 ) N \omega_{c}^{N}(1+z^{-1})^{N} ωcN(1+z1)N

可以写成如下形式:

n u m [ 0 ] ( z − 1 ) 0 + n u m [ 1 ] ( z − 1 ) 1 + . . . n u m [ N ] ( z − 1 ) N num[0](z^{-1})^{0}+ num[1](z^{-1})^{1}+... num[N](z^{-1})^{N} num[0](z1)0+num[1](z1)1+...num[N](z1)N

其中,

n u m [ 0 ] = t m p C N ∗ s b [ 0 ] \begin{aligned} num[0]=&tmpC_{N}*sb[0] \end{aligned} num[0]=tmpCNsb[0]

n u m [ 1 ] = t m p C N − 1 ∗ s b [ 0 ] \begin{aligned} num[1]=&tmpC_{N-1}*sb[0] \end{aligned} num[1]=tmpCN1sb[0]

. . . . . . ...... ......

n u m [ N ] = t m p C 0 ∗ s b [ 0 ] \begin{aligned} num[N]=&tmpC_{0}*sb[0] \end{aligned} num[N]=tmpC0sb[0]

(3)实现代码:

length=N;
for(int i = 0; i <= length; i++)
{
    //分子系数
    if(i == 0)
    {
        for(int j = 0; j <= length; j++)
        {
            numerator[j] = sb[0] * tempCoef2[length - j];
        }
    }
}

7.5 系数归一化(分子分母同除)

目标:使分母常数项=1

即:
num=num/den[0]
den=den/den[0]

实现代码:

length=N;
for(int i = 0; i <= length; i++)
{
    //系数归一化,分母的常数项为1
    for(int i = length; i >= 0; i--)
    {
        numerator[i] = numerator[i] / denominator[0];
        denominator[i] = denominator[i] / denominator[0];
    }
}

7.6 设计完成

得到了num和den数组,即得到如下形式的H(z)
H ( z ) = n u m [ 0 ] ( z − 1 ) 0 + n u m [ 1 ] ( z − 1 ) 1 + . . . n u m [ N ] ( z − 1 ) N d e n [ 0 ] ( z − 1 ) 0 + d e n [ 1 ] ( z − 1 ) 1 + . . . d e n [ N ] ( z − 1 ) N H(z)=\frac { num[0](z^{-1})^{0}+ num[1](z^{-1})^{1}+... num[N](z^{-1})^{N} } { den[0](z^{-1})^{0}+ den[1](z^{-1})^{1}+... den[N](z^{-1})^{N} } H(z)=den[0](z1)0+den[1](z1)1+...den[N](z1)Nnum[0](z1)0+num[1](z1)1+...num[N](z1)N

你可能感兴趣的:(ROS,IMU&GPS组合导航,IMU&GPS组合导航)