%************************************************************
% File:         run_batch_test.m
% Date:         August 21, 2006
% Author:       Jo-Anne Ting
% Description:
%  Tests the functionality of the batch version of
%  Bayesian weighted regression (BWR) for automatic 
%  outlier detection on a linear function.
%
%  Compares performances of the following algorithms:
%  i)   Ordinary Least Squares (OLS) regression
%  ii)  Bayesian weighted regression (BWR)
%  iii) Random Sample Consensus (RANSAC), 
%       (Fischler & Bolles, 1981)
%  iv)  Variational Bayesian Robust regression
%       (Faul & Tipping, 2001)
%  v)   Robust least squares (Matlab's robustfit)
%  vi)  A mixture model using a Gaussian and an
%       uniform distribution to model outliers
%  vii) 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

PLOT_DATA      = 0;    % set to 1 if want to plot data cloud

% Set indicator variable to 1 if want to run algorithm
OLS_ON         = 1;    % OLS regression
BWR_ON         = 1;    % Bayesian weighted regression
RANSAC_ON      = 0;    % RANSAC (by Peter Kovesi)
ROBUSTREG_ON   = 1;    % Robust regression (Faul & Tipping)
ROBUSTLS_ON    = 1;    % Robust Least Squares
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
  %----------------------------------------------------------

  % Create training, test and outlier input 
  X  = (rand(n,numDim) - 0.5)*4;
  Xt = (-2:0.1:2)'*ones(1,numDim);
  nt = size(Xt,1); 
  
  % Beta vector with offset dimension
  %b = randn(numDim+1, 1)*10;
  b = [-10; 5; 4; 8; 2; 3];

  % Create training and test outputs
  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; 
                              
  % Create test output
  Yt = [Xt ones(size(Xt,1),1)]*b;                     
  
  % Std of output noise
  sigmaNoise = std(Ynoise);                           

  % Add outlying outputs (outside the 2*sigma range) 
  %-------------------------------------------------
  outlierX = []; outlierY = [];
  for nn=1:n
     pp = rand(1);
     % This point is an outliers with probability, pOutlier
     if pp <= pOutlier
        % Add the outlier some distance away from the data
        Yn(nn,1) = Yn(nn,1) + (k+rand(1))*sigmaNoise;
        outlierX = [outlierX; X(nn,1)];
        outlierY = [outlierY; Yn(nn,1)];
     end
  end  

  % Visualize data, if desired
  %-----------------------------
  if PLOT_DATA 
  figure()
  subplot(211)
  plot(X, Yn, 'go', outlierX, outlierY, 'pb')
  title(sprintf('Entire dataset'))
  subplot(212)
  plot(X, Y, '.r');
  title(sprintf('Noiseless dataset'))
  end

  % Pad input training data with offset dim. 
  Xpadded = [X ones(size(X,1),1)];
  dpadded = size(Xpadded,2);


 
  % 2.2 Perform OLS regression 
  %----------------------------------------------------------
  if OLS_ON 
  b_ols = inv(Xpadded'*Xpadded)*Xpadded'*Yn;
  nmse_ols(t, 1) = ...
   mean(([Xt ones(nt,1)]*b_ols - Yt).^2) / var(Yt);
  end

  % 2.3 Run BWR 
  %----------------------------------------------------------
  if BWR_ON 
  % Settings for BWR
  numIterations = 2000;
  threshold = 1.e-10;
  bwr_result = bwr(X, Yn, numIterations,  threshold);
  yPred_bwr = [Xt ones(nt,1)]*bwr_result.beta;
  nmse_bwr(t, 1) = mean((yPred_bwr - Yt).^2) / var(Yt);
  end

  % 2.4 Run RANSAC (by Peter Kovesi)
  %----------------------------------------------------------
  if RANSAC_ON 
  fittingfn = @defineline;
  distfn    = @lineptdist;
  degenfn   = @isdegenerate;
  ransac_numPoints = 100;
  [ransac_m, ransac_inliers] = ...
   ransac([X'; Yn'], fittingfn, distfn, degenfn, ...
          ransac_numPoints, 0.01, 1);
  % Find the regression estimate based on data points 
  % found to be in the model
  ransac_x = [ransac_m(1:end-1,:); ones(1,ransac_numPoints)];
  ransac_y = ransac_m(end,:);
  ransac_b = pinv(ransac_x*ransac_x')*ransac_x*ransac_y';
  yPred_ransac = [Xt ones(nt,1)]*ransac_b;
  nmse_ransac(t, 1) = mean((yPred_ransac - Yt).^2) / var(Yt);
  end

  % 2.5 Run Faul & Tipping's robust regression (2001) 
  %----------------------------------------------------------
  if ROBUSTREG_ON
  numIterations = 1000;
  threshold = 1.e-10;
  rr_result = sparse_mixture(X, Yn, numIterations, threshold);
  yPred_rreg = [Xt ones(nt,1)]*rr_result.beta;
  nmse_rreg(t, 1) = mean((yPred_rreg - Yt).^2) / var(Yt);
  end 
 
  % 2.6 Run Robust Least Squares (Matlab's robustfit)
  %----------------------------------------------------------
  if ROBUSTLS_ON 
  [rls_b, rls_stats] = robustfit(X, Yn, 'bisquare', 4.685);
  yPred_rls = [Xt ones(nt,1)]*rls_b;
  nmse_rls(t, 1) = mean((yPred_rls - Yt).^2) / var(Yt);
  end

  % 2.7 Run Mixture Model (a uniform + Gaussian distribution)
  %----------------------------------------------------------
  if MIXTURE_ON 
  numIterations = 20;
  mm_result = mixtureModel(X, Yn, numIterations);

  % mm_result.z1 denotes level of membership in data cloud 
  % & mm.z2 denotes membership level in outlier component

  mm_result.beta = inv( ...
   (Xpadded .* repmat(mm_result.z1,1,dpadded))'*Xpadded ) *...
   (Xpadded .* repmat(mm_result.z1,1,dpadded))' * Yn;
  nmse_mixture(t, 1) = mean( ...
   ([Xt ones(nt,1)]*mm_result.beta - Yt).^2 ) / var(Yt);
  end

  % 2.8 Threshold on the Mahalanobis distance
  %----------------------------------------------------------
  if THRESHOLD_ON 
  jointData = [X Yn];
  s.meanData = mean(jointData,1);
  
  % Calculate covariance of dataset
  s.diff = jointData - repmat(s.meanData, n, 1);
  s.xTx = s.diff'*s.diff;
  
  % Now calculate the Mahalanobis distance for each point
  for nn=1:n
    s.mahala(nn,:) = sqrt( s.diff(nn,:) * inv(s.xTx) * ...
                           s.diff(nn,:)' );
  end

  % Threshold  for a 10-DOF chi-squared distribution 
  % (95% confidence interval) = 4.575 
  s.threshold = 0.100;  % hand-tuned optimal threshold
  s.finalW = ones(n,1);
  s.indexW = find(s.mahala > s.threshold);
  s.finalW(s.indexW) = 0;

  s.beta = inv( ...
    (repmat(s.finalW,1,numDim+1).*Xpadded)'*Xpadded) *... 
    (repmat(s.finalW,1,numDim+1).*Xpadded)' * Yn;
  nmse_thres(t, 1) = mean(([Xt ones(nt,1)]*s.beta - Yt).^2) ...
                      / var(Yt);
  end


end   % for t = 1:totalNumTrials


%------------------------------------------------------------
% 3. Report results 
%------------------------------------------------------------

% Average test nMSE values
disp('Average test nMSE values:')
if OLS_ON
  disp(sprintf('OLS-test-nmse: %f', mean(nmse_ols) ));
end
if THRESHOLD_ON
  disp(sprintf('Threshold-test-nmse: %f', mean(nmse_thres) ));
end
if MIXTURE_ON
  disp(sprintf('Mixture-test-nmse: %f', mean(nmse_mixture) ));
end
if ROBUSTLS_ON
  disp(sprintf('ROBUSTLS-test-nmse: %f', mean(nmse_rls) ));
end
if ROBUSTREG_ON
  disp(sprintf('ROBUSTREG-test-nmse: %f', mean(nmse_rreg) ));
end
if RANSAC_ON
  disp(sprintf('RANSAC-test-nmse: %f', mean(nmse_ransac) ));
end
if BWR_ON
  disp(sprintf('BWR-test-nmse: %f', mean(nmse_bwr) ));
end
