matlab filter与filtfilt函数实现，C语言实现

嗯，算法非常简单，就是网上搜不到C代码实现。filter是个很万能的数字滤波器函数，只要有滤波器的差分方程系数，IIR呀FIR呀都能通过它实现。在MATLAB里面，filter最常用的格式是这两个：

[y,zf] = filter(b,a,X)

[y,zf] = filter(b,a,X,zi)

其中b和a就是差分方程的系数，X是输入向量，zi是“初始状态”。可能这么说明还是不很清晰，那么请看图（注意，a(1)为1，这个可以通过差分方程所有系数除以a(1)所得）：

嗯，这样子很轻松地就能把各个y值给算出来了，哦注意上面式子里面的n是“向量a或者b中最长的长度”，在实际编程的时候，如果a和b长度不一样，短者显然是要用0补齐的。对于那个初始状态zi，忽略的时候，比如（顺便提醒一句MATLAB的下标从1开始）

y(1)=b(1)x(1)

y(2)=b(1)x(2)+Z1(1)= b(1)x(2) + b(2)x(1) - a(2)y(1)

不忽略的时候

y(1)=b(1)x(1) + Z1(0)

y(2)=b(1)x(2)+Z1(1)= b(1)x(2) + b(2)x(1) - a(2)y(1) + Z2(0)

因为实际程序中自己定义的东西比较多（=,=|||这也是没办法的事情不是），而filtfilt这个超级无敌的“零相移滤波函数”更是复杂到稍微调用了一下自己写的矩阵运算函数，所以代码全部贴上来实在是太乱。等这次工程做完会大概地把代码打包发一下。现在就先贴点代码片段好了……但愿我的代码风格没那么难懂……
#include "filter.h"
#include <string.h>

//Transposed Direct-Form II Realization
//initial conditions: zi, length==nfilt-1. ignore when zi==NULL

#ifndef EPS
#define EPS 0.000001
#endif

void
filter(const double* x, double* y, int xlen, double* a, double* b, int nfilt, double* zi){
    double tmp;
    int i,j;

    //normalization
    if( (*a-1.0>EPS) || (*a-1.0<-EPS) ){
        tmp=*a;
        for(i=0;i<nfilt;i++){
            b[i]/=tmp;
            a[i]/=tmp;
        }
    }

    memset(y,0,xlen*sizeof(double));

    a[0]=0.0;
    for(i=0;i<xlen;i++){
        for(j=0;i>=j&&j<nfilt;j++){
            y[i] += (b[j]*x[i-j]-a[j]*y[i-j]);
        }
        if(zi&&i<nfilt-1) y[i] += zi[i];
    }
    a[0]=1.0;

}

OK，接下来是神奇的零相移滤波filtfilt，操作上不复杂，原理上小小有点复杂。它主要是先找到一个“合理的”初始状态Zi，使得无论先从反向滤波还是先从正向滤波结果一致，然后filter一次，逆向，再filter一次，再逆向，就是结果了。这里面包括3个要点：

1. Zi的确定。

2. 边缘效应的消除。

3. 正反向滤波的数学原理。

对于要点1，可以参阅Fredrik Gustafsson的论文Determining the Initial States in Forward-backward Filtering的数学证明。要点2，filtfilt对数据两头做了镜像拓延，最后滤完波再截掉头尾。要点3，这个貌似不大难推（

）

#include <stdlib.h>

#include "filter.h"

#include "mulMat.h"

#include "invMat.h"

int

filtfilt(const double* x, double* y, int xlen, double* a, double* b, int nfilt){

    int nfact;

    int tlen;    //length of tx

    int i;

    double *tx,*tx1,*p,*t,*end;

    double *sp,*tvec,*zi;

    double tmp,tmp1;

    nfact=nfilt-1;    //3*nfact: length of edge transients

    

    if(xlen<=3*nfact || nfilt<2) return -1;    //too short input x or a,b

    

    //Extrapolate beginning and end of data sequence using a "reflection

    //method". Slopes of original and extrapolated sequences match at

    //the end points.

    //This reduces end effects.

    tlen=6*nfact+xlen;

    tx=malloc(tlen*sizeof(double));

    tx1=malloc(tlen*sizeof(double));

    sp=malloc( sizeof(double) * nfact * nfact );

    tvec=malloc( sizeof(double) * nfact );

    zi=malloc( sizeof(double) * nfact );

    if( !tx || !tx1 || !sp || !tvec || !zi ){

        free(tx);

        free(tx1);

        free(sp);

        free(tvec);

        free(zi);

        return 1;

    }

    

    tmp=x[0];

    for(p=x+3*nfact,t=tx;p>x;--p,++t) *t=2.0*tmp-*p;

    for(end=x+xlen;p<end;++p,++t) *t=*p;

    tmp=x[xlen-1];

    for(end=tx+tlen,p-=2;t<end;--p,++t) *t=2.0*tmp-*p;

    //now tx is ok.

    end = sp + nfact*nfact;

    p=sp;

    while(p<end) *p++ = 0.0L; //clear sp

    sp[0]=1.0+a[1];

    for(i=1;i<nfact;i++){

        sp[i*nfact]=a[i+1];

        sp[i*nfact+i]=1.0L;

        sp[(i-1)*nfact+i]=-1.0L;

    }

    for(i=0;i<nfact;i++){

        tvec[i]=b[i+1]-a[i+1]*b[0];

    }

    if(invMat(sp,nfact)){

        free(zi);

        zi=NULL;

    }

    else{

        mulMat(sp,tvec,zi,nfact,nfact,1);

    }//zi is ok

    free(sp);free(tvec);

    //filtering tx, save it in tx1

    tmp1=tx[0];

    if(zi)

        for( p=zi,end=zi+nfact; p<end;) *(p++) *= tmp1;

    filter(tx,tx1,tlen,a,b,nfilt,zi);

    //reverse tx1

    for( p=tx1,end=tx1+tlen-1; p<end; p++,end--){

        tmp = *p;

        *p = *end;

        *end = tmp;

    }

    //filter again

    tmp1 = (*tx1)/tmp1;

    if(zi)

        for( p=zi,end=zi+nfact; p<end;) *(p++) *= tmp1;

    filter(tx1,tx,tlen,a,b,nfilt,zi);

    //reverse to y

    end = y+xlen;

    p = tx+3*nfact+xlen-1;

    while(y<end){

        *y++ = *p--;

    }

    free(zi);

    free(tx);

    free(tx1);

    return 0;

}

与MATLAB对比（MATLAB代码）：

x=[1 2 3 4 5 6 7 8];

a=[1 2 3];

b=[4 5 6];

t=filtfilt(b,a,x);

for i=1:8, fprintf(1,'%f\n',t(i)), end;

结果为：

-6731884.250000

7501778.750000

-2757230.250000

-662443.250000

1360955.750000

-686678.250000

4135.750000

227147.750000

这个例子用上面给出的C语言版的filtfilt计算结果完全一致：