close all; clear all; clc;
% G(s) = 8/(s-5)(s-10)   --> obviously unstable open loop system as it has
% poles at 5 and 10 > 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 = [1 -15 50];

[A B C D] = tf2ss(num,den)
sys_openLoop = ss(A,B,C,D);


%From specifications given
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)];

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

t = 0:0.01:10;
x_initial = [-0.5;0.02];
[y_closedLoop,t,x_closedLoop]=lsim(sys_closedLoop,ones(size(t)),t,x_initial);
figure
plot(t,y_closedLoop,'LineWidth',2)
title('output with full state feedback and nonzero initial state vector')



% 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);


% plot the output with nonzero initial state vector
t = 0:0.01:10;
[y_closedLoop_wthPrescaling,t,x_closedLoop_wthPrescaling]=lsim(sys_closedLoop_wthPrescaling,ones(size(t)),t,x_initial);
figure
plot(t,y_closedLoop_wthPrescaling,'LineWidth',2)
title('output with full state feedback + prescaling and nonzero initial state vector')
lsiminfo(y_closedLoop_wthPrescaling,t)



%--------------------------------------------------
% let's now use the estimated state vector for feedback instead of using
% the actual state vector (combine estimator with regulator assume r=0
% first)

% test
%------



desiredEstimatorPoles = [10*real(poles_ClosedLoop(1)) 9*real(poles_ClosedLoop(2))]  ;   % this is arbitrary but you need this to decay so fast

L = place(A.',C.',desiredEstimatorPoles).'

% construct augmented SS equation to simulate the combined
% regulator+estimator

Aaug = [A-B*K, B*K; zeros(size(A)) A-L*C];
Baug = [B;zeros(size(B))];
Caug = [C zeros(size(C))];
Daug = 0;

sys_regulatorPlusEstimator = ss(Aaug,Baug,Caug,Daug);


t = 0:0.01:10;
e_initial = x_initial;   % assume zero initial estimated state vector
[y_regulatorPlusEstimator,t,x_regulatorPlusEstimator]=lsim(sys_regulatorPlusEstimator,ones(size(t)),t,[x_initial;e_initial]);
figure
plot(t,y_regulatorPlusEstimator,'LineWidth',2)
title('output with state feedback + state estimator and nonzero initial state vector')
lsiminfo(y_regulatorPlusEstimator,t)
figure
plot(t,x_regulatorPlusEstimator(:,3:4),'LineWidth',2)
title('estimation error of both state variables in case of regulator + estimator')

figure
plot(t,x_regulatorPlusEstimator(:,1:2),'LineWidth',2)
hold on
plot(t,x_regulatorPlusEstimator(:,1:2)-x_regulatorPlusEstimator(:,3:4),'--','LineWidth',2)
title('true state variables and estimated state variables versus time')
legend({'$x_1$','$x_2$','$\hat{x_1}$','$\hat{x_2}$'},'Interpreter','Latex')
xlabel('time (sec)')
ylabel('state variable')



%{
e = zeros(2,length(t));
for ii = 1:length(t)
e(:,ii) = expm((A-L*C)*t(ii)) * e_initial;
end
figure
plot(t,e.')
%}


% calculate Nbar (to be added to state feedback+estimator) and plot the new step response
N1 = 1 / (Caug*inv(-Aaug)*Baug);
sys_feedbackPlusEstimatorWthRefInputPrescaled = ss(Aaug,Baug*N1,Caug,Daug);



[y_feedbackPlusEstimatorWthRefInputPrescaled,t,x_feedbackPlusEstimatorWthRefInputPrescaled]=lsim(sys_feedbackPlusEstimatorWthRefInputPrescaled,ones(size(t)),t,[x_initial;e_initial]);
figure
plot(t,y_feedbackPlusEstimatorWthRefInputPrescaled,'LineWidth',2)
title('output with state feedback + state estimator +prescaling and nonzero initial state vector')
lsiminfo(y_feedbackPlusEstimatorWthRefInputPrescaled,t)
