% Design the altitude hold loop (outermost loop on altitude)
% Linearize the simulink model with the alpha and hdot loops closed
disp(' '); disp('Start here <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<'); disp(' ')

disp('Determine the state space model with the first and second loops closed')
disp(' This step is important!!!!!')
[a2,b2,c2,d2]=linmod('controlBack_2p') %Find linear model
%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(a2) % Find properties of the poles of linearized system
poles=eig(a2)
PSYS2=ss(a2,b2,c2,d2); %Create a linear state space system
%set(PSYS2,'statename',{'v(f/s)' 'gam(r)','compa1','compa2','compb1'})
set(PSYS2,'inputname',{'vCf/s)' 'hdotC(f/s)'})
set(PSYS2,'outputname',{'v(f/s)' 'gam(r)' 'hdot(f/s)','h(f)'})

VperVC=zpk(PSYS2(1,1))
disp('Setup for third loop of Backside control strategy')
hperhdotC=zpk(PSYS2(4,2))
[StaticLoopSensitivity,DCgain,num,den]=getTFdata(hperhdotC,1,1)
[EssStep,Kp,EssRamp,Kv,EssParabola,Ka,type]=ErrorConst(hperhdotC)

% Use sisotool to design a compensator for this system.
disp('Save to the workspace AND a file the compensator as a system called Ch')
sisotool
disp('Save to the workspace AND a file the compensator as a system called Ch')