%% Data Fusion
% %%%%%%%%%%%%%%%%%%%%%%%%%%%
% model 
%  Y1=U   + noise(0,1/b1)
%  Y2=U+V + noise(0,1/b2)
%  Y3=V   + noise(0,1/b3)
%  p(U)=p(V)=1


% generate 3 data points!
u=2;v=4;
b1=10;   % precision on 1st measurement
b2=.10;    % precision on 2nd measurement
b3=.0010;   % precision on 3rd measurement

y1 = u   + randn/sqrt(b1);
y2 = u+v + randn/sqrt(b2);
y3 = v   + randn/sqrt(b3);

% calculate posterior means and variances for u and v

mu=(b1*y1+(b2*b3)/(b2+b3)*(y2-y3))/(b1+b2*b3/(b2+b3));
vu=1/(b1+(b2*b3)/(b2+b3));
mv=(b3*y3+(b2*b1)/(b2+b1)*(y2-y1))/(b3+b2*b1/(b2+b1));
vv=1/(b3+(b2*b1)/(b2+b1));

% plot results
figure,hold on
axis([-5 20 0 .2])
x=-5:.1:10;
p=normpdf(x,mu,sqrt(vu));p=p/sum(p);
h1=plot(x,p,'b');
p=normpdf(x,mv,sqrt(vv));p=p/sum(p);
h2=plot(x,p,'r');
legend([h1 h2],{'posterior U','posterior V'});

plot(y1,0,'.','color','k','markersize',30);text(y1,-.005,'y1');
plot(y2,0,'.','color','k','markersize',30);text(y2,-.005,'y2');
plot(y3,0,'.','color','k','markersize',30);text(y3,-.005,'y3');


line([u u],[0 max(p)],'color','c');text(u,max(p)+.001,'U');
line([v v],[0 max(p)],'color','c');text(v,max(p)+.001,'V');




%%
% Simple Bayesian Dynamic System [a.k.a. Kalman Filter]
%
%     x0->x1 -> ... -> xk-1 -> xk -> ... 
%         |            |       |         
%         V            V       V         
%         y1           yk-1    yk        
%
%


% here is some weather data
data       = load('weather_data.txt');
year       = data(:,1);
month      = data(:,2);
maxTemp    = data(:,3);
minTemp    = data(:,4);
hoursFrost = data(:,5);
rain       = data(:,6);
hoursSun   = data(:,7);

% choose your favourite data among the above
y = maxTemp(1:200); 
n = length(y);


% set up noise precisions
b=1;   % observation noise 
b0=1;  % state noise 

% first state is first data point
x0=y(1);

% Model is very simple:
%   yk ~ N(xk,1/b)
%   xk ~ N(xk-1,1/b0)
% or equivalently
% yk = xk + noise(0,1/b)    --> observation equation
% xk = xk-1 + noise(0,1/b0) --> state dynamics equation


xk=zeros(n,1);
bk=zeros(n,1);
% forward updates
xk(1) = (b*y(1)+b0*x0)/(b+b0);   
bk(1) = b+b0;
for k=2:n
    % update
    bk(k) = b+b0*bk(k-1)/(b0+bk(k-1));
    xk(k) = xk(k-1) + b/bk(k)*(y(k)-xk(k-1));
%     b_hat = 1/(1/b0+1/bk(k-1));
    
%     xk(k) = xk(k-1) + b/(b+b_hat)*(y(k)-xk(k-1));
%     bk(k) = b_hat+b;
end

figure
plot(y,'.-');
hold on
plot(xk,'r-');
plot(xk+sqrt(1./bk),'r--');
plot(xk-sqrt(1./bk),'r--');



