c语言filtfilt实现

#include <stdlib.h>
#include "stdio.h"
#include <math.h>
#include <memory.h>


//filter函数

void filter(const float* x, float* y, int xlen, float* a, float* b, int nfilt, float* zi)//nfilt为系数数组长度
{
#define EPS 0.000001
    float 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(float));//将y清零，以双浮点为单位

    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;
}




//矩阵乘法
void trmul(float *a,float *b,float *c,int m,int n,int k)//矩阵乘法  m为a的行数，n为a的列数数，k为b的行数，第一个矩阵列数必须要和第二个矩阵的行数相同
 { 
    int i,j,l,u;
    for (i=0; i<=m-1; i++)
    for (j=0; j<=k-1; j++)
      { 
 u=i*k+j; c[u]=0.0;
          for (l=0; l<=n-1; l++)
          c[u]=c[u]+a[i*n+l]*b[l*k+j];
      }
    return;
  }