clear all
close all
clc

%% Recupero dati, selezione tipologia e adattamento

load dati3input
h=size(dati);

            % %uso dati temp. operativa Top= 0.5 x (Ta + Tr) dove Ta è la temperatura
            % %dell'aria e Tr è quella radiante. é una media. Il ciclo trasforma le
            % %matrici 3D in 2D adatte per l'algoritmo di identificazione
            %         % for i=1:h(3)
            %         %     y(i,:)=dati(:,3,i); 
            %         % end
            % y1 = squeeze(dati(:,3,:))';
            % 
            % %ingressi sono la Temp. esterna ripetuta e con rumore
            %         % for k=1:h(3)
            %         %     u(k,:)=dati(:,4,k);
            %         % end
            % %codice per ingressi multipli!
            % u1 = squeeze(dati(:,4,:))';
            % 
            % %ingresso singolo(infatti la temperatura esterna è unica)
            % %u1 = squeeze(dati(:,4,1))';
            
%tolgo alle uscite il valore dei dati luogo
l=[1 4:size(dati,3)];
y1 = squeeze(dati(:,3,:))';
y1=y1(l,:);

%seleziono gli ingressi: Testerna, Umidità, Solare Diretto.
u2 = squeeze(dati(:,4,1))'; %T esterna
u3 = squeeze(dati(:,1,2))'; %Umidità
u4 = squeeze(dati(:,1,3))'; %solare normale diretto kW/m^2
u5 = squeeze(dati(:,2,3))'; %solare orizzontale diffuso kW/m^2

%compongo l'ingresso totale con quelli desiderati (mettere ;)
u1= [u2;u3];
         
%tolgo la media ai dati con la funzione centra oppure è possibile usare la
%funzione detrend() con medesimi risultati.
[my, y]=centra(y1);
[mu, u]=centra(u1);

%% analisi spettrale dei segnali
%tale analisi è utile per determinare le componenti deterministiche del
%processo

%immetti nomi del segnale da analizzare con fft
sign=u2;

%interval è impostato come minuti per campione, ossia 10.
figure
[fftdata, freq, powerspect] = plotfft(sign, 10);

%% Imposta la parte di identificazione/validazione

frazione = 2/3; 
N = length(dati);

% identificazione
y_id = y(:,1:floor(N*frazione)); 
u_id = u(:,1:floor(N*frazione));

%aggiungo rumore agli ingressi con snr=10
%u_id = awgn(u_id1,10);
%essendo uguali posso usarne solo uno

%plottaggio I/O identificazione
figure
subplot(2,1,1)
plot(u_id');
grid on;
title('Ingressi per identificazione');
ylabel('°C (scalati)');
subplot(2,1,2)
plot(y_id');
grid on;
title('Uscite per identificazione');
ylabel('°C (scalati)');
print(['-f',num2str(gcf)], '-depsc2', 'identificazione')

% validazione
y_val = y(:,ceil(N*frazione):end);
u_val = u(:,ceil(N*frazione):end);

%% Identificazione

%scelta metodo: pbsid_opt/pbsid/n4sid/pem/IOpredictor
tipo='pbsid';

switch tipo
    case 'pbsid_opt'
        disp('metodo: pbsid_opt')
        %PBSIDopt ricorsivo (tolto il -1)
        %qui S1 sono i valori singolari.
        [A,B,C,D,K,S1] = PBSID_opt_ARX_weighted_recursive(y_id,u_id,[ 2 2 ]);
    case 'pbsid'
        disp('metodo: pbsid')
        [A,B,C,D,K,lambda] = PBSID_opt_ARX_weighted(y_id,u_id,[ 2 2 ]);
    case 'n4sid'
        disp('metodo: n4sid')
        d = iddata(y',u');
        %m = n4sid(d,size(d.y,2),'N4Weight','CVA'); é instabile!!
        m = n4sid(d,size(d.y,2),'focus','simulation');
        A = m.A; B = m.B; C = m.C; D = m.D; K = m.K; lambda = m.NoiseVariance; 
        m.EstimationInfo
    case 'pem'
        disp('metodo: pem')
        d = iddata(y_id',u_id');
        m=pem(d);
        A = m.A; B = m.B; C = m.C; D = m.D; K = m.K; lambda = m.NoiseVariance; 
        m.EstimationInfo
    case 'IOpredictor'
        disp('metodo: IO predittore')
        [A,B,C,D,K,lambda] = IO_predictor(y_id,u_id,[ 2 2 ]);
    case 'subspace'
        [A,B,C,D,K,lambda] = subspace_id(y_id,u_id);
        %[A,B,C,D,K,lambda]=subspace_id(y_id,u_id,[],[],[],'proj_ls',[],1,'cva','ls');
end

%costruzione modello in spazio di stato del tipo 
%x(t+1)=Ax(t)+Bu(t)
%y(t)=Cx(t)+ Du(t)
%che non corrisponde al modello stocastico di partenza con K!!
model=ss(A,B,C,0,1);
%raggiungibilità e osservabilità
if rank(ctrb(model))==size(A,1)
    display('SISTEMA RAGGIUNGIBILE')
else display('SISTEMA NON RAGGIUNGIBILE')
end

if rank(obsv(model))==size(A,1)
    display('SISTEMA OSSERVABILE')
else display('SISTEMA NON OSSERVABILE')
end

%verifico qualitativamente la stabilità con gli autovalori di A
autov=abs(eig(model.A));
figure
plot(autov);
grid on;
title('autovalori');
print(['-f',num2str(gcf)], '-depsc2', 'autovalori')

%% stima condizioni iniziali: 

%2 metodi:
%1)dato che D=0 e conosco i dati delle uscite di identificazione, prendo le ultime uscite.
    %x0 = pinv(C)*y_id(:,end); % se conosco le misure!

%2)Compute prediction errors associated with model and data set. (migliora)
    d1 = iddata(y_val',u_val');
    m1 = idss(A,B,C,D,K);
    [e,x0] = pe(m1,d1,'e');
    m1 = idss(A,B,C,D,K,x0);
   

