离线滤波C语言实现

假设输入占用10kRAM，实现这个滤波需要额外的4x10kRAM。
以下是滤波的核心代码：

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

#define EPS 0.000001
//#pragma warning(disable:4244)
//filter函数
void filter(double* x, double* y, int xlen, double* a, double* b, int nfilt, double* zi)//nfilt为系数数组长度
{
	//printf("%f\n", x[0]);
	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));//将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(double *a, double *b, double *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;
}


//求逆矩阵，当返回值为0时成功，a变为逆矩阵
int rinv(double *a, int n)//逆矩阵
{
	int *is, *js, i, j, k, l, u, v;
	double d, p;
	is = (int *)malloc(n * sizeof(int));
	js = (int *)malloc(n * sizeof(int));
	for (k = 0; k <= n - 1; k++)
	{
		d = 0.0;
		for (i = k; i <= n - 1; i++)
			for (j = k; j <= n - 1; j++)
			{
				l = i * n + j; p = fabs(a[l]);
				if (p > d) { d = p; is[k] = i; js[k] = j; }
			}
		if (d + 1.0 == 1.0)
		{
			free(is); free(js); printf("err**not invn");
			return(0);
		}
		if (is[k] != k)
			for (j = 0; j <= n - 1; j++)
			{
				u = k * n + j; v = is[k] * n + j;
				p = a[u]; a[u] = a[v]; a[v] = p;
			}
		if (js[k] != k)
			for (i = 0; i <= n - 1; i++)
			{
				u = i * n + k; v = i * n + js[k];
				p = a[u]; a[u] = a[v]; a[v] = p;
			}
		l = k * n + k;
		a[l] = 1.0 / a[l];
		for (j = 0; j <= n - 1; j++)
			if (j != k)
			{
				u = k * n + j; a[u] = a[u] * a[l];
			}
		for (i = 0; i <= n - 1; i++)
			if (i != k)
				for (j = 0; j <= n - 1; j++)
					if (j != k)
					{
						u = i * n + j;
						a[u] = a[u] - a[i*n + k] * a[k*n + j];
					}
		for (i = 0; i <= n - 1; i++)
			if (i != k)
			{
				u = i * n + k; a[u] = -a[u] * a[l];
			}
	}
	for (k = n - 1; k >= 0; k--)
	{
		if (js[k] != k)
			for (j = 0; j <= n - 1; j++)
			{
				u = k * n + j; v = js[k] * n + j;
				p = a[u]; a[u] = a[v]; a[v] = p;
			}
		if (is[k] != k)
			for (i = 0; i <= n - 1; i++)
			{
				u = i * n + k; v = i * n + is[k];
				p = a[u]; a[u] = a[v]; a[v] = p;
			}
	}
	free(is);
	free(js);
	return(0);
}


//filtfilt函数
int filtfilt(double* x, double* y, int xlen, double* a, double* b, int nfilt, double* tx, double* tx1, double* sp, double* tvec, double* zi)
{
	//printf("%f\n", x[0]);
	int nfact;
	int tlen;    //length of tx
	int i;
	double *p, *t, *end;

	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;
	memset(tx, 0, tlen * sizeof(double));
	memset(tx1, 0, tlen * sizeof(double));
	memset(sp, 0, tlen * sizeof(double));
	memset(tvec, 0, tlen * sizeof(double));

	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 (rinv(sp, nfact)) {
		free(zi);
		zi = NULL;
	}
	else {
		trmul(sp, tvec, zi, nfact, nfact, 1);
	}//zi is ok
	
	//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--;
	}
	return 0;
}

以下分别是低通和带通的调用方式：

#include "filtfilt.h"
#include "lowpass.h"
#include "len.h"
//三阶低通滤波，a,b已经通过python获得，如下
static int NFILT = 4;
static double a[4] = { 1.0, -2.9685844, 2.93766033, -0.96907211};
static double b[4] = { 4.76951789e-07, 1.43085537e-06, 1.43085537e-06, 4.76951789e-07 };

int lowpass(double* x, double* y, double* tx, double* tx1, double* sp, double* tvec, double* zi){
	filtfilt(x, y, LEN, a, b, NFILT, tx, tx1, sp, tvec, zi);
	return 0;
}

#include "filtfilt.h"
#include "bandpass.h"
#include "len.h"
/*
	butterworth filter
	bandpass filter
	order == 3
	b, a were generated by python scipy.signal.butter
*/

static int NFILT = 7;
static double a[7] = {1.0, -5.66016623, 13.36832544, -16.86471382, 11.98614053, -4.55060366, 0.72101783 };
static double b[7] = { 0.00046585, 0.0, -0.00139755, 0.0, 0.00139755, 0.0, -0.00046585 };

int bandpass(double* x, double* y, double* tx, double* tx1, double* sp, double* tvec, double* zi) {

	filtfilt(x, y, LEN, a, b, NFILT, tx, tx1, sp, tvec, zi);
	return 0;
}

如果在嵌入式实现，需要在调用之前为tx,tx1,sp1,tvec申请与输入同样大小的静态内存空间。

#define LEN 1000
static double out[4 * LEN + 200] = { 0 };//额外申请200是为了防止越界，视情况而定，非必须
double* tx = out;
double* tx1 = out + LEN + 40;
double* sp = out + 2 * LEN + 80;
double* tvec = out + 3 * LEN + 120;
double* zi = out + 4 * LEN + 160;//zi占用空间极少，一般在10个单位以内，但也需要申请静态存储空间