filter 函数 c语言实现

int filtfilt(const double*x,double*y,int xlen,int ylen,double *a,double*b,int nfilt)
{
    int nfact=nfilt-1;
    int tlen;//length of tx
    int i;
    double *tx,*tx1,*p,*t,*end;
    double *sp,*tvec,*zi;
    double tmp,tmp1;
    assert (xlen>nfact);
    assert(nfilt>=2);

    tlen =6*nfact+xlen;
    tx=malloc(tlen*sizeof(double));
    tx1 = malloc(tlen*sizeof(double));

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

    tvec =malloc(nfact*sizeof(double));
    zi=malloc(nfact*sizeof(double ));
    //申请地址不成功返回
    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;
    //tx 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;
}