Matlab中filter.m和filtfilt.m函数C语言实现

一、一些基础知识

filter.m函数是依据z变换的一些知识进行的滤波方法。
filtfilt.m则还有另一个名字是零相位滤波，顾名思义，通过filtfilt函数滤波后的信号，幅值会发生变化，但相位不会改变。
如何使用的零相位滤波呢，首先对原始信号进行普通的iir滤波(filter)，滤波后将得到的输出信号进行翻转，再次使用filter进行滤波，最后再将得到的结果进行翻转，这样便实现了零相位滤波。
以上是专业的术语。我希望可以用尽量通俗一些的一眼把这些解释清楚。

二、为什么需要z变换？

我们学习信号处理，是首先从傅里叶变换学起的，Fourier变换又分为连续周期信号的Fourier级数，连续非周期信号的傅里叶变换，离散周期信号的Fourier级数，离散非周期信号的Fourier变换。
在连续系统和连续信号的处理中，我们引入了LapaLace变换，Fourier变换可理解为LapaLace变换的一种特殊情况。在离散系统与离散信号分析的过程中，我们又引入Z变换，Z变换与LapaLace的作用类似。
为什么从Fourier变换扩展到LapaLace变换和Z变换？，我们慢慢一步一步讲。
首先呢，我们知道，如果已知LTI系统对单位冲激信号$\delta (n)$的输出$h(n)$，那么利用叠加性原理可以知道系统对任意复杂信号的输出。
$$y(n)=T[x(n)]=T[\sum _{m=-\infty }^{\infty }x(m)\delta (n-m)](1-1)$$
这就是我们很熟悉的卷积，将$x(m)$提出，可得到
$$y(n)=\sum _{m=-\infty }^{\infty }x(m)h(n-m)(1-2)$$
系统对冲激信号$\delta (n)$的单位冲激响应$h(n)$，是表征系统特性的一个基本方式。好的，这里先暂停一下，后面讲完Z变换再回到这里来。

三、Z变换定义

Z变换严格来说是一种非常数学化的工具，数学公式如下所示：
$$X(z)=\sum _{n=-\infty }^{\infty }x(n)z^{-n}$$
其中z为复变量。从数学的观点看，X(z)就是一个无穷级数的求和问题。应该知道，并非所有的无穷级数都是收敛的，因此，Z变换并不总是存在的。把使得Z变换收敛的z的取值范围称为收敛域ROC。
$$X(z)=\sum _{n=0}^{\infty }x(n)z^{-n}$$
上式称为单边Z变换。因为现实中的信号基本都是因果信号，即当前的输出只依赖于当前时刻和之前时刻的输入输出。因此，Z变换在多数情况下都是单边Z变换。

四、filter.m函数深入研究

这里用一个实际例子来深入研究下。
首先，例如我们得到一段随机信号，这里我们用matlab生成。

close all
clear
clc
Fs=1000;
T=1/Fs;
L=1000;
t=(0:L-1)*T;
x=0.7*sin(2*pi*20*t)+sin(2*pi*120*t);%原始信号由20Hz和120Hz的2段信号组成
% x=x+2*randn(size(t));%可添加随机白噪声，也可不添加
figure(1)
plot(t,x)
hold on

原始信号图片如下：

这里做一个简单的50Hz低通滤波

% 4阶低通滤波
[b,a]=butter(4,50/(Fs/2),'low');
y1=filter(b,a,x);
plot(t,y1,'LineWidth',2)

可以看到，120Hz的信号成分基本完全被滤除了。

滤波数学表达

这里我们看到了，低通滤波用的2行代码是这样的

% 4阶低通滤波
[b,a]=butter(4,50/(Fs/2),'low');
y1=filter(b,a,x);

这两行代码对原数组进行了何种操作呢？
首先，butter是用来生成零极点的滤波器系数。这里我们说明一下，滤波器通常分为FIR有限冲击响应滤波器和IIR无限冲击响应滤波器。
IIR滤波器的转移函数为
$$H(z)=\frac{{\sum }_{r=0}^M{b}_r{z}^{-r}}{1+{\sum }_{k=1}^N{a}_k{z}^{-k}}$$
FIR滤波器的转移函数为
$$H(z)=\sum _{n=0}^{N-1}h(n)z^{-n}$$
这里我们能看出来，这里使用的带通滤波器属于IIR滤波器（为什么这么说，因为a的值不为1，是一个一维矩阵）。
在matlab中可以观察到

