matlab 验证编写离散低通滤波器是否正确

1.matlab中的打印  fprintf('the value of pi is%6.2f\n',pi)

2.数字滤波器，与当前的输入，过去的输入和过去的输出有关系，为递归滤波器。只和现在的输入、过去的输入有关系，为非递归滤波器。

3. 采用差分方程的方式编写了低通滤波器，代码如下，特意跟matlab自带的filter()函数做对比。

%% Main函数
function Main()
    global Xpre Ypre            % 全局变量，用来保存历史值
    global N_low_f N_high_f
    global A_low B_low
    B_low = [0.00569,-0.01537,0.0097,0.00978, -0.015373,0.00569];
    A_low = [1.000000,-4.555306,8.364724,-7.7350,3.600, -0.67471];

    N_low_f = 5;               % 滤波器的阶数，这里采用椭圆滤波器

    for i = 1:1:N_low_f
        Ypre(i) = 0.0;
    end
    for i = 1:1:N_low_f
        Xpre(i) = 0.0;
    end
    [x,y,z,t]=textread('.\data.txt','%n%n%n%n','delimiter', ',','headerlines', 1); %最后一个不需要处理  

   %% 验证低通滤波器设计是否正确
    Fs = 100;
    t = (1:100)/Fs;
    s1 = sin(2*pi*t*0.3);
    s2 = sin(2*pi*t*8);
    s3 = sin(2*pi*t*3);
    s = s1 + s2 + s3;
    sf = filter(B_low, A_low,s);
    figure(3)
    subplot(2,2,1)
    plot(t,s);
    hold on 
    plot(t,sf);
    xlabel('s/t');
    ylabel('value');
    S = fft(s,512);
    SF = fft(sf,512);
    w = (0:255)/256*(Fs/2);
    subplot(2,2,2) 
    plot(w, abs(S(1:256)),'r');
    xlabel('频率/Hz');
    ylabel('幅度');
    hold on
    plot(w,abs(SF(1:256)),'b');
    legend('滤波前傅里叶变换','滤波后傅里叶');
   
    % 采用编写的离散傅里叶确认是否有问题----有问题,滤波效果不太好-已解决2019.9.17 @vi
    % 历史输入赋值错位
    subplot(2,2,3)
    plot(t,s);
    
    for i = 1:6:length(s)-5
        sda = LowFilter(s(i:i+5));
        sff(i:i+5) = sda;        
    end
    hold on
    plot(t(1:length(sff)),sff);
    
    SFF = fft(sff,512);
    subplot(2,2,4)
    plot(w,abs(S(1:256)));
    hold on
    plot(w,abs(SFF(1:256)));    
         
end %Main end

%%  离散低通滤波的实现
%****************************************
% destData 离散低通滤波器的输出
% sourData 离散低通滤波器的输入 6个输入为一组
%****************************************
function destData = LowFilter(sourData)
    global Xpre Ypre
    global N_low_f         % 5
    global A_low  B_low
    A_low(1) = 0;
    for i = 1:1:(N_low_f+1)
        Y(i) = 0.0;
    end

    for iOut =1:1:N_low_f+1
        % 输出数据受输入和输出的影响 递归函数
        iIn = 1;
        while iIn <= iOut
            Y(iOut) = Y(iOut) + B_low(iIn)*sourData(iOut - iIn +1) - A_low(iIn)*Y(iOut - iIn+1);
            iIn = iIn + 1;
        end
        
        while iIn <= N_low_f+1          
            Y(iOut) = Y(iOut) + B_low(iIn) * Xpre(iIn - iOut) - A_low(iIn)*Ypre(iIn - iOut);
            iIn = iIn + 1;
        end
    end
    
    % X(2) X(3) X(4) the num is bigger,the data is older 
    A_low(1) = 1.0;
    for i = 1:1:N_low_f
        Xpre(i) =  sourData(N_low_f - i + 2);
        Ypre(i) =  Y(N_low_f - i + 2);
    end

    for i = 1:1:N_low_f+1
        destData(i) = Y(i);
    end  
