%% Commande Optimale
% Mise en place du Controle de la température d'une plaque chauffant sur 2 capteurs

% Récupération des paramètre du modèle
clc;close all ;clear all ;
load('ABCtild.mat'); % contient 'Atild', 'Btild', 'Ctild','deltaT','n','nu'

%% -- Etape 1 --  Mise en place du controle - etude stabilité   (Simuler Xk+1 avec Uk = -K Xk)
% Stable si X -> 0

R = eye(nu,nu);
Q = eye(n,n)*10;
K = dlqr(Atild,Btild,Q,R);

X  = randn(n,1);
State = [X];

for k = 1:50
    u = -K*X;
    X  = Atild*X + Btild*u;
    % stockage:
    State = [State,X];
    U(:,k)=u;
end


% figure(1),
% for i=1:nu
%     plot(U(i,:)),hold on;
% end
% legend('u1','u2'), ylabel('U'),xlabel('nombre d''itération');

figure,
for i=1:n
    plot(State(i,:)),hold on;
end
legend('x1','x2','x3','x4'), ylabel('X'),xlabel('nombre d''itérations'), title('Méthode 1 : évolution de X');
% Observation : Notre controleur est stable car X -> 0



%% -- Etape 2 -- Controle Avec Régulation et asservissement     (Simuler Xk+1 avec Uk = -Ky y* + KI yI - K Xk)
Ae = [Atild,zeros(n,nu);
    eye(nu,nu)*deltaT,zeros(n-nu,n-nu),eye(nu,nu)];
Be = [Btild;
    zeros(nu,nu)];
Qi = eye(nu,nu)*1e-3;
Qe = [Q,zeros(n,nu);
    zeros(nu,n),Qi];
Re = R;
y_target = [2;-5]; % température ciblées (autour du point d'équilibre) = y*

Ke = dlqr(Ae,Be,Qe,Re);
Ky = Ke(:,1:nu);
K = Ke(:,1:n);
Ki = Ke(:,n+(1:nu));
%K = [Ky,Kz];

X  = randn(n,1);
y =  Ctild * X;
State = [X];
sortieY = [y];
yi = zeros(nu,1);

for k = 1:250
    yi = yi + deltaT*(y-y_target);
    u = -K*X + Ky*y_target - Ki*yi;
    X  = Atild*X + Btild*u;
    y =  Ctild * X;
    % stockage:
    State = [State,X];
    U(:,k)=u;
    sortieY = [sortieY, y];
end


% figure,plot(U(2,:)),hold on,plot(U(2,:));
% legend('u1','u2'), ylabel('U'),xlabel('nombre d''itération');
% 
% figure,
% for i=1:n
%     plot(State(i,:)),hold on;
% end
% legend('x1','x2','x3','x4'), ylabel('X'),xlabel('nombre d''itération');

figure,plot(sortieY(1,:)),hold on,plot(sortieY(2,:));
hold on, plot(1:k,ones(k,1)*y_target(1),'--b'), hold on, plot(1:k,ones(k,1)*y_target(2),'--r');
legend('y1','y2','y1 ciblé','y2 ciblé'), ylabel('sortie Y'),xlabel('nombre d''itérations'), title('Méthode 2 : évolution des sorties Y (t°c)');
% Observation : Le controleur fonctionne bien car les températures désirées
% sont atteintes après un certain nb d'itération.


%% -- Etape 3 -- Controle avec l'observateur      (Simuler Xk+1chapeau avec Uk = -Ky y* + KI yI - K Xkchapeau)
R0 = eye(size(R));
Q0 = eye(size(Q))*1000;
Ko = (dlqr(Atild',Ctild',Q0,R0))';

X  = randn(n,1);
X_est  = randn(n,1);
y =  Ctild * X;
State = [X];

State_est = [X_est];

sortieY = [y];
yi = zeros(nu,1);


for k = 1:250 
    yi = yi + deltaT*(y-y_target);
    u = -K*X_est + Ky*y_target - Ki*yi;
    
    X  = Atild*X + Btild*u;    
    y =  Ctild * X;
    
    X_est  = Atild*X_est + Btild*u + Ko*(y - Ctild * X_est);
        
    % stockage:
    State = [State,X];
    State_est = [State_est ,X_est]; 
    U(:,k)=u;
    sortieY = [sortieY, y];
end

figure,plot(sortieY(1,:)),hold on,plot(sortieY(2,:));
hold on, plot(1:k,ones(k,1)*y_target(1),'--b'), hold on, plot(1:k,ones(k,1)*y_target(2),'--r');
legend('y1','y2','y1 ciblé','y2 ciblé'), ylabel('sortie Y'),xlabel('nombre d''itérations'), title('Méthode 3 : évolution des sorties Y (t°c)');


figure,
hold on
for i = 1:n
    plot(State_est(i,:)-State(i,:)),
end
ylabel('Erreur (Xest - X)'),xlabel('nombre d''itérations'),legend('x1','x2','x3','x4');
title('Méthode 3 : évolution Erreur (Xest - X)');

