% Esercitazioni di Controllo Digitale A.A. 2003/2004
% Titolare del corso Prof. A. Bemporad
% Esercitazioni tenute da Ing. Nicolo' Giorgetti
%
% Problema: 2 masse accoppiate da una molla e uno smorzatore, una forza spinge la 
% prima e si misura la posizione della seconda massa.
%
% Concetti introdotti: + Utilizzo del control system toolbox
%                      + Raggiungibilita'
%                      + Calcolo della sequenza d'ingressi che porta da
%                        uno stato iniziale ad uno stato finale
%                      + Controllo a minima energia su orizzonte di 10
%                        passi

% Parametri del modello
k=1;
beta=2;
m1=10;
m2=10;

% Rappresentazione spazio di stato
A=[0 1 0 0;...
      -k/m1 -beta/m1 k/m1 beta/m1;...
      0 0 0 1;
      k/m1 beta/m2 -k/m2 -beta/m2];
b=[0 1/m1 0 0]';
c=[0 0 1 0];
d=[0];
sys_2m=ss(A,b,c,d)
pause;

% Versione tempo-discreta del sistema
Ts=5;
sys_td_2m=c2d(sys_2m, Ts)
pause;

% Calcolo della matrice di raggiungibilita'
R=ctrb(sys_td_2m)
n=rank(R);
fprintf('Rango di R= %d\n',n);
pause;

% Calcolo di una sequenza d'ingressi
x1=[0 0 0.5 0]' % Condizione iniziale
x2=[10 0 11 0]' % Condizione finale

pause;

X=x2-(sys_td_2m.a^n)*x1;
disp('Sequenza d''ingressi'); 
U=inv(R)*X
pause;

% Simulazione del sistema nota la sequenza d'ingressi
U=flipud(U);


state=x1;
XX=[state];
for i=1:n
   state=sys_td_2m.a*state+sys_td_2m.b*U(i);
   XX=[XX state];
end

XX
pause;

U(5)=U(4);
T=0:Ts:n*Ts; T=T';
[Y,T,XXc]=lsim(sys_2m,U,T,x1,'zoh');

XXc=XXc'
pause;

% Calcolo della sequenza d'ingressi a minima energia per un'orizzonte pari
% a 10 passi
hz=10;
RR=[R];
for i=n:hz-1
   RR=[RR sys_td_2m.a^i*sys_td_2m.b];
end
X=x2-(sys_td_2m.a^hz)*x1;

disp('Sequenza d''ingressi a minima energia per orizzonte 10');
Ustar=RR'*inv(RR*RR')*X
pause;


Ustar=flipud(Ustar);

state=x1;
XX=[state];
for i=1:hz
   state=sys_td_2m.a*state+sys_td_2m.b*Ustar(i);
   XX=[XX state];
end
XX



