互相关原理.m

用互相关原理实现控制系统的频率特性测试仿真

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%   曲线拟和求频率特性    %%%%%%%%%%%%%%%%%
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）');