%
%   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

%% Inizializzazione parametri
       
x0=1.9;     %Parametro da misurare
x0_=1.5;    %Valore del parametro noto a priori
p0=0.5;     %Incertezza a priori sul parametro
r=0.4;      %Varianza rumore di misura
N=1000;      %Numero di misure effettuate
t=0:N-1;     %Indici temporali delle misure

%% Generazione delle misure;
%
randn('seed', sum(1e8*clock));
y = x0*ones(1,N)+sqrt(r)*randn(1,N);

%% Calcolo della successione delle p_k
p=zeros(1,N);
p(1)=p0;        %inizializzazione alla varianza a priori
for j=2:N
    p(j)=p(j-1)*r/(p(j-1)+r);   %Si utilizza la formula ricorsiva di Kalman
end

%% Calcolo delle stime
x=zeros(1,N+1); %È lungo un elemento in più per contenere la stima
x(1)=x0_;      %a posteriori all'istante -1 che poi verrà eliminata
for j=2:(N+1)
    x(j)=x(j-1)+(p(j-1)/(p(j-1)+r))*(y(j-1)-x(j-1)); %Si utilizza la formula ricorsiva di Kalman
end
x=x(1,2:end); % Elimino la stima a posteriori in -1

%Visualizzazione degli andamenti ottenuti
h=figure;
set(h,'Position',[0 0 1200 400]);
subplot(1,2,1)
plot(t,p);
title('Andamento temporale varianza dell''errore di predizione')
xlabel('Campione');
subplot(1,2,2)
plot(t,[x; x0*ones(1,N)])
title('Andamento temporale della stima del parametro')
legend('Stima','Valore vero');
xlabel('Campione');


