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);
VettTracceComplementari=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

   CC_i=C(setdiff(1:N,Combinazioni(i,:)),:);
   [P,eigenvalues,L] = dare(A',CC_i',Q,diag(0.01*ones(N-K,1)));
   VettTracceComplementari(i)=trace(P);
   
end

RigheSelezionate=1*ones(N,1);
for i=1:(N-K)
    OttimoLocale=1e100;
    RigaDaTogliere=-1;
    for j=1:N
        if (ismember(j,find(RigheSelezionate)))
            [P,eigenvalues,L] = dare(A',C([setdiff(find(RigheSelezionate),j)],:)',Q,diag(0.01*ones(N-i,1)));
            if (trace(P)<OttimoLocale)
                OttimoLocale=trace(P);
                RigaDaTogliere=j;
            end            
        end
    end
    RigheSelezionate(RigaDaTogliere)=0;
end


[VettTracce Perm]=sort(VettTracce);
%[P,eigenvalues,L] = dare(A',C(Combinazioni(IndiceBoundMax,:),:)',Q,R);
VettTracceComplementari=VettTracceComplementari(Perm);
[P,eigenvalues,L] = dare(A',C',Q,diag(0.01*ones(N,1)));

% IndiceTracciaMin
% TracciaMin
% IndiceBoundMax
TracciaMinBound=trace(P);
plot(VettTracce,'b');
hold on;
plot(VettTracceComplementari,'r');
plot(OttimoLocale*ones(NComb,1),'m');

fprintf('Sensori ottimi : ');
Combinazioni(IndiceTracciaMin,:)
fprintf('Sensori ottenuti togliendo : ');
find(RigheSelezionate)'

% 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('Varianza d''errore usando k sensori','Varianza d''errore usando gli altri n-k','Valore ottenuto con l''algoritmo greedy che lascia ogni volta gli n-1 sensori + promettenti','Location','NorthWest');
