function [lambda_vec, error_train, error_val] = ...
    validationCurve(X, y, Xval, yval)
%VALIDATIONCURVE Generate the train and validation errors needed to
%plot a validation curve that we can use to select lambda
%   [lambda_vec, error_train, error_val] = ...
%       VALIDATIONCURVE(X, y, Xval, yval) returns the train
%       and validation errors (in error_train, error_val)
%       for different values of lambda. You are given the training set (X,
%       y) and validation set (Xval, yval).
%

% Selected values of lambda (you should not change this)
lambda_vec = [0 0.001 0.003 0.01 0.03 0.1 0.3 1 3 10]';

% You need to return these variables correctly.
error_train = zeros(length(lambda_vec), 1);
error_val = zeros(length(lambda_vec), 1);

% ====================== YOUR CODE HERE ======================
% Instructions: Fill in this function to return training errors in
%               error_train and the validation errors in error_val. The
%               vector lambda_vec contains the different lambda parameters
%               to use for each calculation of the errors, i.e,
%               error_train(i), and error_val(i) should give
%               you the errors obtained after training with
%               lambda = lambda_vec(i)
%
% Note: You can loop over lambda_vec with the following:
%
%       for i = 1:length(lambda_vec)
%           lambda = lambda_vec(i);
%           % Compute train / val errors when training linear
%           % regression with regularization parameter lambda
%           % You should store the result in error_train(i)
%           % and error_val(i)
%           ....
%
%       end
%
%

for i=1:length(lambda_vec)
    [theta] = trainLinearReg(X,y,lambda_vec(i));
    [error_train(i),~] = linearRegCostFunction(X,y,theta,0);
    [error_val(i),~] = linearRegCostFunction(Xval,yval,theta,0);
end;

% =========================================================================

end


function [A, C, Z] = ldsPca(X, k, m)
% Subspace method for learning linear dynamic system.
% Input:
%   X: d x n data matrix
%   k: dimension of hidden variable
%   m: stacking order for the Hankel matrix
% Output:
%   A: k x k transition matrix
%   C: k x d emission matrix
%   Z: k x n latent variable
%   Y: d x n reconstructed data
% reference: Bayesian Reasoning and Machine Learning (BRML) chapter 24.5.3 p.507
% Written by Mo Chen (sth4nth@gmail.com).
[d,n] = size(X);
H = reshape(X(:,hankel(1:m,m:n)),d*m,[]);
[U,S,V] = svd(H,'econ');
C = U(1:d,1:k);
Z = S(1:k,1:k)*V(:,1:k)';
A = Z(:,2:end)/Z(:,1:end-1); % estimated transition
% Y = C*Z; % reconstructions


function varargout = slogfrac(varargin)
%logfrac
%
% y = SLOGFRAC(x)
%
% Computes/declares log(1 + x(1)/x(2))

switch class(varargin{1})
    case 'double'
        x = varargin{1};
        % Safe version with defined negative values (helps fmincon when
        % outside feasible region)
        if all(x==0)
            varargout{1} = log(2);% ?definition...
        elseif x(1)==0
            varargout{1} = log(1);
        else
            aux = 1 + (x(1)./(x(2)+sqrt(eps)));
            if aux <= 0
                varargout{1} = -15;
            else
                varargout{1} = log(1 + (x(1)./(x(2)+sqrt(eps))));
            end

        end

    case 'sdpvar'
        if min(size(varargin{1}))>1
            error('SLOGFRAC only defined for vector arguments');
        else
            varargout{1} = yalmip('define',mfilename,varargin{1});
        end

    case 'char'

        operator = CreateBasicOperator('callback');
        operator.range = [-inf inf];
        operator.domain = [-inf inf];
        operator.bounds = @bounds;
        operator.derivative = @(x) ([1./(x(1)+x(2)+sqrt(eps));1./(x(1)+x(2)+sqrt(eps))-(x(2)+sqrt(eps)).^-1]);

        varargout{1} = [];
        varargout{2} = operator;
        varargout{3} = varargin{3};

    otherwise
        error(['SDPVAR/' upper(mfilename) ' called with weird argument']);
end

function [L,U] = bounds(xL,xU)

% Derive bounds on x1/x2
z = [xL(1)./xL(2) xU(1)./xL(2) xL(1)./xU(2) xU(1)./xU(2)];
fL = min(z);
fU = max(z);

if fL <= -1
    L = -inf;
else
    L = log(1 + fL);
end
if fU <= -1
    U = -inf;
else
    U = log(1 + fU);
end


function [kf] = ext_kalman_filter(F, G, H, u, Q, R, time, last_measure, kf, P)

%EXT_KALMAN_FILTER - This is the extended Kalman filter.
%
% Synthax:
%
% Inputs:
%   uu          - state variables, delta, wind
%   kf.x_prev   - previous a posteriori state estimate
%   kf.P_prev   - previous a posteriori estimate covariance
%   last_measure- (time, sensor measure)
%   P           - parameters
%
% Outputs:
%   kf.x        - a priori/a posteriori state estimate
%   kf.P        - a priori/a posteriori estimate covariance
%   kf.z_res    - post-fit residual

% A -> F
% B -> G
% C -> H

% Q - covariance of the zero-mean Gaussian noise on x_dot
% R - covariance of the zero-mean Gaussian noise on y

time_m = last_measure(:,1);
z_m    = last_measure(:,2);

