%
%   Corso Progettazione Sistemi di Controllo  -  a.a. 2006/07
%   Guido Albertin    (546986)
%   Elena Toffoli     (546946)
%   Giancarlo Baldan  (547324)
%

%% Setup

clear all, close all, clc, warning off

R1 = [2 1; 1 2];
R2 = 1;
R = blkdiag(R1, R2)

C1=[1 2; 3 1];
C2=[4 3];
C = [C1; C2]

P=[4 1; 1 2]

% Genero il valore dello stato con media zero e varianza P

e=randn(2,1);
x=(chol(P)')*e

%x=zeros(2,1000);       
%for i=1:1000;                  %Parte di codice per verificare se genera
%    e=randn(2,1);              %campioni con la giusta varianza... sembra essere OK!
%    x(:,i)=(chol(P)')*e;
%end
%cov(x')

% Generazione di un campione di rumore
e=randn(3,1);
w=(chol(R)')*e;

%Calcolo delle misure ottenute
y=C*x+w
y1=y(1:2);
y2=y(3);

%------- Stima ottima completa -----------
x_=P*C'*inv(C*P*C'+R)*y

%------- Stima solo sensore 1 ------------
x_1=P*C1'*inv(C1*P*C1'+R1)*y1

%------- Stima solo sensore 2 ------------
x_2=P*C2'*inv(C2*P*C2'+R2)*y2


% Calcolo Stima ottima combinando le stime ai due sensori
A=C1*P*C1'+R1;
B=C1*P*C2';
C=C2*P*C1'; %Sovrascrivo la matrice C delle misure... tanto non la uso +
D=C2*P*C2'+R2;
Ai=inv(A);
Di=inv(D);

T1=eye(2)+P*C1'*Ai*B*inv(D-C*Ai*B)*C2-P*C2'*inv(D-C*Ai*B)*C2;
T2=eye(2)-P*C1'*Ai*C1-P*(C2'-C1'*Ai*C1*P*C2')*Di*C*inv(B*Di*C-A)*C1;

x_dist=T1*x_1+T2*x_2 %<- E come per magia abbiamo una combinazione lineare delle due stime distribuite
                     % unico problema: i coefficienti della combinazione
                     % sono di un casino spaventoso!!! chissà se riuscirò
                     % mai a semplificarli!!!


