%% 
% BE de commande optimale
clc

%%
%Question 3 
Ms = 1120/4;     %kg
mu = 45;      %kg
Ks = 20000;   %N/m
Kt = 150000; %N/m
Cs = 1000;    %N.s/m
Ct = 0.0;     %N.s/m

% Représentation d'&état du système

A = [0 1 0 -1 ; -Ks/Ms -Cs/Ms 0 Cs/Ms; 0 0 0 1 ; Ks/mu Cs/mu -Kt/mu -(Cs+Ct)/mu];
B = [0 ; 1/Ms ; 0 ; -1/mu];
E = [0 ; 0 ; -1 ; Ct/mu];
C = [0 1 0 0 ; 0 0 0 1];
C_st=[0 1 0 0]
C_ut=[0 0 0 1]

% Création du système Caisse
sysZs = ss(A,E,C_st,0)
%Modification en fonction transfertx
tf_Zs = tf(sysZs)

% Création système Roue
sysZu = ss(A,E,C_ut,0)
%Modification en tf
tf_Zu = tf(sysZu)

%tracé de bode de nos deux sous systèmes
bode(tf_Zs,tf_Zu)

%Tracé des steps de chacun des sous systèmesh
figure
step(tf_Zs)
title("Réponse indicielle de la caisse")

figure 
step(tf_Zu)
title("Réponse indicielle de la roue")

%%
% Question 5
% Mise en place de la commande optimale

Q=[Ks^2 0 0 0; 0 Cs^2 0 0; 0 0 Kt^2 0; 0 0 0 0];
R=1;

[G, K, lambda]=lqr(A,B,Q,R);
Abf=A-B*G;

sysZsBF = ss(Abf, E, C_st, 0);
Zs_BF = tf(sysZsBF)
sysZuBF = ss(Abf, E, C_ut, 0);
Zu_BF = tf(sysZuBF)

figure
bode(Zs_BF, Zu_BF);

%Réponse indicielle
figure
step(0.08*sysZsBF)
title("Réponse indicielle pous ZsBF")
figure
step(0.08*sysZuBF)
title("Réponse indicielle pour ZuBF")

% Variation de Q
figure
for i = [1:10]
  [G, K, lambda]=lqr(A,B,10*i*Q,R);
  Abf=A-B*G;
  
  sysZsBF = ss(Abf, E, C_st, 0);
  Zs_BF = tf(sysZsBF);

  
  step(0.08*sysZsBF);
  
  hold on;
end 
title("Réponse indicielle Caisse en fonction de Q")

figure
for i= [1:10]
  [G, K, lambda]=lqr(A,B,10*i*Q,R);
  Abf=A-B*G;
  sysZuBF = ss(Abf, E, C_ut, 0);
  Zu_BF = tf(sysZuBF);
  
  step(0.08*sysZuBF);
  
  hold on;
end
title("Réponse indicielle Caisse en fonction de Q")

%commande obtenue par u=-Gx, C=-G
C=-G;
commande=ss(Abf, E, C, 0)