% Matlab Script to compute Gain and Phase Margins for the yaw axis of the AAE451 aircraft
% This script uses the simulink block diagram called 
% S148Linear.mdl
% and optionally uses the Simulink block diagram called servoNL.mdl
%
clear
disp('  ')
disp('Start here')
disp('  ')
[a,b,c,d]=linmod('S148Linear');      % get linear model from linear servo model in state space form
%[aNL,bNL,cNL,dNL]=linmod('servoNL') % get linear model from nonlinear servo model
servoSys=ss(a,b,c,d); % Define a Matlab system from the linear model from the state space
disp('servo')
servoSys3=tf(servoSys) % Define a Matlab system in transfer function form
%
%
% Define the feedback gain
disp('You want to pick a gain, K, to achieve the desired gain and phase margins')
% Note gain margins below are not in dB. If they are less then 1 then the CL is nstable
%K=-.31  % - is the sign here for stabilizing feedback, Gm =20.6908508856175(26.3dB), Pm =Inf
%K=-.33  % - is the sign here for stabilizing feedback,Gm =19.4368599228528(25.77dB), Pm =Inf
K=+.33  % + is the sign here for de-stabilizing feedback,Gm =1.10862926149568(0.8957dB), Pm =Inf
%K=.31    % + is the sign here for destabilizing feedback, Gm =1.18015372997927(1.4dB), Pm =Inf, nice trans resp


if K>=0; sense=' de-stabilizing feedback'; else; sense=' stabilizing feedback'; end; sense
% Define the pitch mode transfer function, numerator first then denominator
num=[-7.334 -61.79]; 
den=[1 11.52 22.63];
% Define a system that is the pitch mode transfer function
disp('Short period mode transfer function ')
pitchSys=tf(num,den)
shortperiod=roots(den)
% Cascade the servo model and the pitch mode model
disp('Properties of the open loop system (pitch + servo + gain)')
openLoop=series(servoSys3,K*pitchSys)
% Plot the open loop bode plot including any gain K that may have been set above.
figure(1)
bode(openLoop)  % plots the bode plot
title(['Including nominal gain = ',num2str(K),' deg/deg/sec'])
[mag,phase,w]=bode(openLoop); % returns the data for the bode plot
% Compute the gain and phase margin of the open loop system (servo + pitch mode + gain)
figure(2)
margin(mag,phase,w)
title(['Stability Margins including nominal gain = ',num2str(K),' deg/deg/sec, ',sense])
[Gm,Pm,Wcg,Wcp]=margin(mag,phase,w) % Note: this gain margin is not in db
% Be careful that you understand the MATLABs definitions of the frequencies Wcg and Wcp.
disp('Be careful that you understand the MATLABs definitions of the frequencies Wcg and Wcp.')
disp('Look carefully at the gain and phase margins on the plot')
Gmdb=20*log10(Gm) % Gain morgin expressed in db
% Compute the Nyquist plot for the open loop system  (servo + pitch mode + gain)
figure(3)
nyquist(openLoop) % Check that you have the proper margins.
title(['Including nominal gain = ',num2str(K),' deg/deg/sec'])
text(-.8,-.05,['Gain margin= ',num2str(Gmdb),' dB, Phase Margin= ',num2str(Pm),' deg'])
text(-.8,-.09,sense)
theta=0:pi/180:2*pi;
hold on; plot(cos(theta),sin(theta));hold off
disp('Use the Nyquist plot to insure that you have the proper margins.')
%
%rlocus(openLoop) % For some strange reason, I can't get root locus to work. Maybe you can.
%rlocus(servoSys) % Works.
%rlocus(servoSys3) % Does not work.
%rlocus(rollSys) % Does not work.
%openLoop=ss(openLoop) % Does not work.
%[aa,bb,cc,dd]=ss(openLoop) % Does not work.
% I have another idea. Get a, b, c, d state space matrices from Simulink for the open loop system.
% get linear model from open loop simulink model in state space form
[a,b,c,d]=linmod('openLoopPitch');   % This uses another Simulink block diagram with the servo + pitch TF (no gain).
pitchSysD=ss(a,b,c,d); 
figure(4)
rlocus(pitchSysD) % I finally figured it out. 
title('Pitch rate feedback to the elevator: De-stabilizing feedback')
pp=rlocus(pitchSysD,abs(K)) % get poles for nominal gain
hold on; plot(real(pp),imag(pp),'s'); hold off
%    [K,POLES] = RLOCFIND(SYS)
[gDS,pDS]=rlocfind(pitchSysD)
text(-10,-10,['Stable for gain < ',num2str(gDS),' deg/deg/sec'])
text(-25,-15,['Square symbols are closed loop poles for the nominal gain, K= ',num2str(K),' deg/deg/sec'])
figure(5)
pitchSys=ss(a,b,-c,d); % note sign change is the c matrix.
rlocus(pitchSys)
title('Pitch rate feedback to the elevator: Stabilizing feedback')
[gS,pS]=rlocfind(pitchSys)
text(-10,40,['Stable for gain < ',num2str(gS),' deg/deg/sec'])
ppp=rlocus(pitchSys,abs(K)) % get poles for nominal gain
hold on; plot(real(ppp),imag(ppp),'s'); hold off
text(-25,-15,['Square symbols are closed loop poles for the nominal gain, K= ',num2str(K),' deg/deg/sec'])


% margin(pitchSys) % This works. This approach was probably the best way to procede.
disp('closed loop, destabilizing')
closedLoop=feedback(pitchSysD,abs(K),-1); 
closedLoopTF=tf(closedLoop)
figure(6)
[yD,tD]=step(closedLoop,25);


disp('closed loop stabilizing')
closedLoop=feedback(pitchSysD,-abs(K),-1); 
closedLoopTF=tf(closedLoop)
[yS,tS]=step(closedLoop,25);


% open loop response
[y,t]=step(pitchSysD,25);
plot(t,y,'-',tS,yS,':',tD,yD,'x')
legend('Open loop', 'Stabilizing feedback','Destabilizing feedback')
title('Pitch rate step response')
xlabel('time (seconds)')
ylabel('pitch rate (deg/sec)')
text(2,-28,['abs(K)= ',num2str(abs(K)),' deg/deg/sec'])

