% Linearize MPX-5 about XR2, UR2
% You must run Step12_Phugoid_Trim before running this script.
disp(' '); disp('Start here'); disp(' ')
disp('Trim conditions')
XR2
UR2
[a,b,c,d]=linmod('PhugoidSubProp',XR2,UR2) %Find linear model of nonlinear pendulum
%Units x(1)=y(1)=v [ft/sec]
%      x(2)=y(2)=gamma [rad]
%           y(3)=hdot [ft/sec]
disp('Natural frequencies, damping ratios, and poles of system matrix a')
[Wn,Z]=damp(a) % Find properties of the poles of linearized system
poles=eig(a)
PSYS=ss(a,b,c,d); %Create a linear state space system
set(PSYS,'statename',{'v(f/s)' 'gam(r)'})
set(PSYS,'inputname',{'alpha(r)' 'Bhp(hp)'})
set(PSYS,'outputname',{'v(f/s)' 'gam(r)' 'hdot(f/s)'})

VperAlpha=zpk(PSYS(1,1))
% GamperAlpha=zpk(PSYS(2,1))
% HdotperAlpha=zpk(PSYS(3,1))
% VperBhp=zpk(PSYS(1,2))
% GamperBhp=zpk(PSYS(2,2))
% HdotperBhp=zpk(PSYS(3,2))

% Use sisotool to design a compensator for this system.

%SYS = FEEDBACK(SYS1,SYS2)
CL1=feedback(VperAlpha,-.01)
eig(CL1)
%[Wn,Z] = DAMP(SYS)
[Wn,Z] = damp(CL1)
%[Gm,Pm,Wcg,Wcp] = MARGIN(SYS)
[Gm,Pm,Wcg,Wcp] = margin(-.01*VperAlpha)
%[EssStep,Kp,EssRamp,Kv,EssParabola,Ka,type]=ErrorConst(SISOsys)
[EssStep,Kp,EssRamp,Kv,EssParabola,Ka,type]=ErrorConst(-.01*VperAlpha)