%% 1° metodo validazione: valutazione visiva

%simulazione con il modello identificato con gli ingressi di validazione e
%poi confronteremo le uscite.
y_sim = lsim(model,u_val,1:length(u_val),x0);

%trasposta della matrice y_val per adattarne le dimensioni
y_val=y_val';    

%plottaggi
figure
subplot(2,1,1)
plot(y_val);
grid on;
title('Uscite di validazione (da simulatore)');
ylabel('°C (scalati)');
subplot(2,1,2)
plot(y_sim);
grid on;
title('Uscite del modello identificato con gli ingressi di validazione');
ylabel('°C (scalati)');
print(['-f',num2str(gcf)], '-depsc2', 'validazione')

%% 2° metodo validazione: errore quadratico medio 
% tra le uscite di validazione e quelle in uscita dal modello identificato con u=u_val

for i= 1:size(y_val,2)
    e2(:,i)=(sum((y_sim(:,i)-y_val(:,i)).^2))/size(y_val,1);
end

%plotto e2
figure
stem(e2);
grid on;
axis([0 size(y_val,2)+1 0 max(e2)+0.5])
title('Errori quadratici medi per ogni uscita');
xlabel('n° uscite');
ylabel('°C');
print(['-f',num2str(gcf)], '-depsc2', 'errore quadratico medio')

%% 3° metodo validazione: compare()

figure
    %compare() confronta l'n-esima uscita reale con la relativa prodotta dal
    %modello identificato con medesimi ingressi. é un altro modo per
    %validare. Si può usare anche il predittore a k-passi, ma non lo uso.
    compare(d1,m1);
    print(['-f',num2str(gcf)], '-depsc2', 'compare')

%% 4° metodo validazione: auto-correlazione e cross-correlazione dei residui
    
figure 
    %residui: Residual analysis consists of two tests: the whiteness test and the
    %independence test. Utilizza la funzione pe.m! già usata prima
    disp('CLICCA INVIO per analizzare la auto-corr. e la cross-corr. di tutte le uscite')
      %risultati diversi se si imposta il predittore a k passi in resid(m1,d1,k)
      %test da fare sui dati di identificazione?        
    resid(m1,d1);
 
%% considerazioni fpe e akaike

%fpe confronta vari modelli. Dovrei mettere più modelli d in fpe().
%N.B: fpe() e aic() funzionano con modelli che derivano direttamente
%dall'identificazione come "m." n4sid. Il tool di chiuso non fornisce
%questo.
    %fp = fpe(m);
%Akaike Information Criterion for estimated model: il modello con valore di
%aic più piccolo è il migliore.
    %Aic = aic(m);    
    
%% valori singolari: Hankel

%matrice Hankel (per oggetti ssdata)
%In control theory, eigenvalues define a system stability, whereas Hankel singular 
%values define the "energy" of each state in the system. Keeping larger energy states 
%of a system preserves most of its characteristics in terms of stability, frequency, 
%and time responses. Model reduction techniques presented here are all based on the Hankel 
%singular values of a system. They can achieve a reduced-order model that preserves 
%the majority of the system characteristics.
figure
hankelsv(model);

%plottaggio valori singolari di un sistema LTI (che non è il nostro caso)
figure
sigma(model);
grid on
title('valori singolari di un sistema LTI');

