clear all, close all, clc, warning off

v=get(0,'ScreenSize');
H=800; %Altezza in pixel grafici
W=1200; %Larghezza in pixel grafici


handle1=figure;
set(handle1,'Position',[round(v(3)/2-W/2) round(v(4)/2-H/2) W H]);

N=10; %N° totale di sensori
K=5;  %N° di sensori da selezionare 

n=4;  %N° di stati del sistema;

Ac = zeros(4,4); Ac(1,2) = 1; Ac(3,4) = 1;          % matrice dinamica continua
A = expm(Ac*0.1);                                     % matrice dinamica discreta

Q(1:2,1:2) = 0.01*[0.1 0.01; 0.01 1];              % struttura covarianza rumore di modello
Q(3:4,3:4) = Q(1:2,1:2);                          %
C=randn(N,n);

R = diag(0.01*ones(K,1));                             % struttura covarianza rumore di misura
Combinazioni=nchoosek(1:N,K);
NComb=size(Combinazioni,1);
TracciaMin=1e100;
BoundMax=-1e100;
VettTracce=zeros(NComb,1);
for i=1:NComb
   C_i=C(Combinazioni(i,:),:);

   % Valore esatto della traccia della varianza d'errore di predizione
   [P,eigenvalues,L] = dare(A',C_i',Q,R);
   VettTracce(i)=trace(P);
   if (VettTracce(i)<TracciaMin) 
       TracciaMin=VettTracce(i);
       IndiceTracciaMin=i;
   end
   
   % Calcolo di un suo bound
   Autovalori=eig(A*inv(inv(Q)+C_i'*inv(R)*C_i)*A'+Q);
   Valorisingolari=svds(C_i'*inv(chol(R)),n);
   if (Autovalori(1)*Valorisingolari(n)^2>BoundMax)
       BoundMax=Autovalori(1)*Valorisingolari(n)^2;
       IndiceBoundMax=i;
   end
end

RigheSelezionate=-1*ones(K,1);
for i=1:K
    OttimoLocale=1e100;
    RangoMax=0;
    TrovatoRangoPieno=0;
    for j=1:N
        if (~ismember(j,RigheSelezionate))
            r=rank(obsv(A,C([RigheSelezionate(1:(i-1)); j],:)));
            if (r<length(A) && ~TrovatoRangoPieno)
                if (r>RangoMax)
                    RangoMax=r;
                    RigheSelezionate(i)=j;
                end
            else
                TrovatoRangoPieno=1;
                [P,eigenvalues,L] = dare(A',C([RigheSelezionate(1:(i-1)); j],:)',Q,diag(0.01*ones(i,1)));
                if (trace(P)<OttimoLocale)
                    OttimoLocale=trace(P);
                    RigheSelezionate(i)=j;
                end            
            end
        end
    end
end

RigheSelezionate2=-1*ones(K,1);
TrovatoRangoPieno2=0;
for i=1:K
    OttimoLocale2=1e100;
    if (~TrovatoRangoPieno2)
        Combinazioni2=nchoosek(1:N,i);
        NComb2=size(Combinazioni2,1);
        for j=1:NComb2
            C_j=C(Combinazioni2(j,:),:);
            if (rank(obsv(A,C_j))==length(A))
                TrovatoRangoPieno2=1;
                [P,eigenvalues,L] = dare(A',C_j',Q,diag(0.01*ones(i,1)));
                if (trace(P)<OttimoLocale2)
                    OttimoLocale2=trace(P);
                    RigheSelezionate2(1:i)=Combinazioni2(j,:);
                end       
            end
        end
    else
        for j=1:N
            if (~ismember(j,RigheSelezionate2))
                [P,eigenvalues,L] = dare(A',C([RigheSelezionate2(1:(i-1)); j],:)',Q,diag(0.01*ones(i,1)));
                if (trace(P)<OttimoLocale2)
                    OttimoLocale2=trace(P);
                    RigheSelezionate2(i)=j;
                end            
            end
        end
    end
end


VettTracce=sort(VettTracce);
[P,eigenvalues,L] = dare(A',C(Combinazioni(IndiceBoundMax,:),:)',Q,R);

IndiceTracciaMin
TracciaMin
IndiceBoundMax
TracciaMinBound=trace(P)
plot(VettTracce);
hold on;
plot(TracciaMinBound*ones(NComb,1),'g');
[P,eigenvalues,L] = dare(A',C',Q,diag(0.01*ones(N,1)));
plot(trace(P)*ones(NComb,1),'r');
TracciaTeorica=trace(P)

plot(OttimoLocale*ones(NComb,1),'k');

plot(OttimoLocale2*ones(NComb,1),'m');
legend('Ottimizzazione ottenuta utilizzando la DARE (complessità esponenziale)','Ottimizzazione ottenuta utilizzando il bound (complessità esponenziale)','Traccia della varianza di predizione se si potessero usare tutte le righe di C','Ottimizzazione ottenuta utilizzando la DARE (complessità polinomiale)','Ottimizzazione ottenuta utilizzando la DARE (complessità pseudo-polinomiale... solo se siamo fortunati)','Location','NorthWest');
