
% file ES_TD.m
%
% Calcolo dell'evoluzione libera e forzata dello stato e dell'uscita
% di un sistema dinamico a tempo discreto mediante la function ss2tf

clear all, close all
pack
format compact

A=[3, 0; -3.5 -0.5]

B=[1, 1; 2, -2]

C=[1, -1]

D=[0, 0]

x0=[1; -2]

n=size(A,1);  % Dimensione del vettore di stato       = # righe di A
p=size(B,2);  % Dimensione del vettore degli ingressi = # colonne di B
q=size(C,1);  % Dimensione del vettore delle uscite   = # righe di C

% Trasformate Zeta degli ingressi Ui(z)/z
% (la divisione per z permette di ricavare i fratti semplici in z
%  nella forma piu' opportuna mediante la function residue)
num_u1=2;
den_u1=[1, -1];
num_u2=3;
den_u2=[1, 0];

% Calcolo della risposta nel tempo

% 1) Evoluzione libera 

% 1.a) Stato
[num_Xl,den_Xl]=ss2tf(A,x0,eye(n),zeros(n,1),1)
% 1.b) Uscita
[num_Yl,den_Yl]=ss2tf(A,x0,C,zeros(q,1),1)

% 2) Evoluzione forzata

% 2.a) Stato
[num_Xf1,den_Xf1]=ss2tf(A,B,eye(n),zeros(n,p),1)
[num_Xf2,den_Xf2]=ss2tf(A,B,eye(n),zeros(n,p),2)
for kk=1:n,
    num_Xf1tot(kk,:)=conv(num_u1,num_Xf1(kk,:));
    num_Xf2tot(kk,:)=conv(num_u2,num_Xf2(kk,:));
end
num_Xf1tot
num_Xf2tot
den_Xf1tot=conv(den_u1,den_Xf1)
den_Xf2tot=conv(den_u2,den_Xf2)

% 2.b) Uscita
[num_Yf1,den_Yf1]=ss2tf(A,B,C,D,1);
[num_Yf2,den_Yf2]=ss2tf(A,B,C,D,2);
for kk=1:q,
    num_Yf1tot(kk,:)=conv(num_u1,num_Yf1(kk,:));
    num_Yf2tot(kk,:)=conv(num_u2,num_Yf2(kk,:));
end
num_Yf1tot 
num_Yf2tot 
den_Yf1tot=conv(den_u1,den_Yf1)
den_Yf2tot=conv(den_u2,den_Yf2)

% Calcolo dei residui

for kk=1:n,
    kk
    [r_Xl,    p_Xl,    k_Xl]    =residue(num_Xl(kk,:),     den_Xl)
    [r_Xf1tot,p_Xf1tot,k_Xf1tot]=residue(num_Xf1tot(kk,:), den_Xf1tot)
    [r_Xf2tot,p_Xf2tot,k_Xf2tot]=residue(num_Xf2tot(kk,:), den_Xf2tot)
end

[r_Yl,    p_Yl,    k_Yl]    =residue(num_Yl,     den_Yl)
[r_Yf1tot,p_Yf1tot,k_Yf1tot]=residue(num_Yf1tot, den_Yf1tot)
[r_Yf2tot,p_Yf2tot,k_Yf2tot]=residue(num_Yf2tot, den_Yf2tot)

% Tracciamento della risposta del sistema mediante simulazione
t=0:1:5;
u1=num_u1*ones(size(t));          % ingresso a gradino
u2=zeros(size(t)); u2(1)=num_u2;  % ingresso a impulso
[Yl,    Xl]    =dlsim(A,B,C,D,0*[u1;u2]',  x0);
[Yf1tot,Xf1tot]=dlsim(A,B,C,D,[u1;0*u2]',0*x0);
[Yf2tot,Xf2tot]=dlsim(A,B,C,D,[0*u1;u2]',0*x0);
[Ytot,  Xtot]  =dlsim(A,B,C,D,  [u1;u2]',  x0);

figure(1), plot(t,Xl(:,1),'r', t,Xf1tot(:,1),'g', t,Xf2tot(:,1),'b', t,Xtot(:,1),'y'), title('X1')
figure(2), plot(t,Xl(:,2),'r', t,Xf1tot(:,2),'g', t,Xf2tot(:,2),'b', t,Xtot(:,2),'y'), title('X2')
figure(3), plot(t,Yl(:,1),'r', t,Yf1tot(:,1),'g', t,Yf2tot(:,1),'b', t,Ytot(:,1),'y'), title('Y')