end

4.高通滤波器采用相同的方式验证，发现结果都一致，说明编写没有问题。

%% Main函数
function Main()
    global Xpre Ypre            % 全局变量，用来保存历史值
    global N_high_f
    global A_high B_high
 
    B_high = [0.74, -3.72, 7.419, -7.419, 3.7204, -0.74849];
    A_high = [1.00, -4.394515, 7.78123, -6.9334, 3.1069,-0.5597];

    N_high_f = 5;
    for i = 1:1:N_low_f
        Ypre(i) = 0.0;
    end
    for i = 1:1:N_low_f
        Xpre(i) = 0.0;
    end
    [x,y,z,t]=textread('.\data.txt','%n%n%n%n','delimiter', ',','headerlines', 1); %最后一个不需要处理   

   %% 验证高通滤波器设计是否正确
    Fs = 100;
    t = (1:100)/Fs;
    s1 = sin(2*pi*t*0.3);
    s2 = sin(2*pi*t*8);
    s3 = sin(2*pi*t*3);
    s = s1 + s2 + s3;
    sf = filter(B_high, A_high,s);
    
    figure(3)
    subplot(2,2,1)
    plot(t,s,'r');
    hold on 
    plot(t,sf,'b');
    xlabel('s/t');
    ylabel('value');
    legend('滤波前','matlab-filter滤波后')
    S = fft(s,512);
    SF = fft(sf,512);
    w = (0:255)/256*(Fs/2);
    subplot(2,2,2) 
    plot(w, abs(S(1:256)),'r');
    xlabel('频率/Hz');
    ylabel('幅度');
    hold on
    plot(w,abs(SF(1:256)),'b');
    legend('滤波前傅里叶变换','滤波后傅里叶');

    % 采用编写的离散傅里叶确认是否有问题----有问题,滤波效果不太好-已解决2019.9.17 @vi
    % 历史输入赋值错位
    subplot(2,2,3)
    plot(t,s,'r');
    
    for i = 1:6:length(s)-5
        sda = HighFilter(s(i:i+5));
        sff(i:i+5) = sda;        
    end
    hold on
    plot(t(1:length(sff)),sff,'b');
    xlabel('s/t');
    ylabel('value');
    legend('滤波前','离散HighFilter滤波后')
    
    SFF = fft(sff,512);
    subplot(2,2,4)
    plot(w,abs(S(1:256)),'r');
    hold on
    plot(w,abs(SFF(1:256)),'b');   
    xlabel('频率/Hz');
    ylabel('幅度');
    legend('滤波前傅里叶变换','滤波后傅里叶');     
end %Main end

%% 离散高通滤波器
function destdata = HighFilter(sourdata)
    global Xpre Ypre
    global N_high_f
    global A_high  B_high
    A_high(1) = 0.0;
    for i = 1:1:(N_high_f+1)
        Y(i) = 0.0;
    end
    
    for iOut = 1:1:N_high_f +1
        iIn = 1;
        while iIn <= iOut
            Y(iOut) = Y(iOut) + B_high(iIn)*sourdata(iOut - iIn +1) -A_high(iIn)*Y(iOut - iIn +1);
            iIn = iIn + 1;
        end
        
        while iIn <= N_high_f +1
            Y(iOut) = Y(iOut) + B_high(iIn)*Xpre(iIn-iOut) - A_high(iIn)*Ypre(iIn -iOut);
            iIn = iIn + 1;
        end
    end
     
    A_high(1) =1.0;
    for iOut =1:1:N_high_f
        Xpre(iOut) = sourdata(N_high_f -iOut + 2);                          %此次原本写错了，后面应该+2，非+1
        Ypre(iOut) = Y(N_high_f -iOut + 2);
    end
    for iOut = 1:1:N_high_f+1
        destdata(iOut) = Y(iOut);
    end   
end

5.放到数据中，发现低通滤波器确实没有问题，说明处理数据存在问题。

