% Matlab Script to compute Ganin and Phase Margins for the roll 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')
%K=-.8 % Feedback gain, gives 3.76 db gain margin and infinite phase margin. - is the sign here for destabilizing feedback
%K=-1.23347363742067   % gives 0 db of gain margin (destabilizing feedback)
%K=+1.23347363742067   % gives 14.86 db of gain margin and 140.6 deg of phase margin (stabilizing feedback)
K=+.001815 % + is the sign here for stabilizing feedback, Gm =18.6818671400488(25.4dB), Pm =Inf
K=-.001815 % - is the sign here for de-stabilizing feedback,
% Define the roll mode transfer function, numerator first then denominator

if K<=0; sense=' de-stabilizing feedback'; else; sense=' stabilizing feedback'; end; sense
num=[4301 5593 34580 0] 
den=[1 14.37 28.24 137.6 2.454]
% Define a system that is the roll mode transfer function
disp('Roll mode transfer function ')
rollSys=tf(num,den)
RollMode=roots(den)
% Cascade the servo model and the roll mode model
disp('Properties of the open loop system (roll + servo + gain')
openLoop=series(servoSys3,K*rollSys)
% 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 + roll 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 + roll mode + gain)
figure(3)
nyquist(openLoop) % Check that you have the proper margins.
disp('Use the Nyquist plot to insure 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
%
%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('openLoopRoll')   % This uses another Simulink block diagram with the servo + roll TF (no gain).
RollSys=ss(a,b,c,d) 
figure(4)
rlocus(RollSys) % I finally figured it out. 
title('Roll rate feedback to the aileron: Stabilizing feedback')
pp=rlocus(RollSys,abs(K)) % get poles for nominal gain
hold on; plot(real(pp),imag(pp),'s'); hold off
v=axis;
axis([v(1),v(2),-80,80])
%    [K,POLES] = RLOCFIND(SYS)
[gS,pS]=rlocfind(RollSys)
text(-10,-10,['Stable for gain < ',num2str(gS),' deg/deg/sec'])
text(-25,-15,['Square symbols are closed loop poles for the nominal gain, K= ',num2str(K),' deg/deg/sec'])

figure(5)
RollSysD=ss(a,b,-c,d) % note sign change is the c matrix.
rlocus(RollSysD)
title('Roll rate feedback to the aileron: De-stabilizing feedback')
% margin(RollSysD) % This works. This approach was probably the best way to procede.
[gD,pD]=rlocfind(RollSysD)
text(-10,40,['Stable for gain < ',num2str(gD),' deg/deg/sec'])
ppp=rlocus(RollSysD,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'])

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

disp('closed loop, destabilizing')
closedLoop=feedback(RollSys,-abs(K),-1); 
closedLoopTF=tf(closedLoop)
figure(6)
[yD,tD]=step(closedLoop,1);
damp(RollSys)




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

damp(closedLoopTF)

