% 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=-.01  % - is the sign here for stabilizing feedback Gm =29.9022601539077, Pm =135.821537179302 (stable, nice response)
%K=.01  % + is the sign here for destabilizing feedback, Gm =0.788776235495788, Pm =33.9721363120902 (slightly unstable, oscillatory)
%K=-.299 % - is the sign for stabilizing feedback, Gm =1.0000755904317, Pm =0.00287958673152389 (unstable oscillatory)
%K=+.299 % + is the sign for destabilizing feedback, Gm =12.2264254602092(21.7dB), Pm =83.3820285388344 (unstable aperiodic)
%K=.008  % Gm =0.985970294369735, Pm =8.08199670948014
%K=.005   % Gm =1.57755247099158, Pm =Inf (oscillatory but stable)
%K=-.005   %Gm =59.8045203078155, Pm =Inf (stable, nice response)
%K=+.007 % + is the sign for destabilizing feedback, Gm =1.12682319356541,Pm =Inf (stable but very oscillatory)
%K=-.007 % - is the sign for stabilizing feedback, Gm =42.7175145055825, Pm =Inf (stable and nice)
%K=+.07 % + is the sign for destabilizing feedback,Gm =0.112682319356541(-18.96dB),Pm =80.7692405427179 (unstable)
%K=-.07 % - is the sign for stabilizing feedback, Gm =4.27175145055825(12.6dB), Pm = 68.7781193230131 (stable)
%K=-.05 % - is the sign for stabilizing feedback,Gm =5.98045203078155(15.53dB), Pm =78.733541763658
K=+.05 % + is the sign for de-stabilizing feedback,

if K>=0; sense=' de-stabilizing feedback'; else; sense=' stabilizing feedback'; end; sense
% Define the Dutch Roll mode transfer function, numerator first then denominator
num=[-142.44 -1.9755]; 
den=[1 1.0917 7.1961];
% Define a system that is the yaw mode transfer function
disp('Dutch roll mode transfer function ')
yawSys=tf(num,den)
dutchroll=roots(den)
% Cascade the servo model and the yaw mode model
disp('Properties of the open loop system (yaw + servo + gain)')
openLoop=series(servoSys3,K*yawSys)
% 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 + yaw 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 + yaw mode + gain)
figure(3)
nyquist(openLoop) % Check that you have the proper margins.
title(['Including nominal gain = ',num2str(K),' deg/deg/sec'])
text(-.8,-.5,['Gain margin= ',num2str(Gmdb),' dB, Phase Margin= ',num2str(Pm),' deg'])
text(-.8,-.9,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('openLoopYaw');   % This uses another Simulink block diagram with the servo + yaw TF (no gain).
YawSysD=ss(a,b,c,d); 
figure(4)
rlocus(YawSysD) % I finally figured it out. 
title('Yaw rate feedback to the rudder: De-stabilizing feedback')
pp=rlocus(YawSysD,abs(K)) % get poles for nominal gain
hold on; plot(real(pp),imag(pp),'s'); hold off
%    [K,POLES] = RLOCFIND(SYS)
[gDS,pDS]=rlocfind(YawSysD)
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)
YawSys=ss(a,b,-c,d); % note sign change is the c matrix.
rlocus(YawSys)
title('Yaw rate feedback to the rudder: Stabilizing feedback')
[gS,pS]=rlocfind(YawSys)
text(-10,40,['Stable for gain < ',num2str(gS),' deg/deg/sec'])
ppp=rlocus(YawSys,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(YawSys) % This works. This approach was probably the best way to procede.
disp('closed loop, destabilizing')
closedLoop=feedback(YawSysD,abs(K),-1); 
closedLoopTF=tf(closedLoop)
figure(6)
[yD,tD]=step(closedLoop,15);

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

% open loop response
[y,t]=step(YawSysD,15);
plot(t,y,'-',tS,yS,':',tD,yD,'--')
legend('Open loop', 'Stabilizing feedback','Destabilizing feedback')
title('Yaw rate rate step response for rudder input')
xlabel('time (seconds)')
ylabel('yaw rate (deg/sec)')
text(2,-28,['abs(K)= ',num2str(abs(K)),' deg/deg/sec'])
vv=axis;
hold on; plot(tD,yD,'x'); hold off
axis([vv(1),vv(2),-50,50])