% %% Feedback
%     
% %Questa prodedura verifica la presenza di feedback dei dati (u,y)
% %una percentuale alta sopra al 90% equivale alla presenza di feedback!
% %(non sarebbe corretto fare la media semplice!)
% 
% [fbck,fbck0,nudir] = feedback(d1);
% for k=1:size(fbck,2)
%     if mean(fbck(k))>90
%         disp(['PRESENZA DI REAZIONE (in media) per ingresso: ' int2str(k)])
%         else disp(['ASSENZA DI REAZIONE (in media) per ingresso: ' int2str(k)])
%     end
% end
% 
% %% advice.m fornisce informazioni sul iddata dei dati di validazione.
% 
% advice(d1)
% 
% %% routine che mi dice il ritardo delle uscite nei confronti dell'ingresso
% for j=1:size(d1,2)
%     nk=delayest(squeeze(d1(:,j,:)));
%     disp(['Uscita: ' int2str(j)]);
%     disp(['affetta da un RITARDO (in campioni per ingresso) di: ' int2str(nk)])
% end

%% predizione k passi con bittanti

%seleziona intervallo di predizione:
k=2;

% In una prima fase, la predizione viene effettuata sui dati identificati, 
% gli stessi usati per l’identificazione:
d_id = iddata(y_id',u_id');
yhat1=predict(m1,d_id,k,'estimate');

%predittore con tutti i dati disponibili
dd=iddata(y',u');
yhat=predict(m1,dd,k,'estimate');
%ricavo i dati finali del predittore per confrontarli con quelli di valid.
y_pred=yhat(1152:end,:).y;

% In una seconda fase, la predizione viene effettuata sui restanti dati,
% così da avere un criterio oggettivo di valutazione della bontà predittiva 
% del modello:

%non è corretto, fornisce forti oscillazioni e la sintassi non è
%compatibile con quello che fà compare.
compare(yhat1,m1,k,y_val,'Init','e');     
   
%confronto visuale con dati validazione
% for i=1:9
% figure(i)
% plot(y_pred(:,i),'r')
% hold on
% grid on
% plot(y_val(:,i))
% hold on
% end

figure()
plot(y_pred(:,1),'r')
hold on
grid on
plot(y_val(:,1))
title(['Confronto prima uscita validaz. Vs predittore a ' int2str(k) ' passi'])
legend('dati validazione','dati predittore')

%andamento errore predittore a 2 passi per tutte uscite
figure()
err_pred1=y_val-y_pred;
plot(err_pred1)
grid on
title('Andamenti errori di predizione')

for i= 1:size(y_val,2)
    err_pred(i)=((sum((y_val(:,i)-y_pred(:,i)).^2))^0.5)/size(y_val,2);
end

%plotto e2
figure()
stem(err_pred);
grid on;
xlabel('n° uscite');
ylabel('°C');
title('err_pred');
axis([0 size(y_val,2)+1 0 max(err_pred)+0.5])
title('Errori quadratici medi per ogni uscita del predittore');

%confronto, tra i errori in catena aperta e il predittore
figure()
plot(e2)
hold on
plot(err_pred,'r')
title('Errori quadratici medi per ogni uscita tra predittore Vs dati validazione');
legend('e2','errore predittore')
grid on


%% predizione a 1 passo con il cambio sistema e sua innovazione
% come conseguenza di un passaggio da un modello di stato
% x(t+1)=Ax(t)+Bu(t)+Ke(t) a uno normale x(t+1)=A*x(t)+Bu(t), ma con
% A*=A-K*C

y_ident2 = lsim(model,u_val,1:length(u_val),x0);%using data saved for validation
model_k=ss(A-K*C,[B K],C,0,1);
y_ident2k = lsim(model_k,[u_val' y_val],1:length(u_val),x0);%using data saved for validation
model_e=ss(A-K*C,[B K],-C,[D eye( size(y_val,2))],1);
e_ident2k = lsim(model_e,[u_val' y_val],1:length(u_val),x0);%errore di innovazion

figure()
plot([y_val(:,1) y_ident2k(:,1)])
grid on
title('Confronto prima uscita di validazione con la predizione');
legend('uscita di validazione','uscita predittore')
xlabel('campioni');
ylabel('°C');
%plot multiplo
figure()
plot(e_ident2k)
grid on
title('Andamenti errore di predizione');
xlabel('campioni');
ylabel('°C');
%plot singolo
figure()
plot(e_ident2k(:,1))
grid on
title('Andamento errore di predizione prima uscita');
xlabel('campioni');
ylabel('°C');

%stima della covarianza per serie di dati
cov_e=covf(e_ident2k,length(e_ident2k));
figure() 
plot(cov_e')
title('Andamenti della covarianza di errore di predizione');
%plot singolo
figure() 
plot(cov_e(1,:)')
title('Andamento della covarianza di errore di predizione prima uscita');
grid on

%errore quadratico medio delle uscite con predittore
for i= 1:size(y_val,2)
    e3(:,i)=(sum((y_ident2k(:,i)-y_val(:,i)).^2))/size(y_val,1);
end

figure
stem(e3);
grid on;
axis([0 size(y_val,2)+1 0 max(e3)+0.5])
title('Errori quadratici medi per ogni uscita del predittore');
xlabel('n° uscite');
ylabel('°C');