代码

    dlen = length(x) - 1;
    xdata = x(1:dlen);
    ydata = y(1:dlen);
    zdata = z(1:dlen);
    tdata = t(1:dlen)/(25*64);                   % 采样频率为25Hz,时间记录每个相差64.
    XGD = EllipticFilter_v(xdata);               % 函数中编写的采用matlab自带的filter
    YGD = EllipticFilter_v(ydata);
    ZGD = EllipticFilter_v(zdata);
    
    if plotFigure == 0
        subplot(2,2,1)
        plot(tdata,xdata);
        hold on
        plot(tdata,ydata);
        hold on
        plot(tdata,zdata);
        xlabel('t/s');ylabel('幅度');
        title('原始数据')
        legend('x','y','z');
    
        subplot(2,2,2)
        plot(tdata,XGD);
        hold on
        plot(tdata,YGD);
        hold on
        plot(tdata,ZGD);
        xlabel('t/s');ylabel('幅度');
        title('经过椭圆低通滤波器后的数据')
        legend('x','y','z');
    
        subplot(2,2,4)
        for i = 1:6:dlen-5
        sda = LowFilter(xdata(i:i+5));    %这里的LowFilter是上面自己编写的
        sff(i:i+5) = sda;        
        end
     
        plot(t(1:length(sff)),sff,'b');
        xlabel('s/t');
        ylabel('value');
        title('离散LowFilter滤波后')
    end

结果（后面只仿真X轴）

找到问题的原因了，在分别计算x y z轴是，共用了相同的全局变量，导致全局变量在x y z轴之间变换。应该计算每个轴有单独的全局变量，用来记录历史数据。不然记录x轴的历史数据也去记录y了。

之前一直算出的数据不对，现在设定多个全局变量，对全局变量作为参数传入，正确代码和结果如下（以高通滤波器为例），

代码

main函数中
   
 % 用来存放历史数据
    for xi = 1:1:N_low_f
        Xprehx(xi) = 0.0;
        Yprehx(xi) = 0.0;
    end
    for xi = 1:1:N_low_f
        Xprehy(xi) = 0.0;
        Yprehy(xi) = 0.0;
    end
    for xi = 1:1:N_low_f
        Xprehz(xi) = 0.0;
        Yprehz(xi) = 0.0;
    end

    % 这个全局变量计算x y z时，全局变量都会变，相互影响。
    [destxh,Xprehx,Yprehx] = HighFilterData(dataX,N_high_f, Xprehx, Yprehx);
    [destyh,Xprehy,Yprehy] = HighFilterData(dataY,N_high_f, Xprehy, Yprehy);
    [destzh,Xprehz,Yprehz] = HighFilterData(dataZ,N_high_f, Xprehz, Yprehz);

%% 离散高通滤波器
function [destdata,XpreH,YpreH] = HighFilterData(sourdata,N_high_f,XpreH,YpreH)
    global A_high  B_high
    A_high(1) = 0.0;
    for i = 1:1:(N_high_f+1)
        Y(i) = 0.0;
    end
    
    for iOut = 1:1:N_high_f +1
        iIn = 1;
        while iIn <= iOut
            Y(iOut) = Y(iOut) + B_high(iIn)*sourdata(iOut - iIn +1) -A_high(iIn)*Y(iOut - iIn +1);
            iIn = iIn + 1;
        end
        
        while iIn <= N_high_f +1
            Y(iOut) = Y(iOut) + B_high(iIn)*XpreH(iIn-iOut) - A_high(iIn)*YpreH(iIn -iOut);
            iIn = iIn + 1;
        end
    end
     
    A_high(1) =1.0;
    for iOut =1:1:N_high_f
        XpreH(iOut) = sourdata(N_high_f -iOut + 2);                          %此次原本写错了，后面应该+2，非+1
        YpreH(iOut) = Y(N_high_f -iOut + 2);
    end
    for iOut = 1:1:N_high_f+1
        destdata(iOut) = Y(iOut);
    end 
 
end

结果