%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%   組合正弦波和chirp法測頻率特性MATLAB仿真    %%%%%%%%%
%%%%%%     虛擬儀器頻率特性測試方法仿真            %%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
close all; 
clear all;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%系統離散化及初始值設置%%%%%%%%%%%%%%%%%%%%%
F0=1;
F1=120;
N=4096;
Fs=2048;
%sys=tf([-5],[2.0e-5 2e-9 1]);
sys=tf([3.355e7],[1 1.504e3 5.394e5 3.291e7]);
%sys=tf([-2.6207],[1 0.0137 0.0000248])
%sys=tf([2500],[1 20 2500]);
sysd=c2d(sys,1/Fs,'tustin');
[num,den]=tfdata(sysd,'v');
[h0,ff0]=freqz(num,den,N/2,Fs);
mag=abs(h0);
ph=angle(h0);
ph=ph*180/pi;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%輸入信號%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
A=0.3;%頻率F1信號的幅度
pr=1;
for i=1:10
    if (F0+40*i)<F1
    f0=F0+40*(i-1);%起始頻率(Hz)
    f1=40*i;%結束頻率
    elseif ((F0+40*i)>F1)& ((F0+40*(i-1))<F1)
        f0=F0+40*(i-1);
        f1=F1;
    else
        break;
    end;
    N1=20*f1;
    N=40*f1;
    Fs=10*f1;
    dfs=1;
    t1=[0:1/Fs:N1/Fs]; %采樣時刻
    t=[0:1/Fs:N/Fs]; 
    F=([1:N]-1)*Fs/N; %換算成實際的頻率值
    F=F(1:N/2);%取N/2個實際頻率點
    df=4*dfs;%頻率間隔
    S=0;
    for i=0:1:((f1-f0)/df)
     f=f0+df*i;
     S=A*cos(2*pi*f*t1)+S;  %組合正弦波
    end;
    x=8*chirp(t1,f0,N/Fs,f1);%chirp
    for i=N1:N+1 %補零消除柵欄效應
    S(i)=0;
    x(i)=0;
    end;
    sysd=c2d(sys,1/Fs,'tustin');
[num,den]=tfdata(sysd,'v'); 
[Am,Pm,A1m,P1m,A0m,P0m,ym]=qiuzhi(S,N,num,den);
[Axm,Pxm,A1xm,P1xm,A0xm,P0xm,y1m]=qiuzhi(x,N,num,den);
 k1=round(f0/(Fs/N));
 k2=round(f1/(Fs/N));
 for i=k1:1:k2
    %if A1m(i)>0.003
   AA(pr)=Am(i);
   P(pr)=Pm(i);
   Ax(pr)=Axm(i);
   Px(pr)=Pxm(i);
   ff(pr)= F(i);
   g=round(2*F(i));
   dA(pr)=Ax(pr)-mag(g);
   dP(pr)=Px(pr)-ph(g);
   pr=pr+1;
    %end;
 end;
end;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%繪制頻率特性曲線%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
w0=ff;
figure(1);
subplot(2,1,1);semilogx(w0,20*log10(AA),'b',ff0,20*log10(mag),'g');%幅頻特性 
%subplot(2,1,1);plot(w0,Amp,'r',w0,Amp1,'b',w0,mag,'g');%幅頻特性
grid;ylabel('增益（dB）');xlabel('頻率（HZ）');
legend('組合正弦波','理論波形');
title('幅頻特性');
axis([0 50 -10 2]);
subplot(2,1,2);semilogx(w0,P,'b',ff0,ph,'g');   %相頻特性
grid;ylabel('相角（deg)');
title('相頻特性');
legend('組合正弦波','理論波形');xlabel('頻率（HZ）');
axis([0 50 -150 10]);
figure(2);
subplot(2,1,1),plot(w0,dA);
grid;ylabel('增益誤差（dB）');xlabel('頻率（HZ）');
subplot(2,1,2),plot(w0,dP);
grid;ylabel('相位誤差（deg）');xlabel('頻率（HZ）');