FDAtool轉C

1.      首先設計低通濾波器

在simulink裡面模擬

得到濾波器係數，並產生標頭檔：

 

 

解得差分方程

           acc_xxx(index)=-IIR_A(2)*acc_xxx(index-1)-IIR_A(3)*acc_xxx(index-2)-IIR_A(4)*acc_xxx(index-3);

           acc_xxx(index)=acc_xxx(index)+IIR_B(1)*acc_x(index)+IIR_B(2)*acc_x(index-1)+IIR_B(3)*acc_x(index-2)+IIR_B(4)*acc_x(index-3);

編寫程式：

%%用來測試低通濾波器的c語言標頭檔    clear,clc;    load('motor(1k0320).mat')    load('acc_xx(5Hz).mat')    [len a]=size(acc_x);        MWSPT_NSEC=5;    IIR_B=[2.191480047329e-005,2.727814787772e-006,2.727814787772e-006,2.191480047329e-005];    IIR_A=[1,   -2.938376706887,    2.879344290419,  -0.9409182983008];           acc_xxx=zeros(1,len);            in=0;    acc_x_iir=zeros(1,len);    for index=1:len        if(index==1)            acc_xxx(index)=acc_xxx(index)+IIR_B(1)*acc_x(index);        elseif(index==2)            acc_xxx(index)=-IIR_A(2)*acc_xxx(index-1);            acc_xxx(index)=acc_xxx(index)+IIR_B(1)*acc_x(index)+IIR_B(2)*acc_x(index-1);        elseif(index==3)            acc_xxx(index)=-IIR_A(2)*acc_xxx(index-1)-IIR_A(3)*acc_xxx(index-2);            acc_xxx(index)=acc_xxx(index)+IIR_B(1)*acc_x(index)+IIR_B(2)*acc_x(index-1)+IIR_B(3)*acc_x(index-2);        else            acc_xxx(index)=-IIR_A(2)*acc_xxx(index-1)-IIR_A(3)*acc_xxx(index-2)-IIR_A(4)*acc_xxx(index-3);            acc_xxx(index)=acc_xxx(index)+IIR_B(1)*acc_x(index)+IIR_B(2)*acc_x(index-1)+IIR_B(3)*acc_x(index-2)+IIR_B(4)*acc_x(index-3);        end%         x(1,1)=acc_x(index);%         for i=1:MWSPT_NSEC%             y(i,1)=0;%             y(i,1)=y(i,1)+x(i,1)*IIR_B(i,1);%             y(i,1)=y(i,1)+x(i,2)*IIR_B(i,2);%             y(i,1)=y(i,1)+x(i,3)*IIR_B(i,3);%             y(i,1)=y(i,1)-y(i,2)*IIR_A(i,2);%             y(i,1)=y(i,1)-y(i,3)*IIR_A(i,3);%             y(i,1)=y(i,1)/IIR_A(i,1);%             y(i,3)=y(i,2);%             y(i,2)=y(i,1);%             x(i,3)=x(i,2);%             x(i,2)=x(i,1);%             x(i+1,1)=y(i,1);%         end%         acc_x_iir(index)=x(MWSPT_NSEC+1,1);    end    xx=acc_xxx-acc_xx(1:len)';    figure(12)     subplot(3,1,1);    plot(acc_xx(1:len));    title('FDATool');    subplot(3,1,2);        plot(acc_xxx);    title('C語言');    ylim([-0.6 0.4]);    subplot(3,1,3);    plot(xx);        ylim([-0.01 0.01]);            

 結果：