clc;
clear;
close all;
%%
s = tf('s') ;
sys = (s+1)/(s-1)/(s+2)^2 ;
sys = ss(sys) ;
A = sys.a ;
B = sys.b ;
Poles = [-5 -6 -7] ;
K = place(A , B , Poles); %% u = -6x1-26x2-14x3
sys_cl = ss(A-B*K , B , sys.C , sys.D) ;
X0 =  ones(3,1) ;
t = 0:0.01:10 ;
[y , t , x] = initial(sys_cl , X0 , t) ;
u = zeros(size(t)) ;
for i = 1:numel(t)
    u(i) = -K * x(i , :)';
end
figure(1);
subplot(211) ;
plot(t , x(: , 1)  , '-o' , 'LineWidth' , 2) ; hold on
grid on
title('States')
subplot(212) ;
plot(t , u  , '-o' , 'LineWidth' , 2) ; hold on
grid on
title('Input Signal') ; 
%%  lqr design
Q = diag([0.1 1 1]) ; 
R = 10 ;
N = 0  ; 
K = lqr(A , B , Q , R , N) ; 
sys_cl = ss(A-B*K , B , sys.C , sys.D) ;
t = 0:0.01:10 ;
[y , t , x] = initial(sys_cl , X0 , t) ;
u = zeros(size(t)) ;
for i = 1:numel(t)
    u(i) = -K * x(i , :)';
end

figure(1);
subplot(211) ;
plot(t , x(: , 1)  , '-o' , 'LineWidth' , 2) ;
grid on
title('States')
subplot(212) ;
plot(t , u  , '-o' , 'LineWidth' , 2) ;
grid on
title('Input Signal') ; 
