filtfilt函数的c语言实现

[y,zf]=filter(b,a,X)[y,zf]=filter(b,a,X,zi)其中b和a就是差分方程的系数，X是输入向量，zi是“初始状态”。可能这么说明还是不很清晰，那么请看图（注意，a(1)为1，这个可以通过差分方程所有系数除以a(1)所得）：FILTFILTZero-phaseforwardandreversedigitalfiltering.Y=FILTFILT(B,A,X)filtersthedatainvectorXwiththefilterdescribedbyvectorsAandBtocreatethefiltereddataY.Thefilterisdescribedbythedifferenceequation: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)Afterfilteringintheforwarddirection,thefilteredsequenceisthenreversedandrunbackthroughthefilter;Yisthetimereverseoftheoutputofthesecondfilteringoperation.Theresulthaspreciselyzerophasedistortionandmagnitudemodifiedbythesquareofthefilter'smagnituderesponse.Careistakentominimizestartupandendingtransientsbymatchinginitialconditions.Thelengthoftheinputxmustbemorethanthreetimesthefilterorder,definedasmax(length(b)-1,length(a)-1).NotethatFILTFILTshouldnotbeusedwithdifferentiatorandHilbertFIRfilters,sincetheoperationofthesefiltersdependsheavilyontheirphaseresponse.Seealsofilter.ReferencepageinHelpbrowserdocfiltfilt零相移滤波filtfilt，操作上不复杂，原理上小小有点复杂。它主要是先找到一个“合理的”初始状态Zi，使得无论先从反向滤波还是先从正向滤波结果一致，然后filter一次，逆向，再filter一次，再逆向，就是结果了。这里面包括3个要点：1.Zi的确定。2.边缘效应的消除。3.正反向滤波的数学原理。对于要点1，可以参阅FredrikGustafsson的论文DeterminingtheInitialStatesinForward-backwardFiltering的数学证明。要点2，filtfilt对数据两头做了镜像拓延，最后滤完波再截掉头尾。要点3，这个貌似不大难推#include<stdlib.h>#include"filter.h"#include"mulMat.h"#include"invMat.h"intfiltfilt(constdouble*x,double*y,intxlen,double*a,double*b,intnfilt){   intnfact;   inttlen;   //lengthoftx   inti;   double*tx,*tx1,*p,*t,*end;   double*sp,*tvec,*zi;   doubletmp,tmp1;   nfact=nfilt-1;   //3*nfact:lengthofedgetransients       if(xlen<=3*nfact||nfilt<2)return-1;   //tooshortinputxora,b       //Extrapolatebeginningandendofdatasequenceusinga"reflection   //method".Slopesoforiginalandextrapolatedsequencesmatchat   //theendpoints.   //Thisreducesendeffects.   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);       return1;   }       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;   //nowtxisok.   end=sp+nfact*nfact;   p=sp;   while(p<end)*p++=0.0L;//clearsp   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);   }//ziisok   free(sp);free(tvec);   //filteringtx,saveitintx1   tmp1=tx[0];   if(zi)       for(p=zi,end=zi+nfact;p<end;)*(p++)*=tmp1;   filter(tx,tx1,tlen,a,b,nfilt,zi);   //reversetx1   for(p=tx1,end=tx1+tlen-1;p<end;p++,end--){       tmp=*p;       *p=*end;       *end=tmp;   }   //filteragain   tmp1=(*tx1)/tmp1;   if(zi)       for(p=zi,end=zi+nfact;p<end;)*(p++)*=tmp1;   filter(tx1,tx,tlen,a,b,nfilt,zi);   //reversetoy   end=y+xlen;   p=tx+3*nfact+xlen-1;   while(y<end){       *y++=*p--;   }   free(zi);   free(tx);   free(tx1);   return0;}