text:
function zi = interp2_mine(varargin)
%INTERP2 2-D interpolation (table lookup).
%   ZI = INTERP2(X,Y,Z,XI,YI) interpolates to find ZI, the values of the
%   underlying 2-D function Z at the points in matrices XI and YI.
%   Matrices X and Y specify the points at which the data Z is given.
%
%   XI can be a row vector, in which case it specifies a matrix with
%   constant columns. Similarly, YI can be a column vector and it
%   specifies a matrix with constant rows.
%
%   ZI = INTERP2(Z,XI,YI) assumes X=1:N and Y=1:M where [M,N]=SIZE(Z).
%   ZI = INTERP2(Z,NTIMES) expands Z by interleaving interpolates between
%   every element, working recursively for NTIMES.  INTERP2(Z) is the
%   same as INTERP2(Z,1).
%
%   ZI = INTERP2(...,METHOD) specifies alternate methods.  The default
%   is linear interpolation.  Available methods are:
%
%     'nearest' - nearest neighbor interpolation
%     'linear'  - bilinear interpolation
%     'spline'  - spline interpolation
%     'cubic'   - bicubic interpolation as long as the data is
%                 uniformly spaced, otherwise the same as 'spline'
%
%   For faster interpolation when X and Y are equally spaced and monotonic,
%   use the syntax ZI = INTERP2(...,*METHOD).
%
%   ZI = INTERP2(...,METHOD,EXTRAPVAL) specificies a method and a scalar
%   value for ZI outside of the domain created by X and Y.  Thus, ZI will
%   equal EXTRAPVAL for any value of YI or XI which is not spanned by Y
%   or X respectively. A method must be specified for EXTRAPVAL to be used,
%   the default method is 'linear'.
%
%   All the interpolation methods require that X and Y be monotonic and
%   plaid (as if they were created using MESHGRID).  If you provide two
%   monotonic vectors, interp2 changes them to a plaid internally.
%   X and Y can be non-uniformly spaced.
%
%   For example, to generate a coarse approximation of PEAKS and
%   interpolate over a finer mesh:
%       [x,y,z] = peaks(10); [xi,yi] = meshgrid(-3:.1:3,-3:.1:3);
%       zi = interp2(x,y,z,xi,yi); mesh(xi,yi,zi)
%
%   Class support for inputs X, Y, Z, XI, YI:
%      float: double, single
%
%   See also INTERP1, INTERP3, INTERPN, MESHGRID, TriScatteredInterp.

%   Copyright 1984-2011 The MathWorks, Inc.
%   $Revision: 5.33.4.24 $  $Date: 2011/05/17 02:32:27 $

% error(nargchk(1,7,nargin,'struct')); % allowing for an ExtrapVal

