close all; clear all; clc
% say we have a third order system
% G(s) = 8/(s-5)(s-10)(s-7)   --> obviously unstable open loop system as it has
% poles at 5 and 10 and 7 > 0
% we would like to design a SS feedback controller (find K) such that the
% resulting closed loop system is stable and satisfies the follwoing two
% transient specifications
% maximum overshoot < 5%
% 2% settling time <= 2 second
%--------------------------------------------------------------------------
% solution
% 1) realize the system (find A, B, C, D)
num = 8;
den = conv(conv([1 -5],[1 -10]),[1 -7]);
[A B C D] = tf2ss(num,den)
sys_openLoop = ss(A,B,C,D);
figure;step(sys_openLoop,1); title('step response of open loop system')
eig(A)

%From specifications (we will still assume the system is a second order
%system and find two poles from the specifications and place the remaining
%third pole at 5-10 times further from the jw axis to make sure it does not
%impact the transient behavior of the overall system)

overshoot = 0.05; settlingTime = 4;

%maximum overshoot for a second order system
% max overshoot = exp(-pi*zeta/sqrt(1-z^2))
zeta = sqrt((log(overshoot))^2 / (pi^2+(log(overshoot))^2))
% 2% settling time = -ln(0.02*sqrt(1-zeta^2)/zeta*wn or approximately= 4/(zeta*wn)
Omega_n = -log(0.02*sqrt(1-zeta^2))/zeta /settlingTime;  

% closed loop poles of a second order prototype system are
% (-zeta*wn + i*wn*sqrt(1-zeta^2)) and -zeta*wn - i*wn*sqrt(1-zeta^2)

poles_ClosedLoop = [-zeta*Omega_n + 1i*Omega_n*sqrt(1-zeta^2) -zeta*Omega_n - 1i*Omega_n*sqrt(1-zeta^2)]

% now calculate the third pole and force it to be far from jw axis
remainingPole = 10 * real(poles_ClosedLoop(1));
poles_ClosedLoop = [poles_ClosedLoop remainingPole]

% find K that achieves the desired specifications
K = place(A,B,poles_ClosedLoop)

% let's now make sure the closed loop system has the desired specifications
A_closedLoop = A-B*K;
eig(A_closedLoop)
sys_closedLoop = ss(A-B*K,B,C,D);

% the closed loop TF is Y(f)/R(f)
figure;step(sys_closedLoop,10); title('step response of closed loop system without pre-scaling')
text(2,0.2,['K= [ ',num2str(round(K*100)/100),' ]'],'FontSize',14)
stepinfo(sys_closedLoop)



% calculate Nbar and plot the new step response
Nbar = 1 / (C*inv(-A+B*K)*B);
sys_closedLoop_wthPrescaling = ss(A-B*K,Nbar*B,C,D);
figure;step(sys_closedLoop_wthPrescaling,10); title('step response of closed loop system with pre-scaling')
text(1,0.3,['K= [ ',num2str(round(K*100)/100),' ],     Nbar=' num2str(round(Nbar*100)/100)],'FontSize',14)
stepinfo(sys_closedLoop_wthPrescaling)


% plot the response to any input with finite steady state value (you will
% find that the output still convrges to the steady state value and tracks
% the reference input)
t = 0:0.1:10;
figure; lsim(sys_closedLoop_wthPrescaling,2-3*exp(-t),t);