% prediction (a priori)
kf.x = F * kf.x_prev    + G * u;    % a priori state estimate
kf.P = F * kf.P_prev * (F') + Q;    % a priori estimate covariance

% update (a posteriori)
if time - time_m < P.dt
    kf.z_tilde = z_m - H*kf.x;      % innovation (a priori residual)
    S = R + H*kf.P*(H');            % innovation covariance
    K = kf.P*(H')/S;                 % optimal kalman gain
    kf.x = kf.x + K*kf.z_tilde;     % a posteriori state estimate
    kf.P = kf.P - K*S*(K');         % a posteriori estimate covariance
end

kf.z_res = z_m - H*kf.x;            % a posteriori residual

end


function ellipsoid_monte_carlo_test01 ( )

%*****************************************************************************80
%
%% ELLIPSOID_MONTE_CARLO_TEST01 uses ELLIPSOID_SAMPLE on a 2D ellipse centered at (0,0).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 August 2014
%
%  Author:
%
%    John Burkardt
%
  m = 2;

  a = [ ...
    9.0, 1.0;
    1.0, 4.0 ]';
  e_test = [ ...
    0, 0;
    1, 0;
    0, 1;
    2, 0;
    1, 1;
    0, 2;
    3, 0 ]';
  r = 2.0;
  v = [ 0.0, 0.0 ]';

  fprintf ( 1, '\n' );
  fprintf ( 1, 'ELLIPSOID_MONTE_CARLO_TEST01\n' );
  fprintf ( 1, '  Use ELLIPSOID_SAMPLE to estimate integrals\n' );
  fprintf ( 1, '  in a 2D ellipse x'' * A * x <= r^2.\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Ellipsoid radius R = %g\n', r );
  r8vec_print ( m, v, '  Ellipsoid center V:' );
  r8mat_print ( m, m, a, '  Ellipsoid matrix A:' );

  volume = ellipsoid_volume ( m, a, v, r );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Ellipsoid volume = %g\n', volume );

  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, '         N        1              X               Y  ' );
  fprintf ( 1, '             X^2               XY             Y^2             X^3\n' );
  fprintf ( 1, '\n' );

  n = 1;

  while ( n <= 65536 )

    [ x, seed ] = ellipsoid_sample ( m, n, a, v, r, seed );

    fprintf ( 1, '  %8d', n );

    for j = 1 : 7

      e(1:m) = e_test(1:m,j);

      value = monomial_value ( m, n, e, x );

      result = volume * sum ( value(1:n) ) / n;

      fprintf ( 1, '  %14.6g', result );

    end

    fprintf ( 1, '\n' );

    n = 2 * n;

  end

  return
end


%% Author: epokh
%% Website: www.epokh.org/drupy
%% This software is under GPL

function [Mdh]=DHmatrix(theta,d,a,alpha)
%%output: DH matrix
%%input: DH parameters:
%%theta: rotation along x(n) axis
%%d: traslation along z(n) axis
%%a: translation along x(n+1) axis
%%alpha: rotation along x(n+1) axis
%%all angles expressed in degrees

Mdh=[cosd(theta) -sind(theta)*cosd(alpha) sind(theta)*sind(alpha) a*cosd(theta);
     sind(theta) cosd(theta)*cosd(alpha)  -cosd(theta)*sind(alpha) a*sind(theta);
     0,sind(alpha),cosd(alpha),d;
     0,0,0,1];

end


function disk_monte_carlo_test01 ( )

%*****************************************************************************80
%
%% DISK_MONTE_CARLO_TEST01 uses DISK01_SAMPLE with an increasing number of points.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 January 2014
%
%  Author:
%
%    John Burkardt
%
  e_test = [ ...
    0, 0; ...
    2, 0; ...
    0, 2; ...
    4, 0; ...
    2, 2; ...
    0, 4; ...
    6, 0 ]';

  fprintf ( 1, '\n' );
  fprintf ( 1, 'DISK_MONTE_CARLO_TEST01\n' );
  fprintf ( 1, '  Use DISK01_SAMPLE to estimate integrals in the unit disk.\n' );

  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, '         N        1              X^2             Y^2        ' );
  fprintf ( 1, '     X^4             X^2Y^2           Y^4             X^6\n' );
  fprintf ( 1, '\n' );

  n = 1;

  while ( n <= 65536 )

    [ x, seed ] = disk01_sample ( n, seed );

    fprintf ( 1, '  %8d', n );
    for j = 1 : 7

      e(1:2) = e_test(1:2,j);

      value = monomial_value ( 2, n, e, x );

      result(j) = disk01_area ( ) * sum ( value(1:n) ) / n;
      fprintf ( 1, '  %14.6g', result(j) );
    end
    fprintf ( 1, '\n' );

    n = 2 * n;

  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '     Exact' );
  for j = 1 : 7

    e(1:2) = e_test(1:2,j);

    result(j) = disk01_monomial_integral ( e );
    fprintf ( 1, '  %14.6g', result(j) );
  end

  fprintf ( 1, '\n' );

  return
end


function [yi, ypi] = hermint(x, y, yp, xi, c)

% HERMINT 1-D piecewise hermite interpolation
%    HERMINT(X,Y,YP,XI,C) interpolates to find YI, the values of
%    the underlying function Y at the points in the array XI,
%    using piecewise hermite interpolation.  X and Y must be
%    vectors of length N.
%
%    C specifies the number of data points to use in the
%    interpolation.  The default is to use all points.
%
%    [YI,YPI] = HERMINT() also returns the interpolated derivative
%    of the underlying function Y at points XI.

% Joe Henning - Fall 2011

if (nargin < 5)
   c = 0;
end

n = length(x);

if (n < c)
   fprintf('??? Bad c input to hermint ==> c <= length(x)\n');
   yi = [];
   ypi = [];
   return
end

for i = 1:length(xi)
   % Find the right place in the table by means of a bisection.
   klo = 1;
   khi = n;
   while (khi-klo > 1)
      k = fix((khi+klo)/2.0);
      if (x(k) > xi(i))
         khi = k;
      else
         klo = k;
      end
   end

   h = x(khi) - x(klo);
   if (h == 0.0)
      fprintf('??? Bad x input to hermint ==> x values must be distinct\n');
      yi(i) = NaN;
      ypi(i) = NaN;
      continue;
   end

   isiny = 0;
   for k = 1:n
      if (xi(i) == x(k))
         yi(i) = y(k);
         ypi(i) = yp(k);
         isiny = 1;
         break
      end
   end

   if (isiny)
      continue
   end

   % Evaluate hermite polynomial
   yi(i) = 0;
   ypi(i) = 0;
   if (c == 0)
      for k = 1:n
         f = 0;
         g = 0;
         h = 1;

         for m = 1:n
            if (k ~= m)
               f = f + 1/(x(k)-x(m));
               h = h*(xi(i)-x(m))/(x(k)-x(m));
            end
         end

         for m = 1:n
            if (k ~= m)
               g = g + h/(xi(i)-x(m));
            end
         end

         a = h*h;
         b = 2*h*g;
         u = xi(i) - x(k);

         b1 = a*u;
         b2 = b*u + a;
         a1 = a - 2*f*b1;
         a2 = b - 2*f*b2;

         yi(i) = yi(i) + a1*y(k) + b1*yp(k);
         ypi(i) = ypi(i) + a2*y(k) + b2*yp(k);
      end
   else
      if (mod(c,2) == 0)   % even
         c2 = c/2;
         if (klo < c2)
            klo = c2;
         end
         if (klo > n-c2)
            klo = n-c2;
         end
         khi = klo + 1;
         for k = klo-(c2-1):klo+c2
            f = 0;
            g = 0;
            h = 1;

            for m = klo-(c2-1):klo+c2
               if (k ~= m)
                  f = f + 1/(x(k)-x(m));
                  h = h*(xi(i)-x(m))/(x(k)-x(m));
               end
            end

            for m = klo-(c2-1):klo+c2
               if (k ~= m)
                  g = g + h/(xi(i)-x(m));
               end
            end

            a = h*h;
            b = 2*h*g;
            u = xi(i) - x(k);

            b1 = a*u;
            b2 = b*u + a;
            a1 = a - 2*f*b1;
            a2 = b - 2*f*b2;

            yi(i) = yi(i) + a1*y(k) + b1*yp(k);
            ypi(i) = ypi(i) + a2*y(k) + b2*yp(k);
         end
      else   % odd
         c2 = floor(c/2);
         if (klo < c2+1)
            klo = c2+1;
         end
         if (klo > n-c2)
            klo = n-c2;
         end
         khi = klo + 1;
         for k = klo-c2:klo+c2
            f = 0;
            g = 0;
            h = 1;

            for m = klo-c2:klo+c2
               if (k ~= m)
                  f = f + 1/(x(k)-x(m));
                  h = h*(xi(i)-x(m))/(x(k)-x(m));
               end
            end

            for m = klo-c2:klo+c2
               if (k ~= m)
                  g = g + h/(xi(i)-x(m));
               end
            end

            a = h*h;
            b = 2*h*g;
            u = xi(i) - x(k);

            b1 = a*u;
            b2 = b*u + a;
            a1 = a - 2*f*b1;
            a2 = b - 2*f*b2;

            yi(i) = yi(i) + a1*y(k) + b1*yp(k);
            ypi(i) = ypi(i) + a2*y(k) + b2*yp(k);
         end
      end
   end
end


function [evects,evals] = pca(X)
% [evects,evals] = pca(X)
%
% finds principal components of
%
% input:
%  X  dxn matrix (each column is a dx1 input vector)
%
% output:
% evects  columns are principal components (leading from left->right)
% evals   corresponding eigenvalues
%
% See also applypca
%
% copyright by Kilian Q. Weinberger, 2006

[d,N]  = size(X);

mm = mean(X,2);
X = X - mm*ones(1,N); % remove mean from data

cc = cov(X',1); % compute covariance
[cvv,cdd] = eig(cc); % compute eignvectors
[zz,ii] = sort(diag(-cdd)); % sort according to eigenvalues
evects = cvv(:,ii); % pick leading eigenvectors
cdd = diag(cdd); % extract eigenvalues
evals = cdd(ii); % sort eigenvalues


function b = r8blt_vxm ( n, ml, a, x )

%*****************************************************************************80
%
%% R8BLT_VXM multiplies a vector by a R8BLT matrix.
%
%  Discussion:
%
%    The R8BLT storage format is appropriate for a banded lower triangular matrix.
%    The matrix is assumed to be zero below the ML-th subdiagonal.
%    The matrix is stored in an ML+1 by N array, in which the diagonal
%    appears in the first row, followed by successive subdiagonals.
%    Columns are preserved.
%
%  Example:
%
%    N = 5, ML = 2
%
%    A11   0   0   0   0
%    A21 A22   0   0   0
%    A31 A32 A33   0   0
%      0 A42 A43 A44   0
%      0   0 A53 A54 A55
%                --- ---
%                    ---
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 March 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, integer ML, the lower bandwidth.
%
%    Input, real A(ML+1,N), the R8BLT matrix.
%
%    Input, real X(N), the vector to be multiplied by A.
%
%    Output, real B(N), the product X*A.
%
  b(1:n) = 0.0;

  for i = 1 : n
    jlo = max ( 1, i - ml );
    jhi = i;
    for j = jlo : jhi
      b(j) = b(j) + x(i) * a(i-j+1,j);
    end
  end

  return
end


function [ o, x, w ] = en_r2_03_2 ( n )

%*****************************************************************************80
%
%% EN_R2_03_2 implements the Stroud rule 3.2 for region EN_R2.
%
%  Discussion:
%
%    The rule has order O = 2^N.
%
%    The rule has precision P = 3.
%
%    EN_R2 is the entire N-dimensional space with weight function
%
%      w(x) = exp ( - x1^2 - x2^2 ... - xn^2 )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 January 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Arthur Stroud,
%    Approximate Calculation of Multiple Integrals,
%    Prentice Hall, 1971,
%    ISBN: 0130438936,
%    LC: QA311.S85.
%
%  Parameters:
%
%    Input, integer N, the spatial dimension.
%
%    Output, integer O, the order.
%
%    Output, real X(N,O), the abscissas.
%
%    Output, real W(O), the weights.
%
  o = 2^n;
  volume = sqrt ( pi^n );

  a = volume / o;
  r = sqrt ( 1 / 2 );

  x = zeros ( n, o );
  w = zeros ( o, 1 );

  k = 0;
%
%  2^N points.
%
  k = k + 1;
  x(1:n,k) = - r;
  w(k) = a;
  more = 1;

  while ( more )
    more = 0;
    for i = n : -1 : 1
      if ( x(i,k) < 0.0 )
        k = k + 1;
        x(1:n,k) = x(1:n,k-1);
        x(i,k)     = + r;
        x(i+1:n,k) = - r;
        w(k) = a;
        more = 1;
        break;
      end
    end
  end

  return
end


function [sMin,sMax,s0]=projEllipseOntoLine(z,A,gammaVal,x0,v)
%%PROJELLIPSEONTOLINE Determine the orthogonal projection of an ellipse or
%                     ellipsoid onto a line.
%
%INPUTS: z The numDimX1 center of the ellipsoid.
%        A A numDimXnumDim symmetric, positive definite matrix that
%          specifies the size and shape of the ellipse or ellipsoid, where
%          a point zp is on the ellipse/ellipsoid if
%          (zp-z)'*A*(zp-z)=gammaVal.
% gammaVal The threshold for declaring a point to be in the ellipsoid. If
%          an empty matrix is passed, the default value of 1 is used.
%     x0,v Two numDimX1 vectors that specify the line. A point y is on the
%          line if y=x0+s*v, where s is a scalar quantity.
%
%OUTPUTS: sMin,sMax The range of values of s on the line y=x0+s*v onto
%                   which the ellipsoid is orthogonally projected. These
%                   are scalar quantities.
%
%The mathematics for projecting an ellipsoid onto a line are described in
%Section 12 of [1].
%
%EXAMPLE:
% A=[1,0;
%    0,10];
% M=[0.413074198133900,  0.910697373904216;
%    -0.910697373904216,   0.413074198133900];%A rotation matrix
% A=M*A*M';
% z=[5;6];
% x0=[0;0];
% v=[1;2];
% [sMin,sMax,s0]=projEllipseOntoLine(z,A,[],x0,v);
% figure(1)
% clf
% hold on
% axis([-0.5, 7.5, 2, 10])%Same size in x and y
% axis square
% drawEllipse(z,A,1,'b','linewidth',2)
% %Draw a segment of the line.
% xStart=x0+v;
% xEnd=x0+5*v;
% plot([xStart(1);xEnd(1)],[xStart(2);xEnd(2)],'-k','linewidth',4)
% %Draw the segment of the orthogonal projection onto the line.
% xStart=x0+sMin*v;
% xEnd=x0+sMax*v;
% plot([xStart(1);xEnd(1)],[xStart(2);xEnd(2)],'-r','linewidth',2)
% %Draw the line from the center of the ellipse to the place on the line onto
% %which it is orthogonally projected.
% xS0=x0+s0*v;
% plot([z(1);xS0(1)],[z(2);xS0(2)],'--c')
%
%REFERENCES:
%[1] S. B. Pope, "Algorithms for ellipsoids," Cornell University, Tech.
%    Rep. FDA-08-01, Feb. 2008. [Online].
%    Available: https://tcg.mae.cornell.edu/pubs/Pope_FDA_08.pdf
%
%May 2016 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(isempty(gammaVal))
   gammaVal=1;
end

A=A/gammaVal;

L=chol(A,'lower');

s0=v'*(z-x0)/(v'*v);
w=norm(L\v/(v'*v));

sMin=s0+w;
sMax=s0-w;
end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function [MBox] = MBoxtest(X,alpha)
% Multivariate Statistical Testing for the Homogeneity of Covariance Matrices by the Box's M.
%
%   Syntax: function [MBox] = MBoxtest(X,alpha)
%
%     Inputs:
%          X - data matrix (Size of matrix must be n-by-(1+p); sample=column 1, variables=column 2:p).
%      alpha - significance level (default = 0.05).
%     Output:
%          MBox - the Box's M statistic.
%          Chi-sqr. or F - the approximation statistic test.
%          df's - degrees' of freedom of the approximation statistic test.
%          P - observed significance level.
%
%    If the groups sample-size is at least 20 (sufficiently large), Box's M test
%    takes a Chi-square approximation; otherwise it takes an F approximation.
%
%    Example: For a two groups (g = 2) with three independent variables (p = 3), we
%             are interested to test the homogeneity of covariances matrices with a
%             significance level = 0.05. The two groups have the same sample-size
%             n1 = n2 = 5.
%                                       Group
%                      ---------------------------------------
%                            1                        2
%                      ---------------------------------------
%                       x1   x2   x3             x1   x2   x3
%                      ---------------------------------------
%                       23   45   15             277  230   63
%                       40   85   18             153   80   29
%                      215  307   60             306  440  105
%                      110  110   50             252  350  175
%                       65  105   24             143  205   42
%                      ---------------------------------------
%
%             Total data matrix must be:
%          X=[1 23 45 15;1 40 85 18;1 215 307 60;1 110 110 50;1 65 105 24;
%          2 277 230 63;2 153 80 29;2 306 440 105;2 252 350 175;2 143 205 42];
%
%             Calling on Matlab the function:
%                MBoxtest(X,0.05)
%
%             Answer is:
%
%  ------------------------------------------------------------
%       MBox         F           df1          df2          P
%  ------------------------------------------------------------
%     27.1622     2.6293          6           463       0.0162
%  ------------------------------------------------------------
%  Covariance matrices are significantly different.
%

%  Created by A. Trujillo-Ortiz and R. Hernandez-Walls
%             Facultad de Ciencias Marinas
%             Universidad Autonoma de Baja California
%             Apdo. Postal 453
%             Ensenada, Baja California
%             Mexico.
%             atrujo@uabc.mx
%             And the special collaboration of the post-graduate students of the 2002:2
%             Multivariate Statistics Course: Karel Castro-Morales, Alejandro Espinoza-Tenorio,
%             Andrea Guia-Ramirez, Raquel Muniz-Salazar, Jose Luis Sanchez-Osorio and
%             Roberto Carmona-Pina.
%  November 2002.
%
%  To cite this file, this would be an appropriate format:
%  Trujillo-Ortiz, A., R. Hernandez-Walls, K. Castro-Morales, A. Espinoza-Tenorio, A. Guia-Ramirez
%    and R. Carmona-Pina. (2002). MBoxtest: Multivariate Statistical Testing for the Homogeneity of
%    Covariance Matrices by the Box's M. A MATLAB file. [WWW document]. URL http://www.mathworks.com/
%    matlabcentral/fileexchange/loadFile.do?objectId=2733&objectType=FILE
%
%  References:
%
%  Stevens, J. (1992), Applied Multivariate Statistics for Social Sciences. 2nd. ed.
%              New-Jersey:Lawrance Erlbaum Associates Publishers. pp. 260-269.

if nargin < 1,
    error('Requires at least one input arguments.');
end;

if nargin < 2,
    alpha = 0.05; %(default)
end;

if (alpha <= 0 | alpha >= 1)
   fprintf('Warning: significance level must be between 0 and 1\n');
   return;
end;

g = max(X(:,1)); %Number of groups.

n = []; %Vector of groups-size.
indice = X(:,1);
for i = 1:g
   Xe = find(indice==i);
   eval(['X' num2str(i) '= X(Xe,2);']);
   eval(['n' num2str(i) '= length(X' num2str(i) ') ;'])
   eval(['xn= n' num2str(i) ';'])
   n = [n,xn];
end;

[f,c] = size(X);
X = X(:,2:c);

[N,p]=size(X);
r=1;
r1=n(1);
bandera=2;
for k=1:g
   if n(k)>=20;
      bandera=1;
   end
end
%Partition of the group covariance matrices.
for k=1:g
   eval(['S' num2str(k) '=cov(X(r:r1,:));';]);
   if k<g
    r=r+n(k);
    r1=r1+n(k+1);
   end
end

deno=sum(n)-g;
suma=zeros(size(S1));

for k=1:g
   eval(['suma =suma + (n(k)-1)*S' num2str(k) ';']);
end

Sp=suma/deno;  %Pooled covariance matrix.
Falta=0;

for k=1:g
   eval(['Falta =Falta + ((n(k)-1)*log(det(S' num2str(k) ')));']);
end

MB=(sum(n)-g)*log(det(Sp))-Falta;  %Box's M statistic.
suma1=sum(1./(n(1:g)-1));
suma2=sum(1./((n(1:g)-1).^2));
C=(((2*p^2)+(3*p)-1)/(6*(p+1)*(g-1)))*(suma1-(1/deno));  %Computing of correction factor.
if bandera==1
X2=MB*(1-C);  %Chi-square approximation.
v=(p*(p+1)*(g-1))/2;  %Degrees of freedom.
P=1-chi2cdf(X2,v);  %Significance value associated to the observed Chi-square statistic.
disp(' ')
;
fprintf('------------------------------------------------\n');
disp('     MBox     Chi-sqr.         df          P')
fprintf('------------------------------------------------\n');
fprintf('%10.4f%11.4f%12.i%13.4f\n',MB,X2,v,P);
fprintf('------------------------------------------------\n');
   if P >= alpha
      disp('Covariance matrices are not significantly different.');
   else
      disp('Covariance matrices are significantly different.');
   end
else
%To obtain the F approximation we first define Co, which combined to the before C value
%are used to estimate the denominator degrees of freedom (v2); resulting two possible cases.
Co=(((p-1)*(p+2))/(6*(g-1)))*(suma2-(1/(deno^2)));
  if Co-(C^2)>= 0;
	v1=(p*(p+1)*(g-1))/2;  %Numerator degrees of freedom.
   v21=fix((v1+2)/(Co-(C^2)));  %Denominator degrees of freedom.
   F1=MB*((1-C-(v1/v21))/v1);  %F approximation.
   P1=1-fcdf(F1,v1,v21);  %Significance value associated to the observed F statistic.
disp(' ')
  ;
fprintf('------------------------------------------------------------\n');
disp('     MBox         F           df1          df2          P')
fprintf('------------------------------------------------------------\n');
fprintf('%10.4f%11.4f%11.i%14.i%13.4f\n',MB,F1,v1,v21,P1);
fprintf('------------------------------------------------------------\n');
   if P1 >= alpha
      disp('Covariance matrices are not significantly different.');
   else
      disp('Covariance matrices are significantly different.');
   end

 else
   v1=(p*(p+1)*(g-1))/2;  %Numerator degrees of freedom.
   v22=fix((v1+2)/((C^2)-Co));  %Denominator degrees of freedom.
   b=v22/(1-C-(2/v22));
   F2=(v22*MB)/(v1*(b-MB));  %F approximation.
   P2=1-fcdf(F2,v1,v22);  %Significance value associated to the observed F statistic.
disp(' ')
  ;
fprintf('------------------------------------------------------------\n');
disp('     MBox         F           df1          df2          P')
fprintf('------------------------------------------------------------\n');
fprintf('%10.4f%11.4f%11.i%14.i%13.4f\n',MB,F2,v1,v22,P2);
fprintf('------------------------------------------------------------\n');

   if P2 >= alpha
      disp('Covariance matrices are not significantly different.');
   else
      disp('Covariance matrices are significantly different.');
   end
 end
end


%................................................................

% MATLAB codes for Finite Element Analysis
% problem16vibrations.m
% Timoshenko beam in free vibrations
% AJM Ferreira 2008
% modified by Manuel Diaz, 2013.07.25

% clear memory
clear all

% E; modulus of elasticity
% G; shear modulus
% I: second moments of area
% L: length of beam
% thickness: thickness of beam
<<<<<<< HEAD
E=30e6; poisson = 0.30;L  = 1;thickness=0.01;
=======
E=10e7; poisson = 0.30;L = 3;thickness=0.001;
>>>>>>> 47f4967135b009f9128c106a6744c97a2ffd18ce
I=thickness^3/12;
EI=E*I;
kapa=5/6;
rho=1;
A=1*thickness;
%

P = 0; % uniform pressure
%P = -1; % uniform pressure
% constitutive matrix
G=E/2/(1+poisson);
C=[   EI   0; 0    kapa*A*G];

% mesh
<<<<<<< HEAD
numberElements     = 50;
=======
numberElements  = 40;
>>>>>>> 47f4967135b009f9128c106a6744c97a2ffd18ce
nodeCoordinates=linspace(0,L,numberElements+1);
xx=nodeCoordinates';x=xx';
for i=1:size(nodeCoordinates,2)-1
    elementNodes(i,1)=i;
    elementNodes(i,2)=i+1
end
% generation of coordinates and connectivities
numberNodes=size(xx,1);

% GDof: global number of degrees of freedom
GDof=2*numberNodes;

% computation of the system stiffness, force, mass
[stiffness,force,mass]=...
    formStiffnessMassTimoshenkoBeam(GDof,numberElements,...
    elementNodes,numberNodes,xx,C,P,rho,I,thickness);

% boundary conditions (simply-supported at both bords)
%fixedNodeW =[1 ; numberNodes];
%fixedNodeTX=[];
% boundary conditions (clamped at both bords)
%fixedNodeW =[1 ; numberNodes];
%fixedNodeTX=[1 ; numberNodes];
% boundary conditions (cantilever)
%fixedNodeW =[1];
%fixedNodeTX=[1];
<<<<<<< HEAD
% Free-free
%fixedNodeW =[];
%fixedNodeTX=[];
=======
% Free-Free conditions
fixedNodeW =[];
fixedNodeTX=[];
>>>>>>> 47f4967135b009f9128c106a6744c97a2ffd18ce
prescribedDof=[fixedNodeW; fixedNodeTX+numberNodes];

% solution (optional*)
displacements=solution(GDof,prescribedDof,stiffness,force);

% output displacements/reactions (optional*)
outputDisplacementsReactions(displacements,stiffness,...
    GDof,prescribedDof)

% free vibration problem
activeDof=setdiff([1:GDof]',[prescribedDof]);
modeNumber=4;

[V,D]=eig(stiffness(activeDof,activeDof),...
    mass(activeDof,activeDof));
D = diag(sqrt(D)*L*L*sqrt(rho*A/E/I));
[D,ii] = sort(D);

V1=zeros(GDof,1);
V1(activeDof,1:modeNumber)=V(:,1:modeNumber);
!
% drawing eigenmodes
drawEigenmodes1D(modeNumber,numberNodes,V1,xx,x)


%  Figure 4.16      Feedback Control of Dynamic Systems, 6e
%                   Franklin, Powell, Emami
%% script to generate Figure 4.16(a), 4.16 (b)
% case I, P control by reaction curve data.
% First, input the plant model and compute the reaction curve.
sysp=tf(1,[600 70 1])
set(sysp,'td', 5)

% From the curve, kp = 6.92 for proportional control
sysdP = tf(6.92,1)
% To get the closed loop response, we must use a pade approximation to the
% delay
syspade=pade(sysp,3);
sysforward=syspade*sysdP;
sysback=tf(1,1)
syscl=feedback(sysforward,sysback)
figure(1)
clf
step(syscl)
title('Figure 4.16a Closed loop response using Reaction Curve data')
gtext('P')
hold on
%case II, PI control
%kp = 6.22, TI=43.3
sysdPI=6.22*tf([43.3 1],[43.3 0])
sysforward=syspade*sysdPI;
syscl=feedback(sysforward,sysback)
step(syscl)
gtext('PI')
nicegrid
%
sysdP = tf(6.92/3,1)
% To get the closed loop response, we must use a pade approximation to the
% delay
syspade=pade(sysp,3);
sysforward=syspade*sysdP;
sysback=tf(1,1)
syscl=feedback(sysforward,sysback)
figure(2)
clf
step(syscl)
title('Figure 4.16b Closed loop response using Reaction Curve data')
gtext('P')
hold on
%case II, PI control
%kp = 6.22, TI=43.3
sysdPI=(6.22/3)*tf([43.3 1],[43.3 0])
sysforward=syspade*sysdPI;
syscl=feedback(sysforward,sysback)
step(syscl)
gtext('PI')
nicegrid;


function [logml log10ml ml]=mmlikdata(X,y,n,T,q,sigma,beta0,omega0,betabar)

% function [logml log10ml ml]=bear.mmlik(X,y,n,T,q,sigma,omega0,beta0,betabar)
% computes the marginal likelihood for a Minesota prior, by implementing algorithm XXX
% inputs:  - matrix 'X': matrix of regressors for the VAR model (defined in 1.1.8)
%          - vector 'y': vectorised regressands for the VAR model (defined in 1.1.12)
%          - integer 'n': number of endogenous variables in the BVAR model (defined p 7 of technical guide)
%          - integer 'T': number of sample time periods (defined p 7 of technical guide)
%          - integer 'q': total number of coefficients to estimate for the BVAR model (defined p 7 of technical guide)
%          - matrix 'sigma': 'true' variance-covariance matrix of VAR residuals, for the original Minnesota prior
%          - vector 'beta0': vector of prior values for beta (defined in 1.3.4)
%          - matrix 'omega0': prior covariance matrix for the VAR coefficients (defined in 1.3.8)
%          - vector 'betabar': posterior mean vector (defined in 1.3.18)
% outputs: - scalar 'logml': base e log of the marginal likelihood (defined in 1.2.9)
%          - scalar 'log10ml': base 10 log of the marginal likelihood (defined in 1.2.9)
%          - scalar 'ml': marginal likelihood (defined in 1.2.9)

% compute the constant part of the marginal likelihood
temp1=(-n*T/2)*log(2*pi)+(-T/2)*log(det(sigma));

% compute the log determinant part
% create the square root matrix of omega0
% because omega0 is diagonal, this is simply the square root of the diagonal terms of omega0
Fomega=spdiags(diag(omega0).^0.5,0,q,q);
% compute the inverse of sigma
C=chol(bear.nspd(sigma));
invC=C\speye(n);
invsigma=invC*invC';
% compute the product
product=Fomega'*kron(invsigma,X'*X)*Fomega;
% compute the eigenvalues of the product
if n == 1
    eigenvalues=eig(full(product));
else
    eigenvalues=eig(product);
end
% now compute the full determinant term
temp2=(-1/2)*log(prod(diag(eye(q)+diag(eigenvalues))));

% compute the final term
% first compute the inverse of omega0, which is a diagonal matrix (hence simply invert element wise the diagonal terms)
invomega0=spdiags(1./diag(omega0),0,q,q);
% compute the inverse of omegabar
invomegabar=invomega0+kron(invsigma,X'*X);
% now compute the whole matrix sum
summ=beta0'*invomega0*beta0-betabar'*invomegabar*betabar+y'*kron(invsigma,speye(T))*y;
% finally, compute the whole exponential term
temp3=-0.5*summ;

% compute the marginal likelihood
logml=real(temp1+temp2+temp3);
log10ml=logml/log(10);
ml=exp(logml);


function [ Ix, Iy, Iz, It ] = imageDerivatives3D( image1, image2 )
%This fucntion computes 3D derivatives between two 3D images.
%
%   Description :
%
%   There are four derivatives here; three along X, Y, Z axes and one along
%   timeline axis.
%
%   -image1, image2 :   two subsequent images or frames
%   -dx, dy, dz : vectors along X, Y and Z axes respectively
%   -dt : vectors along timeline axis
%   -Ix, Iy, Iz : derivatives along X, Y and Z axes respectively
%   -It : derivatives along timeline axis
%
%   Author : Mohammad Mustafa
%   By courtesy of The University of Nottingham and
%   Mirada Medical Limited, Oxford, UK
%
%   Published under a Creative Commons Attribution-Non-Commercial-Share Alike
%   3.0 Unported Licence http://creativecommons.org/licenses/by-nc-sa/3.0/
%
%   June 2012

dx=zeros(2,2,2);
dx(:,:,1)=[-1 1; -1 1 ]; dx(:,:,2)=[-1 1; -1 1 ];
dx=0.25*dx;

dy=zeros(2,2,2);
dy(:,:,1)=[-1 -1; 1 1 ]; dy(:,:,2)=[-1 -1; 1 1 ];
dy=0.25*dy;

dz=zeros(2,2,2);
dz(:,:,1)=[-1 -1; -1 -1 ]; dz(:,:,2)=[1 1; 1 1 ];
dz=0.25*dz;

dt=ones(2,2,2);
dt=0.25*dt;

% Computing derivatives
Ix = 0.5 * (convn(image1,dx) + convn(image2,dx) );
Iy = 0.5 * (convn(image1,dy) + convn(image2,dy) );
Iz = 0.5 * (convn(image1,dz) + convn(image2,dz) );
It = 0.5 * (convn(image1,dt) - convn(image2,dt) );

% Adjusting sizes
Ix=Ix(1:size(Ix,1)-1, 1:size(Ix,2)-1, 1:size(Ix,3)-1);
Iy=Iy(1:size(Iy,1)-1, 1:size(Iy,2)-1, 1:size(Iy,3)-1);
Iz=Iz(1:size(Iz,1)-1, 1:size(Iz,2)-1, 1:size(Iz,3)-1);
It=It(1:size(It,1)-1, 1:size(It,2)-1, 1:size(It,3)-1);

end


function gray = rgb_to_gray ( rgb, equal )

%*****************************************************************************80
%
%% RGB_TO_GRAY returns a grayscale version of an RGB image.
%
%  Discussion:
%
%    An RGB image is an (M,N,3) array.
%
%    A grayscale image is an (M,N) array.
%
%    A grayscale version of an RGB image can be made by averaging
%    the RGB components of each pixel.
%
%    A more sophisticated approach uses the luminance function:
%
%      GRAY = 0.2126 * R + 0.7152 * G + 0.0722 * B
%
%    which attempts to better model the contributions to brightness
%    of the different colors.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 February 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, uint8 RGB(M,N,3), the original data.
%
%    Input, logical EQUAL, is TRUE (1) if the R, G and B channels
%    are to be assigned equal weight.  EQUAL is FALSE by default.
%
%    Output, uint8 GRAY(M,N), the grayscale version of the data.
%
  if ( nargin < 1 )
    error ( 'RGB_TO_GRAY - Fatal error! Missing RGB input argument.' )
  end

  if ( nargin < 2 )
    equal = 0;
  end

  if ( equal )
    v = [ 1, 1, 1 ]' / 3;
  else
    v = [ 0.2126, 0.7152, 0.0722 ]';
  end
%
%  This is really a matrix multiply, but the obvious equation
%    gray = rgb * v
%  is illegal according to MATLAB.
%
  gray = uint8 ( double ( rgb(:,:,1) ) * v(1) ...
               + double ( rgb(:,:,2) ) * v(2) ...
               + double ( rgb(:,:,3) ) * v(3) );

  return
end


function yp = p15_fun ( neqn, t, y )

%*****************************************************************************80
%
%% P15_FUN evaluates the function for problem P15.
%
%  Discussion:
%
%    30 equations.
%
%    This system models the motion of the five outer planets of the
%    solar system.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 February 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Wayne Enright, John Pryce,
%    Algorithm 648,
%    ACM Transactions on Mathematical Software,
%    Volume 13, Number 1, pages 28-34.
%
%  Parameters:
%
%    Input, integer NEQN, the number of equations.
%
%    Input, real T, Y(NEQN), the arguments of the derivative
%    function.
%
%    Output, real YP(NEQN), the value of the derivative function.
%
  yp = zeros ( neqn, 1 );

  m = zeros ( 5, 1 );
  k2 = p15_param ( 'GET', 'K2', [] );
  m0 = p15_param ( 'GET', 'M0', [] );
  m(1) = p15_param ( 'GET', 'M1', [] );
  m(2) = p15_param ( 'GET', 'M2', [] );
  m(3) = p15_param ( 'GET', 'M3', [] );
  m(4) = p15_param ( 'GET', 'M4', [] );
  m(5) = p15_param ( 'GET', 'M5', [] );
  i = 0;
  for l = 3 : 3 : 15
    i = i + 1;
    p = y(l-2).^2 + y(l-1).^2 + y(l).^2;
    r(i) = 1.0 / ( p * sqrt ( p ) );
    j = 0;
    for ll = 3 : 3 : 15
      j = j + 1;
      if ( ll ~= l )
        p = ( y(l-2) - y(ll-2) ).^2 + ( y(l-1) - y(ll-1) ).^2 ...
          + ( y(l) - y(ll) ).^2;
        q(i,j) = 1.0 / ( p * sqrt ( p ) );
        q(j,i) = q(i,j);
      end
    end
  end
  i3 = 0;
  for i = 1 : 5
    i3 = i3 + 3;
    for ll = i3-2 : i3
      mm = ll - i3;
      yp(ll) = y(ll+15);
      p = 0.0;
      for j = 1 : 5
        mm = mm + 3;
        if ( j ~= i )
          p = p + m(j) ...
             * ( y(mm) * ( q(i,j) - r(j) ) - y(ll) * q(i,j) );
        end
      end
      yp(ll+15) = k2 * ( - ( m0 + m(i) ) * y(ll) * r(i) + p );
    end
  end

  return
end


function [x, w, v] = radaupts(n, alp, bet)
%RADAUPTS   Gauss-Jacobi-Radau quadrature nodes and weights.
%  RADAUPTS(N) returns N Legendre-Radau points X in [-1,1).
%
%  [X, W] = RADAUPTS(N) returns also a row vector W of weights for
%  Gauss-Legendre-Lobatto quadrature.
%
%  [X, W, V] = RADAUPTS(N) returns additionally a column vector V of weights in
%  the barycentric formula corresponding to the points X. The weights are scaled
%  so that max(abs(V)) = 1.
%
%  [...] = RADUAPTS(N, ALP, BET) is similar, but for the Gauss-Jacobi-Radau
%  nodes and weights. Here ALP and BET should be scalars > -1.
%
%  In each case, N should be a positive integer.
%
% See also CHEBPTS, LEGPTS, JACPTS, LEGPOLY, JACPOLY, LOBPTS.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Developer note:
%   The approach used here is to observe that the Gauss-Radau points are
%   precisely the roots of (1+x)P^(0,1)_{n-2}(x), which can be obtained from
%   JACPTS. A similar identity [NIST, (18.9.5) and (18.9.17)] is used for the
%   computation of the quadrature weights from those of JACPTS, and the missing
%   barycentric weights are determined by enforcing the interpolation of f(x) =
%   x at x = 0.
%
%    x_j = roots of (1+x)P^(0,1)_{n-2}(x)
%    w_j = { 2/n^2                                 : x_j = -1
%          { 1/(1-x_j) * 1/[d/dx P_{n-1}(x_j)]^2   : otherwise
%
%   (Note that the weights for n-1 point Gauss-Jacobi with a = 0, b = 1 satisfy
%    u_j = C/(1-x_j^2)/[d/dx P^(0,1)_{n-1}(x_j)]^2)
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% TODO: Scaled domains?

if ( nargin < 3 )
    % Default to Gauss-Legendre-Lobatto:
    alp = 0; bet = 0;
end

%% Trivial cases:
if ( n == 1 )
    x = -1;
    w = 2^(1+alp+bet)*beta(1+alp,1+bet);
    v = 1;
    return
end

%% Call JACPTS():
[xi, w, v] = jacpts(n-1, alp, bet+1);

%% Nodes:
x = [-1 ; xi];

%% Quadrature weights:
wi = w./(1 + xi.');
if ( alp == 0 && bet == 0 )
    w = [2/n^2, wi];
else
    % See Walter Gautschi, "Gauss–Radau formulae for Jacobi and Laguerre
    % weight functions", Mathematics and Computers in Simulation, (2000).
    w = [2^(alp+bet+1)*beta(bet+1,n)*beta(alp+n,bet+1)*(bet+1), wi];
end

%% Barycentric weights:
v = v./(1 + xi);
v = v/max(abs(v));
v1 = -sum(v);
v = [v1 ; v];

end


function [a1, b1, c1, a2, b2, c2] = aaad(A, B, C)
%AAAD  gives both solutions to the angle-angle-angle problem, in degrees.
%
%   AAAD(A, B, C) will result in NaNs if the existence condition
%   |pi - |A|-|B|| <= |C| <= pi - ||A| - |B||
%   is not met.
%
%   See also AAAD.

% Rody P.S. Oldenhuis
% Delft University of Technology
% oldenhuis@gmail.com
%
% Crated     : 23/Feb/2009
% Last edited: 30/Nov/2012

    % find both solutions by calling aaa directly
    r2d = 180/pi;
    d2r = 1/r2d;
    [a1, b1, c1, a2, b2, c2] = aaa(A*d2r, B*d2r, C*d2r);
    [a1, b1, c1, a2, b2, c2] = deal(a1*r2d, b1*r2d, c1*r2d, a2*r2d, b2*r2d, c2*r2d);

end


function [xr,Yr]=noisergeo(x,dim,tau,r,q,p,theta)
%Syntax: [xr,Yr]=noisergeo(x,dim,tau,r,q,p,theta)
%________________________________________________
%
% Noise reduction by Local Geometric Projection.
%
% xr is the vector/matrix with the cleaned time series.
% Yr is the phase space of the last cleaned xr.
% x is the time series.
% dim is the embedding dimension.
% tau is the time delay.
% r can be either
%   real defining the neighborhood range.
%   integer defining the number of nearest neighbors.
% q can take one of the following values
%  'wAV' for the weighted average.
%  [an integer from 0 to dim-1] for the local geometric projection.
%  'mod' for the adaptive selection of the local neighborhood dimensions.
% p defines the norm.
% theta is the correction paprameter [0,1]
%   1: full correction.
%   0: no correction.
%
%
% References:
%
% Kantz H, Schreiber T, Hoffmann I, Buzug T, Pfister G, Flepp L G, Simonet
% J, Badii R, Brun E (1993): Nonlinear noise reduction: A case study on
% experimental data. Physical Review E 48: 1529-1538
%
% Leontitsis A., Bountis T., Pange J. (2004): An adaptive way for improving
% noise reduction using local geometric projection. CHAOS 14(6): 106-110
%
%
% Alexandros Leontitsis
% Department of Education
% University of Ioannina
% Ioannina
% Greece
%
% University e-mail: me00743@cc.uoi.gr
% Lifetime e-mail: leoaleq@yahoo.com
% Homepage: http://www.geocities.com/CapeCanaveral/Lab/1421
%
% 14 Jul 2001

if nargin<1 | isempty(x)==1
   error('You should provide a time series.');
else
   % x must be a vector
   if min(size(x))>1
      error('Invalid time series.');
   end
   x=x(:);
   % n is the time series length
   n=length(x);
end

if nargin<2 | isempty(dim)==1
   dim=2;
else
   % dim must be either a scalar or a vector
   if min(size(dim))>1
      error('dim must be a scalar or a vector.');
   end
   % dim must be an integer
   if round(dim)-dim~=0
      error('dim must be an integer.');
   end
   % dim values must be above 1
   if any(dim<1)==1
      error('dim values must be above 1');
   end
end

if nargin<3 | isempty(tau)==1
   tau=1;
else
   % tau must be either a scalar or a vector
   if min(size(tau))>1
      error('tau must be a scalar or a vector.');
   end
   % tau must be an integer
   if round(tau)-tau~=0
      error('tau must be an integer.');
   end
   % tau values must be above 1
   if any(tau<1)==1
      error('tau values must be above 1');
   end
end

if nargin<4 | isempty(r)==1
   r=dim+1;
else
   % r must be either a scalar or a vector
   if min(size(r))>1
      error('r must be a scalar or a vector.');
   end
   % r values must be above 0
   if any(r<=0)==1
      error('r values must be greater than 0');
   end
end

if nargin<5 | isempty(q)==1
   q=1;
end

if nargin<6 | isempty(p)==1
   p=2;
else
   % p must be either a scalar or a vector
   if min(size(p))>1
      error('p must be a scalar or a vector.');
   end
end

if nargin<7 | isempty(theta)==1
   theta=1;
else
   % theta must be either a scalar or a vector
   if min(size(theta))>1
      error('theta must be a scalar or a vector.');
   end
   % theta must be above 0 and bellow 1
   if theta<0 | theta>1
      error('theta must be above 0 and bellow 1.')
   end
end

% Only one of dim, tau, r, p or theta should be vector
l=[length(dim),length(tau),length(r),length(p),length(theta)];
if length(find(l>1))>1
   error('Only one of dim, tau, r, p, or theta should be vector.');
end

% Make the phase-space
[Y,T]=phasespace(x,dim,tau);

m=max(l);
dim=ones(1,m).*dim;
tau=ones(1,m).*tau;
r=ones(1,m).*r;
p=ones(1,m).*p;
theta=ones(1,m).*theta;

for i=1:m

    % Initialize Yr
    Yr=zeros(T,dim(i));

    % For every phase-space point
    for j=1:T

        % Locate the j-th point
        y=Y(j,:);

        % Check neighborhood or neighbors
        if mod(r(i),floor(r(i)))==0
            lock=Knearest(y,Y,r(end)+1,p(i));
        else
            lock=radnearest(y,Y,T,r(i),p(i));
            lock(find(j==lock))=[];
        end

        if isempty(lock)==1
            Yr(j,:)=y;
        elseif q=='wAV'
            % The calculations for the weighted average
            Ynearest=Y(lock,:);
            w=[];
            for k=1:length(lock)
                w(k)=norm(y-Ynearest(k,:))/2/r(i);
                w(k)=exp(-w(k)^2);
                Ynearest(k,:)=Ynearest(k,:)*w(k);
            end
            Yr(j,:)=sum(Ynearest)/sum(w);
        else
            % All the neighboring points have equal weight
            if length(lock)<dim(i)
                Yr(j,:)=mean(Y(lock,:));
            else
                % Make the matrix of the local neighborhood
                Ynearest=Y(lock,:);
                %Ynearest=quality3(Ynearest,2);
                %Ynearest=quality2(Ynearest,2,.99,r,y);
                Ynearest=quality(Ynearest,2,.7);
                % Neigborhood average
                ybar=mean(Ynearest);
                % Subtract the average from the neighborhood poionts
                Ynearest=Ynearest-ones(size(Ynearest,1),1)*ybar;
                % SVD on the local neighborhood
                [u,s,v]=svd(Ynearest,0);
                % Either use the Adaptive selection ...
                if q=='mod'
                    s=diag(s);
                    d=s(1:dim(i)-1)./(s(2:dim(i))+eps);
                    q=find(d==max(d));
                    Yr(j,:)=y-(y-ybar)*(v(:,q+1:dim(i))*v(:,q+1:dim(i))');
                % ... or use a predifined q
                else
                    Yr(j,:)=y-(y-ybar)*(v(:,dim(i)-q:dim(i))*v(:,dim(i)-q:dim(i))');
                end
            end
        end
        Yr(j,:)=theta(i)*Yr(j,:)+(1-theta(i))*Y(j,:);
    end

    % Initialize xr
    xr(:,i)=zeros(n,1);
    times(:,i)=zeros(n,1);

    % Calculate the reconstructed time series
    for j=1:dim
        xr(1+(j-1)*tau:T+(j-1)*tau,i)=xr(1+(j-1)*tau:T+(j-1)*tau,i)+Yr(:,j);
        times(1+(j-1)*tau:T+(j-1)*tau,i)=times(1+(j-1)*tau:T+(j-1)*tau,i)+1;
    end
    xr(:,i)=xr(:,i)./times(:,i);
    xr(:,i)=xr(:,i)-mean(xr(:,i)-x);

end


% Nonnegativity Constrained Least Squares with Multiple Righthand Sides
%      using Block Principal Pivoting method
%
% This software solves the following problem: given A and B, find X such that
%              minimize || AX-B ||_F^2 where X>=0 elementwise.
%
% Reference:
%      Jingu Kim and Haesun Park. Fast Nonnegative Matrix Factorization: An Activeset-like Method and Comparisons,
%      SIAM Journal on Scientific Computing, 33(6), pp. 3261-3281, 2011.
%
% Written by Jingu Kim (jingu.kim@gmail.com)
%            School of Computational Science and Engineering,
%            Georgia Institute of Technology
%
% Note that this algorithm assumes that the input matrix A has full column rank.
% This code comes with no guarantee or warranty of any kind.
% Please send bug reports, comments, or questions to Jingu Kim.
%
% Modified Feb-20-2009
% Modified Mar-13-2011: numChol and numEq
%
% <Inputs>
%        A : input matrix (m x n) (by default), or A'*A (n x n) if isInputProd==1
%        B : input matrix (m x k) (by default), or A'*B (n x k) if isInputProd==1
%        isInputProd : (optional, default:0) if turned on, use (A'*A,A'*B) as input instead of (A,B)
%        init : (optional) initial value for X
% <Outputs>
%        X : the solution (n x k)
%        Y : A'*A*X - A'*B where X is the solution (n x k)
%        success : 0 for success, 1 for failure.
%                  Failure could only happen on a numericall very ill-conditioned problem.
%        numChol : number of unique cholesky decompositions done
%        numEqs : number of systems of linear equations solved

function [ X,Y,success,numChol,numEq ] = nnlsm_blockpivot( A, B, isInputProd, init )
    if nargin<3, isInputProd=0;, end
    if isInputProd
        AtA = A;, AtB = B;
    else
        AtA = A'*A;, AtB = A'*B;
    end

    if size(AtA,1)==1
        X = AtB/AtA; X(X<0) = 0;
        Y = AtA*X - AtB;
        numChol = 1; numEq = size(AtB,2); success = 1;
        return
    end

    [n,k]=size(AtB);
    MAX_BIG_ITER = n*5;
    % set initial feasible solution
    X = zeros(n,k);
    if nargin<4
        Y = - AtB;
        PassiveSet = false(n,k);
        numChol = 0;
        numEq = 0;
    else
        PassiveSet = (init > 0);
        [ X,numChol,numEq] = normalEqComb(AtA,AtB,PassiveSet);
        Y = AtA * X - AtB;
    end
    % parameters
    pbar = 3;
    P = zeros(1,k);, P(:) = pbar;
    Ninf = zeros(1,k);, Ninf(:) = n+1;

    NonOptSet = (Y < 0) & ~PassiveSet;
    InfeaSet = (X < 0) & PassiveSet;
    NotGood = sum(NonOptSet)+sum(InfeaSet);
    NotOptCols = NotGood > 0;

    bigIter = 0;, success=0;
    while(~isempty(find(NotOptCols)))
        bigIter = bigIter+1;
        if ((MAX_BIG_ITER >0) && (bigIter > MAX_BIG_ITER))   % set max_iter for ill-conditioned (numerically unstable) case
            success = 1;, break
        end

        Cols1 = NotOptCols & (NotGood < Ninf);
        Cols2 = NotOptCols & (NotGood >= Ninf) & (P >= 1);
        Cols3Ix = find(NotOptCols & ~Cols1 & ~Cols2);
        if ~isempty(find(Cols1))
            P(Cols1) = pbar;,Ninf(Cols1) = NotGood(Cols1);
            PassiveSet(NonOptSet & repmat(Cols1,n,1)) = true;
            PassiveSet(InfeaSet & repmat(Cols1,n,1)) = false;
        end
        if ~isempty(find(Cols2))
            P(Cols2) = P(Cols2)-1;
            PassiveSet(NonOptSet & repmat(Cols2,n,1)) = true;
            PassiveSet(InfeaSet & repmat(Cols2,n,1)) = false;
        end
        if ~isempty(Cols3Ix)
            for i=1:length(Cols3Ix)
                Ix = Cols3Ix(i);
                toChange = max(find( NonOptSet(:,Ix)|InfeaSet(:,Ix) ));
                if PassiveSet(toChange,Ix)
                    PassiveSet(toChange,Ix)=false;
                else
                    PassiveSet(toChange,Ix)=true;
                end
            end
        end
        [ X(:,NotOptCols),tempChol,tempEq ] = normalEqComb(AtA,AtB(:,NotOptCols),PassiveSet(:,NotOptCols));
        numChol = numChol + tempChol;
        numEq = numEq + tempEq;
        X(abs(X)<1e-12) = 0;            % One can uncomment this line for numerical stability.
        Y(:,NotOptCols) = AtA * X(:,NotOptCols) - AtB(:,NotOptCols);
        Y(abs(Y)<1e-12) = 0;            % One can uncomment this line for numerical stability.

        % check optimality
        NotOptMask = repmat(NotOptCols,n,1);
        NonOptSet = NotOptMask & (Y < 0) & ~PassiveSet;
        InfeaSet = NotOptMask & (X < 0) & PassiveSet;
        NotGood = sum(NonOptSet)+sum(InfeaSet);
        NotOptCols = NotGood > 0;
    end
end


function cvt_test14 ( )

%*****************************************************************************80
%
%% CVT_TEST14 generates a 10 point CVT on [0,1].
%
%  Discussion:
%
%    Generate 10 CVT points on the interval [0,1].
%    We expect them to be at 1/20, 3/20, 5/20, ..., 19/20.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 September 2010
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST14\n' );
  fprintf ( 1, '  Generate a CVT in the interval [0,1] using 10 points.\n' );
  fprintf ( 1, '  Exact answer: { 0.05, 0.15, 0.25, ..., 0.85, 0.95 }\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  It turns out that, for a fixed number of points N,\n' );
  fprintf ( 1, '  a 1D problem will converge much more slowly than for\n' );
  fprintf ( 1, '  cases where the dimension is higher.\n' );

  dim_num = 1;
  n = 10;
  batch = 10000;
  init = 1;
  init_string = 'uniform';
  it_max = 50;
  it_fixed = 1;
  sample = 1;
  sample_num = 100000;
  sample_string = 'uniform';
  seed = 123456789;
  r = [];

  seed_init = seed;

  [ r, seed, it_num, it_diff, energy ] = cvt ( dim_num, n, batch, init, ...
    sample, sample_num, it_max, it_fixed, seed, r );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Dimension DIM_NUM =        %12d\n', dim_num );
  fprintf ( 1, '  Number of points N =       %12d\n', n );
  fprintf ( 1, '  Initial SEED =             %12d\n', seed_init );
  fprintf ( 1, '  Current SEED =             %12d\n', seed );
  fprintf ( 1, '  INIT =                    "%s".\n', init_string );
  fprintf ( 1, '  Max iterations IT_MAX =    %12d\n', it_max );
  fprintf ( 1, '  IT_FIXED (fixed samples) = %12d\n', it_fixed );
  fprintf ( 1, '  Iterations IT_NUM =        %12d\n', it_num );
  fprintf ( 1, '  Difference IT_DIFF =       %14f\n', it_diff );
  fprintf ( 1, '  CVT ENERGY =               %14f\n', energy );
  fprintf ( 1, '  SAMPLE =                  "%s".\n', sample_string );
  fprintf ( 1, '  Samples SAMPLE_NUM    =    %12d\n', sample_num );
  fprintf ( 1, '  Sampling BATCH size =      %12d\n', batch );
  fprintf ( 1, '  EPSILON (unit roundoff) =  %12e\n', eps );

  r = sort ( r );

  r8mat_transpose_print ( dim_num, n, r, '  Generators (rows):' );

  return
end


%% Spherical Functions
%
%%
% By a variable of type @S2Fun it is possible to represent an entire
% function on the two dimensional sphere. A typical example of such a
% function is the pole density function of a given ODF with respect to a
% fixed crystal direction.

% the famouse Santa Fe orientation distribution function
odf = SantaFe;

% the (100) pole density function
pdf = odf.calcPDF(Miller(1,0,0,odf.CS))

%%
% Since, the variable |pdf| stores all information about this function we
% may evaluate it for any direction |r|

% take a random direction
r = vector3d.rand;

% and evaluate the pdf at this direction
pdf.eval(r)

%%
% We may also plot the function in any spherical projection

plot(pdf)

%%
% or find its local maxima

[~,localMax] = max(pdf,'numLocal',12)

annotate(localMax)

%%
% A complete list of operations that can be performed with spherical
% functions can be found in section <S2FunOperations.html Operations>.
%

%% Representation of Spherical Functions
%
% In MTEX there exist different ways for representing spherical functions
% internally.
%
% || harmonic expansion || @S2FunHarmonic ||
% || finite elements || @S2FunTri ||
% || function handle || @S2FunHandle ||
% || Bingham distribution || @BinghamS2 ||
%
% All representations allow for the same operations which are specified for
% the abstact class @S2Fun. In particular it is possible
% to calculate with spherical functions as with ordinary numbers, i.e., you
% can add, multiply arbitrary functions, take the mean, integrate them or
% compute gradients.
%
%% Generalizations of Spherical Functions
%
% || spherical vector fields || @S2VectorField ||
% || spherical axis fields || @S2AxisField ||
% || radial spherical functions || @S2Kernel ||
% || symmetric spherical functions || @S2FunHarmonicSym ||
%


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Function (ldiv) : calculate inverse Z-transform by long division
% Author : Tamer mohamed samy abdelazim Mellik
% Contact information :
%Department of Electrical & Computer Engineering,
%University of Calgary,
%2500 University Drive N.W. ,
%Calgary, AB T2N 1N4 ,
%Canada .
% email :abdelasi@enel.ucalgary.ca
% email : tabdelaz@ucalgary.ca
% Webpage : http://www.enel.ucalgary.ca/~abdelasi/
% Date    : 2-5-2002
% Version : 1.0.0
%Example
% This function like deconv but it help if the numerator less or equal degree of denominator
% if you have this function (It must arranged in terms of minus power of Z):
%              1
% G(z)= -----------------
%             -1     -2
%      ( 5 - Z  - 3 Z  )
% and you want to calculate long division or inverse Z transform :
% The numerator  is a=[1] and  the denominator is b= [5 -1 -3 ]
% call the function ldiv(a,b) to get the funresult 20 items (default)
% another example :
%                -2     -3
%       ( 5 - 3 Z  + 4 Z  )
%  G(z)-----------------
%              -1     -2
%        (5 - Z  - 3 Z )
% a=[5 0 -3 4] , b= [5 -1 -3 ] and you want the funresult 100 terms !
% ldiv(a,b,100)
% Note : The author doesn't have any responsibility for any harm caused by the use of this file
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function funresult=ldiv(a,b,N)
%a numerator
%b denominator
%default order of the filter == 20
funresult=[];
if nargin < 3
    if nargin > 1
            N=20;
    else
            disp('Usage: M = ldiv(a,b,N)')
            disp('a:numerator , b denominator and N is the order of the resultant filter')
            return
    end
end

if size(a) < 1
    disp('Error: numerator must at least have one element not empty')
    return
end
if size(b) < 1
    disp('Error: denominator must at least have one element not empty')
    return
end

if b(1)==0
    disp('Error: The first element of denominator must have nonzero value')
    return
end
if size(b) < 2
    funresult=a./b;
    for i =length(funresult)+1:N
       funresult(i)=0;
    end
    return
end

for i = length(a)+1:N
    a(i)=0;
end
for i = 1 : N
    funresult(i)=a(1)/b(1);
    if length(a)>1
            for k= 2:length(b)
                if k > length(a)
                    a(k)=0;
                end
                a(k)=a(k)-funresult(length(funresult))*b(k);
            end

        for i = 1:length(a)-1
                a(i)=a(i+1);
        end
        a=a(1:length(a)-1);
    end
end


function a = frank_inverse ( n )

%*****************************************************************************80
%
%% FRANK_INVERSE returns the inverse of the FRANK matrix.
%
%  Formula:
%
%    if ( I = J-1 )
%      A(I,J) = -1
%    elseif ( I = J )
%      if ( I = 1 )
%        A(I,J) = 1
%      else
%        A(I,J) = N + 2 - I
%    elseif ( J < I )
%      A(I,J) = - (N+1-I) * A(I-1,J)
%    else
%      A(I,J) = 0
%
%  Example:
%
%    N = 5
%
%     1  -1  0  0  0
%    -4   5 -1  0  0
%    12 -15  4 -1  0
%   -24  30 -8  3 -1
%    24 -30  8 -3  2
%
%  Properties:
%
%    A is generally not symmetric: A' /= A.
%
%    A is lower Hessenberg.
%
%    det ( A ) = 1.
%
%    A is unimodular.
%
%    A is integral: int ( A ) = A.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Output, real A(N,N), the matrix.
%
  a = zeros ( n, n );

  for i = 1 : n
    for j = 1 : n

      if ( i == j-1 )
        a(i,j) = - 1.0;
      elseif ( i == j )
        if ( i == 1 )
          a(i,j) = 1.0;
        else
          a(i,j) = ( n + 2 - i );
        end
      elseif ( j < i )
        a(i,j) = - ( n + 1 - i ) * a(i-1,j);
      else
        a(i,j) = 0.0;
      end

    end
  end

  return
end


function [detJ,J] = dtiJacobian(inData)
%
% [detJ,J] = dtiJacobian(inData)
%
% Computes the Jacobian (J) and the determinant of the Jacobian (detJ)
% given an XxYxZx3 or Nx3 input array. The array is assumed to represent a
% vector field. The Jacobian is a useful metric for interpreting
% deformations. E.g., abs(detJ) represents the local volume change for a
% deformation field.
%
% HISTORY:
% 2008.06.25 RFD wrote it.

sz = size(inData);

if((numel(sz)~=4 && numel(sz)~=2) || sz(end)~=3)
    error('inData must be XxYxZx3 or Nx3.');
end

% Approximate the Jacobian of the input array using grad.

if(numel(sz)==2)
    inData = reshape(inData,[1 1 1 sz(1) sz(2)]);
end

dim = size(inData);
J = zeros(dim(1),dim(2),dim(3),3,3);

for(ii=1:3)
    [gradX,gradY,gradZ] = gradient(inData(:,:,:,ii),1);
    J(:,:,:,ii,1) = gradX;
    J(:,:,:,ii,2) = gradY; % Or Y,X,Z?
    J(:,:,:,ii,3) = gradZ;
end
% Compute the determinant of J
% det(J) for a 3x3 [a b c; d e f; g h i] is: (aei+bfg+cdh)-(gec+hfa+idb)
% a=1,1; b=1,2; c=1,3; d=2,1; e=2,2; f=2,3; g=3,1; h=3,2; i=3,3;
detJ =  J(:,:,:,1,1).*J(:,:,:,2,2).*J(:,:,:,3,3) ...
      + J(:,:,:,1,2).*J(:,:,:,2,3).*J(:,:,:,3,1) ...
      + J(:,:,:,1,3).*J(:,:,:,2,1).*J(:,:,:,3,2) ...
      - J(:,:,:,3,1).*J(:,:,:,2,2).*J(:,:,:,1,3) ...
      - J(:,:,:,3,2).*J(:,:,:,2,3).*J(:,:,:,1,1) ...
      - J(:,:,:,3,3).*J(:,:,:,2,1).*J(:,:,:,1,2);

if(numel(sz)==2)
    J = squeeze(J);
    detJ = squeeze(detJ);
end
return;


function chi = RoadInclination_Arkussinus(z_I_Ground_m,l_WheelbaseFB_m,w_TrackFB_m,phiz_IK_rad)
%% Computation of Road Angles from Street Profile in Inertial Frame
% Input parameters:
% phiz_IK_rad     [rad]    Euler-Angle around z-Axis from Inertial Reference Frame I to Vehicle Fixed Reference Frame K [Psi]
% w_TrackFB_m     [m]      Track width at Front and Rear Axle [w_TrackF_m w_TrackR_m]
% l_WheelbaseFB_m [m]      Wheelbase from Cog to Front and Rear Axle [l_WheelbaseF_m l_WheelbaseR_m]
% z_I_Ground_m    [m]      Input Vector with z-Profile of Street in Inertial Coordinate System at the Axle Locations [x1 x2 x3 x4] [3x4]
% Output parameters:
% Y               [rad]    Approximated Cardan Angles from Inertial Reference Frame to Road Fixed Reference Frame [chi_x chi_y Psi] [3x1]

%% Computation of Road Angles
% Inclination Angle in x-direction for negative z-road-profile
chi_y = -asin( 0.5*((z_I_Ground_m(1)+z_I_Ground_m(2))-(z_I_Ground_m(3)+z_I_Ground_m(4)))/sum(abs(l_WheelbaseFB_m)) );
% Inclination Angle in y-direction for negative z-road-profile
chi_x = -asin( 0.5*((z_I_Ground_m(1)+z_I_Ground_m(3))-(z_I_Ground_m(2)+z_I_Ground_m(4)))/mean(abs(w_TrackFB_m)) );

%% Output Parameters
chi = [chi_x;chi_y;phiz_IK_rad];


%%***************************************************************
%% logcheby: logarithmic Chebyshev approximation problem.
%%           SDP formulation.
%%    minimize   t
%%      x,t
%%    such that  1/t <= (x'*B(i,:))/f(i) <= t,  i = 1:p.
%%
%%    B = pxm matrix,  f = p-vector,  p > m.
%%
%%    Ref: Vanderberghe & Boyd.
%%--------------------------------------------------------------
%% [blk,Avec,C,b,X0,y0,Z0,obj,x] = logcheby(B,f,feas,solve);
%%
%% B = pxm matrix  (p > m).
%% f = px1 vector,  [B(:,j)./f must be positive for each j]
%% feas  = 1 if want feasible starting point
%%       = 0 if otherwise.
%% solve = 0 if just want initialization.
%%       = 1 if want to solve the problem.
%%
%% SDPT3: version 3.0
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last modified: 2 Feb 01
%%***************************************************************

  function [blk,Avec,C,b,X0,y0,Z0,objval,x] = logcheby(B,f,feas,solve);

     if (nargin < 4); solve = 0; end;
     if (nargin < 3); feas = 1;  end;

     if ~isreal(B); error('B must be real'); end;

     [p,m] = size(B);
     blk{1,1} = 's'; blk{1,2} = 2*ones(1,p);
     blk{2,1} = 'l'; blk{2,2} = p;

     E = zeros(p,m);
     for j = [1:m];  E(:,j) = B(:,j)./f;  end;
     if any(E < 1e-10);
        error(' B(:,j)./f must have all entry positive');
     end;
     for i = [1:p];  beta(i) = sum(E(i,:)); end;
%%
     temp = zeros(2*p+1,1);
     temp(2:2:2*p) = ones(p,1);
     C{1,1} = spdiags(temp,1,2*p,2*p); C{1,1} = C{1,1} + C{1,1}';
     C{2,1} = zeros(p,1);
     b  = [zeros(m,1); 1];

     A = cell(2,m+1);
     temp = zeros(2*p,1);
     for k = 1:m
         temp(1:2:2*p-1) = -E(:,k);
         A{1,k} = spdiags(temp,0,2*p,2*p);
         A{2,k} = E(:,k);
     end;
     temp = zeros(2*p,1);
     temp(2:2:2*p) = ones(p,1);
     A{1,m+1} = spdiags(temp,0,2*p,2*p);
     A{2,m+1} = ones(p,1);
%%
    Avec = svec(blk,A,ones(size(blk,1),1));
    if (feas == 1);
        X0{1,1} = speye(2*p)/(2*p);
        X0{2,1} = ones(p,1)/(2*p);
        y0 = [ones(m,1); -1.1*max(beta)]/min(beta);
        Z0 = ops(C,'-',Atyfun(blk,Avec,[],[],y0));
    elseif (feas == 0);
        [X0,y0,Z0] = infeaspt(blk,Avec,C,b);
    end;
    if (solve)
       [obj,X,y,Z] = sqlp(blk,Avec,C,b,[],X0,y0,Z0);
       objval = -mean(obj);
       x = y(1:m);
    else
       objval = []; x = [];
    end
%%***************************************************************


%% Machine Learning Online Class
%  Exercise 1: Linear regression with multiple variables
%
%  Instructions
%  ------------
%
%  This file contains code that helps you get started on the
%  linear regression exercise.
%
%  You will need to complete the following functions in this
%  exericse:
%
%     warmUpExercise.m
%     plotData.m
%     gradientDescent.m
%     computeCost.m
%     gradientDescentMulti.m
%     computeCostMulti.m
%     featureNormalize.m
%     normalEqn.m
%
%  For this part of the exercise, you will need to change some
%  parts of the code below for various experiments (e.g., changing
%  learning rates).
%

%% Initialization

%% ================ Part 1: Feature Normalization ================

%% Clear and Close Figures
clear ; close all; clc

fprintf('Loading data ...\n');

%% Load Data
data = load('ex1data2.txt');
X = data(:, 1:2);
y = data(:, 3);
m = length(y);

% Print out some data points
fprintf('First 10 examples from the dataset: \n');
fprintf(' x = [%.0f %.0f], y = %.0f \n', [X(1:10,:) y(1:10,:)]');

fprintf('Program paused. Press enter to continue.\n');
pause;

% Scale features and set them to zero mean
fprintf('Normalizing Features ...\n');

[X mu sigma] = featureNormalize(X);

% Add intercept term to X
X = [ones(m, 1) X];

%% ================ Part 2: Gradient Descent ================

% ====================== YOUR CODE HERE ======================
% Instructions: We have provided you with the following starter
%               code that runs gradient descent with a particular
%               learning rate (alpha).
%
%               Your task is to first make sure that your functions -
%               computeCost and gradientDescent already work with
%               this starter code and support multiple variables.
%
%               After that, try running gradient descent with
%               different values of alpha and see which one gives
%               you the best result.
%
%               Finally, you should complete the code at the end
%               to predict the price of a 1650 sq-ft, 3 br house.
%
% Hint: By using the 'hold on' command, you can plot multiple
%       graphs on the same figure.
%
% Hint: At prediction, make sure you do the same feature normalization.
%

fprintf('Running gradient descent ...\n');

% Choose some alpha value
alpha = 0.01;
num_iters = 400;

% Init Theta and Run Gradient Descent
theta = zeros(3, 1);
[theta, J_history] = gradientDescentMulti(X, y, theta, alpha, num_iters);

% Plot the convergence graph
figure;
plot(1:numel(J_history), J_history, '-b', 'LineWidth', 2);
xlabel('Number of iterations');
ylabel('Cost J');

% Display gradient descent's result
fprintf('Theta computed from gradient descent: \n');
fprintf(' %f \n', theta);
fprintf('\n');

% Estimate the price of a 1650 sq-ft, 3 br house
% ====================== YOUR CODE HERE ======================
% Recall that the first column of X is all-ones. Thus, it does
% not need to be normalized.

x = [1 1650 3]';
price = theta' * x;

% ============================================================

fprintf(['Predicted price of a 1650 sq-ft, 3 br house ' ...
         '(using gradient descent):\n $%f\n'], price);

fprintf('Program paused. Press enter to continue.\n');
pause;

%% ================ Part 3: Normal Equations ================

fprintf('Solving with normal equations...\n');

% ====================== YOUR CODE HERE ======================
% Instructions: The following code computes the closed form
%               solution for linear regression using the normal
%               equations. You should complete the code in
%               normalEqn.m
%
%               After doing so, you should complete this code
%               to predict the price of a 1650 sq-ft, 3 br house.
%

%% Load Data
data = csvread('ex1data2.txt');
X = data(:, 1:2);
y = data(:, 3);
m = length(y);

% Add intercept term to X
X = [ones(m, 1) X];

% Calculate the parameters from the normal equation
theta = normalEqn(X, y);

% Display normal equation's result
fprintf('Theta computed from the normal equations: \n');
fprintf(' %f \n', theta);
fprintf('\n');

% Estimate the price of a 1650 sq-ft, 3 br house
% ====================== YOUR CODE HERE ======================

x = [1 1650 3]';
price = theta' * x;

% ============================================================

fprintf(['Predicted price of a 1650 sq-ft, 3 br house ' ...
         '(using normal equations):\n $%f\n'], price);


function [ xout, yout ] = quaerotate ( xin, yin )

%*****************************************************************************80
%
%% QUAEROTATE applies a rotation.
%
%  Licensing:
%
%    This code is distributed under the GNU GPL license.
%
%  Modified:
%
%    26 June 2014
%
%  Author:
%
%    Original FORTRAN77 version by Hong Xiao, Zydrunas Gimbutas.
%    This MATLAB version by John Burkardt.
%
%  Reference:
%
%    Hong Xiao, Zydrunas Gimbutas,
%    A numerical algorithm for the construction of efficient quadrature
%    rules in two and higher dimensions,
%    Computers and Mathematics with Applications,
%    Volume 59, 2010, pages 663-676.
%
%  Parameters:
%
%    Input, real XIN, YIN, the coordinates of the point.
%
%    Output, real XOUT, YOUT, the coordinates of the point
%    after rotation.
%

%
%  Initialize the matrix of rotation.
%
  theta = 2.0 * pi / 3.0;
  a11 = cos ( theta );
  a22 = cos ( theta );
  a12 = - sin ( theta );
  a21 = -a12;
%
%  Apply the rotation matrix to the input vector.
%
  xout = a11 * xin + a12 * yin;
  yout = a21 * xin + a22 * yin;

  return
end


function jac = p18_jac ( neqn, t, y )

%*****************************************************************************80
%
%% P18_JAC evaluates the jacobian for problem p18.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 February 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NEQN, the number of equations.
%
%    Input, real T, Y(NEQN), the arguments of the jacobian.
%
%    Output, real JAC(NEQN,NEQN), the jacobian matrix.
%
  jac = zeros ( neqn, neqn );

  d = ( sqrt ( ( y(1).^2 + y(2).^2 ) ) ).^5;

  jac(1,3) = 1.0;
  jac(2,4) = 1.0;
  jac(3,1) = ( 2.0 * y(1).^2 - y(2).^2 ) / d;
  jac(3,2) = 3.0 * y(1) * y(2) / d;
  jac(4,1) = 3.0 * y(1) * y(2) / d;
  jac(4,2) = ( - y(1).^2 + 2.0 * y(2).^2 ) / d;

  return
end


function [W,fun,iter] = wLassomtl(X,y,XtX,Xty,lambda,W0,tol,maxiter,L,Lflag)
% Lasso Multi-Task Learning
% min_W ||XW - Y||_F^2 + \sum_i lambda_ij*|W^ij|
% By Nesterov's method
% ------------------------------- Input ---------------------------------------------
% X: block diagonal data matrix whose i-th block (n_i*d) is the data matrix of the i-th task
% y: \sum_i{n_i}*1 vector; response vector which is stacked by the reponses of all tasks
% XtX: X'*X
% Xty: X'*y
% lambda: regularized parameter (vector)
% W0: d*m matrix; starting point of W
% tol: stopping tolerance
% maxiter: maximum iterative steps
% Lflag: estimate the upper bound of Lipschitz constant if nonzero, zero otherwise
%
% ------------------------------- Output -------------------------------------------
%
% W: output weight
% fun: function values
% iter: iterative steps
%
% -----------------------------------------------------------------------------------

W = W0;
[d,m] = size(W); % d: dimension, m: the number of tasks

Wn = W;
t_new = 1;
fun = zeros(maxiter+1,1);

% Initial function value
fun(1) = norm(X*W(:) - y)^2 + wL1norm(W,lambda);
count = 0;
for iter = 1:maxiter
    W_old = W;
    t_old = t_new;
    gradvec = 2*(XtX*Wn(:) - Xty);
    gradmat = reshape(gradvec,d,m);
    % If we estimate the upper bound of Lipschitz constant, no line search
    % is needed.
    if Lflag
        W = proximalwL1norm(Wn - gradmat/L, lambda/L);
    else
        % line search
        for inneriter = 1:20
            W = proximalwL1norm(Wn - gradmat/L, lambda/L);
            dW = W - Wn;
            if 2*(dW(:)'*XtX*dW(:)) <= L*sum(sum((dW.*dW)))
                break;
            else
                L = L*2;
            end
        end
    end
    fun(iter+1) = norm(X*W(:) - y)^2 + wL1norm(W,lambda);
    % stopping condition
    if abs(fun(iter) - fun(iter+1))/fun(iter+1) < tol
        count = count + 1;
    else
        count = 0;
    end
    if count >= 1
        break;
    end

    % Update the coefficient
    t_new = (1+sqrt(1+4*t_old^2))/2;
    Wn = W + (t_old-1)/t_new*(W - W_old);

end


function I=getNextCombo(I,n,startVal)
%%GETNEXTCOMBO Return the next combination in lexicographic order given the
%              current combination. If the final combination in the
%              sequence has been reached, then an empty matrix is returned.
%              The first element in the combination is the least
%              significant element for defining the lexicographic order.
%
%INPUTS: I The rX1 or 1Xr current combination of r elements. The next
%          combination in lexicographic order is desired. The first element
%          is the least significant and one begins with I=[0;1;2;...;r-1]
%          is startVal=0 and I=[1;2;3;...;r] if startVal=1.
%        n The number of items from which r items are chosen for
%          combinations. The elements of I can range from 0 to n-1. if
%          startVal=0 or they range from 1 to n if startVal=1.
% startVal This is zero or 1, indicating which value the value at which the
%          elements in I can start. The default if omitted or an empty
%          matrix is passed is 0.
%
%OUTPUTS: I The next combination in the lexicographic sequence, or an empty
%           matrix if the final combination in the lexicographic ordering
%           is provided.
%
%This function can be useful for generating combinations when used in a
%loop. It is more computationally efficient than sequentially unranking the
%combinations. If the final combination is put in, an empty matrix will be
%returned.
%
%The algorithm is from [1], with an option to start at 0 instead of 1.
%
%REFERENCES:
%[1] C. J. Mifsud, "Algorithm 154: Combination in lexicographical order,"
%    Communications of the ACM, vol. 6, no. 3 pp. 103, Mar. 1963.
%    modified to start from zero instead of one.
%
%September 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<3||isempty(startVal))
    startVal=0;
end

if(startVal==0)
    r=length(I);
    if(I(r)<n-1)
        I(r)=I(r)+1;
        return
    else
        for j=r:-1:2
           if(I(j-1)<n-r+j-2)
               I(j-1)=I(j-1)+1;
               for s=j:1:r
                  I(s)=I(j-1)+s-(j-1);
               end
               return;
           end
        end
        I=[];
    end
else%startVal=1
    r=length(I);
    if(I(r)<n)
        I(r)=I(r)+1;
        return
    else
        for j=r:-1:2
           if(I(j-1)<n-r+j-1)
               I(j-1)=I(j-1)+1;
               for s=j:1:r
                  I(s)=I(j-1)+s-(j-1);
               end
               return;
           end
        end
        I=[];
    end
end
end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function x = tuple_next_fast ( m, n, rank )

%*****************************************************************************80
%
%% TUPLE_NEXT_FAST computes the next element of a tuple space, "fast".
%
%  Discussion:
%
%    The elements are N vectors.  Each entry is constrained to lie
%    between 1 and M.  The elements are produced one at a time.
%    The first element is
%      (1,1,...,1)
%    and the last element is
%      (M,M,...,M)
%    Intermediate elements are produced in lexicographic order.
%
%    This code was written as a possibly faster version of TUPLE_NEXT.
%
%  Example:
%
%    N = 2,
%    M = 3
%
%    INPUT        OUTPUT
%    -------      -------
%    Rank          X
%    ----          ----
%   -1            -1 -1
%    0             1  1
%    1             1  2
%    2             1  3
%    3             2  1
%    4             2  2
%    5             2  3
%    6             3  1
%    7             3  2
%    8             3  3
%    9             1  1
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 July 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the maximum entry.
%
%    Input, integer N, the number of components.
%
%    Input, integer RANK, indicates the rank of the tuples.
%    However, if RANK < 0, this indicates that this is the first
%    call for the given values of (M,N).  Initialization is done,
%    and X is set to a dummy value.
%
%    Output, integer X(N), the next tuple of the given rank.
%
  global tuple_next_fast_BASE

  if ( rank < 0 )

    tuple_next_fast_BASE(n) = 1;
    for i = n-1 : -1 : 1
      tuple_next_fast_BASE(i) = tuple_next_fast_BASE(i+1) * m;
    end

    x(1:n) = -1;

  else

    x(1:n) = mod ( floor ( rank ./ tuple_next_fast_BASE(1:n) ), m ) + 1;

  end

  return
end


function ydata = tsne_d(D, labels, no_dims, perplexity)
%TSNE_D Performs symmetric t-SNE on the pairwise Euclidean distance matrix D
%
%   mappedX = tsne_d(D, labels, no_dims, perplexity)
%   mappedX = tsne_d(D, labels, initial_solution, perplexity)
%
% The function performs symmetric t-SNE on the NxN pairwise Euclidean
% distance matrix D to construct an embedding with no_dims dimensions
% (default = 2). An initial solution obtained from an other dimensionality
% reduction technique may be specified in initial_solution.
% The perplexity of the Gaussian kernel that is employed can be specified
% through perplexity (default = 30). The labels of the data are not used
% by t-SNE itself, however, they are used to color intermediate plots.
% Please provide an empty labels matrix [] if you don't want to plot
% results during the optimization.
% The data embedding is returned in mappedX.
%
%

% This file is part of the Matlab Toolbox for Dimensionality Reduction.
% The toolbox can be obtained from http://homepage.tudelft.nl/19j49
% You are free to use, change, or redistribute this code in any way you
% want for non-commercial purposes. However, it is appreciated if you
% maintain the name of the original author.
%
% (C) Laurens van der Maaten, Delft University of Technology

    if ~exist('labels', 'var')
        labels = [];
    end
    if ~exist('no_dims', 'var') || isempty(no_dims)
        no_dims = 2;
    end
    if ~exist('perplexity', 'var') || isempty(perplexity)
        perplexity = 30;
    end

    % First check whether we already have an initial solution
    if numel(no_dims) > 1
        initial_solution = true;
        ydata = no_dims;
        no_dims = size(ydata, 2);
    else
        initial_solution = false;
    end

    % Compute joint probabilities
    D = D / max(D(:));                                                      % normalize distances
    P = d2p(D .^ 2, perplexity, 1e-5);                                      % compute affinities using fixed perplexity

    % Run t-SNE
    if initial_solution
        ydata = tsne_p(P, labels, ydata);
    else
        ydata = tsne_p(P, labels, no_dims);
    end


function [oHDev] = calculateHDEV(tau,sPeriod,readings)
%Overlapping Hadamard (ADEV) function that uses phase error or time error values
%input argument is readings already grouped as tau values. tau is the
%desired gate or tau time needed for calculating overlap.
%sPeriod is used to determine the overlap

N = numel(readings); %get the reading count
n = tau / sPeriod; %calculate averaging factor
n = floor(n); %make sure this is an integer
const = 1/(6*(N - (3*n))*tau^2); %calculate the const mult 1/(2*(N - (2*n))*tau^2)
%sum from i=1 to N-(3*n) (Xi+3m - 3Xi+2m + 3Xi+m - Xi)^2
sum = 0; %variable to store summation calculation

%loop for performing summation
for i = 1:(N-(3*n))
   sum = sum + (readings(i+(3*n)) - (3*readings(i+(2*n))) + (3*readings(i+n)) - readings(i))^2;
end

oHDev = sqrt(const*sum); %square root for Allan Dev


function J2=numDiff2(x,f,fDim,N,epsilon)
%%NUMDIFF2 Numerically compute the second derivatives using central
%          difference formulae based on Lagrange interpolating polynomials.
%          The algorithm is made to alert (and not crash) in the event that
%          f fails. Specifically, if f returns an empty matrix or it any of
%          the components of f are NaNs, then the numDiff function will
%          terminate early, returning an empty matrix to indicate failure.
%          The function only computes second derivatives, not considering
%          cross terms (it does not find a Hessian matrix). Use numDiffHess
%          to get a Hessian matrix with cross terms.
%
%INPUTS: x The xDimX1 vector or scalar point at which the second derivative
%          of the (possibly vector) function is desired.
%        f The scalar or vector function that is to be differentiated. The
%          function f must take x as its parameter and its output should be
%          a scalar or a column vector.
%     fDim The dimensionality of the output of f.
%        N A number >=1 specifying the order of the second derivative
%          approximation. Values for n=1 through 3 are explicitly coded in.
%          For values 3 and above, the coefficients of the second
%          derivative of the Lagrange interpolating polynomial are
%          explicitly solved. If omitted, a value of N=1 is used.
%  epsilon A scalar or xDimX1 vector quantity specifying the finite step
%          size used for numerical differentiation. If a scalar value is
%          given, that value is used for differentiating with respect to
%          elements of xDim. If an xDimX1 value is given, then the
%          corresponding element of epsilon is used to differentiate each
%          element of x. If epsilon is omitted, then
%          epsilon=max(1e-5*x,1e-7); is used.
%
%OUTPUTS: J2 A fDimXxDim matrix of second derivatives. Each column is the
%            derivative vector of f with respect to the corresponding
%            element of x. If at any point the function f returned a NaN or
%            an empty matrix, J2 will be an empty matrix.
%
%The function is similar to the numDiff function. Central-difference
%numerical differentiation is discussed in terms of Lagrange interpolating
%polynomials in Chapter 4.1 of [1]. The Lagrange interpolating polynomials
%themselves are discussed in Chapter 3 of [1]. In general, the coefficients
%of the interpolating polynomial for first derivative central difference
%numerical differentiation come from Equation 4.2, which  expresses them in
%terms of the first derivative of a Lagrange interpolating polynomial. This
%function just extends the concept by using the second-derivative
%coefficients.
%
%REFERENCES:
%[1] R. L. Burden and J. D. Faires, Numerical Analysis, 9th ed. Boston, MA:
%    Brooks/ Cole, 2011.
%
%April 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<4)
   N=1;
end

switch(N)
    case 1
        a=[1; -2; 1];
    case 2
        a=[-1/12; 4/3; -5/2; 4/3; -1/12];
    case 3
        a=[1/90; -3/20; 3/2; -49/18; 3/2; -3/20; 1/90];
    otherwise
        [~,~,d2Li]=LagrangeInterpPoly(-N:1:N);
        a=d2Li(end,:);
end

xDim=size(x,1);

if(nargin<5)
    %If epsilon is not specified, then use some ad-hoc default value
    epsilon=max(1e-5*x,1e-7);
end

if(isscalar(epsilon))
   epsilon=repmat(epsilon,[xDim,1]);
end

J2=zeros(fDim,xDim);
for curEl=1:xDim
    epsCur=epsilon(curEl);

    curIdx=1;
    for curP=-N:1:N
        xP=x;
        xP(curEl)=xP(curEl)+curP*epsCur;
        fxP=f(xP);
        if(isempty(fxP)||any(isnan(fxP)))
            J2=[];
            return;
        end
        J2(:,curEl)=J2(:,curEl)+a(curIdx)*fxP;
        curIdx=curIdx+1;
    end
    J2(:,curEl)=J2(:,curEl)/(epsCur^2);
end

end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function fn = facenormals( V,F )
%FACENORMALS Compute face normals of triangular mesh
%   V - nverts by 3 matrix containing vertex positions
%   F - ntri by 3 matrix containing triangle vertex indices

% Get the triangle vertices
v1      = F(:, 1);
v2      = F(:, 2);
v3      = F(:, 3);

% Compute the edge vectors
e1s = V(v2, :) - V(v1, :);
e2s = V(v3, :) - V(v1, :);

% Compute cross products between edge vectors
fn    = cross(e1s, e2s, 2);

% Normalise face normals to unit length
norms = sqrt(fn(:,1).^2+fn(:,2).^2+fn(:,3).^2);
fn=fn./repmat(norms,[1 3]);

end


function [ output_maps ] = max_pooling( input_maps)
%POOLING Summary of this function goes here
%   Detailed explanation goes here

    orig_rows = size(input_maps,1);
    orig_cols = size(input_maps,2);

    pooled_rows = ceil(orig_rows / 2);
    pooled_cols = ceil(orig_cols / 2);

    up_to_rows_out = floor(orig_rows / 2);
    up_to_cols_out = floor(orig_cols / 2);

    if(mod(orig_cols,2) == 0)
        up_to_cols = orig_cols;
    else
        up_to_cols = orig_cols - 1;
    end

    if(mod(orig_rows,2) == 0)
        up_to_rows = orig_rows;
    else
        up_to_rows = orig_rows - 1;
    end

    output_maps = zeros(pooled_rows, pooled_cols, size(input_maps,3));
    for i=1:size(input_maps,3)
        temp = im2col(input_maps(1:up_to_rows,1:up_to_cols,i), [2,2], 'distinct');
        max_val = max(temp);
        output_maps(1:up_to_rows_out,1:up_to_cols_out,i) = reshape(max_val, up_to_rows_out, up_to_cols_out);
    end

    % A bit of a hack for non-even number of rows or columns
    if(mod(orig_cols,2) ~= 0)
        for i=1:size(input_maps,3)
            temp = im2col(input_maps(1:up_to_rows,end,i), [2,1], 'distinct');
            max_val = max(temp);
            output_maps(1:up_to_rows_out,end,i) = max_val;
        end
    end

    if(mod(orig_rows,2) ~= 0)
        for i=1:size(input_maps,3)
            temp = im2col(input_maps(end, 1:up_to_cols,i), [1,2], 'distinct');
            max_val = max(temp);
            output_maps(end, 1:up_to_cols_out,i) = max_val;
        end
    end

    if(mod(orig_cols,2) ~= 0 && mod(orig_rows,2) ~= 0)
        output_maps(end,end,:) = input_maps(end,end,:);
    end

end