%************************************************************
% File:         run_realtime_test.m
% Date:         August 21, 2006
% Author:       Jo-Anne Ting
% Description:
%  Tests the functionality of the real-time version of
%  Bayesian weighted regression (BWR) for automatic
%  outlier detection on a linear function. Uses a 
%  forgetting rate to implement the real-time version.
%
%  Compares performances of the real-time versions of
%  the following algorithms:
%  i)   Ordinary least squares reggression (in batch form)
%  ii)  Bayesian weighted regression (in batch & real-time
%       forms)
%  iii) Variational Bayesian Robust regression
%       (Faul & Tipping, 2001)
%  iv)  A mixture model using a Gaussian and an
%       uniform distribution to model outliers
%  v)   An approach that thresholds on the
%       Mahalanobis distance to see if a sample is
%       an outlier or not. Optimal threshold value
%       is found experimentally.
%
% Please see Ting, D'Souza & Schaal (2007) in ICRA
% proceedings for more details on this test.
%
%************************************************************
clear all;
close all;

%------------------------------------------------------------
% 1. Initialize test settings
%------------------------------------------------------------

totalNumTrials = 1;    % number of trials to average over
numDim         = 5;    % dimensionality of input data
snry           = 10;   % signal-to-noise ratio of output data
n              = 1000; % number of data samples in data set
pOutlier       = 0.2;  % probability of an outlier occuring
k              = 3;    % scaling factor which determines how
                       % far outliers are from the data cloud
SMALL          = 1e-6;

% Size of window is 1/ (1-lambda) data points
lambda         = 0.999;    

% Set indicator variable to 1 if want to run algorithm
OLS_ON         = 1;    % OLS regression
BWR_ON         = 1;    % Bayesian weighted regression
ROBUSTREG_ON   = 1;    % Robust regression (Faul & Tipping)
MIXTURE_ON     = 1;    % Mixture model approach
THRESHOLD_ON   = 1;    % Thresholding approach


%------------------------------------------------------------
% 2. Run the algorithms
%------------------------------------------------------------

