%% Forme canonique d'observabilite
clear all; close all; clc;
load('parametres_du_modele.mat');

%% Ce système est-il controlable ? 
[n,nu] = size(B);
Pc = [B,A*B,A.^2*B,A.^3*B]; %matrice de controlabilite Pc
rang_Pc = rank(Pc); %=n donc le systeme est controlable

%% Ce système est-il observable ?
Po = [C;C*A;C*A.^2;C*A.^3]; %matrice d'observabilite Oc
rang_Po = rank(Po); %=n donc le systeme est observable

%% Mise sous forme normale d'observabilite du systeme en faisant un changement de variable
% Recheche de la matrice de Passage P tel que y = Ctild*Xtild avec Xtild =P*X
% avec P = (C (2 lignes) , 2 autres ligne de Po) afin d'obtenir rank(P) = n

% Recherche de 2 lignes de Po pour former la matrice de passage P de telle sorte que P est de rang plein

for i=3:size(Po,1) % on parcours les lignes de la matrice d'observabilite (apartir ligne 3 car les 2 avant sont C)
    P_temp = [C; Po(i,:)]; 
    if  rank(P_temp) == 3
        ligne = i;
        break
    end
end
for j=ligne+1:size(Po,1)
    P_temp2 = [P_temp; Po(j,:)];
    if  rank(P_temp2) == 4
        break
    end
end
P = P_temp2;

% Construction de Atild, Btild, Ctild
Atild = P*A*inv(P);
Btild = P*B;
Ctild = [eye(2),zeros(2,size(A,1)-2)];

save('ABCtild.mat','Atild', 'Btild', 'Ctild','deltaT','n','nu');
