% reponse en boucle ouverte
num=1;den=[1 10 20];
figure(1); step(num,den); % gain statique =1/20 etc ...
    
% ETUDE PID
% appliquer chaque fois la methode de Routh pour connaitre les 
% contraintes sur les gains du PID qui assurent la stabilite

    
% controle proportionnel
% appliquer routh pour connaitre les contraintes de stabilite
G=tf(num,den); KP=300;
T=G*KP/(1+G*KP);
figure(2);step(T);

% reduction de depassement par compensation derivee
% appliquer routh pour connaitre les contraintes de stabilite
KD=10; KP=300;
K=tf([KD KP],1);
T=G*K/(1+G*K);
figure(3);step(T);

figure(4);
for KD=0:1:10;
K=tf([KD KP],1);
T=G*K/(1+G*K);
step(T); hold on;
end


% elimination erreur statique par compensation integrale
% appliquer routh pour connaitre les contraintes de stabilite
KP=30; KI=70;
KPI=tf([KP KI],[1 0]);
T=G*KPI/(1+G*KPI);
figure(5);step(T); hold on;


% compensation PID
% appliquer routh pour connaitre les contraintes de stabilite
KP=350; KD=50; KI=300;
KPID=tf([KD KP KI],[1 0]);
T=G*KPID/(1+G*KPID);
figure(6);step(T); hold on;


% EFFETS DES BRUITS
% bode de K ou GK
% GK doit etre petit pour avoir attenuation de bruit dans
% une bande de frequences
figure(7);bode(G*KPI); %sensibilite au bruit
figure(8);bode(G*KPID); %sensibilite au bruit


% sous simulink
% PI.mdl
% PID.mdl
% faire les simulations avec random de variance = 0.01 
% en pratique, on peut etre amener a modifier le terme derive
% KD s ---> KD s/(1+tau s) avec tau petit > 0


% commande modale avec retour integral
% on calcule un modele d'etat puis 
% on commence par augmenter le systeme avec l'integrale
% de la position pour avoir une action integrale

% systeme augmente
k=20; b=10;M=1;
A=[0 1 0;-k/M -b/M 0;1 0 0]; B=[0;1/M;0];
C=eye(3);D=zeros(3,1);

% ensuite, on applique la methode modale, choix des modes
% resolution des problemes de noyau, etc
% poles  -0.1  -del*wn +-j wn *(1-del^2)^0.5
tr=2;
del=0.7;wn=3/(tr*del);
lam1=-3/tr; lam2=-del*wn + j*wn*sqrt(1-del^2); lam3=conj(lam2);

NM1=null([A-lam1*eye(3) B]);
NM2=null([A-lam2*eye(3) B]);
NM3=null([A-lam3*eye(3) B]);

v1=NM1(1:3,1); w1=NM1(4,1);
v2=NM2(1:3,1); w2=NM2(4,1);
v3=NM3(1:3,1); w3=NM3(4,1);

K=[w1 w2 w3]/[v1 v2 v3];
K=real(K); % suppression des residus imaginaires

eig(A+B*K), k1=K(1,1); k2=K(1,2); k3=K(1,3);
% pour la simulation on forme asim, bsim,csim,dsim
% simulateur MODAL.mdl
asim=[0 1 ;-k/M -b/M]; bsim=[0;1/M]; csim=eye(2); dsim=zeros(2,1);

% marges de stabilite
sys=ss([0 1;-k/M -b/M],[0;1/M],[1 0],0);
systf=tf(sys);
Ktf = -tf([k2 k2 k3],[1 0]),  % changement de signe car retroaction
BO=tf(sys)*Ktf;
BF= feedback(systf*Ktf,1);
margin(BO), % check margins of LQ
nichols(BO), % pour voir les marges







% commande LQ avec retour integral
% permet de calculer un PID par LQ
% idem a la methode modale
% attention au signe du gain pour le LQ
k=20; b=10;M=1;
sys=ss([0 1;-k/M -b/M],[0;1/M],eye(2),zeros(2,1));
A=[0 1 0;-k/M -b/M 0;1 0 0]; B=[0;1/M;0]; % systeme augmente
C=eye(3);

Q=diag([1 0.1 1]); R=0.0001 ;
[K,S,E] = lqr(A,B,Q,R);
eig(A-B*K), % verifie stabilite en boucle fermee
k1=K(1,1); k2=K(1,2); k3=K(1,3);
% simulateur LQsim.mdl


Ktf = tf([k2 k2 k3],[1 0]),

sys=ss([0 1;-k/M -b/M],[0;1/M],[1 0],0);
systf=tf(sys);
BO=tf(sys)*Ktf;
BF= feedback(systf*Ktf,1);
pole(BF),
step(BF),
margin(BO), % check margins of LQ
nichols(BO), % pour voir les marges

% plus facile a regler a partir de l'etat integral
Q=diag([1 0.1 100]); R=1 ;