bypass = false;
uniform = true;
if (nargin > 1)
    if nargin == 7 && ~isnumeric(varargin{end})
        error(message('MATLAB:interp2:extrapvalNotNumeric'));
    end
    if ischar(varargin{end})
        narg = nargin-1;
        method = [varargin{end} '    ']; % Protect against short string.
        if strncmpi(method,'s',1) || strncmpi(method, '*s', 2)
            ExtrapVal = 'extrap'; % Splines can extrapolate
        else
            ExtrapVal = nan; % setting default ExtrapVal as NAN
        end
        index = 1; %subtract off the elements not in method
    elseif ischar(varargin{end-1}) && isnumeric(varargin{end})
        narg = nargin-2;
        method = [ varargin{end-1} '    '];
        ExtrapVal = varargin{end}; % user specified ExtrapVal
        index = 2; % subtract off the elements not in method and ExtrapVal
    else
        narg = nargin;
        method = 'linear';
        ExtrapVal = nan; % protecting default
        index = 0;
    end
    if strncmpi(method,'*',1) % Direct call bypass.
        if (narg ==5 || narg ==3)
            xitemp = varargin{end-index - 1};
            yitemp = varargin{end-index};
            if isrow(xitemp) && iscolumn(yitemp)
                varargin{end-index - 1} = repmat(xitemp, [size(yitemp,1), 1]);
                varargin{end-index} = repmat(yitemp, [1, size(xitemp,2)]);
            elseif iscolumn(xitemp) && isrow(yitemp)
                varargin{end-index - 1} = repmat(xitemp', [size(yitemp, 2), 1]);
                varargin{end-index} = repmat(yitemp', [1, size(xitemp,1)]);
            end
        end
        if strcmpi(method(2),'l') || strcmpi(method(2:4),'bil')
            % bilinear interpolation.
            zi = linear(ExtrapVal, varargin{1:end-index});
            return
        elseif strcmpi(method(2),'c') || strcmpi(method(2:4),'bic')
            % bicubic interpolation
            zi = cubic(ExtrapVal, varargin{1:end-index});
            return
        elseif strcmpi(method(2),'n')
            % Nearest neighbor interpolation
            zi = nearest(ExtrapVal, varargin{1:end-index});
            return
        elseif strcmpi(method(2),'s')
            % spline interpolation
            method = 'spline'; bypass = true;
        else
            error(message('MATLAB:interp2:InvalidMethod', deblank( method )));
        end
    elseif strncmpi(method,'s',1), % Spline interpolation
        method = 'spline'; bypass = true;
    end
else
    narg = nargin;
    method = 'linear';
    ExtrapVal = nan; % default ExtrapVal is NaN
end

% if narg==1, % interp2(z), % Expand Z
%     [nrows,ncols] = size(varargin{1});
%     xi = 1:.5:ncols; yi = (1:.5:nrows)';
%     x = 1:ncols; y = 1:nrows;
%     [msg,x,y,z,xi,yi] = xyzchk(x,y,varargin{1},xi,yi);
%
% elseif narg==2. % interp2(z,n), Expand Z n times
%     [nrows,ncols] = size(varargin{1});
%     ntimes = floor(varargin{2}(1));
%     xi = 1:1/(2^ntimes):ncols; yi = (1:1/(2^ntimes):nrows)';
%     x = 1:ncols; y = 1:nrows;
%     [msg,x,y,z,xi,yi] = xyzchk(x,y,varargin{1},xi,yi);
%
% elseif narg==3, % interp2(z,xi,yi)
%     [nrows,ncols] = size(varargin{1});
%     x = 1:ncols; y = 1:nrows;
%     [msg,x,y,z,xi,yi] = xyzchk(x,y,varargin{1:3});
%
% elseif narg==4,
%     error(message('MATLAB:interp2:nargin'));

% elseif narg==5, % linear(x,y,z,xi,yi)
%     [msg,x,y,z,xi,yi] = xyzchk(varargin{1:5});
%
% end
x = varargin{1};
y = varargin{2};
z = varargin{3};
xi = varargin{4};
yi = varargin{5};

% if ~isempty(msg)
%     error(message(msg.identifier));
% end

%
% Check for plaid data.
%
xx = x(1,:); yy = y(:,1);
% if (size(x,2)>1 && ~isequal(repmat(xx,size(x,1),1),x)) || ...
%    (size(y,1)>1 && ~isequal(repmat(yy,1,size(y,2)),y)),
%     error(message('MATLAB:interp2:meshgrid'));
% end

%
% Check for non-equally spaced data.  If so, map (x,y) and
% (xi,yi) to matrix (row,col) coordinate system.
%
if ~bypass,
    xx = xx.'; % Make sure it's a column.
    dx = diff(xx); dy = diff(yy);
    xdiff = max(abs(diff(dx))); if isempty(xdiff), xdiff = 0; end
    ydiff = max(abs(diff(dy))); if isempty(ydiff), ydiff = 0; end
    if (xdiff > eps(class(xx))*max(abs(xx))) || (ydiff > eps(class(yy))*max(abs(yy)))
        if any(dx < 0), % Flip orientation of data so x is increasing.
            x = fliplr(x); y = fliplr(y); z = fliplr(z);
            xx = flipud(xx); dx = -flipud(dx);
        end
        if any(dy < 0), % Flip orientation of data so y is increasing.
            x = flipud(x); y = flipud(y); z = flipud(z);
            yy = flipud(yy); dy = -flipud(dy);
        end

        if any(dx<=0) || any(dy<=0),
            error(message('MATLAB:interp2:XorYNotMonotonic'));
        end

        % Bypass mapping code for cubic
        if ~strncmp(method(1),'c',1)
            % Determine the nearest location of xi in x
            [xxi,j] = sort(xi(:));
            [~,i] = sort([xx;xxi]);
            ui(i) = 1:length(i);
            ui = (ui(length(xx)+1:end)-(1:length(xxi)))';
            ui(j) = ui;

            % Map values in xi to index offset (ui) via linear interpolation
            ui(ui<1) = 1;
            ui(ui>length(xx)-1) = length(xx)-1;
            ui = ui + (xi(:)-xx(ui))./(xx(ui+1)-xx(ui));

            % Determine the nearest location of yi in y
            [yyi,j] = sort(yi(:));
            [~,i] = sort([yy;yyi(:)]);
            vi(i) = 1:length(i);
            vi = (vi(length(yy)+1:end)-(1:length(yyi)))';
            vi(j) = vi;

            % Map values in yi to index offset (vi) via linear interpolation
            vi(vi<1) = 1;
            vi(vi>length(yy)-1) = length(yy)-1;
            vi = vi + (yi(:)-yy(vi))./(yy(vi+1)-yy(vi));

            [x,y] = meshgrid(ones(class(x)):size(x,2),ones(class(y)):size(y,1));
            xi(:) = ui; yi(:) = vi;
        else
            uniform = false;
        end
    end
end

% Now do the interpolation based on method.
if strncmpi(method,'l',1) || strncmpi(method,'bil',3) % bilinear interpolation.
    zi = linear(ExtrapVal,x,y,z,xi,yi);

elseif strncmpi(method,'c',1) || strncmpi(method,'bic',3) % bicubic interpolation
    if uniform
        zi = cubic(ExtrapVal,x,y,z,xi,yi);
    else
        zi = spline2(x,y,z,xi,yi,ExtrapVal);
    end

elseif strncmpi(method,'n',1) % Nearest neighbor interpolation
    zi = nearest(ExtrapVal,x,y,z,xi,yi);

elseif strncmpi(method,'s',1) % Spline interpolation
    % A column is removed from z if it contains a NaN.
    % Orient to preserve as much data as possible.
    [inan, jnan] = find(isnan(z));
    ncolnan = length(unique(jnan));
    nrownan = length(unique(inan));
    if ncolnan > nrownan
        zi = spline2(y',x',z',yi,xi,ExtrapVal);
    else
        zi = spline2(x,y,z,xi,yi,ExtrapVal);
    end
else
    error(message('MATLAB:interp2:InvalidMethod', deblank( method )));

end

%------------------------------------------------------
function F = linear(ExtrapVal,arg1,arg2,arg3,arg4,arg5)
%LINEAR 2-D bilinear data interpolation.
%   ZI = LINEAR(EXTRAPVAL,X,Y,Z,XI,YI) uses bilinear interpolation to
%   find ZI, the values of the underlying 2-D function in Z at the points
%   in matrices XI and YI.  Matrices X and Y specify the points at which
%   the data Z is given.  X and Y can also be vectors specifying the
%   abscissae for the matrix Z as for MESHGRID. In both cases, X
%   and Y must be equally spaced and monotonic.
%
%   Values of EXTRAPVAL are returned in ZI for values of XI and YI that are
%   outside of the range of X and Y.
%
%   If XI and YI are vectors, LINEAR returns vector ZI containing
%   the interpolated values at the corresponding points (XI,YI).
%
%   ZI = LINEAR(EXTRAPVAL,Z,XI,YI) assumes X = 1:N and Y = 1:M, where
%   [M,N] = SIZE(Z).
%
%   ZI = LINEAR(EXTRAPVAL,Z,NTIMES) returns the matrix Z expanded by
%   interleaving bilinear interpolates between every element, working
%   recursively for NTIMES. LINEAR(EXTRAPVAL,Z) is the same as
%   LINEAR(EXTRAPVAL,Z,1).
%
%   See also INTERP2, CUBIC.

if nargin==2 % linear(extrapval,z), Expand Z
    [nrows,ncols] = size(arg1);
    s = 1:.5:ncols; lengths = length(s);
    t = (1:.5:nrows)'; lengtht = length(t);
    s = repmat(s,lengtht,1);
    t = repmat(t,1,lengths);

elseif nargin==3 % linear(extrapval,z,n), Expand Z n times
    [nrows,ncols] = size(arg1);
    ntimes = floor(arg2);
    s = 1:1/(2^ntimes):ncols; lengths = length(s);
    t = (1:1/(2^ntimes):nrows)'; lengtht = length(t);
    s = repmat(s,lengtht,1);
    t = repmat(t,1,lengths);

elseif nargin==4 % linear(extrapval,z,s,t), No X or Y specified.
    [nrows,ncols] = size(arg1);
    s = arg2; t = arg3;

elseif nargin==5
    error(message('MATLAB:interp2:linear:nargin'));

elseif nargin==6 % linear(extrapval,x,y,z,s,t), X and Y specified.
    [nrows,ncols] = size(arg3);
    mx = numel(arg1); my = numel(arg2);
    if (mx ~= ncols || my ~= nrows) && ~isequal(size(arg1),size(arg2),size(arg3))
        error(message('MATLAB:interp2:linear:XYZLengthMismatch'));
    end
    if nrows < 2 || ncols < 2
        error(message('MATLAB:interp2:linear:sizeZ'));
    end
    s = 1 + (arg4-arg1(1))/(arg1(end)-arg1(1))*(ncols-1);
    t = 1 + (arg5-arg2(1))/(arg2(end)-arg2(1))*(nrows-1);

end

if nrows < 2 || ncols < 2
    error(message('MATLAB:interp2:linear:sizeZsq'));
end
if ~isequal(size(s),size(t))
    error(message('MATLAB:interp2:linear:XIandYISizeMismatch'));
end

% Check for out of range values of s and set to 1
sout = find((s<1)|(s>ncols));
if ~isempty(sout), s(sout) = 1; end

% Check for out of range values of t and set to 1
tout = find((t<1)|(t>nrows));
if ~isempty(tout), t(tout) = 1; end

% Matrix element indexing
ndx = floor(t)+floor(s-1)*nrows;

% Compute intepolation parameters, check for boundary value.
if isempty(s), d = s; else d = find(s==ncols); end
s(:) = (s - floor(s));
if ~isempty(d), s(d) = s(d)+1; ndx(d) = ndx(d)-nrows; end

% Compute intepolation parameters, check for boundary value.
if isempty(t), d = t; else d = find(t==nrows); end
t(:) = (t - floor(t));
if ~isempty(d), t(d) = t(d)+1; ndx(d) = ndx(d)-1; end

% Now interpolate.
onemt = 1-t;
if nargin==6,
    F =  ( arg3(ndx).*(onemt) + arg3(ndx+1).*t ).*(1-s) + ...
         ( arg3(ndx+nrows).*(onemt) + arg3(ndx+(nrows+1)).*t ).*s;
else
    F =  ( arg1(ndx).*(onemt) + arg1(ndx+1).*t ).*(1-s) + ...
         ( arg1(ndx+nrows).*(onemt) + arg1(ndx+(nrows+1)).*t ).*s;
end

% Now set out of range values to ExtrapVal.
if ~isempty(sout), F(sout) = ExtrapVal; end
if ~isempty(tout), F(tout) = ExtrapVal; end

%------------------------------------------------------
function F = cubic(ExtrapVal,arg1,arg2,arg3,arg4,arg5)
%CUBIC 2-D bicubic data interpolation.
%   CUBIC(...) is the same as LINEAR(....) except that it uses
%   bicubic interpolation.
%
%   This function needs about 7-8 times SIZE(XI) memory to be available.
%
%   See also LINEAR.

%   Based on "Cubic Convolution Interpolation for Digital Image
%   Processing", Robert G. Keys, IEEE Trans. on Acoustics, Speech, and
%   Signal Processing, Vol. 29, No. 6, Dec. 1981, pp. 1153-1160.

if nargin==2, % cubic(extrapval,z), Expand Z
    [nrows,ncols] = size(arg1);
    s = 1:.5:ncols; lengths = length(s);
    t = (1:.5:nrows)'; lengtht = length(t);
    s = repmat(s,lengtht,1);
    t = repmat(t,1,lengths);

elseif nargin==3, % cubic(extrapval,z,n), Expand Z n times
    [nrows,ncols] = size(arg1);
    ntimes = floor(arg2);
    s = 1:1/(2^ntimes):ncols; lengths = length(s);
    t = (1:1/(2^ntimes):nrows)'; lengtht = length(t);
    s = repmat(s,lengtht,1);
    t = repmat(t,1,lengths);

elseif nargin==4, % cubic(extrapval,z,s,t), No X or Y specified.
    [nrows,ncols] = size(arg1);
    s = arg2; t = arg3;

elseif nargin==5,
    error(message('MATLAB:interp2:cubic:nargin'));

elseif nargin==6, % cubic(extrapval,x,y,z,s,t), X and Y specified.
    [nrows,ncols] = size(arg3);
    mx = numel(arg1); my = numel(arg2);
    if (mx ~= ncols || my ~= nrows) && ~isequal(size(arg1),size(arg2),size(arg3))
        error(message('MATLAB:interp2:cubic:XYZLengthMismatch'));
    end
    if nrows < 3 || ncols < 3
        error(message('MATLAB:interp2:cubic:sizeZ'));
    end
    s = 1 + (arg4-arg1(1))/(arg1(end)-arg1(1))*(ncols-1);
    t = 1 + (arg5-arg2(1))/(arg2(end)-arg2(1))*(nrows-1);

end

if nrows < 3 || ncols < 3
    error(message('MATLAB:interp2:cubic:sizeZsq'));
end
if ~isequal(size(s),size(t)),
    error(message('MATLAB:interp2:cubic:XIandYISizeMismatch'));
end

% Check for out of range values of s and set to 1
sout = find((s<1)|(s>ncols));
if ~isempty(sout), s(sout) = 1; end

% Check for out of range values of t and set to 1
tout = find((t<1)|(t>nrows));
if ~isempty(tout), t(tout) = 1; end

% Matrix element indexing
ndx = floor(t)+floor(s-1)*(nrows+2);

% Compute intepolation parameters, check for boundary value.
if isempty(s), d = s; else d = find(s==ncols); end
s(:) = (s - floor(s));
if ~isempty(d), s(d) = s(d)+1; ndx(d) = ndx(d)-nrows-2; end

% Compute intepolation parameters, check for boundary value.
if isempty(t), d = t; else d = find(t==nrows); end
t(:) = (t - floor(t));
if ~isempty(d), t(d) = t(d)+1; ndx(d) = ndx(d)-1; end

if nargin==6,
    % Expand z so interpolation is valid at the boundaries.
    zz = zeros(size(arg3)+2);
    zz(1,2:ncols+1) = 3*arg3(1,:)-3*arg3(2,:)+arg3(3,:);
    zz(2:nrows+1,2:ncols+1) = arg3;
    zz(nrows+2,2:ncols+1) = 3*arg3(nrows,:)-3*arg3(nrows-1,:)+arg3(nrows-2,:);
    zz(:,1) = 3*zz(:,2)-3*zz(:,3)+zz(:,4);
    zz(:,ncols+2) = 3*zz(:,ncols+1)-3*zz(:,ncols)+zz(:,ncols-1);
    nrows = nrows+2; %also ncols = ncols+2;
else
    % Expand z so interpolation is valid at the boundaries.
    zz = zeros(size(arg1)+2);
    zz(1,2:ncols+1) = 3*arg1(1,:)-3*arg1(2,:)+arg1(3,:);
    zz(2:nrows+1,2:ncols+1) = arg1;
    zz(nrows+2,2:ncols+1) = 3*arg1(nrows,:)-3*arg1(nrows-1,:)+arg1(nrows-2,:);
    zz(:,1) = 3*zz(:,2)-3*zz(:,3)+zz(:,4);
    zz(:,ncols+2) = 3*zz(:,ncols+1)-3*zz(:,ncols)+zz(:,ncols-1);
    nrows = nrows+2; %also ncols = ncols+2;
end

% Now interpolate using computationally efficient algorithm.
t0 = ((2-t).*t-1).*t;
t1 = (3*t-5).*t.*t+2;
t2 = ((4-3*t).*t+1).*t;
t(:) = (t-1).*t.*t;
F     = ( zz(ndx).*t0 + zz(ndx+1).*t1 + zz(ndx+2).*t2 + zz(ndx+3).*t ) ...
    .* (((2-s).*s-1).*s);
ndx(:) = ndx + nrows;
F(:)  = F + ( zz(ndx).*t0 + zz(ndx+1).*t1 + zz(ndx+2).*t2 + zz(ndx+3).*t ) ...
    .* ((3*s-5).*s.*s+2);
ndx(:) = ndx + nrows;
F(:)  = F + ( zz(ndx).*t0 + zz(ndx+1).*t1 + zz(ndx+2).*t2 + zz(ndx+3).*t ) ...
    .* (((4-3*s).*s+1).*s);
ndx(:) = ndx + nrows;
F(:)  = F + ( zz(ndx).*t0 + zz(ndx+1).*t1 + zz(ndx+2).*t2 + zz(ndx+3).*t ) ...
    .* ((s-1).*s.*s);
F(:) = F/4;

% Now set out of range values to ExtrapVal.
if ~isempty(sout), F(sout) = ExtrapVal; end
if ~isempty(tout), F(tout) = ExtrapVal; end

%------------------------------------------------------
function F = nearest(ExtrapVal,arg1,arg2,arg3,arg4,arg5)
%NEAREST 2-D Nearest neighbor interpolation.
%   ZI = NEAREST(EXTRAPVAL,X,Y,Z,XI,YI) uses nearest neighbor interpolation
%   to find ZI, the values of the underlying 2-D function in Z at the points
%   in matrices XI and YI.  Matrices X and Y specify the points at which
%   the data Z is given.  X and Y can also be vectors specifying the
%   abscissae for the matrix Z as for MESHGRID. In both cases, X
%   and Y must be equally spaced and monotonic.
%
%   Values of EXTRAPVAL are returned in ZI for values of XI and YI that are
%   outside of the range of X and Y.
%
%   If XI and YI are vectors, NEAREST returns vector ZI containing
%   the interpolated values at the corresponding points (XI,YI).
%
%   ZI = NEAREST(EXTRAPVAL,Z,XI,YI) assumes X = 1:N and Y = 1:M, where
%   [M,N] = SIZE(Z).
%
%   F = NEAREST(EXTRAPVAL,Z,NTIMES) returns the matrix Z expanded by
%   interleaving interpolates between every element.  NEAREST(EXTRAPVAL,Z)
%   is the same as NEAREST(EXTRAPVAL,Z,1).
%
%   See also INTERP2, LINEAR, CUBIC.

if nargin==2, % nearest(z), Expand Z
    [nrows,ncols] = size(arg1);
    u = 1:.5:ncols; lengthu = length(u);
    v = (1:.5:nrows)'; lengthv = length(v);
    u = repmat(u,lengthv,1);
    v = repmat(v,1,lengthu);

elseif nargin==3, % nearest(z,n), Expand Z n times
    [nrows,ncols] = size(arg1);
    ntimes = floor(arg2);
    u = 1:1/(2^ntimes):ncols; lengthu = length(u);
    v = (1:1/(2^ntimes):nrows)'; lengthv = length(v);
    u = repmat(u,lengthv,1);
    v = repmat(v,1,lengthu);

elseif nargin==4, % nearest(z,u,v)
    [nrows,ncols] = size(arg1);
    u = arg2; v = arg3;

elseif nargin==5,
    error(message('MATLAB:interp2:nearest:nargin'));

elseif nargin==6, % nearest(x,y,z,u,v), X and Y specified.
    [nrows,ncols] = size(arg3);
    mx = numel(arg1); my = numel(arg2);
    if (mx ~= ncols || my ~= nrows) && ...
            ~isequal(size(arg1),size(arg2),size(arg3))
        error(message('MATLAB:interp2:nearest:XYZLengthMismatch'));
    end
    if nrows > 1 && ncols > 1
        u = 1 + (arg4-arg1(1))/(arg1(mx)-arg1(1))*(ncols-1);
        v = 1 + (arg5-arg2(1))/(arg2(my)-arg2(1))*(nrows-1);
    else
        u = 1 + (arg4-arg1(1));
        v = 1 + (arg5-arg2(1));
    end
end

if ~isequal(size(u),size(v))
    error(message('MATLAB:interp2:nearest:XIandYISizeMismatch'));
end

% Check for out of range values of u and set to 1
uout = (u<.5)|(u>=ncols+.5);
anyuout = any(uout(:));
if anyuout, u(uout) = 1; end

% Check for out of range values of v and set to 1
vout = (v<.5)|(v>=nrows+.5);
anyvout = any(vout(:));
if anyvout, v(vout) = 1; end

% Interpolation parameters
u = round(u); v = round(v);

% Now interpolate
ndx = v+(u-1)*nrows;
if nargin==6,
    F = arg3(ndx);
else
    F = arg1(ndx);
end

% Now set out of range values to ExtrapVal.
if anyuout, F(uout) = ExtrapVal; end
if anyvout, F(vout) = ExtrapVal; end

%----------------------------------------------------------
function F = spline2(varargin)
%2-D spline interpolation

% Determine abscissa vectors
varargin{1} = varargin{1}(1,:);
varargin{2} = varargin{2}(:,1).';

%
% Check for plaid data.
%
xi = varargin{4}; yi = varargin{5};
xxi = xi(1,:); yyi = yi(:,1);

if ~isequal(repmat(xxi,size(xi,1),1),xi) || ...
        ~isequal(repmat(yyi,1,size(yi,2)),yi)
    F = splncore(varargin(2:-1:1),varargin{3},varargin(5:-1:4));
else
    F = splncore(varargin(2:-1:1),varargin{3},{yyi(:).' xxi},'gridded');
end

ExtrapVal = varargin{6};
% Set out-of-range values to ExtrapVal
if isnumeric(ExtrapVal)
    d = xi < min(varargin{1}) | xi > max(varargin{1}) | ...
        yi < min(varargin{2}) | yi > max(varargin{2});
    F(d) = ExtrapVal;
end

author: TadasBaltrusaitis

repo: OpenFace


text:
function cv2tifs(y,f)
%CV2TIFS Decodes a TIFS2CV compressed image sequence.
%   Y = CV2TIFS(Y,F) decodes compressed sequence Y (a structure
%   generated by TIFS2CV) and creates a multiframe TIFF file F.
%
%   See also TIFS2CV.
%
%   Copyright 2002-2020 Gatesmark
%
%   This function, and other functions in the DIPUM Toolbox, are based
%   on the theoretical and practical foundations established in the
%   book Digital Image Processing Using MATLAB, 3rd ed., Gatesmark
%   Press, 2020.
%
%   Book website: http://www.imageprocessingplace.com
%   License: https://github.com/dipum/dipum-toolbox/blob/master/LICENSE.txt

% Get the number of frames, block size, and reconstruction quality.
fcnt = double(y.frames);
m = double(y.blksz);
q = double(y.quality);

% Reconstruct the first image in the sequence and store.
if q == 0
    r = double(huff2mat(y.video(1)));
else
    r = double(jpeg2im(y.video(1)));
end
imwrite(uint8(r),f,'Compression','none','WriteMode','overwrite');

% Get the frame size and motion vectors.
fsz = size(r);
mvsz = [fsz/m 2 fcnt];
mv = int16(huff2mat(y.motion));
mv = reshape(mv,mvsz);

% For frames except the first, get a motion conpensated prediction
% residual and add to the proper reference subimages.
for i = 2:fcnt
    if q == 0
        pe = double(huff2mat(y.video(i)));
    else
        pe = double(jpeg2im(y.video(i)) - 255);
    end
    peC = im2col(pe,[m m],'distinct');

    for col = 1:size(peC,2)
        u = 1 + mod(m * (col - 1),fsz(1));
        v = 1 + m * floor((col - 1) * m / fsz(1));
        rx = u - mv(1 + floor((u - 1)/m), 1 + floor((v - 1)/m), ...
           1, i);
        ry = v - mv(1 + floor((u - 1)/m), 1 + floor((v - 1)/m), ...
           2, i);

        subimage = r(rx:rx + m - 1,ry:ry + m - 1);
        peC(:,col) = subimage(:) - peC(:,col);
    end

    r = col2im(double(uint16(peC)),[m m],fsz,'distinct');
    imwrite(uint8(r),f,'Compression','none', ...
        'WriteMode','append');
end

author: dipum

repo: dipum-toolbox

save_path: github-repos/MATLAB/dipum-dipum-toolbox


text:
function [K,C,strain,A,M] = linear_elasticity_stiffness(V,F,varargin)
  %
  % [K,C,strain,A,M] = linear_elasticity_stiffness(V,F)
  %
  % Inputs:
  %   V  #V by d list of vertex positions
  %   F  #F by d+1 list of  element indices into V
  %   Optional:
  %     'Lambda'  followed by first Lamé parameter {1.7423333}, scalar
  %       (homogeneous) or #F by 1 list of per-element values
  %     'Mu'  followed by shear modulus {0.0115}, scalar (homogeneous) or #F by
  %       1 list of per-element values
  %     'Young'  followed by Young's modulus, scalar (homogeneous) or #F by 1
  %       list of per-element values
  %     'Nu'  followed by Poisson's ratio, scalar (homogeneous) or #F by 1 list
  %       of per-element values
  % Outputs:
  %   K  #V*d by #V*d sparse stiffness matrix
  %   C  #F**(d*(d+1)/2) by #F**(d*(d+1)/2) sparse constituitive model matrix
  %   strain  #F*(d*(d+1)/2) by #V*d sparse strain matrix
  %   A  #F*(d*(d+1)/2) by #F*(d*(d+1)/2) diagonal element area matrix
  %   M  #V*d by #V*d sparse mass matrix
  %

  % Silicone rubber: http://www.azom.com/properties.aspx?ArticleID=920
  mu = 0.0115;
  % Bulk modulus
  K = 1.75;
  lambda = K-2/3*mu;
  young = [];
  nu = [];
  % Map of parameter names to variable names
  params_to_variables = containers.Map( ...
    {'Lambda','Mu','Nu','Young'}, ...
    {'lambda','mu','nu','young'});
  v = 1;
  while v <= numel(varargin)
    param_name = varargin{v};
    if isKey(params_to_variables,param_name)
      assert(v+1<=numel(varargin));
      v = v+1;
      % Trick: use feval on anonymous function to use assignin to this workspace
      feval(@()assignin('caller',params_to_variables(param_name),varargin{v}));
    else
      error('Unsupported parameter: %s',varargin{v});
    end
    v=v+1;
  end

  assert( ...
    (~isempty(lambda) && ~isempty(mu))||(~isempty(young) && ~isempty(nu)), ...
    'Must define either lambda and mu or young and nu');
  if (~isempty(young) && ~isempty(nu))
    lambda = young.*nu./((1+nu).*(1-2.*nu));
    mu = .5.*young./(1+nu);
  end

  dim = size(V,2);

  % This matches the matlab code by Jonas Koko, in
  % "Vectorized Matlab Codes for Linear Two-Dimensional Elasticity"

  % Gradient/divergence operator
  G = grad(V,F);

  I = speye(size(F,1));
  Z = sparse(size(F,1),size(V,1));
  lambda = diag(sparse(lambda));
  mu = diag(sparse(mu));
  % Strain tensor
  %
  %   ϵ = ½(∇u + (∇u)')
  %   ϵ = ½ // ∂u₁/∂x₁  ∂u₂/∂x₁ \  + / ∂u₁/∂x₁  ∂u₁/∂x₂ \\
  %         \\ ∂u₁/∂x₂  ∂u₂/∂x₂ /    \ ∂u₂/∂x₁  ∂u₂/∂x₂ //
  %
  %                                "Voigt" notation
  %   ϵ₁₁ = ∂u₁/∂x₁              = ϵ₁
  %   ϵ₂₂ = ∂u₂/∂x₂              = ϵ₂
  %   ϵ₁₂ = ½(∂u₂/∂x₁ + ∂u₁/∂x₁) = ½ ϵ₃
  %   ϵ₂₁ = ϵ₁₂                  = ½ ϵ₃
  %
  switch dim
  case 2
    G1 = G(1:size(F,1),:);
    G2 = G(size(F,1)+(1:size(F,1)),:);
    % 3#F by 2#V
    strain = [G1 Z;Z G2;G2 G1];
    % Stiffness tensor
    %
    %    σ = C:ϵ        %  A:B = Aij Bij
    %                   %      = ∑∑ Aij Bij, where in this case Aij is a 2x2
    %                   %                    matrix, and Bij is a scalar
    %
    % For each face we have:
    %
    %    2x2 = 2x2x2x2 2x2
    %    σf  = Cf : ϵf
    %    σ = ∑∑ Cij ϵij, where Cij is a 2x2 matrix
    %    σkl = ∑∑ Cijkl ϵij, where Cijkl is a scalar
    %
    % But really ϵf and σf are just 3 distinct values:
    %
    %   σ₁ = [ϵ₁ ϵ₂ ϵ₃] [ c₁₁ ; c₁₂ ; c₁₃ ]
    %   σ₂ = [ϵ₁ ϵ₂ ϵ₃] [ c₂₁ ; c₂₂ ; c₂₃ ]
    %   σ₃ = [ϵ₁ ϵ₂ ϵ₃] [ c₃₁ ; c₃₂ ; c₃₃ ]
    %
    %    /σ₁\     /c₁₁ c₁₂ c₁₃\  /ϵ₁\
    %   | σ₂ | = | c₂₁ c₂₂ c₂₃ || ϵ₂ |
    %    \σ₃/     \c₃₁ c₃₂ c₃₃/  \ϵ₃/
    %
    % So if σ is a 3#F by 1 vector and ϵ is a 3#F vector then:
    %
    %   σ = C ϵ
    %        /C₁₁ C₁₂ C₁₃\  /ϵ₁\
    %   σ = | C₂₁ C₂₂ C₂₃ || ϵ₂ |
    %        \C₃₁ C₃₂ C₃₃/  \ϵ₃/
    %
    %  where C is 3#F by 3#F matrix and Cij = diagonal #F by #F matrix.
    %
    % For Isotropic homogeneous media, we have that:
    %
    %   σij = λ δij ϵkk + 2μ ϵij
    %   σij = λ δij (∑ ϵkk) + 2μ ϵij
    %
    % where λ is Lamé's first parameter and μ is the shear modulus: the bulk
    % modulus is thus K := λ + ⅔ μ
    %
    % Or in Voigt notation:
    %
    %   σ₁ = σ₁₁ = λ (ϵ₁ + ϵ₂) + 2μ ϵ₁
    %   σ₂ = σ₂₂ = λ (ϵ₁ + ϵ₂) + 2μ ϵ₂
    %   σ₃ = σ₁₂ = λ (ϵ₁ + ϵ₂) + 2μ ϵ₁₂
    %            = λ (ϵ₁ + ϵ₂) + μ ϵ₃
    %
    %        //λ  λ 0\   /2μ  0  0\\
    %  σ =  || λ  λ 0 |+|  0 2μ  0 || ϵ
    %        \\0  0 0/   \ 0  0  μ//
    %
    %
    %Z = sparse(size(F,1),size(F,1));
    %I = speye(size(F,1));
    %C = lambda*[[I I Z;I I Z;Z Z Z]] + mu*[2*I Z Z;Z 2*I Z;Z Z I];
    %C = lambda*[1 1 0;1 1 0;0 0 0] + mu*diag([2 2 1]);
    %C = kroneye(C,size(F,1));
    C = [ ...
      (lambda+2*mu)*I        lambda*I  0*I; ...
            lambda*I (lambda+2*mu)*I  0*I; ...
                  0*I             0*I mu*I];
    %   ∇⋅σ = /∇⋅/σ₁₁\  ∇⋅/σ₁₂\\
    %         \  \σ₂₁/    \σ₂₂//
    %
    % If D is the divergence operator then D is 2#V by 3#F, where σ is 3#F by 1
    % vectorized stress tensor using Voigt notation:
    %
    %   X = D σ
    %
    A = diag(sparse(doublearea(V,F)/2));
  case 3
    G1 = G(1:size(F,1),:);
    G2 = G(size(F,1)+(1:size(F,1)),:);
    G3 = G(2*size(F,1)+(1:size(F,1)),:);
    strain = [G1 Z Z;Z G2 Z; Z Z G3; Z G3 G2; G3 Z G1; G2 G1 Z];
    C = [ ...
        (lambda+2*mu)*I        lambda*I        lambda*I     0*I     0*I     0*I ; ...
        lambda*I (lambda+2*mu)*I        lambda*I     0*I     0*I     0*I ; ...
        lambda*I        lambda*I (lambda+2*mu)*I     0*I     0*I     0*I ; ...
        0*I             0*I             0*I    mu*I     0*I     0*I ; ...
        0*I             0*I             0*I     0*I    mu*I     0*I ; ...
        0*I             0*I             0*I     0*I     0*I    mu*I ];
    A = diag(sparse(volume(V,F)));
  end
  Z = sparse(size(V,1),size(F,1));
  D = strain';
  A = repdiag(A,dim*(dim+1)/2);
  K = D * A * C * strain;

  M = massmatrix(V,F);
  M = repdiag(M,size(V,2));

end

author: alecjacobson

repo: gptoolbox


text:
function cS = corundum

cs_Crn = crystalSymmetry('-32/m',[1 1 2.729]);
N = Miller({0,0,1},{1,0,2},{1,1,3},{2,-1,-1},{1,1,-1},{7,-14,-3},cs_Crn);
dist = [0.5, 1.49, 2.35, 1.53, 9.95, 9.95];
cS = crystalShape(N./dist);

author: mtex-toolbox

repo: mtex

save_path: github-repos/MATLAB/mtex-toolbox-mtex


text:
function rank = rgf_rank ( m, f )

%*****************************************************************************80
%
%% RGF_RANK ranks a restricted growth function.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 January 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Donald Kreher, Douglas Simpson,
%    Combinatorial Algorithms,
%    CRC Press, 1998,
%    ISBN: 0-8493-3988-X,
%    LC: QA164.K73.
%
%  Parameters:
%
%    Input, integer M, the domain of the RGF is the integers
%    from 1 to M.  M must be positive.
%
%    Input, integer F(M), the restricted growth function.
%
%    Output, integer RANK, the rank of the restricted growth
%    function.
%

%
%  Check.
%
  ierror = rgf_check ( m, f );

  if ( ierror ~= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'RGF_RANK - Fatal error!\n' );
    fprintf ( 1, '  The input array is illegal!\n' );
    fprintf ( 1, '  IERROR = %d\n', ierror );
    error ( 'RGF_RANK - Fatal error!' );
  end
%
%  Get the generalized restricted growth function table.
%
  offset = 1;
  d = rgf_g_table ( m );

  rank = 0;
  j = 1;
  for i = 2 : m
    rank = rank + ( f(i) - 1 ) * d(m-i+offset,j+offset);
    j = max ( j, f(i) );
  end

  return
end

author: johannesgerer

repo: jburkardt-m


text:
function [err, XYZ] = AfniIndex2AfniXYZ (Indx, Nxx, Nyy)
%
%    [err, XYZ] = AfniIndex2AfniXYZ (Indx, Nxx, Nyy);
%
% Returns  the XYZ coordinates of an AFNI voxel with
%  an AFNI index of Indx
%
% Indx : Nx1 vector containing the AFNI indices
%   Indx must be an integer vector. If it is not, it's values
%   are rounded to the nearest integer
%
% Nxx, Nyy  : Number of pixels in the slice in the X and Y directions
%
% err = 0 No problem
% err = 1 input Matrix size problems
%
% XYZ : is the XYZ triplets matrix Nx3
%
%      Ziad Saad   Sun Mar 15 19:24:39 CST 1998

	[n1,m1] = size(Indx);

	if (m1 ~= 1),
		fprintf (1,'\a\nError in AfniIndex2AfniXYZ : Bad size for Indx\n\n');
		err = 1;
		return;
	end

	Indx = round (Indx);

	NxxNyy = Nxx .* Nyy;

	Z = floor (Indx ./ NxxNyy);
	Y = floor ((Indx - Z .* NxxNyy) ./ Nxx);
	X = Indx - ( Y .* Nxx) - (Z .* NxxNyy) ;

	XYZ = [X Y Z];

err = 0;
return;

author: fieldtrip

repo: fieldtrip

save_path: github-repos/MATLAB/fieldtrip-fieldtrip


text:
function hP = plugin_board()
% Zynq Platform PCore
% Use Plugin API to create board plugin object

%   Copyright 2015 The MathWorks, Inc.

% Call the common board definition function
hP = AnalogDevices.adrv9371x.common.plugin_board('ZCU102');

author: analogdevicesinc


text:
function [ahm,AHM_f,AHM_ahmf] = fromFrameAhm(F,ahmf)

% FROMFRAMEAHM  Transforms AHM from local frame to global frame.
%   AHM = FROMFRAMEAHM(F,AHMF) transforms the Inverse Depth point IF from the
%   local frame F to the global frame. The frame F can be specified either
%   with a 7-vector F=[T;Q], where T is the translation vector and Q the
%   orientation quaternion, of via a structure containing at least the
%   fields F.t, F.q, F.R and F.Rt (translation, quaternion, rotation matrix
%   and its transpose).
%
%   [AHM,AHM_f,AHM_if] = FROMFRAMEAHM(...) returns the Jacobians wrt F and AHMF.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

xf  = ahmf(1:3,:);
mf  = ahmf(4:6,:);
sf  = ahmf(7,:);

[t,q,R] = splitFrame(F);

if nargout == 1

    x  = fromFrame(F,xf);
    m  = R*mf;

    ahm  = [x;m;sf];

else

    if size(ahmf,2) > 1
        error('Jacobians not available for multiple ahms')
    else

        [x, X_f, X_xf] = fromFrame(F,xf);
        [m,M_q,M_mf]   = Rp(q,mf);

        ahm    = [x;m;sf];

        AHM_f = [...
            X_f
            zeros(3,3) M_q
            zeros(1,7)];

        AHM_ahmf = [...
            X_xf          zeros(3,4)
            zeros(3,3) M_mf zeros(3,1)
            zeros(1,6)           1];

    end
end

return

%% jac

syms x y z a b c d X Y Z U V W R real
F   = [x;y;z;a;b;c;d];
ahmf = [X;Y;Z;U;V;W;R];

[ahm,AHM_f,AHM_ahmf] = fromFrameAhm(F,ahmf);

simplify(AHM_f  - jacobian(ahm,F))
simplify(AHM_ahmf - jacobian(ahm,ahmf))

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB is free software: you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation, either version 3 of the License, or
%   (at your option) any later version.
%
%   SLAMTB is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You should have received a copy of the GNU General Public License
%   along with SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE

author: joansola

repo: slamtb

save_path: github-repos/MATLAB/joansola-slamtb


text:
%pls_train Partial Least Squares (training)
%
%  [B,XRes,YRes,Options] = pls_train(X,Y)
%  [B,XRes,YRes,Options] = pls_train(X,Y,Options)
%
% INPUT
%  X   [N -by- d_X]  the training (input)  data matrix, N samples, d_X variables
%  Y   [N -by- d_Y]  the training (output) data matrix, N samples, d_Y variables
%
%   Options.
%   maxLV        maximal number of latent variables (will be corrected
%                if > rank(X));
%                maxLV=inf means maxLV=min(N,d_X) -- theoretical maximum
%                number of LV;
%                by default =inf
%   method       'NIPALS' or 'SIMPLS'; by default ='SIMPLS'
%
%   X_centering  do nothing (=[] or 0), do mean centering (=nan), center
%                around some vaue v (=v);
%                by default  =[]
%   Y_centering  do nothing (=[] or 0), do mean centering (=nan), center
%                around some vaue v (=v);
%                by default  =[]
%   X_scaling    do nothing (=[] or 1), divide each col by std (=nan),
%                divide by some v (=v);
%                by default =[]
%   Y_scaling    do nothing (=[] or 1), divide each col by std (=nan),
%                divide by some v (=v);
%                by default =[]
%
% OUTPUT
%  B      [d_X -by- d_Y -by- nLV]  collection of regression matrices:
%         Y_new = X_new*B(:,:,n) represents regression on the first n
%         latent variables (X_new here after preprocessing, Y_new before
%         un-preprocessing)
%  XRes.
%   ssq  [1 -by- nLV]    the part of explaind sum of squares of
%        (preprocessed) X matrix
%   T    [N -by- nLV]    scores   (transformed (preprocessed) X)
%   R    [d_X -by- nLV]  weights  (transformation matrix)
%   P    [d_X -by- nLV]  loadings (back-transformation matrix)
%                        P(:,k) are the regression coef of
%                        (preprocessed) X on T(:,k)
%   W    [d_X -by- nLV]  local weights (local transformation matrix);
%                        ONLY FOR NIPALS
%   V    [d_X -by- nLV]  first k columns of this matrix are the
%                        orthornormal basis of the space spanned by k
%                        first columns of P; ONLY FOR SIMPLS
%  YRes.
%   ssq  [1 -by- nLV]    the part of explained sum of squares of
%                        (preprocessed) Y matrix
%   U    [N -by- nLV]    scores   (transformed (preprocessed) Y)
%   Q    [d_Y -by- nLV]  weights  (transformation matrix)
%   C    [d_Y -by- nLV]  C(:,k) are the regression coeff of
%                        (preprocessed) Y on T(:,k)
%   bin  [1 -by- nLV]    bin(k) is the regression coeff of U(:,k) on T(:,k)
%
% Options. contains the same fields as in input, but the values can be changed
% (e.g. after mean centering X_centering = mean(X,1))
%
% DESCRIPTION
% Trains PLS (Partial Least Squares) regression model
%
% Relations between matrices (X end Y are assumed to be preprocessed):
% NIPALS:
% T = X*R (columns of T are orthogonal)
% R = W*prinv(P'*W)
% P = X'*T*prinv(T'*T)
% U = Y*Q - T*(C'*Q-tril(C'*Q))
% C = Y'*T*prinv(T'*T) = Q*diag(bin)
% bin = sqrt(diag(T'*Y*Y'*T))'*prinv(T'*T))
% B = R*C' = W*prinv(P'*W)*C'
%
% SIMPLS:
% T = X*R (columns of T are orthonormal)
% P = X'*T
% U = Y*Q
% C = Y'*T = Q*diag(bin)
% bin = sqrt(diag(T'*Y*Y'*T))'
% B = R*C'
%
% BOTH:
% T_new = X_new*R
% Y_new = X_new*B
%
% SEE ALSO (PRTools Guide)
% PLS_APPLY, PLS_TRANSFORM

% Copyright: S.Verzakov, s.verzakov@ewi.tudelft.nl
% Faculty EWI, Delft University of Technology
% P.O. Box 5031, 2600 GA Delft, The Netherlands

% $Id: pls_train.m,v 1.2 2010/02/08 15:29:48 duin Exp $

function [B, XRes, YRes, Options] = pls_train(X,Y,Options)

[N_X, d_X] = size(X);
[N_Y, d_Y] = size(Y);

if N_X ~= N_Y
  error('size(X,1) must be equal to size(Y,1)');
else
  N = N_X;
end

if nargin < 3
  Options  = [];
end

DefaultOptions.X_centering = [];
DefaultOptions.Y_centering = [];
DefaultOptions.X_scaling = [];
DefaultOptions.Y_scaling = [];
DefaultOptions.maxLV = inf;
DefaultOptions.method = 'SIMPLS';

Options = pls_updstruct(DefaultOptions, Options);

if isinf(Options.maxLV)
  Options.maxLV = min(N,d_X);
elseif Options.maxLV > min(N,d_X)
  error('PLS: The number of LV(s) cannot be greater then min(N,d_X)');
end

[X, Options.X_centering, Options.X_scaling] = pls_prepro(X, Options.X_centering, Options.X_scaling);
[Y, Options.Y_centering, Options.Y_scaling] = pls_prepro(Y, Options.Y_centering, Options.Y_scaling);

ssq_X = sum(X(:).^2);
ssq_Y = sum(Y(:).^2);

B = zeros(d_X,d_Y,Options.maxLV);
XRes.ssq = zeros(1,Options.maxLV);
XRes.T   = zeros(N,Options.maxLV);
XRes.R   = zeros(d_X,Options.maxLV);
XRes.P   = zeros(d_X,Options.maxLV);
XRes.W   = [];
XRes.V   = [];

YRes.ssq = zeros(1,Options.maxLV);
YRes.U   = zeros(N,Options.maxLV);
YRes.Q   = zeros(d_Y,Options.maxLV);
YRes.C   = zeros(d_Y,Options.maxLV);
YRes.bin = zeros(1,Options.maxLV);

ev = zeros(1,Options.maxLV);
nLV = Options.maxLV;
opts.disp   = 0;
opts.issym  = 1;
opts.isreal = 1;

switch upper(Options.method)
case 'NIPALS'
  XRes.W = zeros(d_X,Options.maxLV);
  for LV = 1:Options.maxLV
    S = X'*Y;
    if d_X <= d_Y
      if d_X > 1
        [w, ev(LV)] = eigs(S*S',1,'LA',opts);
      else
        w = 1;
        ev(LV) = S*S';
      end

      t = X*w;
  	  t2 = (t'*t);
      proj = t/t2;
  	  p = X'*proj;

      c = Y'*proj;
      bin = norm(c);  %bin = sqrt(ev)/t2:
      q = c/bin;
      u = Y*q;

    else
      if d_Y > 1
        [q, ev(LV)] = eigs(S'*S,1,'LA',opts);
      else
        q = 1;
        ev(LV) = S'*S;
      end
      u = Y*q;

      w = X'*u;
  	  w = w/norm(w);
  	  t = X*w;
   	  t2 = (t'*t);
      proj = t/t2;
  	  p = X'*proj;

      bin = u'*proj;  %bin = sqrt(ev)/t2:
      c = q*bin;
  	end

	  if LV == 1
      if ev(LV) == 0
        error('PLS: Rank of the covariation matrix X''*Y is zero.');
      end
	  elseif ev(LV) <= 1e-16*ev(1)
      nLV = LV-1;
      WarnMsg = sprintf(['\nPLS: Rank of the covariation matrix X''*Y is exausted ' ...
                         'after removing %d Latent Variable(s).\n'...
                         'Results only for the %d LV(s) will be returned'],nLV,nLV);
      if exist('prwarning')
        prwarning(1, WarnMsg);
      else
        warning(WarnMsg);
      end
      break;
	  end

    %R = W*prinv(P'*W)
    %the next portion of the code makes use of the fact taht P'*W is the upper triangle matrix:
    %
    %if LV == 1
    %  InvPTW(1,1) = 1/(p'*w);
    %  r = w*InvPTW(1,1);
    %else
    %  InvPTW(1:LV,LV) = [(-InvPTW(1:LV-1,1:LV-1)*(XRes.P(:,1:LV-1)'*w)); 1]/(p'*w);
    %  r = [XRes.W(:,1:LV-1), w] * InvPTW(1:LV,LV);
    %end
    %
    %some optimization of the above code (notice that ones(m,0)*ones(0,n) == zeros(m,n)):
    %
    r = (w - (XRes.R(:,1:LV-1)*(XRes.P(:,1:LV-1)'*w)))/(p'*w);

    B(:,:,LV) = r*c';

  	XRes.ssq(:,LV) = t2*(p'*p)/ssq_X;
  	XRes.T(:,LV)   = t;
	  XRes.R(:,LV)   = r;
  	XRes.P(:,LV)   = p;
  	XRes.W(:,LV)   = w;

	  YRes.ssq(:,LV) = t2*(bin.^2)/ssq_Y;
  	YRes.U(:,LV)   = u;
  	YRes.Q(:,LV)   = q;
  	YRes.C(:,LV)   = c;
  	YRes.bin(:,LV) = bin;

  	X = X - t*p';
  	Y = Y - t*c';
  end

  if nLV < Options.maxLV
    XRes.W = XRes.W(:,1:nLV);
  end

case 'SIMPLS'
  XRes.V = zeros(d_X,Options.maxLV);
  S = X'*Y;
  for LV = 1:Options.maxLV
	  if d_X <= d_Y
		  if d_X > 1
        [r, ev(LV)] = eigs(S*S',1,'LA',opts);
      else
        r = 1;
        ev(LV) = S*S';
      end

      t = X*r;
		  norm_t = norm(t);
		  r = r/norm_t;
		  t = t/norm_t;
		  p = X'*t;

		  c = Y'*t;      %c = S'*r;
		  bin = norm(c); %bin = sqrt(ev)/norm_t:
		  q = c/bin;
		  u = Y*q;

    else
      if d_Y > 1
        [q, ev(LV)] = eigs(S'*S,1,'LA',opts);
      else
        q = 1;
        ev(LV) = S'*S;
      end
      r = S*q;
		  t = X*r;
		  norm_t = norm(t);
		  r = r/norm_t;
		  t = t/norm_t;
		  p = X'*t;

			u = Y*q;
		  bin = u'*t;    %bin = ev/norm_t:
		  c = q*bin;
	  end

	  if LV == 1
		  if ev(LV) == 0
        error('PLS: Rank of the covariation matrix X''*Y is zero.');
      else
        v = p;
      end
	  elseif ev(LV) <= 1e-16*ev(1)
      nLV = LV-1;
      WarnMsg = sprintf(['\nPLS: Rank of the covariation matrix X''*Y is exausted ' ...
                         'after removing %d Latent Variable(s).\n'...
                         'Results only for the %d LV(s) will be returned'],nLV,nLV);
      if exist('prwarning')
        prwarning(1, WarnMsg);
      else
        warning(WarnMsg);
      end
      break;
    else
  	  v = p - XRes.V(:,1:LV-1)*(XRes.V(:,1:LV-1)'*p);
	  end

	  v = v/norm(v);

    B(:,:,LV) = r*c';

    XRes.ssq(:,LV) = (p'*p)/ssq_X;
	  XRes.T(:,LV)   = t;
	  XRes.R(:,LV)   = r;
	  XRes.P(:,LV)   = p;
	  XRes.V(:,LV)   = v;

	  YRes.ssq(:,LV) = (bin.^2)/ssq_Y;
	  YRes.U(:,LV)   = u;
	  YRes.Q(:,LV)   = q;
	  YRes.C(:,LV)   = c;
	  YRes.bin(:,LV) = bin;

	  S = S - v*(v'*S);
  end

  if nLV < Options.maxLV
    XRes.V = XRes.V(:,1:nLV);
  end
end

if nLV < Options.maxLV
  B = B(:,:,1:nLV);

  XRes.ssq = XRes.ssq(:,1:nLV);
  XRes.T   = XRes.T(:,1:nLV);
  XRes.R   = XRes.R(:,1:nLV);
  XRes.P   = XRes.P(:,1:nLV);

  YRes.ssq = YRes.ssq(:,1:nLV);
  YRes.U   = YRes.U(:,1:nLV);
  YRes.Q   = YRes.Q(:,1:nLV);
  YRes.C   = YRes.C(:,1:nLV);
  YRes.bin = YRes.bin(:,1:nLV);

  Options.maxLV = nLV;
end

B = cumsum(B,3);

return;

function A = MakeSym(A)
A = 0.5*(A+A');
%A = max(A,A');

return

author: marianux

repo: ecg-kit

save_path: github-repos/MATLAB/marianux-ecg-kit


text:
function rez = final_clustering(rez)

wPCA  = rez.wPCA;
wroll = [];
tlag = [-2, -1, 1, 2];
for j = 1:length(tlag)
    wroll(:,:,j) = circshift(wPCA, tlag(j), 1)' * wPCA;
end

%% split templates into batches
rmin = 0.75;
nlow = 100;
n0 = 0;

Nchan = rez.ops.Nchan;
Nk = size(iC,2);
yunq = unique(rez.yc);

ktid = int32(st3(:,2));

uweigh = abs(rez.U(:,:,1));
uweigh = uweigh ./ sum(uweigh,1);
ycup = sum(uweigh .* rez.yc, 1);

Nfilt =  size(rez.W,2);
dWU = gpuArray.zeros(ops.nt0, ops.Nchan, Nfilt, 'double');
for j = 1:Nfilt
    dWU(:,:,j) = rez.mu(j) * squeeze(rez.W(:, j, :)) * squeeze(rez.U(:, j, :))';
end

ops = rez.ops;
NchanNear   = min(ops.Nchan, 16);

[iC, mask, C2C] = getClosestChannels(rez, ops.sigmaMask, NchanNear);

[~, iW] = max(abs(dWU(ops.nt0min, :, :)), [], 2);
iW = int32(squeeze(iW));

iC = gather(iC(:,iW));
%%
ss = double(st3(:,1)) / ops.fs;

ycenter = 25:40:3025;

Wpca = zeros(6, Nchan, 1000, 'single');
nst = numel(ktid);
hid = zeros(nst,1 , 'int32');
amp = zeros(1000,1 , 'single');

% ycup = rez.yc;

tic
for j = 1:numel(ycenter)
    fprintf('GROUP %d/%d \n', j, numel(ycenter))
    y0 = ycenter(j);

    xchan = abs(ycup - y0) < 20;
    itemp = find(xchan);

    if isempty(itemp)
        continue;
    end
    tin = ismember(ktid, itemp);
    pid = ktid(tin);
    data = tF(tin, :, :);

    ich = unique(iC(:, itemp));
    ch_min = ich(1)-1;
    ch_max = ich(end);

    nsp = size(data,1);
    dd = zeros(nsp, 6, ch_max-ch_min, 'single');
    for k = 1:length(itemp)
        ix = pid==itemp(k);
        dd(ix, :, iC(:,itemp(k))-ch_min) = data(ix,:,:);
    end

    [kid, aj] = run_pursuit(dd, nlow, rmin, n0, wroll, ss(tin));

    nmax = max(kid);
    for t = 1:nmax
        Wpca(:, ch_min+1:ch_max, t + n0) = gather(sq(mean(dd(kid==t,:,:),1)));
    end

    hid(tin) = gather(kid + n0);
    amp(n0+[1:nmax]) = aj;
    n0 = n0 + nmax;
end
Wpca = Wpca(:,:,1:n0);
amp = amp(1:n0);
toc
%%
clust_good = check_clusters(hid, ss);
sum(clust_good)

rez.W = zeros(61,0, 3, 'single');
rez.U = zeros(ops.Nchan,0,3, 'single');
rez.mu = zeros(1,0, 'single');
for  t = 1:n0
    dWU = wPCA * gpuArray(Wpca(:,:,t));
    [w,s,u] = svdecon(dWU);
    wsign = -sign(w(21,1));
    rez.W(:,t,:) = wsign * w(:,1:3);
    rez.U(:,t,:) = wsign * u(:,1:3) * s(1:3,1:3);
    rez.mu(t) = sum(sum(rez.U(:,t,:).^2))^.5;
    rez.U(:,t,:) = rez.U(:,t,:) / rez.mu(t);
end

%%
rez1 = rez;
rez1.st3 = st3;
rez1.st3(:,1) = rez1.st3(:,1); % +  ops.ntbuff;
rez1.st3(:,2) = hid;
rez1.st3(:,3) = amps;
rez1.simScore = (U(:,:) * U(:,:)') .* (W(:,:) * W(:,:)')/6;

amps = sq(sum(sum(tF.^2,2),3)).^.5;

rez1.cProj = [];
rez1.iNeigh = [];
rez1.cProjPC = [];
rez1.iNeighPC = [];
% rez1.st3(:,1) = rez1.st3(:,1); % - 30000 * 2000;

rez1.U = permute(Wpca, [2,3,1]);

rez1.mu = sum(sum(rez1.U.^2, 1),3).^.5;
rez1.U = rez1.U ./ rez1.mu;
rez1.mu = rez1.mu(:);

rez1.W = reshape(rez.wPCA, [61, 1, 6]);
rez1.W = repmat(rez1.W, [1, n0, 1]);
rez1.est_contam_rate = ones(n0,1);

U = permute(rez1.U, [2,1,3]);
W = permute(rez1.W, [2,1,3]);
%%

rez1 = find_merges(rez1, wroll, 1);

%%
hid = int32(rez1.st3(:,2));
clust_good = check_clusters(hid, ss);
sum(clust_good)
rez1.good = clust_good;

%%
rezToPhy2(rez1, rootZ);

%%
tmp_chan = iC(1,:);
y0 = 700;
xchan = abs(rez.yc - y0) < 20;
itemp = find(xchan(tmp_chan));

tin = ismember(ktid, itemp);
pid = ktid(tin);
data = rez.cProjPC(tin, :, :);

ich = unique(iC(:, itemp));
ch_min = ich(1)-1;
ch_max = ich(end);

nsp = size(data,1);
dd = zeros(nsp, 6, ch_max-ch_min, 'single');
for k = 1:length(itemp)
    ix = pid==itemp(k);
    dd(ix, :, iC(:,itemp(k))-ch_min) = data(ix,:,:);
end

size(dd)
%%
tic
kid = run_pursuit(dd, nlow, rmin, 0);
toc
%%
subplot(2,1,1)
[~, isort] = sort(pid);
imagesc(dd(isort(1:10:end), :)', [-10, 10])
colormap(redblue)
subplot(2,1,2)
[~, isort] = sort(kid);
imagesc(dd(isort(1:10:end), :)', [-10, 10])

hold on
ksort = sort(kid);
nc = max(kid);
plot(size(dd,2) * ksort(1:10:end) / nc, 'k', 'linewidth', 3)

author: MouseLand

repo: Kilosort

save_path: github-repos/MATLAB/MouseLand-Kilosort


text:
function varargout=Whos(varargin)
%Whos (Upper case W) is  a customized version of whos (lower case w).
%
%The differences are
%
% (1) "Whos" will compute/display memory in kilobytes whereas "whos" displays in bytes.
%
% (2) "Whos" will always display all array dimensions whereas "whos" will
%     not display array dimensions for 4th and higher dimensional arrays.
%
%
%EXAMPLE: Given arrays
%
%  A=rand(10,20,30,40);
%  B=rand(5,10,15,20,25,'single')*i;
%
%using "whos" displays the following:
%
%>> whos
%   Name      Size              Bytes  Class     Attributes
%
%   A         4-D             1920000  double
%   B         5-D             3000000  single    complex
%
%
%whereas "Whos" will display the following
%
%>> Whos
%   Name   Size              Kilobytes     Class    Attributes
%
%   A      10x20x30x40            1875     double
%   B      5x10x15x20x25          2930     single   complex
%
%
%An unfortunate limitation is that Whos relies on EVALIN and so will not work
%correctly in a workspace context reached using DBUP and DBDOWN.
%I elaborate on this somewhat in this Newsgroup thread:
%
%      http://www.mathworks.com/matlabcentral/newsreader/view_thread/303357
%
%Hopefully, TMW will provide a way around this eventually.
%
%by Matt Jacobson
%
%Copyright, Xoran Technologies, Inc. 2011

if nargin==0

  cmdstr='whos;';
  s=evalin('caller', cmdstr) ;

else

     in=['''' varargin{1} ''''];

     numargs=length(varargin);

     if numargs>1
       for ii=2:numargs
         in= [in ',' '''' varargin{ii} ''''];
       end
     end

     cmdstr=['whos(' in  ');'];

     s=evalin('caller', cmdstr) ;

end

flds=fieldnames(s);
idx=strmatch('bytes',flds);
flds{idx}='kilobytes';
S=rmfield(s,'bytes');
[S.kilobytes]=deal([]);
S=orderfields(S,flds);

deepestLevel=S(1).nesting.level;
setAns=strmatch('ans',{S.name});
idx=[];
for ii=setAns(:).'

    if isequal(S(ii).class,'(unassigned)') && S(ii).nesting.level==deepestLevel
       idx=ii;
    end

end
S(idx)=[];

N=length(S);

sizeStrings=cell(N,1);
memStrings=cell(N,1);
attributesStrings=cell(N,1);
nestFuncs=cell(1,N);
nestLevels=nan(N,1);
 lastLevel=nan;

blockCounts=zeros(1,N);
blockIndex=0;

for ii=1:N

    sizeStrings{ii}=regexprep(num2str(S(ii).size),'\s*','x');

    S(ii).kilobytes=s(ii).bytes/2^10;
    if S(ii).kilobytes>=10

     memStrings{ii}=num2str(ceil(S(ii).kilobytes));
    else
     memStrings{ii}=num2str(S(ii).kilobytes,'%2.3f');
    end

    str='';

    if S(ii).persistent,  str=[str,'persistent, ']; end
    if S(ii).global,   str=[str,'global, ']; end
    if S(ii).sparse,   str=[str,'sparse, ']; end
    if S(ii).complex,  str=[str,'complex, ']; end

    idx=find(str==',',1,'last');
    str(idx:end)='';
    if isempty(str), str=' '; end

    attributesStrings{ii}=str;

    thisLevel=S(ii).nesting.level;
    nestLevels(ii)=thisLevel;

    if thisLevel~=lastLevel

       lastLevel=thisLevel;
       blockIndex=blockIndex+1;
       nestFuncs{ii}=S(ii).nesting.function;

    end

    blockCounts(blockIndex)=blockCounts(blockIndex)+1;

end

if ~nargout

  blockCounts=nonzeros(blockCounts);
  nestFuncs=nestFuncs( ~cellfun('isempty',nestFuncs)) ;
    nestFuncs=cellfun(@(str) ['  ---- ',str,' -'] , nestFuncs(:), 'UniformOutput',0);

  NameCol=char('Name',' ',S.name);
  SizeCol=char('Size',' ',sizeStrings{:});

  ClassCol=char('Class',' ',S.class);
  AttributesCol=char('Attributes',' ',attributesStrings{:});

  %%Right justify the memory footprints
  memStrings=cellfun(@fliplr, memStrings,'UniformOutput',0);
  MegCol=fliplr( char(fliplr('Kilobytes'),' ',memStrings{:}));

  %% set up the display table

  d=repmat(' ',N+2,1);
  dd=[d,d];
  ddd=[dd,d];

  DisplayTable=[dd,NameCol,ddd,...
                SizeCol,ddd,dd,...
                MegCol, ddd,dd,...
                ClassCol, ddd,...
                AttributesCol];

  DisplayTable=char(DisplayTable,' ');

  if length(blockCounts)>1

      subTable=DisplayTable(3:end-1,:);
      subTable=mat2cell(subTable,blockCounts,size(subTable,2));
      subTable=[repmat({' '},size(subTable,1),1)  , nestFuncs,subTable ];
      subTable=subTable.';
      subTable=char(subTable{:});

      DisplayTable=char( DisplayTable(1,:), subTable, DisplayTable(end,:) );

      divs=strmatch('  ----',DisplayTable);

      for ii=1:length(divs)

          jj=divs(ii);

          kk=find(DisplayTable(jj,:)=='-',1,'last');

          DisplayTable(jj,kk+1:end)='-';
      end

   end

  disp(DisplayTable)

else

    varargout{1}=S;

end

author: Sable

repo: mcbench-benchmarks


text:
function [UAV] = UAV_SetUp2
%UAV_SETUP2 在此设置多无人机协同航迹规划任务
% 论文1的环境

% 航迹点设置
% （每行为一个无人机的参数）
UAV.S = [  0,           0;
                  0,           100;
                  300,       0;      ];     % 起点位置 (x,y)或(x,y,z)

UAV.G = [  875,      875;
                  800,       875;
                  875,       800;  ];      % 目标位置 (x,y)或(x,y,z)

UAV.PointNum = [  26;
                                  24;
                                  24;  ];                 % 每个无人机导航点个数（起始点之间点的个数）

UAV.PointDim = size(UAV.S, 2);        % 坐标点维度 （由 起点 坐标决定）
UAV.num = size(UAV.S, 1);                 % UAV数量 （由 起点 个数决定）

% 威胁点设置 (x,y,r) 或 (x,y,z,r)
% （每行为一个威胁的坐标和半径）
UAV.Menace.radar = [  200, 200,    20;
                                        600, 700,    20;  ];  % 雷达威胁（数学模型和其余威胁不同）

UAV.Menace.other = [  80, 40,   40;
                                        300, 300,   40;
                                        350, 600,    40;
                                        480, 450,    20;
                                        700, 700,    40;
                                        720, 760,    20;
                                        680, 760,    20;
                                        200, 400,    40;
                                        200, 520,    40;
                                        200, 600,    20;
                                        500, 200,    40;
                                        700, 200,    40;  ];   % 导弹、火炮、气象等威胁

% 无人机约束设置（min,max)
% （可单独为每个无人机设置，每行为一个无人机约束的上下限）
UAV.limt.v = 0.34*repmat([0.3, 0.7], UAV.num, 1);                 % 速度约束 （0.3Ma ~ 0.7Ma）
UAV.limt.phi = deg2rad(repmat([-60, 60], UAV.num, 1));      % 偏角约束 （-60° ~ 60°）
UAV.limt.theta = deg2rad(repmat([-45, 45], UAV.num, 1));   % 倾角约束 （-45° ~ 45°）
UAV.limt.h = repmat([0.02, 20], UAV.num, 1);                         % 高度约束 （0.02km ~ 20km）
UAV.limt.x = repmat([0, 875], UAV.num, 1);                            % 位置x约束 （0 ~ 875km）
UAV.limt.y = repmat([0, 875], UAV.num, 1);                            % 位置y约束 （0 ~ 875km）
UAV.limt.z = UAV.limt.h;                                                            % 位置z约束 （忽略地球弧度）
UAV.limt.L = zeros(UAV.num, 2);                                              % 航程约束 （最短航迹片段2km，最大航程1.5倍起始距离）
for i =1:UAV.num
    zz.max = 1.5 * norm(UAV.G(i, :) - UAV.S(i, :));
    zz.min = 2;
    UAV.limt.L(i, :) = [zz.min, zz.max];
end

% 多无人机协同设置
% （说明略）
UAV.tc = 6850;        % 协同时间 （单位s）
UAV.ds = 25;           % 安全距离 （单位km）

% 报错
ErrorCheck(UAV)
end

%% 程序自检
function ErrorCheck(UAV)

dim = UAV.PointDim;
if dim ~= size(UAV.G,2) || dim ~= size(UAV.Menace.radar,2)-1 || dim ~= size(UAV.Menace.other,2)-1
    if dim ~= size(UAV.G,2)
        error('仿真维度为%d，但目标点坐标为%d维', dim, size(UAV.G,2))
    else
        error('仿真维度为%d，但威胁点坐标为%d维', dim, size(UAV.Menace.radar,2)-1)
    end
end

num = UAV.num;
if num ~= size(UAV.G,1) || num ~= size(UAV.limt.v,1) || num ~= size(UAV.limt.phi,1) ...
        || num ~= size(UAV.limt.theta,1) || num ~= size(UAV.limt.h,1) || num ~= size(UAV.limt.x,1) ...
        || num ~= size(UAV.limt.y,1) || num ~= size(UAV.limt.z,1) || num ~= size(UAV.limt.L,1)
    if num ~= size(UAV.G,1)
        error('无人机个数为%d, 但目标点有%d个', num, size(UAV.G,1))
    else
        error('约束条件个数与无人机个数不一致')
    end
end

if num ~= size(UAV.PointNum, 1)
    error('无人机个数为%d, 但为%d个无人机设置了导航点', num, size(UAV.PointNum, 1))
end

MaxPoint = floor(UAV.limt.L(:,2) ./ UAV.limt.L(:,1)) - 1;   % 每个无人机支持的最大航迹点数量
for i = 1 : UAV.num
    if UAV.PointNum(i) > MaxPoint(i)
        error('%d号无人机导航点个数超出任务需求，请尝试减少导航点个数', i)
    end
end

end

repo: Grey-Wolf-Optimizer-for-Path-Planning


text:
% Codes= written by Soheil Bahrampour
% Jan 2, 2014

% to perform stochastic gradiant descent (SDG) for multiclass quadratic
% cost function (formulation of 3.3.1 in PAMI paper)
% Inputs:
%       X is data matrix with n (dimension of feature) rows and N (number of train
%           samples) columns
%       Y is the output vector of size q*N where each column is the output
%            vector for one observation in X
%       nu is the regularization parameter
%       iter: Number of epochs over whole data
%       intercept: fit intercept b as well (1) or not (0) (without regularization)
%       batchSize: Mini batch size for stochastic gradient descent
%       computeCost: Flag to compute the cost and plot it
%       ro: constant for computing learnning rate, ro = 5 worked well!

% Output:
%       linear parameter modelQuad.W and modelQuad.b

function modelQuad = SGDMultiClassQuadC(X, Y, nu, iter, intercept, batchSize, ro, computeCost)

N = size(X,2); % number of train samples
t0 = floor(N/batchSize)*iter/10; % for setting the learning rate according to the task-driven dic learning paper: we set t0=T/10 where T is total number of updates
n = size(X,1); % number of features
number_classes = size(Y,1);
W = zeros(number_classes, n);
b = zeros(number_classes, 1);

% optimization
if computeCost
    cost = zeros(iter*N,1); %cost value at each iteration
    costIter = 0;
    costStep = floor(N/batchSize); % Compute cost every costStep over the last costStep batch of train samples. For each batch of train samples, the cost will be computed before updating the dic using that trian samples
    costTemp = zeros(costStep,1); % to store cost over lasr costStep samples
    costTempCount = 0; % to count how many train sample are passed
end
step = 0;
permut = randperm(N); %randomly shuffle data
X = X(:,permut);
Y = Y(:, permut);

for iteration = 1: iter % number of iterations over whole training samples
    for t = 1: batchSize: N-rem(N,batchSize)
        step = step + 1;
        temp = W*X(:,t:t+batchSize-1) + repmat(b, 1, batchSize) - Y(:,t:t+batchSize-1);
        gradW = (temp)*X(:,t:t+batchSize-1)'/batchSize + nu*W;
        if intercept
            gradb = sum(temp,2)/batchSize; % Intercept wil not be regularized
        end

        % compute cost (before update)
        if computeCost
            if costTempCount == costStep
                costIter = costIter + 1;
                cost(costIter,1) = mean(costTemp);
                costTempCount = 0;
                costTemp = zeros(costStep,1);
            end
            costTempCount = costTempCount + 1;
            costTemp(costTempCount,1)= 0.5*sum(sum((Y(:,t:t+batchSize-1) - W*X(:,t:t+batchSize-1)-repmat(b, 1, batchSize)).^2))/batchSize + nu/2*sum(sum(W.^2));
        end

        % update
        learnRate = min(ro, ro*t0/step);
        W = W - learnRate*gradW;
        if intercept
            b = b - learnRate*gradb;
        end
    end
end
if computeCost
    cost = cost(1:costIter,1); % remove extra elements
    figure;plot(cost);
end
modelQuad.W = W';
modelQuad.b = b';
end

author: zhangqianqianQQ

repo: MachineVisionAlgorithm


text:
function cost = snd_costfunction(mpcModel, u, varargin)

%%
% costfunction(mpcModel, u, varargin)
%UNTITLED3 Summary of this function goes here
%	evaluates the cost function of the
%   optimal control problem over the horizon
%   N with sampling time T for the current
%   data of the optimization method t0, snd.xmeasure
%  	and u.
% 	The function return the computed cost
%	function value.

    cost = 0;
    x = zeros(mpcModel.horizon+1, length(mpcModel.xmeasure));
    x = snd_computeOpenloopSolution(mpcModel, u );

    for k=1:mpcModel.horizon
        cost = cost+mpcModel.runningcosts(k, x(k,:), u(:,k), mpcModel.u0_ref, mpcModel.battery); %, varargin
    end
    cost = cost+ 0.5*mpcModel.terminalcosts((mpcModel.horizon+1), x(mpcModel.horizon+1,:));
     %, varargin
end

author: juchengquan


text:
function p05_story ( )

%*****************************************************************************80
%
%% P05_STORY prints the "story" for problem p05.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 August 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Larry Irvine, Samuel Marin, Philip Smith,
%    Constrained Interpolation and Smoothing,
%    Constructive Approximation,
%    Volume 2, Number 1, December 1986, pages 129-151.
%
%  Parameters:
%
%    None
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  This example is due to Larry Irvine, Samuel Marin and Philip Smith.\n' );
  fprintf ( 1, '  This data can cause problems for interpolation methods.\n' );
  fprintf ( 1, '  There are sudden changes in direction, and at the same time,\n' );
  fprintf ( 1, '  sparsely-placed data.  This can cause an interpolant to overshoot\n' );
  fprintf ( 1, '  the data in a way that seems implausible.\n' );

  return
end


text:
%% plot movement of vehicle

close all;
clearvars -except SimRealState input debug logsout

%% User Input

%% data source

   % 1: from simulation
   % 2: from script which loads data for simulink replay
   % 3: from debug file logged on vehicle; raw import with mat load

    i_datasource = 1;

% visualization settings

    % timestep of simulation in seconds
    sim_timestep = 0.004;       % in s

    % use every i-ith data point to reduce computational load
    i = 50;

    % timestep where to start visualization in seconds
    t_start = 0;

    % set replay speed: if > 1, replay is faster than reality and vice versa
    replay_speed_factor = 2;

% vehicle parameters
    length_front_m = 2;
    length_total_m = 4.5;

    width_m = 2;

%% convert data from different sources

if i_datasource == 1
    % from simulation
    dummy = logsout{1}.Values;
    VehicleData.x_m = dummy.x_m;
    VehicleData.y_m = dummy.y_m;
    VehicleData.psi_rad = dummy.psi_rad;

    VehicleData.vx_mps = dummy.vx_mps;
    VehicleData.vy_mps = dummy.vy_mps;

    VehicleData.ax_mps2 = dummy.ax_mps2;
    VehicleData.ay_mps2 = dummy.ay_mps2;

elseif i_datasource == 2

    % from script which loads data for simulink replay
    VehicleData = input{2};
    VehicleData.x_m = SimRealState.Pos.x_m;
    VehicleData.y_m = SimRealState.Pos.y_m;
    VehicleData.psi_rad = SimRealState.Pos.psi_rad;

elseif i_datasource == 2

    % from debug file logged on vehicle; raw import with mat load
    VehicleData.x_m = debug.debug_mvdc_state_estimation_debug_StateEstimate_Pos_x_m;
    VehicleData.y_m = debug.debug_mvdc_state_estimation_debug_StateEstimate_Pos_y_m;
    VehicleData.psi_rad = debug.debug_mvdc_state_estimation_debug_StateEstimate_Pos_psi_rad;

    VehicleData.vx_mps = debug.debug_mvdc_state_estimation_debug_StateEstimate_vx_mps;
    VehicleData.vy_mps = debug.debug_mvdc_state_estimation_debug_StateEstimate_vy_mps;

    VehicleData.ax_mps2 = debug.debug_mvdc_state_estimation_debug_StateEstimate_ax_mps2;
    VehicleData.ay_mps2 = debug.debug_mvdc_state_estimation_debug_StateEstimate_ay_mps2;

end

%% get recorded data and transform for use in visualization

    time = VehicleData.x_m.Time((t_start/sim_timestep)+1:i:end);

    x_m = VehicleData.x_m.Data((t_start/sim_timestep)+1:i:end);
    y_m = VehicleData.y_m.Data((t_start/sim_timestep)+1:i:end);

    psi_rad = VehicleData.psi_rad.Data((t_start/sim_timestep)+1:i:end);

    psi_rad = normalizeAngle(psi_rad) ;

    vx_mps = VehicleData.vx_mps.Data((t_start/sim_timestep)+1:i:end);
    vy_mps = VehicleData.vy_mps.Data((t_start/sim_timestep)+1:i:end);

    ax_mps2 = VehicleData.ax_mps2.Data((t_start/sim_timestep)+1:i:end);
    ay_mps2 = VehicleData.ay_mps2.Data((t_start/sim_timestep)+1:i:end);

    % calculate tangent to vehicle's CoG path
    diff_x_m = diff(x_m);
    diff_y_m = diff(y_m);

    heading_traj_rad = normalizeAngle(atan2(diff_y_m,diff_x_m)-pi/2);

%% main plotting

fig1 = figure;

% subplot 1 (top left)
    sp1 = subplot(3,2,1);
    title('vehicle velocity')
    grid on

  	xlabel('time in s')

    % set color of both y-axes to specific values matching the plot color
    ax_sp1 = gca;
  	yyaxis left
 	ax_sp1.YColor = [0, 0.4470, 0.7410];

    h1 = animatedline('Color',	[0, 0.4470, 0.7410]);
    ylabel('x-velocity in m/s')

    yyaxis right
    ax_sp1.YColor = [0.8500, 0.3250, 0.0980];

    h11 = animatedline('Color',	[0.8500, 0.3250, 0.0980]);
    ylabel('y-velocity in m/s')

% subplot 3 (middle left)
    sp3 = subplot(3,2,3);
    title('vehicle acceleration')
  	grid on

    h31 = animatedline('Color',	[0, 0.4470, 0.7410]);
    h32 = animatedline('Color',	[0.8500, 0.3250, 0.0980]);

    xlabel('time in s')
    ylabel('acceleration in m/s^2')

    legend('ax','ay')

% subplot 5 (bottom left)
    sp5 = subplot(3,2,5);
    title('vehicle location - global')

    xlabel('x-coordinate in m')
    ylabel('y-coordinate in m')

    xlim([min(x_m)-10,max(x_m)+10])
    ylim([min(y_m)-10,max(y_m)+10])

    hold on
    axis equal

    h5 = animatedline;

    plot(x_m(1),y_m(1),'+')

% subplot right side (subplot no. 2,4,6)
    sp246 = subplot(3,2,[2 4 6]);
    title('vehicle location - local')

    xlabel('x-coordinate in m')
    ylabel('y-coordinate in m')

    xlim([x_m(1)-50,x_m(1)+50])
    ylim([y_m(1)-50,y_m(1)+50])
    hold on
    axis equal

    h246 = animatedline;

    plot(x_m(1),y_m(1),'+')

    R = [cos(psi_rad) -sin(psi_rad) ;sin(psi_rad) cos(psi_rad)] ;

    g_veh = hgtransform;
    x = [-width_m/2 0 width_m/2 ];
    y = [length_front_m-length_total_m length_front_m length_front_m-length_total_m];
    vehicle_arrow = patch(x,y,'red');
    set(vehicle_arrow,'Parent',g_veh)

    g_head = hgtransform;
    heading_line = patch([0 0 0],[0 20 NaN],[0 0 0],'EdgeColor','red');
    set(heading_line,'Parent',g_head)

    g_traj_tangent = hgtransform;
    tangent_traj = patch([0 0 0],[0 20 NaN],[0 0 0],'EdgeColor','black');
    set(tangent_traj,'Parent',g_traj_tangent)

    h = zeros(2, 1);
    h(1) = plot(0,0,'r');
    h(2) = plot(0,0,'k');
    legend([h(1) h(2)], {'vehicle heading',"tangent to vehicle's CoG path"});

% for loop which updates every subplot
for timestep=1:size(time,1)-1

   	trans = makehgtform('translate',[x_m(timestep),y_m(timestep),0]);
    rotz = makehgtform('zrotate',psi_rad(timestep));

  	rotz_tangent = makehgtform('zrotate',heading_traj_rad(timestep));

    % update subplot 1
    sp1;

  	addpoints(h1,time(timestep), vx_mps(timestep));
  	addpoints(h11,time(timestep), vy_mps(timestep));

    % update subplot 3
    sp3;

  	addpoints(h31,time(timestep), ax_mps2(timestep));
  	addpoints(h32,time(timestep), ay_mps2(timestep));

    % update subplot 5
    sp5;

    addpoints(h5,x_m(timestep), y_m(timestep));

    % update subplot 246
  	sp246;

    xlim([x_m(timestep)-50,x_m(timestep)+50])
    ylim([y_m(timestep)-50,y_m(timestep)+50])

    set(g_veh,'Matrix',trans*rotz)
    set(g_head,'Matrix',trans*rotz)

    set(g_traj_tangent,'Matrix',trans*rotz_tangent)

    addpoints(h246,x_m(timestep), y_m(timestep));

    % update plot
    drawnow
    % pause to match specified visualization speed
    pause((sim_timestep*i)/replay_speed_factor)

end

author: TUMFTM


text:
function DoHouseKeeping(added_paths, previous_open_figure_handles, bRestoreErrorStatus)

our_fig_hdl = [findall(0, 'Tag', 'a2hbc');findall(0, 'Tag', 'progress_bar')];
close(our_fig_hdl);

on_exit_open_handles = findall(0);
handles2_close = setdiff(on_exit_open_handles, previous_open_figure_handles);
delete(handles2_close);

prwaitbar('off')

if( bRestoreErrorStatus )
    dbclear if caught error
end

rmpath(added_paths);


text:
function rmgismo()

% RMGISMO(SOURCE) removes all existing paths containing the phrase
% 'GISMO'.

% Author: Michael West, Geophysical Institute, Univ. of Alaska Fairbanks
% $Date$
% $Revision$

admin.deprecated(mfilename,'admin.remove');

% REMOVE EXISTING GISMO PATHS
pathList = path;
n = 1;
while true
    t = strtok(pathList(n:end), pathsep);
    OnePath = sprintf('%s', t);

    if strfind(OnePath,'GISMO');
        %disp(['removing: ' OnePath])
        rmpath(OnePath);
    end;
    n = n + length(t) + 1;
    if isempty(strfind(pathList(n:end),':'))
        break
    end;
end

author: geoscience-community-codes

repo: GISMO


text:
%GOBJ_POLYGON Create polygon geometry object.
%
%   [ GOBJ ] = GOBJ_POLYGON( P, TAG ) Creates a polygon
%   geometry object. Accepts the following input parameters.
%
%       Parameter   Value/{Default}           Description
%       -----------------------------------------------------------------------------------
%       p           array  {[0 0;1 0;0 1]}    Polygon vertex points
%       tag         string {P1}               Geometry object tag/name

% Copyright 2013-2022 Precise Simulation, Ltd.

author: precise-simulation

repo: featool-multiphysics