%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%   曲線擬和求頻率特性    %%%%%%%%%%%%%%%%%
close all; 
clear all;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%系統離散化及初始值設置%%%%%%%%%%%%%%%%%%%%%
sys=tf([3.355e7],[1 1.504e3 5.394e5 3.291e7]);
Fs=400;
h=1/Fs;
N=40000;
F0=1;
F1=80;
df=1;
t=[0:h:N*h]; 
%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=20*log10(abs(h0));
ph=angle(h0);
ph=ph*180/pi;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%輸入信號%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
A=1;%頻率F1信號的幅度
M=(F1-F0)/df+1;
ff=[1:M];
AA=[1:M];
PP=[1:M];
dA=[1:M];
dP=[1:M];
for i=1:M
  f=F0+df*(i-1);
  x=A*cos(2*pi*f*t); 
  y=filter(num,den,x);
   U=zeros(N+1,2);
   for j=1:N+1
     U(j,1)=sin(2*pi*f*t(j));
     U(j,2)=cos(2*pi*f*t(j));
   end;
  V=zeros(N+1,1);
  for j=1:N+1
      V(j,1)=y(j);
  end;
 C=zeros(2,1);
 C=inv(U'*U)*U'*V;
 c1=C(1,1);
 c2=C(2,1);
 k=round(f/(Fs/N));
 AA(i)=20*log10(sqrt(c1*c1+c2*c2)/A);
 PP(i)=atan(c2/c1)*180/pi-90;
 dA(i)=AA(i)-mag(k);
 dP(i)=PP(i)-ph(k);
 ff(i)=f;
end;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%繪制頻率特性曲線%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
w0=ff;
figure(1);
subplot(2,1,1);semilogx(w0,AA,'b.',ff0(1:N/2),mag,'g');%幅頻特性 
grid;ylabel('增益（dB）');xlabel('頻率（HZ）');
legend('組合正弦波','理論波形');
title('幅頻特性');
axis([0 50 -10 2]);
subplot(2,1,2);semilogx(w0,PP,'b.',ff0(1:N/2),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）');