% movimento di stato nominale e perturbato
% esempio con sistema LTI a tempo discreto, instabile

close all
clear
clc

% matrici delle equazioni di stato del sistema
A = [1 -2; 0 1]; B = [1 0; 0 1];
C = [1 0;0 1]; D = zeros(2);
% NB autovalore doppio in +1, con molteplicità geometrica 1 --> sistema
% instabile

% definizione di sistema LTI in Matlab
LTIsys1 = ss(A,B,C,D,-1);

% stato iniziale nominale
x0NOM = [1;-2];

% sequenza di ingresso nominale
Useq = [ 1   0   2  -1  0  1  0   2 -1  0   1   0   2  -1  0  1  0   2 -1  0; ...
        -1   2  -1   0  0 -1  2  -1  0  1   -1   2  -1   0  0 -1  2  -1  0  1];

% orizzonte temporale -> 20 istanti di tempo
timeSTEPS = (0:19);

% movimento di stato ed uscita nominali
[Ynom, ~, Xnom] = lsim(LTIsys1, Useq, timeSTEPS, x0NOM);

% stato iniziale perturbato
x0PERT = x0NOM + rand(size(x0NOM));

% movimento perturbato, a aprtire dallo stesso stato iniziale e con la
% stessa sequenza di ingresso
[Ypert, ~ , Xpert] = lsim(LTIsys1, Useq, timeSTEPS, x0PERT);

% grafici
%
% traiettoria di stato, nominale e perturbata
hf = figure('Name', 'traiettoria dello stato');
plot(Xnom(:,1), Xnom(:,2), '-o', 'linewidth', 1.5); hold on
plot(Xpert(:,1), Xpert(:,2), '-x', 'linewidth', 1.5);
legend('x_1','x_2');

% movimento dello stato, nominale e perturbato
hf2 = figure('Name','movimento dello stato');
subplot(2,1,1); 
plot(timeSTEPS, Xnom(:,1),'-o', 'linewidth', 1.5);
hold on; grid on;
plot(timeSTEPS, Xpert(:,1),'-x', 'linewidth', 1.5);
xlabel('tempo'); ylabel('x_1')

subplot(2,1,2); 
plot(timeSTEPS, Xnom(:,2),'-o', 'linewidth', 1.5);
hold on; grid on;
plot(timeSTEPS, Xpert(:,2),'-x', 'linewidth', 1.5);
xlabel('tempo'); ylabel('x_2')

% ----------
    pause
% ----------

% allungo il movimento di stato, arrivando fino all'istante Tend
Tend = 2e2;
tSTEPS2 = (0:Tend-1); % nuovo vettore con gli istanti di tempo

% NB dall'istante 20 in poi NON applico più ingresso = applico ingresso
% nullo dall'istante 20 fino all'istante Tend
U2 = [Useq, zeros(2,Tend-20)];

% simulo il movimento di stato ed uscita, partendo sempre dallo stesso
% stato iniziale ed utilizzando la sequenza di ingressi iniziale fino
% all'istante 19, poi tutta nulla
[Y2pert, ~ , X2pert] = lsim(LTIsys1, U2, tSTEPS2, x0PERT);

% aggiorno i grafici inserendo la nuopva traiettoria
figure(hf);
plot(X2pert(:,1), X2pert(:,2), '-x', 'linewidth', 1.5);

% aggiorno i grafici, inserendo il nuovo movimento di stato
figure(hf2);
subplot(2,1,1);
plot(tSTEPS2, X2pert(:,1),'-o', 'linewidth', 1.5);
hold on; grid on;
xlabel('tempo'); ylabel('x_1')

subplot(2,1,2); 
plot(tSTEPS2, X2pert(:,2),'-o', 'linewidth', 1.5);
hold on; grid on;
xlabel('tempo'); ylabel('x_2')

% ============= FINE ==============