a=[1,-3.1806,3.8612,-2.1122,0.4383];
b=[-0.0004,0.0017,0.0025,0.0017,0.0004];

在filter函数中做了何种处理呢？我们来看下官方matlab对filter.m的解释

%FILTER One-dimensional digital filter.
%   Y = FILTER(B,A,X) filters the data in vector X with the
%   filter described by vectors A and B to create the filtered
%   data Y.  The filter is a "Direct Form II Transposed"
%   implementation of the standard difference equation:
%
%   a(1)*y(n) = b(1)*x(n) + b(2)*x(n-1) + ... + b(nb+1)*x(n-nb)
%                         - a(2)*y(n-1) - ... - a(na+1)*y(n-na)
%
%   If a(1) is not equal to 1, FILTER normalizes the filter
%   coefficients by a(1). 

这里我们需要理解注释中间的公式是如何得出来的。
我们知道，在Z变换中
$$H(z)=\frac{Y(Z)}{X(Z)}$$
因此，由IIR的转移函数，我们可知
$$Y(z)[1+\sum _{k=1}^{N}a(k)z^{-k}]=X(z)[b(0)+\sum _{r=1}^{M}b(r)z^{-r}]$$
进一步可知
$$Y(z)=-Y(z)\sum _{k=1}^{N}a(k)z^{-k}+X(z)\sum _{r=0}^{M}b(r)z^{-r}$$
由Z的逆变换，最后可推出
$$y(n)=-\sum _{k=1}^{N}a(k)y(n-k)+\sum _{r=0}^{M}b(r)x(n-r)$$
因此，在filter中，进行的操作如下

%这里需要注意在matlab中数组下标由1开始，而前面公式推导中默认下标从0开始
y(1)=x(1);
y(2)=-(a(2)*y(1))+b(1)*x(2)+b(2)*x(1)=0.000321;
y(3)=-(a(2)*y(2)+a(3)*y(1))+b(1)*x(3)+b(2)*x(2)+b(3)*x(1)=0.0028;
y(4)=-(a(2)*y(3)+a(3)*y(2)+a(4)*y(1))+b(1)*x(4)+b(2)*x(3)+b(3)*x(2)+b(4)*x(1)=0.0120
......

我们将由b,a组成的滤波器系统的频率响应图画出

freqz(b,a,[],Fs)

可看出，50Hz以上的频率成分被衰减了。

滤波与卷积的关系

滤波实质意义上就是卷积。回想一下我们曾经学过的卷积公式
$$\begin{gathered} y[n]=\sum _{k=-\infty }^{\infty }x[k]h[n-k]=\sum _{k=-\infty }^{\infty }x[n-k]h[k] \\ =...+x[-2]h[n+2]+x[-2]h[n+1]+x[0]h[n]+x[1]h[n-1]+x[2]h[n-2]+... \end{gathered}$$
x[n]为原始输入信号，h[n]为单位冲击响应，y[n]为输出信号。对比下上面公式和前面filter过程的区别，及其类似。

%   a(1)*y(n) = b(1)*x(n) + b(2)*x(n-1) + ... + b(nb+1)*x(n-nb)
%                         - a(2)*y(n-1) - ... - a(na+1)*y(n-na)

五、filter.m函数matlab实现

现在我们可以来自己写函数实现filter.m函数了。

function [y]=self_filter(b,a,x)
%% 自己实现matlab中的filter.m函数,x和y均为列向量
M=length(x);
order=length(b);
x0=zeros(order-1,1);
x=[x0;x;x0];
y=zeros(2*order-2+M,1);
for i=order:order+M-1
    y(i)=b(1)*x(i);
    for j=2:order
        y(i)=y(i)+b(j)*x(i-j+1);
        y(i)=y(i)-a(j)*y(i-j+1);
    end
end
y=y(order:order+M-1);
end

可以对比self_filter和filter的区别，会发现和matlab的filter函数实现效果一致。

