%%% Section 1.3
%%% Weighted Least Squares

% Ax = B
% x = inv(A'WA)A'WB
% W = diag(1./sigma.^2)

clear
close all
% Az = Y
A = 1;
z = -3;
sigma1 = 0.02;
sigma2 = 0.01;


%%% estimate z 1000 times

A = ones(4,1);
zm = z+ [sigma1*randn(2,1000);sigma2*randn(2,1000)];

W = diag(1./[sigma1 sigma1 sigma1 sigma2].^2);

zest = inv(A'*A)*A'*zm;
zwest = inv(A'*W*A)*A'*W*zm;

vest = std(zest)
vwest = std(zwest)





[n,x] = hist(zest,40);
[n2,x2] = hist(zwest,40);

figure(1)
bar(x,n,'hist')
hold on; 
h=bar(x2,n2,'hist'); 
hold off
set(h,'facecolor','y')
legend('LLSE','WLLSE')
text(-3.025, 65, ['STD = 2x 0.02 meters'])
text(-3.025, 60, ['        = 2x 0.01 meters'])
text(-3.025, 50, ['LLSE = ',num2str(round(10000*std(zest))/10000),'meters'])
text(-3.025, 45, ['WLLSE = ',num2str(round(10000*std(zwest))/10000),'meters'])