%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  File: statEfficencyDemo.m
%  Matlab script file
%  Date: Nov, 08
%
% Demonstrate weighted LS and statistical efficiency on simple Gaussian data.

% Dependencies: none
% Author: ADJ
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%% Check Path and Constants  %%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear;
%close all;

%%%%%%%%  Initialize random number generator %%%%%%%%%%%%%%%%%%%%%%%
% Random number generator seed:
seed = round(sum(1000*clock));
rand('state', seed);
seed0 = seed;

mn = [1; 2];  % Data mean
aspectRatio = 3;  % Aspect ratio of noise standard deviations
K = 20;       % Number of data samples
% Random weights for weighted mean.
w = exp(-(10*rand(K,1)));
T = 10000;     % Number of trials for estimating estimator variance.
mSize = 12; % Marker size for plots

% Random orientations for covariance matrices for K samples
theta = pi * rand(K,1);
ct = cos(theta);
st = sin(theta);

R = zeros(2,2,K);
R(1,1,:) = ct;
R(1,2,:) = st;
R(2,1,:) = -st;
R(2,2,:) = ct;

% Random scale factors for each data's covariance
stdev = 50*rand(K,1)+1;

% Generate data
x = zeros(2, K, T);
covHalf = zeros(2,2,K);
for j = 1:K
  covHalf(:,:,j) = stdev(j) * R(:,:,j) * diag([1,aspectRatio]);
  r = repmat(mn, 1, T) + covHalf(:,:,j) * randn(2,T);
  x(:,j,:) = reshape(r, [2, 1, T]);
end

% Show estimates for 10 different random data sets.
for trial = 1:10
  figure(1); clf;
  plot(x(1,:, trial), x(2,:, trial), 'ob');
  hold on;
  h = plot(mn(1), mn(2), 'ok', 'MarkerSize', mSize, 'MarkerFaceColor', 'k');

  % Estimate mean by maximum log likelihood (known covariances)
  %  grad L(m) = 0 implies normal equations
  %  sum (x_k - m)' covInv_k  = 0
  %  sum x_k' covInv_k = m' sum covInv_k
  %  m = [sum covInv_k ]^-1 [sum covInv_k x_k ]

  C = zeros(2,2);
  b = zeros(2,1);
  for j = 1:K
    covInv = stdev(j)^(-2) * R(:,:,j)*diag([1,aspectRatio^(-2)])*R(:,:,j)';
    C = C + covInv;
    b = b + covInv * x(:,j, trial);
  end
  mL = inv(C) * b;
  figure(1); hold on;
  plot(mL(1), mL(2), '*g', 'MarkerSize', mSize);

  % Estimate mean by sample mean
  mS = sum(x(:,:,trial),2)/K;
  figure(1); hold on;
  plot(mS(1), mS(2), '*b', 'MarkerSize', mSize);

  % Estimate mean by weigthed mean  (random weights)
  mW = sum(repmat(w', 2,1).*x(:,:,trial),2)/sum(w);
  figure(1); hold on;
  plot(mW(1), mW(2), '*r', 'MarkerSize', mSize);
  set(get(figure(1),'CurrentAxes'), 'FontSize', 16);
  axis equal;
  title('Estimates: SSSE(g), LS(b), WLS(r), true (k)');

  pause;
end

%% Do T trials of each type of estimate

% Estimate mean by weighted mean
mW = reshape(sum(repmat(w', [2,1,T]).*x,2), 2,T)/sum(w);
figure(1); clf;
figure(1); hold on;
plot(mW(1,:), mW(2,:), '*r', 'MarkerSize', mSize);

% Estimate mean by sample mean
mS = reshape(sum(x,2), 2, T)/K;
figure(1); hold on;
plot(mS(1,:), mS(2,:), '*b', 'MarkerSize', mSize);

%  Estimate mean by maximum log likelihood (known covariances)
C = zeros(2,2);
b = zeros(2,T);
for j = 1:K
  covInv = stdev(j)^(-2) * R(:,:,j)*diag([1,aspectRatio^(-2)])*R(:,:,j)';
  C = C + covInv;
  b = b + covInv * reshape(x(:,j,:), 2, T);
end
mL = inv(C) * b;

figure(1); hold on;
plot(mL(1,:), mL(2,:), '*g', 'MarkerSize', mSize);

h = plot(mn(1), mn(2), 'ok', 'MarkerSize', mSize, 'MarkerFaceColor', 'k');
axis equal;
set(get(figure(1),'CurrentAxes'), 'FontSize', 16);
title('Estimates: SSSE(g), LS(b), WLS(r), true (k)');

% Get covariances of estimators (assuming mean zero)
emL = mL - repmat(mn, 1, T);
emS = mS - repmat(mn, 1, T);
emW = mW - repmat(mn, 1, T);

CovL = (emL * emL') / T;
CovS = (emS * emS') / T;
CovW = (emW * emW') / T;

% Statistical efficiency. 
% The Fisher information for this problem is F = C, and the
% efficiency of an estimator in the direction n is defined as 
%  sqrt( (n' invF  n) / (n' CovL n) )

invF = inv(C);
eL = eig(invF, CovL); 
fprintf('Standard error (maxLike) efficiency: %e %e\n', sqrt(eL'));
eS = eig(invF, CovS); 
fprintf('Ordinary Least Squares efficiency: %e %e\n', sqrt(eS'));
eW = eig(invF, CovW); 
fprintf('(Randomly) Weighted Least Squares efficiency: %e %e\n', sqrt(eW'));

% The maximum likelihood estimator is efficient (eL is close to 1),
% and the other estimators are increasingly inefficient (e << 1)