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計算結果完全一致：

總結

以上是生活随笔為你收集整理的MATLAB里面的filter和filtfilt的C语言源代码的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。