frs_rock.m

导弹控制系统的鲁棒控制设计实例

% Frequency responses of the rocket stabilization system
%
% flight time
t = input ('Enter time t = ');
% closed-loop interconnection
sim_rock
clp = starp(sim_ic,K);
%
% closed-loop frequency response
ref_loop = sel(clp,10,8);
[A,B,C,D] = unpck(ref_loop);
[magp,phasep,w] = bode(A,B,C,D,1);
mag = 20*log10(magp);
figure(1)
semilogx(w,mag,'r'), grid
axis([10^(-1) 10^3 -100 20])
xlabel('Frequency (rad/s)'), ylabel('Magnitude (dB)')
title('Closed-loop magnitude plot')
figure(2)
semilogx(w,phasep,'b'), grid
axis([10^(-1) 10^3 -250 0])
xlabel('Frequency (rad/s)'), ylabel('Phase')
title('Closed-loop phase plot')
%
% sensitivity to accelerometer and rate gyro noises
noise_loop = sel(clp,10,9:10);
[A,B,C,D] = unpck(noise_loop);
[magp1,phasep1,w1] = bode(A,B,C,D,1);
mag1 = 20*log10(magp1);
[magp2,phasep2,w2] = bode(A,B,C,D,2);
mag2 = 20*log10(magp2);
figure(3)
semilogx(w1,mag1,'r-',w2,mag2,'r--'), grid
axis([10^(-1) 10^4 -160 -40])
xlabel('Frequency (rad/s)'), ylabel('Magnitude (dB)')
title('Sensitivity to accelerometer and rate gyro noises')
legend('Sensitivity to accelerometer noise','Sensitivity to rate gyro noise',3)
%
% sensitivity of the control effort to the reference
ref_u = sel(clp,11,8);
[A,B,C,D] = unpck(ref_u);
[magp,phasep,w] = bode(A,B,C,D,1);
mag = 20*log10(magp);
figure(4)
semilogx(w,mag,'r'), grid
axis([10^(-1) 10^3 -100 0])
xlabel('Frequency (rad/s)'), ylabel('Magnitude (dB)')
title('T_{\delta^o r} magnitude plot')
%
% sensitivity of the control effort to accelerometer and rate gyro noises
ref_u = sel(clp,11,9:10);
[A,B,C,D] = unpck(ref_u);
[magp1,phasep1,w1] = bode(A,B,C,D,1);
mag1 = 20*log10(magp1);
[magp2,phasep2,w2] = bode(A,B,C,D,2);
mag2 = 20*log10(magp2);
figure(5)
semilogx(w1,mag1,'r-',w2,mag2,'r--'), grid
axis([10^(-1) 10^3 -160 -60])
xlabel('Frequency (rad/s)'), ylabel('Magnitude (dB)')
title('Sensitivity of the control to accelerometer and rate gyro noises')
legend('Sensitivity to accelerometer noise','Sensitivity to rate gyro noise',3)
%
% controller frequency responses
[A,B,C,D] = unpck(K);
[magp1,phasep1,w1] = bode(A,B,C,D,1);
mag1 = 20*log10(magp1);
[magp2,phasep2,w2] = bode(A,B,C,D,2);
mag2 = 20*log10(magp2);
figure(6)
semilogx(w1,mag1,'r-',w2,mag2,'r--'), grid
axis([10^(-1) 10^4 -80 60])
xlabel('Frequency (rad/s)'), ylabel('Magnitude (dB)')
title('Controller magnitude plots')
legend('K_{\delta^o n_y}','K_{\delta^o d\theta/dt}',3)
figure(7)
semilogx(w1,phasep1,'b-',w2,phasep2,'b--'), grid
axis([10^(-1) 10^4 -200 200])
xlabel('Frequency (rad/s)'), ylabel('Phase')
title('Controller phase plots')
legend('K_{\delta^o n_y}','K_{\delta^o d\theta/dt}',3)
%
% open-loop frequency response
G_ny1_u = sel(sim_ic,13:14,11);
olp = mmult(K,G_ny1_u);
[A,B,C,D] = unpck(olp);
[magp,phasep,w] = bode(A,B,C,D,1);
mag = 20*log10(magp);
figure(8)
semilogx(w,mag,'r'), grid
axis([10^(0) 10^4 -140 60])
xlabel('Frequency (rad/s)'), ylabel('Magnitude (dB)')
title('Open-loop magnitude plot')
figure(9)
semilogx(w,phasep,'b'), grid
axis([10^(0) 10^4 -200 250])
xlabel('Frequency (rad/s)'), ylabel('Phase')
title('Open-loop phase plot')