MATLAB中filter函数的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)所得):

MATLAB中filter函数的C代码实现_第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

//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 - 1y [ 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,这个貌似不大难推(span style=color:E8A02B - 亮点儿 - 我的博客

#include
#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<2return -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=mallocsizeof(double* nfact * nfact );
    tvec=mallocsizeof(double* nfact );
    zi=mallocsizeof(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;
        *= *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计算结果完全一致:

MATLAB中filter函数的C代码实现_第2张图片

你可能感兴趣的:(MATLAB中filter函数的C代码实现)