for t = 1:totalNumTrials

  % 2.1 Create a synthetic data set
  %----------------------------------------------------------
  X  = (rand(n,numDim)-0.5)*4;
  
  % Beta vector with offset dimension
  b  = [-10; 5; 4; 8; 2; 3];  

  % Create training output
  Y = [X ones(size(X,1),1)]*b;   
  
  % Create output noise                 
  Ynoise = randn(size(X,1),1)*sqrt(1/snry)*std(Y); 
  
  % Add noise to training output
  Yn = Y + Ynoise;         
  
  % Std of output noise
  sigmaNoise = std(Ynoise);

  % Add outliers in data
  %----------------------
  Ynn = Yn;
  tempYn = Ynn;

  % For each point, make it an outlier with some probability
  for nn=1:n
    pp = rand(1);
    if pp <= pOutlier
       tempYn(nn, :) = Ynn(nn,:) + (k+rand(1))*sigmaNoise;
    end
  end
  Yn = tempYn;

  % Pad input training data with offset dimension
  Xpadded = [X ones(n,1)];


  % 2.2 Perform OLS regression (batch)
  %----------------------------------------------------------
  if OLS_ON
  b_ols = inv(Xpadded'*Xpadded)*Xpadded'*Yn;
  nmse_ols = mean(([X ones(n,1)]*b_ols - Y).^2) / var(Y);
  rmse_ols = sqrt( mean(([X ones(n,1)]*b_ols - Y).^2) );
  end

  % 2.3 Run BWR
  %----------------------------------------------------------
  if BWR_ON 

  % Batch version of BWR
  %----------------------
  numIterations = 2000;
  threshold = 1.e-10;
  r = bwr(X, Yn, numIterations, threshold);
  nmse_bwr = mean(([X ones(n,1)]*r.beta - Y).^2) / var(Y);

  % Real-time version of BWR
  %--------------------------
  % (assume each sample is available at each time step)
  numIterations = 20; 
  threshold = 1.e-12;

  for nn=1:n
  rOnline = bwr_online(X(nn,:), Yn(nn), ...
             numIterations, threshold, lambda, ss, nn);
  
  % Store predicted output 
  yPred_bwr(nn,:) = [X(nn,:) 1]*rOnline.beta;
  nmse_bwrOnline(nn,1) = mean( ...
        ([X ones(n,1)]*rOnline.beta - Y).^2 ) / var(Y);
  rmse_bwrOnline(nn,1) = ...
        sqrt( mean(([X ones(n,1)]*rOnline.beta - Y).^2) );
  
  % Store sufficient statistics
  ss.sum_wxxT = rOnline.sum_wxxT;
  ss.sum_wyx = rOnline.sum_wyx;
  ss.sum_wyy = rOnline.sum_wyy;
  ss.n = rOnline.n;
  end
  end

  % 2.4 Run Faul & Tipping's robust regression (2001)
  %----------------------------------------------------------
  if ROBUSTREG_ON
  % Real-time version
  numIterations = 20;
  threshold = 1.e-12;
  ss.max_minY = max(Yn) - min(Yn);
  ss.true_varY = var(Yn);
 
  for nn=1:n
  rrOnline = sparse_mixture_online(X(nn,:), Yn(nn), ...
             numIterations, threshold, lambda, ss, nn);
  
  % Store predicted output
  yPred_rr(nn,:) = [X(nn,:) 1]*rrOnline.beta;
  nmse_rreg(nn,1) = mean( ...
           ([X ones(n,1)]*rrOnline.beta - Y).^2 ) / var(Y);
  rmse_rref(nn,1) = ...
           sqrt( mean(([X ones(n,1)]*rrOnline.beta - Y).^2) );
 
  % Store sufficient statistics
  ss.maxY = rrOnline.maxY;
  ss.minY = rrOnline.minY;
  ss.sum_zxx = rrOnline.sum_zxx;
  ss.sum_zxy = rrOnline.sum_zxy;
  ss.sumY = rrOnline.sumY;
  ss.sum_Ym2 = rrOnline.sum_Ym2;
  ss.varY = rrOnline.varY;
  ss.sum_z = rrOnline.sum_z;
  ss.sum_za = rrOnline.sum_za;
  ss.sum_n = rrOnline.sum_n;
  end
  end

  % 2.5 Run Mixture Model (a uniform + Gaussian distribution)
  %----------------------------------------------------------
  if MIXTURE_ON
  % Real-time version
  numIterations = 50;
  threshold = 1.e-12;

  % Real-time version of mixture model approach 
  % (assume each sample is available at each time step)
  for nn=1:n
  tOnline = mixtureModel_online(X(nn,:), Yn(nn), ...
            numIterations, threshold, lambda, ss, nn);
  
  % Store predicted output
  yPred_mix(nn,:) = [X(nn,:) 1]*tOnline.beta;
  nmse_mixture(nn,1) = mean( ...
           ([X ones(n,1)]*tOnline.beta - Y).^2 ) / var(Y);
  rmse_mixture(nn,1) = ...
           sqrt( mean(([X ones(n,1)]*tOnline.beta - Y).^2) );
  
  % Store sufficient statistics
  ss.sum_z1yx = tOnline.sum_z1yx;
  ss.sum_z1xx = tOnline.sum_z1xx;
  ss.sum_z1xxT = tOnline.sum_z1xxT;
  ss.sum_z1x = tOnline.sum_z1x;
  ss.sum_z1 = tOnline.sum_z1;
  ss.sum_z2x = tOnline.sum_z2x;
  ss.sum_z2 = tOnline.sum_z2;
  ss.sum_zxu2 = tOnline.sum_zxu2;
  ss.n = tOnline.n;
  end
  end

  % 2.6 Threshold on the Mahalanobis distance
  %----------------------------------------------------------
  if THRESHOLD_ON 
  s.threshold = 0.8;  %0.89;
  s.numData = 0;
  s.sumData = zeros(1,numDim+1);
  s.xxT = zeros(numDim+1, numDim+1);
  s.wxxT = zeros(numDim+1, numDim+1);
  s.wyx = zeros(numDim+1, 1);
  
  % Assume each sample is available at each time step
  for nn=1:n
   tempData = [X(nn,:) Yn(nn,:)]; 
   tempPadded = [X(nn,:) 1];

   % Store sufficient statistics
   s.sumData = tempData + lambda*s.sumData;
   s.numData = 1 + lambda*s.numData;
   s.xxT = tempData'*tempData + lambda*s.xxT;

   s.meanData = s.sumData ./ s.numData;
   s.mahala(nn,:) = sqrt(...
       (tempData - s.meanData)*pinv(s.xxT)*...
       (tempData - s.meanData)');

   if s.mahala(nn,:) > s.threshold
     s.finalW(nn,:) = 0;
   else
     s.finalW(nn,:) = 1;
   end

   s.wxxT = (s.finalW(nn,:).*tempPadded)' *tempPadded ...
           + lambda*s.wxxT;
   s.wyx = (s.finalW(nn,:).*tempPadded)'*Yn(nn,:) ...
           + lambda*s.wyx;
   s.beta = inv(s.wxxT + SMALL*eye(numDim+1))*s.wyx;
   nmse_thres(nn,1) = ...
            mean(([X ones(n,1)]*s.beta - Y).^2) / var(Y); 
   rmse_thres(nn,1) = ...
            sqrt( mean(([X ones(n,1)]*s.beta - Y).^2) ); 
  end
  end

end  % End of main loop (numTrials)


%------------------------------------------------------------
% 3. Report nMSE results
%------------------------------------------------------------
% Report the nMSE results on the last time step
disp('nMSE values from the last time step');
if OLS_ON
  disp(sprintf('OLS-batch-nmse: %f', nmse_ols));
end
if THRESHOLD_ON
  disp(sprintf('Threshold-nmse: %f', nmse_thres(end) ));
end
if MIXTURE_ON
  disp(sprintf('Mixture-nmse: %f', nmse_mixture(end) ));
end
if ROBUSTREG_ON
  disp(sprintf('ROBUSTREG-nmse: %f', nmse_rreg(end) ));
end
if BWR_ON
  disp(sprintf('BWR-nmse: %f', nmse_bwrOnline(end) ));
  disp(sprintf('BWR-batch-nmse: %f', nmse_bwr));
end

% Display nMSE values over all time steps
%---------------------------------------- 
figure()
hold on;
if THRESHOLD_ON
  plot([1:length(nmse_thres)], nmse_thres, 'g-.');
end
if MIXTURE_ON
  plot([1:length(nmse_mixture)], nmse_mixture, 'r');
end
if ROBUSTREG_ON
  plot([1:length(nmse_rreg)], nmse_rreg, 'k--');
end
if BWR_ON 
  plot([1:length(nmse_bwrOnline)], nmse_bwrOnline, 'b--');
end
axis([0 n 0 0.2])
xlabel('Sample Index');
ylabel('nMSE');
hold off;
