matlab中滤波器函数filter的C语言实现

使用matlab对传感器采集的数据进行低通滤波处理，选定的是切比雪夫I型滤波器，使用matlab自带的函数滤波正常，滤波程序如下：

%设计低通滤波器
wp=3*2/fs;  %通带边界频率15Hz（归一化频率）
ws=15*2/fs;  %阻带边界频率15Hz（归一化频率）
rp=1;rs=30;Nn=512;  %通带波纹和阻带衰减，以及绘制频率特性的数据点数
[nn,wn]=cheb1ord(wp,ws,rp,rs);  %求得数字滤波器的最小阶数和归一化截止频率
[b,a]=cheby1(nn,rp,wn);  %按最小阶数、通带波纹和截止频率设计数字滤波器


DACCx=filter(b,a,ACCx1);%对输入信号进行滤波
DACCy=filter(b,a,ACCy1);%对输入信号进行滤波
DACCz=filter(b,a,ACCz1);%对输入信号进行滤波


DGROx=filter(b,a,GROx1);%对输入信号进行滤波
DGROy=filter(b,a,GROy1);%对输入信号进行滤波

其中ACCx1、ACCy1、ACCz1、GROx1、GROy1是采集的传感器原始数据序列，这里就不再添加数据。计算出的滤波器参数如下：

a(1)=1;a(2)=-2.969516349642659;a(3)=2.940162369075828;a(4)=-0.970632740348324;
b(1)=0.165988560552055*10^(-5);b(2)=0.497965681656165*10^(-5);b(3)=0.497965681656165*10^(-5);b(4)=0.165988560552055*10^(-5);

在STM32中进行相同的低通滤波操作，所以需要使用C语言实现filter函数，根据自己的理解编写的filter程序如下：

%使用差分方程进行低通滤波，观察效果
a1=1;a2=-2.9695;a3=2.9402;a4=-0.9706;
b1=0.1660*10^(-5);b2=0.4980*10^(-5);b3=0.4980*10^(-5);b4=0.1660*10^(-5);
x=ACCx1;
y(1)=b1*x(1);
y(2)=b1*x(2)+b2*x(1)-a2*y(1);
y(3)=b1*x(3)+b2*x(2)+b3*x(1)-a2*y(2)-a3*y(1);
for i=4:len
   y(i)=(b1*x(i)+b2*x(i-1)+b3*x(i-2)+b4*x(i-3)-a2*y(i-1)-a3*y(i-2)-a4*y(i-3)); 
end

先将filter的计算过程说明一下，以三阶为例：
当n=1时，x(n-1)、x(n-2)、x(n-3)、y(n-1)、y(n-2)、y(n-3)全都赋值为0，所以y(1)=b1*x(1)；
当n=2时，x(n-2)、x(n-3)、y(n-2)、y(n-3)全都赋值为0，所以y(2)=b1*x(2)+b2*x(1)-a2*y(1);
当n=3时，x(n-3)、y(n-3)全都赋值为0，所以y(3)=b1*x(3)+b2*x(2)+b3*x(1)-a2*y(2)-a3*y(1);
自此计算出y(1)、y(2)、y(3)；
当n>=4时就可以采用递推运算
y(i)=b1*x(i)+b2*x(i-1)+b3*x(i-2)+b4*x(i-3)-a2*y(i-1)-a3*y(i-2)-a4*y(i-3);其中i=n,n>=4；
但是需要注意的是，进行递推运算是如果a、b的系数误差太大会产生累积误差，所以系数尽量精确。