% Matlab Script to compute Ganin and Phase Margins for the roll axis of the AAE451 aircraft

% Define the feedback gain
close all
clear all
disp('You want to pick a gain, K, to achieve the desired gain and phase margins')

K=0.49 % + is the sign here for stabilizing feedback, Gm =18.6818671400488(25.4dB), Pm =Inf

if K<=0; sense=' de-stabilizing feedback'; else; sense=' stabilizing feedback'; end; sense
num=[-583.8 -7736 -722.6 -2208];
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)
disp('Properties of the open loop system (roll + gain')
openLoop=series(1,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

% 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(-70,-10,['Stable for gain < ',num2str(gS),' deg/deg/sec'])
text(-95,-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(-40,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(-50,-40,['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);

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



% 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'])