六、filter.m函数C语言实现

严格来说，filter函数的调用是有2种形式的，

y=filter(b,z,x); %一般情况下的滤波，在第五小节已经实现，自己依照matlab代码很容易写出C代码
y=filter(b,a,x,zi) %带有滤波器延迟情况的滤波实现

我们本小节主要实现的是y=filter(b,a,x,zi)的C语言实现，该功能主要是为了filtfilt函数实现做铺垫

static void filter(const float* b, const float* a, const float*zi, const float* signal, const int signal_len, float* filter_result)
{
    int i = 0;
    int j = 0;
    float state_signal[FILTER_LEN + 1] = { 0 };//FILTER_LEN为阶数，即系数b的长度
    float state_result[FILTER_LEN] = { 0 };
    
    for (i = 0; i < signal_len; i++)
    {
        state_signal[0] = signal[i];
        if (i == 0)
        {
            state_result[0] = 0;
        }
        else
        {
            state_result[0] = iir_result[i - 1];
        }

        for (j = 0; j < FILTER_LEN; j++)
        {
            iir_result[i] += b[j] * state_signal[j];
        }
        for (j = 1; j < FILTER_LEN; j++)
        {
            iir_result[i] -= a[j] * state_result[j - 1];
        }
        //
        if (i < FILTER_LEN - 1)
        {
            iir_result[i] += zi[i];
        }

        for (j = FILTER_LEN - 1; j > -1; j--)
        {
            state_signal[j + 1] = state_signal[j];
        }
        for (j = FILTER_LEN - 2; j > -1; j--)
        {
            state_result[j + 1] = state_result[j];
        }
        filter_result[i] /= a[0];
    }
}

七、filtfilt.m函数C语言实现

static void filter(const float* b, const float* a, const float* zi, const float* signal, const int signal_len, float* iir_result)
{
    int i = 0;
    int add_num = 3 * (FILTER_LEN - 1);//matlab延拓长度是3 * (FILTER_LEN - 1),
    float tmp_signal[SIG_NUM + 2 * 3 * (FILTER_LEN - 1)] = {0};//SIG_NUM即为signal_len，由于是在C语言中，只能这样初始化数组
    float tmp_result[SIG_NUM + 2 * 3 * (FILTER_LEN - 1)] = {0};//初始化缓存
    float zi_temp[FILTER_LEN - 1] = { 0 };
    //进行数据延拓
    for (i = 0; i < signal_len + add_num * 2; i++)
    {
        if (i < add_num)
        {
            tmp_signal[i] = 2 * signal[0] - signal[add_num - i]; //前延拓
        }
        else if (i < signal_len + add_num)
        {
            tmp_signal[i] = signal[i - add_num]; //中间数据不变
        }
        else
        {
            tmp_signal[i] = 2 * signal[signal_len - 1] - signal[signal_len - 2 - (i - signal_len - add_num)]; //后延拓
        }
    }

    //正向滤波
    for (i = 0; i < FILTER_LEN - 1; i++)
    {
        zi_temp[i] = zi[i] * tmp_signal[0];//计算初始状态
    }
    iir(b, a, zi_temp, tmp_signal, signal_len + add_num * 2, tmp_result);

    //数据翻转
    for (i = 0; i < signal_len + add_num * 2; i++)
    {
        tmp_signal[i] = tmp_result[signal_len + add_num * 2 - 1 - i];
    }
    //初始化缓存
    for (i = 0; i < signal_len + add_num * 2; i++)
    {
        tmp_result[i] = 0;
    }
    //逆向滤波
    for (i = 0; i < FILTER_LEN - 1; i++)
    {
        zi_temp[i] = zi[i] * tmp_signal[0];
    }
    iir(b, a, zi_temp, tmp_signal, signal_len + add_num * 2, tmp_result);
    
    //数据翻转
    for (i = 0; i < signal_len + add_num * 2; i++)
    {
        tmp_signal[i] = tmp_result[signal_len + add_num * 2 - 1 - i];
    }
    //数据截取
    for (i = 0; i < signal_len; i++)
    {
        iir_result[i] = tmp_signal[i + add_num];
    }

}