%% Efficient subpixel image registration by cross-correlation.
% Registers two images (2-D rigid translation) within a  fraction
% of a pixel specified by the user. Instead of computing a zero-padded FFT
% (fast Fourier transform), this code uses selective upsampling by a
% matrix-multiply DFT (discrete FT) to dramatically reduce computation time and memory
% without sacrificing accuracy. With this procedure all the image points are used to
% compute the upsampled cross-correlation in a very small neighborhood around its peak. This
% algorithm is referred to as the single-step DFT algorithm in [1].
%
% [1] Manuel Guizar-Sicairos, Samuel T. Thurman, and James R. Fienup,
% "Efficient subpixel image registration algorithms," Opt. Lett. 33,
% 156-158 (2008).

%% Syntax
% The code receives the FFT of the reference and the shifted images, and an
% (integer) upsampling factor. The code expects FFTs with DC in (1,1) so do not use
% fftshift.
%
%    output = dftregistration(fft2(f),fft2(g),usfac);
%
% The images are registered to within 1/usfac of a pixel.
%
% output(1) is the normalized root-mean-squared error (NRMSE) [1] between f and
% g.
%
% output(2) is the global phase difference between the two images (should be
% zero if images are real-valued and non-negative).
%
% output(3) and output(4) are the row and column shifts between f and g respectively.
%
%    [output Greg] = dftregistration(fft2(f),fft2(g),usfac);
%
% Greg is an optional output, it returns the Fourier transform of the registered version of g,
% where the global phase difference [output(2)] is also compensated.

%% Obtain a reference and shifted images
% To illustrate the use of the algorithm, lets obtain a reference and a
% shifted image. First we read the reference image f(x,y)
f = im2double(imread('cameraman.tif'));
%%
% Define g(x,y) as a version of f(x,y) shifted by fractional values of a
% pixel and multiplied by a global phase.
deltar = 3.48574;
deltac = 8.73837;
phase = 2;
[nr,nc]=size(f);
Nr = ifftshift([-fix(nr/2):ceil(nr/2)-1]);
Nc = ifftshift([-fix(nc/2):ceil(nc/2)-1]);
[Nc,Nr] = meshgrid(Nc,Nr);
g = ifft2(fft2(f).*exp(i*2*pi*(deltar*Nr/nr+deltac*Nc/nc))).*exp(-i*phase);
figure(1);
subplot(1,2,1);
imshow(abs(f));
title('Reference image, f(x,y)')
subplot(1,2,2);
imshow(abs(g));
title('Shifted image, g(x,y)')
%%
% We have shifted the image by 8.73837 and 3.48574 pixels in the x and
% y direction, respectively, and added a phase of 2 radians to g(x,y). The shift
% was implemented by applying a linear phase on its
% FT, thus we have assumed that the images wrap around (features leaving one
% side of the window reappear on the opposite side) and that the image is band-limited
% (interpolated by a sinc function). Cross-correlation image registration by DFTs
% (both the matrix-multiply DFT and the zero-padded FFT) share these assumptions.
%
% This registration technique is well suited to compare images that are captured
% in Fourier domain (i.e. to evaluate an image reconstruction by holography
% or phase retrieval) which are strictly band-limited and exhibit the
% wrap-around effect.
%
% Even though the registration code assumes band-limited images that wrap around, we
% have obtained very good results when applying it to
% band-limited microscope images, and aliased imagery. That is when shifting
% the image brings in new content instead of wrapping it around or when the
% images are not band-limited.

%% Sample Image Registration
% dftregistration.m receives the FT of f and g and the upsampling factor.
% The code expects DC of the FTs at (1,1) so don't use fftshift.
%
% We now use the image registration code to register f and g within 0.01
% pixels by specifying an upsampling parameter of 100
[output Greg] = dftregistration(fft2(f),fft2(g),100);
display(output),
%%
% The pixel shift error (difference between the true and obtained shifts)
% is 0.0016 and 0.0043 in the x and y directions respectively. Well within
% the expected accuracy of 0.01. Notice that using the conventional zero-padded
% FFT approach with the same accuracy, would
% require computation of a 25,600x25,600 FFT, which would require more than
% 19 Gbytes of RAM and a very comfortable chair.
%
% The following plot shows the reference image and the registered image.
figure(1);
subplot(1,2,1);
imshow(abs(f));
title('Reference image, f(x,y)')
subplot(1,2,2);
imshow(abs(ifft2(Greg)));
title('Registered image, gr(x,y)')
%% Disclaimer
% I have made every effort to evaluate the proper working of this code
% under many different conditions. However, it is the responsibility of
% the user to ensure that this registration code is adequate for their
% application.
%
% Feel free to e-mail me with questions or comments.


function p = predict(Theta1, Theta2, X)
%PREDICT Predict the label of an input given a trained neural network
%   p = PREDICT(Theta1, Theta2, X) outputs the predicted label of X given the
%   trained weights of a neural network (Theta1, Theta2)

% Useful values
m = size(X, 1);
num_labels = size(Theta2, 1);

% You need to return the following variables correctly
p = zeros(size(X, 1), 1);

% ====================== YOUR CODE HERE ======================
% Instructions: Complete the following code to make predictions using
%               your learned neural network. You should set p to a
%               vector containing labels between 1 to num_labels.
%
% Hint: The max function might come in useful. In particular, the max
%       function can also return the index of the max element, for more
%       information see 'help max'. If your examples are in rows, then, you
%       can use max(A, [], 2) to obtain the max for each row.
%

X = [ones(m, 1) X];
t1 = sigmoid(X * Theta1');
t1 = [ones(m, 1) t1];

t2 = sigmoid( t1 * Theta2');

[~, p] = max(t2, [], 2);

end


function M = morans_I(grid,W,s)
% PURPOSE: calculate global Moran's I for an input grid (matrix) by calculating all
%          local Moran's I for a given moving windows size using a weight matrix.
% -------------------------------------------------------------------
% USAGE: M = moransI(grid, W, s);
% where: [grid] is the matrix to analyse
%        [W] is the normalized weight matrix of the size the local Moran's
%            I will be calculated for (uneven sized!)
%        [s] is an optional flag to use zscores of input values for
%        calculation. Set to 'true' if zscores of local grid should be
%        calculated. Leave blank if not desired or input values are already
%        standardized.
% -------------------------------------------------------------------------
% OUTPUTS:
%        [M] matrix of all local Moran's I
% -------------------------------------------------------------------
% NOTES: Weight matrix needs to be 'moving window' style, not contiguity
%        matrix: Moran's I is calculated and weighted for neighbours to center cell.
%        Matrix needs to be normalized (weights sum to 1) and center cell weight
%        will be set to 0 if not already. Uses localmoran.m
%        -> Use nanmean(M(:)) to get the average global Moran's I.
%
% See Anselin (1995, 'LISA.', Geogr. Analysis 27(2),p.93f) for details on
% standardized variables in calculation of local Moran's I.
%
% EXAMPLE:  M = moransI(rand(20,20),ones(5,5),'true')
%
% Felix Hebeler, Geography Dept.,de University Zurich, March 2006.

%% Check if standardising should be done
if exist('s','var')
    if strcmp(s,'true');
        grid=zscore(grid);
    elseif strcmp(s,'false')
        %do nothing
    else
        error('Invalid option for s: set [true] to calculated zscores to determine local Moran or leave blank if values are already standardized.');
    end
end
if (mod(size(W,1),2)| mod(size(W,2),2))~=1
   error('Weight matrix W needs to have uneven size (eg. 5x5)')
end
%% Do local Morans I calc of the grid.
M = NaN(size(grid,1),size(grid,2));
wsx=floor(size(W,1)/2);
wsy=floor(size(W,2)/2);
% Do local morans I calc for moving window ws
for row=1+wsy:1:size(grid,1)-wsy;
    for col=1+wsx:1:size(grid,2)-wsx;
        M(row,col) = get_moran(grid(row-wsx:row+wsx,col-wsy:col+wsy),W);
    end
end

%% calculate local Moran's I
function m=get_moran(raster,W)
ncols= size(raster,2);
nrows= size(raster,1);
zi = raster(ceil(nrows/2),ceil(ncols/2));%  value of center cell (note: no weight applied!)
if (isnan(zi));
    m=NaN;
    return;
end;
raster=raster.* W; % Weight values in window
raster(ceil(nrows/2),ceil(ncols/2))=0; %set center cell to zero to exclude zi from sum
zj = nansum(raster(:)); % sum of weighted values excluding zi
m = zi * zj; % calculate local Moran's I and return


function [dcos] = triang2(pp,tt)
%TRIANG2 calc. enclosed angles for a 2-simplex triangulation
%embedded in the two-dimensional plane.
%   [ADEG] = TRIANG2(VERT,TRIA) returns the enclosed angles
%   associated with each triangle, where ADEG is a T-by-3
%   array of the angles subtended at each vertex, VERT is a
%   V-by-2 array of XY coordinates, and TRIA is a T-by-3 ar-
%   ray of vertex indexing, where each row defines a triang-
%   le, such that VERT(TRIA(II,1),:), VERT(TRIA(II,2),:) and
%   VERT(TRIA(II,3),:) are the coordinates of the II-TH tri-
%   angle. Angles are returned in degrees.
%
%   See also TRISCR2, TRIAREA, TRIBAL2

%   Darren Engwirda : 2017 --
%   Email           : de2363@columbia.edu
%   Last updated    : 08/07/2018

%---------------------------------------------- basic checks
    if (~isnumeric(pp) || ~isnumeric(tt) )
        error('triang2:incorrectInputClass' , ...
            'Incorrect input class.') ;
    end

%---------------------------------------------- basic checks
    if (ndims(pp) ~= +2 || ndims(tt) ~= +2 )
        error('triang2:incorrectDimensions' , ...
            'Incorrect input dimensions.');
    end
    if (size(pp,2)~= +2 || size(tt,2) < +3 )
        error('triang2:incorrectDimensions' , ...
            'Incorrect input dimensions.');
    end

    nnod = size(pp,1) ;

%---------------------------------------------- basic checks
    if (min(min(tt(:,1:3))) < +1 || ...
            max(max(tt(:,1:3))) > nnod )
        error('triang2:invalidInputs', ...
            'Invalid TRIA input array.') ;
    end

%----------------------------------- compute enclosed angles
    dcos = zeros(size(tt,1),3) ;

    ev12 = pp(tt(:,2),:)-pp(tt(:,1),:) ;
    ev23 = pp(tt(:,3),:)-pp(tt(:,2),:) ;
    ev31 = pp(tt(:,1),:)-pp(tt(:,3),:) ;

    lv11 = sqrt(sum(ev12.^2,2));
    lv22 = sqrt(sum(ev23.^2,2));
    lv33 = sqrt(sum(ev31.^2,2));

    ev12 = ev12 ./ ...
        lv11(:,ones(1,size(pp,2)));
    ev23 = ev23 ./ ...
        lv22(:,ones(1,size(pp,2)));
    ev31 = ev31 ./ ...
        lv33(:,ones(1,size(pp,2)));

    dcos(:,1) = sum(-ev12.*ev23,2);
    dcos(:,2) = sum(-ev23.*ev31,2);
    dcos(:,3) = sum(-ev31.*ev12,2);

    dcos(:,1) = max(-1.,dcos(:,1));
    dcos(:,1) = min(+1.,dcos(:,1));
    dcos(:,2) = max(-1.,dcos(:,2));
    dcos(:,2) = min(+1.,dcos(:,2));
    dcos(:,3) = max(-1.,dcos(:,3));
    dcos(:,3) = min(+1.,dcos(:,3));

    dcos = acos(dcos) * 180. / pi ;

end


function vecCross = crossMat(vec)
%
% Computes the cross-product matrix of a 3x1 vector
%
    if(size(vec,1) ~= 3 || size(vec,2) ~= 1)
        error('Input vector must be 3x1');
    end

    vecCross = [ 0, 		-vec(3), 	vec(2);
				 vec(3),	0,			-vec(1);
				 -vec(2),	vec(1),		0        ];
end


function [tfr,t,f]=tfrideal(iflaws,t,N,trace)
%TFRIDEAL Ideal TFR for given instantaneous frequency laws.
%	[TFR,T,F]=TFRIDEAL(IFLAWS,T,N,TRACE) generates the ideal
%	time-frequency representation corresponding to the
%	instantaneous frequency laws of the components of a signal.
%
%	IFLAWS : (M,P)-matrix where each column corresponds to
%		 the instantaneous frequency law of an (M,1)-signal,
%		 These P signals do not need to be present at the same time.
%		 The values of IFLAWS must be between 0 and 0.5.
%	T      : the time instant(s)      (default : 1:M).
%	N      : number of frequency bins (default : M).
%	TRACE  : if nonzero, the progression of the algorithm is shown
%                                         (default : 0).
%	TFR    : output time-frequency matrix, of size (N,length(t)).
%		 If nargout=0, a contour plot of TFR is automatically
%		 displayed on the screen.
%	F      : vector of normalized frequencies.
%
%	Example :
%         N=140; t=0:N-1; [x1,if1]=fmlin(N,0.05,0.3);
%         [x2,if2]=fmsin(70,0.35,0.45,60);
%         if2=[zeros(35,1)*NaN;if2;zeros(35,1)*NaN];
%         tfrideal([if1 if2]);
%
%	See also PLOTIFL, PLOTSID, and all the time-frequency
%        representations listed in the CONTENTS file (TFR*)

%	O. Lemoine, F. Auger - March, April 1996.
%	Copyright (c) 1996 by CNRS (France).
%
%  This program 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 2 of the License, or
%  (at your option) any later version.
%
%  This program 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 this program; if not, write to the Free Software
%  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

if (nargin==0),
 error('at least one parameter required');
end;

[ifrow,ifcol]=size(iflaws);

if (nargin==1),
 t=1:ifrow; N=ifrow; trace=0;
elseif (nargin==2),
 N=ifrow; trace=0;
elseif (nargin==3),
 trace=0;
end;

[trow,tcol]=size(t);
if (trow~=1),
 error('T must only have one row');
end;

tfr=zeros(N,tcol);

if any(any(iflaws>0.5)) | any(any(iflaws<0)),
 error('The values of IFLAWS must be between 0 and 0.5');
end

if trace, disp('Ideal time-frequency distribution'); end;

for icol=1:tcol,
 if trace, disprog(icol,tcol,10); end;
 ti= t(icol);
 for fi=1:ifcol,
  if isnan(iflaws(ti,fi)),
   tfr(fi,icol)=NaN;
  else
   tfr(round(iflaws(ti,fi)*2*(N-1))+1,icol)=1;
  end
 end
end;

if (nargout==0),
 f=(0:N-1)/(2*N);
 axes(gca); contour(t,f,tfr,1,'k');
 xlabel('Time'); ylabel('Normalized frequency');
 title('Ideal time-frequency representation');
 grid;
elseif (nargout==3),
 f=(0.5*(0:N-1)/N)';
end;


function [f, e] = fv(B, nk, thmax, noplot)
%FV     Field of values (or numerical range).
%       FV(A, NK, THMAX) evaluates and plots the field of values of the
%       NK largest leading principal submatrices of A, using THMAX
%       equally spaced angles in the complex plane.
%       The defaults are NK = 1 and THMAX = 16.
%       (For a `publication quality' picture, set THMAX higher, say 32.)
%       The eigenvalues of A are displayed as `x'.
%       Alternative usage: [F, E] = FV(A, NK, THMAX, 1) suppresses the
%       plot and returns the field of values plot data in F, with A's
%       eigenvalues in E.   Note that NORM(F,INF) approximates the
%       numerical radius,
%                 max {abs(z): z is in the field of values of A}.

%       Theory:
%       Field of values FV(A) = set of all Rayleigh quotients. FV(A) is a
%       convex set containing the eigenvalues of A.  When A is normal FV(A) is
%       the convex hull of the eigenvalues of A (but not vice versa).
%               z = x'Ax/(x'x),  z' = x'A'x/(x'x)
%               => REAL(z) = x'Hx/(x'x),   H = (A+A')/2
%       so      MIN(EIG(H)) <= REAL(z) <= MAX(EIG(H)),
%       with equality for x = corresponding eigenvectors of H.  For these x,
%       RQ(A,x) is on the boundary of FV(A).
%
%       Based on an original routine by A. Ruhe.
%
%       References:
%       R. A. Horn and C. R. Johnson, Topics in Matrix Analysis, Cambridge
%            University Press, 1991; sec. 1.5.
%       A. S. Householder, The Theory of Matrices in Numerical Analysis,
%            Blaisdell, New York, 1964; sec. 3.3.
%       C. R. Johnson, Numerical determination of the field of values of a
%            general complex matrix, SIAM J. Numer. Anal., 15 (1978),
%            pp. 595-602.

if nargin < 2 | isempty(nk), nk = 1; end
if nargin < 3 | isempty(thmax), thmax = 16; end
thmax = thmax - 1;  % Because code below uses thmax + 1 angles.

iu = sqrt(-1);
[n, p] = size(B);
if n ~= p, error('Matrix must be square.'), end
f = [];
z = zeros(2*thmax+1,1);
e = eig(B);

% Filter out cases where B is Hermitian or skew-Hermitian, for efficiency.
if isequal(B,B')

   f = [min(e) max(e)];

elseif isequal(B,-B')

   e = imag(e);
   f = [min(e) max(e)];
   e = iu*e; f = iu*f;

else

for m = 1:nk

   ns = n+1-m;
   A = B(1:ns, 1:ns);

   for i = 0:thmax
      th = i/thmax*pi;
      Ath = exp(iu*th)*A;               % Rotate A through angle th.
      H = 0.5*(Ath + Ath');             % Hermitian part of rotated A.
      [X, D] = eig(H);
      [lmbh, k] = sort(real(diag(D)));
      z(1+i) = rq(A,X(:,k(1)));         % RQ's of A corr. to eigenvalues of H
      z(1+i+thmax) = rq(A,X(:,k(ns)));  % with smallest/largest real part.
   end

   f = [f; z];

end
% Next line ensures boundary is `joined up' (needed for orthogonal matrices).
f = [f; f(1,:)];

end
if thmax == 0; f = e; end

if nargin < 4

   ax = cpltaxes(f);
   plot(real(f), imag(f))      % Plot the field of values
   axis(ax);
   axis('square');

   hold on
   plot(real(e), imag(e), 'x')    % Plot the eigenvalues too.
   hold off

end

function z = rq(A,x)
%RQ      Rayleigh quotient.
%        RQ(A,x) is the Rayleigh quotient of A and x, x'*A*x/(x'*x).

z = x'*A*x/(x'*x);


function y = geo_mean( x, dim, w )

%GEO_MEAN   Geometric mean.
%   Y=GEO_MEAN(X), where X is a vector, computes the geometrix mean of X. If any
%   of the elements of X are negative, then Y=-Inf. Otherwise, it is equivalent
%   to Y=PROD(X).^(1/LENGTH(X)). All elements must be real.
%
%   For matrices, GEO_MEAN(X) is a row vector containing the geometric means of
%   the columns. For N-D arrays, GEO_MEAN(X) is an array of the geometric means
%   taken along the first non-singleton dimension of X.
%
%   GEO_MEAN(X,DIM) takes the geometric mean along the dimension DIM of X.
%
%   GEO_MEAN(X,DIM,W), where W is a vector of nonnegative integers, computes a
%   weighted geometric mean Y = PROD(X.^W)^(1/SUM(W)). This is more efficient
%   than replicating the values of X W times. Note that W must be a vector,
%   even if X is a matrix, and its length must be the same as SIZE(X,DIM).
%
%   Disciplined convex programming information:
%       GEO_MEAN is concave  and nondecreasing; therefore, when used in CVX
%       specifications, its argument must be concave.

%
% Check arguments
%

error( nargchk( 1, 3, nargin ) );
if ~isreal( x ),
    error( 'First argument must be real.' );
elseif nargin < 2,
    dim = cvx_default_dimension( size( x ) );
elseif ~cvx_check_dimension( dim ),
    error( 'Second argument must be a positive integer.' );
end
sx = size( x );
nx = sx( dim );

%
% Third argument check
%

if nargin < 3 || isempty( w ),
    w = [];
elseif numel( w ) ~= length( w ) || ~isnumeric( w ) || ~isreal( w ) || any( w < 0 ) || any( w ~= floor( w ) ),
    error( 'Third argument must be a vector of nonnegative integers.' );
elseif length( w ) ~= nx,
    error( 'Third argument must be a vector of length %d.', nx );
else
    w = reshape( w, 1, nx );
end

if nx == 0,
    sx( dim ) = 1;
    y = ones( sx );
else
    if nx == 1,
        y = x;
    elseif isempty( w ) || ~any( diff( w ) ),
        y = exp( sum( log( max( x, realmin ) ), dim ) * ( 1 / nx ) );
    elseif dim == 1,
        y = exp( w * log( max( x, realmin ) ) * ( 1 / sum( w ) ) );
    else
        pvec = [ dim, 1 : dim - 1, dim + 1 : ndims( x ) ];
        y = ipermute( exp( w * log( max( permute( x, pvec ), realmin ) ) * ( 1 / sum( w ) ) ), pvec );
    end
    xmin = min( x, [], dim );
    y( xmin <  0 ) = -Inf;
    y( xmin == 0 ) = 0;
end

% Copyright 2010 Michael C. Grant and Stephen P. Boyd.
% See the file COPYING.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function p = part_table ( n )

%*****************************************************************************80
%
%% PART_TABLE tabulates the number of partitions of N.
%
%  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 N, the integer to be partitioned.
%    N must be positive.
%
%    Output, integer P(1:N+1), P(I+1) is the number of partitions of I.
%
  offset = 1;

  p = zeros ( n + 1, 1 );

  p(0+offset) = 1;
  p(1+offset) = 1;

  for i = 2 : n

    sign = 1;
    psum = 0;
    w = 1;
    j = 1;
    wprime = w + j;

    while ( w < n )

      if ( 0 <= i - w )
        if ( sign == 1 )
          psum = psum + p(i-w+offset);
        else
          psum = psum - p(i-w+offset);
        end
      end

      if ( wprime <= i )

        if ( sign == 1 )
          psum = psum + p(i-wprime+offset);
        else
          psum = psum - p(i-wprime+offset);
        end

      end

      w = w + 3 * j + 1;
      j = j + 1;
      wprime = w + j;
      sign = - sign;

    end

    p(i+offset) = psum;

  end

  return
end


function [rectx,recty,area,perimeter] = minboundrect(x,y,metric)
% minboundrect: Compute the minimal bounding rectangle of points in the plane
% usage: [rectx,recty,area,perimeter] = minboundrect(x,y,metric)
%
% arguments: (input)
%  x,y - vectors of points, describing points in the plane as
%        (x,y) pairs. x and y must be the same lengths.
%
%  metric - (OPTIONAL) - single letter character flag which
%        denotes the use of minimal area or perimeter as the
%        metric to be minimized. metric may be either 'a' or 'p',
%        capitalization is ignored. Any other contraction of 'area'
%        or 'perimeter' is also accepted.
%
%        DEFAULT: 'a'    ('area')
%
% arguments: (output)
%  rectx,recty - 5x1 vectors of points that define the minimal
%        bounding rectangle.
%
%  area - (scalar) area of the minimal rect itself.
%
%  perimeter - (scalar) perimeter of the minimal rect as found
%
%
% Note: For those individuals who would prefer the rect with minimum
% perimeter or area, careful testing convinces me that the minimum area
% rect was generally also the minimum perimeter rect on most problems
% (with one class of exceptions). This same testing appeared to verify my
% assumption that the minimum area rect must always contain at least
% one edge of the convex hull. The exception I refer to above is for
% problems when the convex hull is composed of only a few points,
% most likely exactly 3. Here one may see differences between the
% two metrics. My thanks to Roger Stafford for pointing out this
% class of counter-examples.
%
% Thanks are also due to Roger for pointing out a proof that the
% bounding rect must always contain an edge of the convex hull, in
% both the minimal perimeter and area cases.
%
%
% See also: minboundcircle, minboundtri, minboundsphere
%
%
% default for metric
if (nargin<3) || isempty(metric)
  metric = 'a';
elseif ~ischar(metric)
  error 'metric must be a character flag if it is supplied.'
else
  % check for 'a' or 'p'
  metric = lower(metric(:)');
  ind = strmatch(metric,{'area','perimeter'});
  if isempty(ind)
    error 'metric does not match either ''area'' or ''perimeter'''
  end

  % just keep the first letter.
  metric = metric(1);
end

% preprocess data
x=x(:);
y=y(:);

% not many error checks to worry about
n = length(x);
if n~=length(y)
  error 'x and y must be the same sizes'
end

% if var(x)==0

% start out with the convex hull of the points to
% reduce the problem dramatically. Note that any
% points in the interior of the convex hull are
% never needed, so we drop them.
if n>3

    %%%%%%%%%%%%%%%%%%%%%%%%%
    if (var(x)== 0|| var(y)==0)
        if var(x)== 0
            x = [x-1;x(1); x+1 ];
            y = [y ;y(1);y];
            flag = 1;
        else
            y = [y-1;y(1); y+1 ];
            x = [x ;x(1);x];
            flag = 1;
        end

    else
        flag = 0;
     %%%%%%%%%%%%%%%%%%%%%%
    edges = convhull(x,y);  % 'Pp' will silence the warnings

    end

  % exclude those points inside the hull as not relevant
  % also sorts the points into their convex hull as a
  % closed polygon

  %%%%%%%%%%%%%%%%%%%%
  if flag == 0
  %%%%%%%%%%%%%%%%%%%%

  x = x(edges);
  y = y(edges);
  %%%%%%%%%%%%%%%%%%
  end
  %%%%%%%%%%%%%
  % probably fewer points now, unless the points are fully convex
  nedges = length(x) - 1;
elseif n>1
  % n must be 2 or 3
  nedges = n;
  x(end+1) = x(1);
  y(end+1) = y(1);
else
  % n must be 0 or 1
  nedges = n;
end

% now we must find the bounding rectangle of those
% that remain.

% special case small numbers of points. If we trip any
% of these cases, then we are done, so return.
switch nedges
  case 0
    % empty begets empty
    rectx = [];
    recty = [];
    area = [];
    perimeter = [];
    return
  case 1
    % with one point, the rect is simple.
    rectx = repmat(x,1,5);
    recty = repmat(y,1,5);
    area = 0;
    perimeter = 0;
    return
  case 2
    % only two points. also simple.
    rectx = x([1 2 2 1 1]);
    recty = y([1 2 2 1 1]);
    area = 0;
    perimeter = 2*sqrt(diff(x).^2 + diff(y).^2);
    return
end
% 3 or more points.

% will need a 2x2 rotation matrix through an angle theta
Rmat = @(theta) [cos(theta) sin(theta);-sin(theta) cos(theta)];

% get the angle of each edge of the hull polygon.
ind = 1:(length(x)-1);
edgeangles = atan2(y(ind+1) - y(ind),x(ind+1) - x(ind));
% move the angle into the first quadrant.
edgeangles = unique(mod(edgeangles,pi/2));

% now just check each edge of the hull
nang = length(edgeangles);
area = inf;
perimeter = inf;
met = inf;
xy = [x,y];
for i = 1:nang
  % rotate the data through -theta
  rot = Rmat(-edgeangles(i));
  xyr = xy*rot;
  xymin = min(xyr,[],1);
  xymax = max(xyr,[],1);

  % The area is simple, as is the perimeter
  A_i = prod(xymax - xymin);
  P_i = 2*sum(xymax-xymin);

  if metric=='a'
    M_i = A_i;
  else
    M_i = P_i;
  end

  % new metric value for the current interval. Is it better?
  if M_i<met
    % keep this one
    met = M_i;
    area = A_i;
    perimeter = P_i;

    rect = [xymin;[xymax(1),xymin(2)];xymax;[xymin(1),xymax(2)];xymin];
    rect = rect*rot';
    rectx = rect(:,1);
    recty = rect(:,2);
  end
end
% get the final rect

% all done

end % mainline end


function a = vand1_inverse ( n, x )

%*****************************************************************************80
%
%% VAND1_INVERSE returns the inverse of the VAND1 matrix.
%
%  Formula:
%
%    A(I,J) = coefficient of X^(J-1) in I-th Lagrange basis polynomial.
%
%  Example:
%
%    N = 5,
%    X = ( 2, 3, 4, 5, 6 )
%
%     15.00  -14.25    4.96  -0.75   0.04
%    -40.00   44.67  -17.33   2.83  -0.17
%     45.00  -54.00   22.75  -4.00   0.25
%    -24.00   30.00  -13.33   2.50  -0.17
%      5.00   -6.42    2.96  -0.58   0.04
%
%  Properties:
%
%    The sum of the entries of A is
%
%      1 - product ( 1 <= I <= N ) ( 1 - 1 / X(I) ).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, real X(N), the values that define A.
%
%    Output, real A(N,N), the matrix.
%
  a = zeros ( n, n );

  for i = 1 : n
    for j = 1 : n
      if ( j == 1 )
        a(i,j) = 1.0;
      else
        a(i,j) = 0.0;
      end
    end
  end

  for i = 1 : n

    index = 0;

    for k = 1 : n

      if ( k ~= i )

        index = index + 1;

        for j = index + 1: - 1 : 1

          a(i,j) = - x(k) * a(i,j) / ( x(i) - x(k) );

          if ( 1 < j )
            a(i,j) = a(i,j) + a(i,j-1) / ( x(i) - x(k) );
          end

        end

      end

    end

  end

  return
end


function x=isleap(Year)
%ISLEAP True for leap year.
%     ISLEAP(Year) returns 1 if Year is a leap year and 0 otherwise.
%     ISLEAP is only set for gregorian calendar, so Year >= 1583
%
% Syntax: 	ISLEAP(YEAR)
%
%     Inputs:
%           YEAR - Year of interest (default = current year).
%           You can input a vector of years.
%     Outputs:
%           Logical vector.
%
%      Example:
%
%           Calling on Matlab the function: isleap
%
%           Answer is: 0
%
%
%           Calling on Matlab the function: x=isleap([2007 2008])
%
%           Answer is:
%           x = 0 1
%
%           Created by Giuseppe Cardillo
%           giuseppe.cardillo-edta@poste.it
%           Modified after Simon Jan suggestions
% To cite this file, this would be an appropriate format:
% Cardillo G. (2007) Isleap: a simple routine to test if a year is a leap
% year.
% http://www.mathworks.com/matlabcentral/fileexchange/14172

%Input Error handling
switch nargin
    case 0
         c=clock; Year=c(1); clear c
    case 1
        if ~isvector(Year) || ~all(isnumeric(Year)) || ~all(isfinite(Year)) || isempty(Year)
            error('Warning: Year values must be numeric and finite')
        end
        if ~isequal(Year,round(Year))
            error('Warning: Year values must be integer')
        end
        L=Year-1583;
        if L(L<0)
            error('Warning: Every value of Year must be >1582')
        end
    otherwise
        error('stats:Isleap:TooMuchInputs','Year must be a scalar or a vector.');
end

% The Gregorian calendar has 97 leap years every 400 years:
% Every year divisible by 4 is a leap year.
% However, every year divisible by 100 is not a leap year.
% However, every year divisible by 400 is a leap year after all.
% So, 1700, 1800, 1900, 2100, and 2200 are not leap years,
% but 1600, 2000, and 2400 are leap years.
x = ~mod(Year, 4) & (mod(Year, 100) | ~mod(Year, 400));
return


function ex2 (n)
%EX2: create an n-by-n 2D mesh, four different ways

% Example:
%   ex2
% See also: cs_demo

%   Copyright 2006-2007, Timothy A. Davis.
%   http://www.cise.ufl.edu/research/sparse

if (nargin < 1)
    n = 30 ;
end

subplot (1,2,1) ;

% method 1: create an n-by-n 2D mesh for the 2nd difference operator
tic
ii = zeros (5*n^2, 1) ;
jj = zeros (5*n^2, 1) ;
xx = zeros (5*n^2, 1) ;
k = 1 ;
for j = 0:n-1
    for i = 0:n-1
        s = j*n+i + 1 ;
        ii (k:k+4) = [(j-1)*n+i j*n+(i-1) j*n+i j*n+(i+1) (j+1)*n+i ] + 1 ;
        jj (k:k+4) = [s s s s s] ;
        xx (k:k+4) = [-1 -1 4 -1 -1] ;
        k = k + 5 ;
    end
end

% remove entries beyond the boundary
keep = find (ii >= 1 & ii <= n^2 & jj >= 1 & jj <= n^2) ;
ii = ii (keep) ;
jj = jj (keep) ;
xx = xx (keep) ;
A = sparse (ii,jj,xx) ;
t1 = toc ; disp (t1) ;
% subplot (2,2,1) ;
spy (A)
title (sprintf ('%d-by-%d 2D mesh\n', n, n)) ;

% method 2, using no for loops
tic
nn = 1:n^2 ;
i2 = [nn-n ; nn-1 ; nn ; nn+1 ; nn+n] ;
j2 = repmat (nn, 5, 1) ;
x2 = repmat ([-1 -1 4 -1 -1]', 1, n^2) ;
keep = find (i2 >= 1 & i2 <= n^2 & j2 >= 1 & j2 <= n^2) ;
i2 = i2 (keep) ;
j2 = j2 (keep) ;
x2 = x2 (keep) ;
C = sparse (i2,j2,x2) ;
t2 = toc ; disp (t2) ;

% subplot (2,2,2) ; plot (j2) ;
% title ('2D fast j2') ;
disp (A-C) ;

any (ii-i2)
any (jj-jj)

% method 3: create an n-by-n-by-n 3D mesh for the 2nd difference operator
tic
[A, keep, ii, jj, xx] = mesh3d1 (n) ;
ii = ii (keep) ;
jj = jj (keep) ;
xx = xx (keep) ;
t3 = toc ; disp (t3) ;
tic
E = sparse (ii,jj,xx) ;
t3b = toc ; disp (t3b) ;
subplot (1,2,2) ; spy (E) ;
title (sprintf ('%d-by-%d-by-%d 3D mesh\n', n, n, n)) ;

% method 4, using no for loops
tic
nn = 1:n^3 ;
i2 = [nn-n^2 ; nn-n ; nn-1 ; nn ; nn+1 ; nn+n ; nn+n^2] ;
j2 = repmat (nn, 7, 1) ;
x2 = repmat ([-1 -1 -1 6 -1 -1 -1]', 1, n^3) ;
keep = find (i2 >= 1 & i2 <= n^3 & j2 >= 1 & j2 <= n^3) ;
i2 = i2 (keep) ;
j2 = j2 (keep) ;
x2 = x2 (keep) ;
t4 = toc ; disp (t4) ;
tic
F = sparse (i2,j2,x2) ;
t4b = toc ; disp (t4b) ;
disp (E-F) ;


% EX_MAXWELL_SRC_PERIODIC_SQUARE: solve Maxwell source problem in the unit square.

% 1) PHYSICAL DATA OF THE PROBLEM
clear problem_data
% Physical domain, defined as NURBS map given in a text file
problem_data.geo_name = 'geo_square.txt';
% The domain must also have the right continuity conditions on the periodic
%  sides, otherwise the convergence rate may deteriorate.
%  See the example in ex_laplace_square_periodic.

% Type of boundary conditions
problem_data.nmnn_sides     = [];
problem_data.drchlt_sides   = [3 4];
problem_data.periodic_directions = [1];

% Physical parameters
problem_data.c_stiff = @(x, y) ones(size(x));
problem_data.c_mass  = @(x, y) pi^2 * ones(size(x));

% Source and boundary terms
problem_data.f = @(x, y) cat(1, zeros (1, size (x, 1), size (x, 2)), ...
                             reshape (17*pi^2*sin(4*pi*x), [1, size(x)]));
problem_data.g = @(x, y, ind) zeros (2, size (x, 1), size (x, 2));
problem_data.h = @(x, y, ind) zeros (2, size (x, 1), size (x, 2));

% Exact solution (optional)
problem_data.uex     = @(x, y) cat(1, zeros (1, size (x, 1), size (x, 2)), ...
                                      reshape ( sin(4*pi*x), [1, size(x)]));
problem_data.curluex = @(x, y) 4*pi*cos(4*pi*x);

% 2) CHOICE OF THE DISCRETIZATION PARAMETERS
clear method_data
method_data.degree     = [3 3];     % Degree of the bsplines
method_data.regularity = [2 2];     % Regularity of the splines
method_data.nsub       = [9 9];   % Number of subdivisions
method_data.nquad      = [4 4];     % Points for the Gaussian quadrature rule

% 3) CALL TO THE SOLVER
[geometry, msh, space, u] = solve_maxwell_src (problem_data, method_data);

% 4) POST-PROCESSING
vtk_pts = {linspace(0, 1, 30), linspace(0, 1, 30)};
% 4.1) EXPORT TO PARAVIEW
output_file = 'maxwell_square_periodic_Deg3_Reg2_Sub8';
fprintf ('The result is saved in the file %s \n \n', output_file);
sp_to_vtk (u, space, geometry, vtk_pts, output_file, 'u')

% 4.2) Plot in Matlab. Comparison with the exact solution
[eu, F] = sp_eval (u, space, geometry, vtk_pts);
[X, Y]  = deal (squeeze(F(1,:,:)), squeeze(F(2,:,:)));
eu2     = problem_data.uex (X, Y);

subplot(1,2,1)
quiver (X, Y, squeeze(eu(1,:,:)), squeeze(eu(2,:,:)))
axis equal tight
title('Computed solution')
ylim([0,1]); xlim([0,1]);
subplot(1,2,2)
quiver (X, Y, squeeze(eu2(1,:,:)), squeeze(eu2(2,:,:)))
axis equal tight
title('Exact solution')
ylim([0,1]); xlim([0,1]);

[error_hcurl, error_l2] = ...
    sp_hcurl_error (space, msh, u, problem_data.uex, problem_data.curluex)


%% Defining Tensorial Properties
%
% Physical laws describe the relationship between physical properties. The
% most simplest laws are linear ones and are of the form
%
% $$ y = \mathbf A x $$
%
% where $x$ and $y$ are the physical properties and $\mathbf A$ is a
% material constant. In a typical example $y$ could be the force applied to
% a spring, $x$ the displacement and $A$ describes the stiffnes of the
% spring which is essentially Hooks law.
%
% As soon as we consider more general forces and displacements they can not
% be described anymore by scalar numbers $x$ and $y$ but vectors or
% matrices are required. In its most general form the displacment is
% describes by a <strainTensor.html strain matrix> $\sigma_{ij}$ and the
% force is described by a stiffness matrix $\varepsilon_{kl}$. In this setting
% the linear relationship between the two matrices is described by the
% <complianceTensor.complianceTensor.html compliance tensor> $\mathbf
% C_{ijkl}$ which can be seen as a 4 dimensional generalization of a matrix.
%
% More, general a tensor of rank $r$ is a "matrix" of dimension $r$. If
% $r=0$ we speek of scalars, if $r=1$ these are vectors and for $r=2$ they
% are classical $3 \times 3$ matrices.
%
% In the following we explain how tensors of arbitrary rank can be defined
% in MTEX. Independent of the rank any tensor is represented in MTEX by a
% variable of type @tensor.
%
%
%% Scalars (tensors of zero rank)
%
% In physics, properties like temperature or density are not connected
% to any specific direction of the body they are measured. These
% non-directional physical quantities are called scalars, and they are
% defined by a single number. In MTEX, a scalar is defined by:

M = 5;
t = tensor(M,'rank',0)

%% Vectors (tensors of first rank)
%
% In contrast to scalars, other physical quantities can only be defined in
% reference to a specific direction. If we need to specify completely the
% mechanical force acting into a point for example, we need to specify
% the magnitude and its direction. As an alternative, we can choose three
% mutually perpendicular axes (A1,A2 and A3) and give the vector components
% along them. In MTEX, this is done by:

t = tensor([1;2;3],'rank',1)

%%
% where 1, 2 and 3 are the components related to the axes A1, A2 and A3.
% As rank 1 tensors are essentialy vectors we can freely convert tensors to
% @vector3d and vice verca.

% define a tensor from a vector
t = tensor(vector3d.X)

% convert a tensor into a vector
vector3d(t)

%% Matrices (tensors of second rank)
%
% We have now to expand the idea of a vector to three-dimensional space.
% Let's take the example of stress (force per unit of area). Imagine a cube
% of material subjected to load as shown below. As can be seen, one can
% measure ther stresses in this cube in various directions, and in various
% planes. These measurements will for a second rank sensor, where each
% component is associated with a pair of axes, taken in an specific order.
% The generalized second rank stress tensor can be written as
%
% $$
% \sigma_{ij} =
% \left[\begin{array}{ccc}
% \sigma_{11} & \sigma_{12} & \sigma_{13}  \\
% \sigma_{21} & \sigma_{22} & \sigma_{23}  \\
% \sigma_{31} & \sigma_{32} & \sigma_{33}  \\
% \end{array}\right]
% $$
%
% In MTEX, a second-rank tensor where only the main diagonal components are
% of interest is defined as

t = tensor(diag([1,2,3]), 'rank',2)

%%
% If all the components are of interest, the definition is as follow

M = [1     0.75  0.5;...
     0.75  1     0.25;...
     0.5   0.25  1];

t = tensor(M,'rank',2)

%% Tensors (tensors of third rank)
%
% Smart materials are materials that have one or more properties that
% change significantly under external stimuli. A typical example is the
% voltage resulting to applied stress that certain materials have, named
% piezoeletric effect. This property is described as a third rank tensor
% that relates induced electric displacement vector to the second-order
% stress tensor. This is expressed in the form $P_i=d_{ijk} \sigma_{jk}$.
% In MTEX, a third rank tensor can be described as

M =[[-1.9222  1.9222    0   -0.1423     0         0    ];...
    [   0        0      0       0     0.1423    3.8444];...
    [   0        0      0       0       0         0    ]];

t = tensor(M,'rank',3)

%% Tensors (tensors of fourth rank)
%
% Fourth rank tensors are tensors that describe the relation between 2
% second rank tensors. A typical example is the tensor describing the
% elastic properties of materials, which translate the linear relationship
% between the second rank stress and infinitesimal strain tensors. The
% Hooke's Law describing the shape changes in a material subject to stress
% can be written as $\sigma_{ij}=c_{ijkl} \epsilon_{kl}$, where $c_{ijkl}$
% is a fourth rank tensor.
%
% The four indices (ijkl) of the elastic tensor have values between 1 and
% 3, so that there are $3^4=81$ coefficients. As the stress and strain
% tensors are symmetric, both stress and strain second rank tensors only
% have 6 independent values rather than 9. In addition, crystal symmetry
% reduces even more the number of independent components on the elastic
% tensor, from 21 in the case of triclinic phases, to 3 in the case of
% cubic materials. In MTEX, a fourth rank tensor can be defined as:

M = [[320   50  50   0     0     0];...
    [  50  320  50   0     0     0];...
    [  50   50 320   0     0     0];...
    [   0    0   0  64     0     0];...
    [   0    0   0   0    64     0];...
    [   0    0   0   0     0    64]];

C = tensor(M,'rank',4)

%%
% Note the repetition in values in this matrix is related to crystal
% symmetry, in this case, a cubic example, where only $C_{11}$, $C_{12}$
% and $C_{44}$ are independent components.
%
%% Specific tensors
%
% MTEX includes specific classes for the following tensors.
%
% || *name* || *rank* || *symbol* || *name* || *rank* || *symbol* ||
% || @complianceTensor || 4 || $S_{ijkl}$ || @stiffnessTensor || 4 || $C_{ijkl}$ ||
% || @strainTensor || 2 || $\sigma_{ij}$  || @stressTensor || 2 || $\varepsilon_{ij}$ ||
% || @strainRateTensor || 2 || $E$ || @velocityGradientTensor || 2 || $L$ ||
% || @curvatureTensor || 2 || $\kappa_{ij}$ || @deformationGradientTensor || 2 || $F$ ||
% || @refractiveIndexTensor || 2 || $\chi$ || @ChristoffelTensor || 2 || $M_{ij}$ ||
% || @dislocationDensityTensor || 2 || $\alpha$  || <SchmidTensor.html |SchmidTensor|> || 2 || $M_{ij}$ ||
% || <tensor.leviCivita.html |leviCivita|> || 3 || $\varepsilon_{ijk}$ || @spinTensor || 2 || $\Omega$ ||
%
% Those specific tensors are defined by the syntax

M = [0 0 0;...
  0 0 0; ...
  0 0 1];

e = strainTensor(M)

%%
% In many cases shortcuts exist like

e = stressTensor.uniaxial(vector3d.Z)

%%
% The advantage of using these specific tensor classes is that some tensor
% operations like <stressTensor/calcShearStress.html |calcShearStress(e)|>
% are defined only for specific tensor classes.
%
%% Predefined tensors
%
% For certain applications, one may want to have a tensor where all the
% components are 1. In MTEX this is computed as

t = tensor.ones('rank',2)

%%
% *Identity tensor*
%
% The Identity tensor is a second order tensor that has ones n the main
% diagonal and zeros otherwise. The identity matrix has some special
% properties, including (i) When multiplied by itself, the result is itself
% and (ii) rows and columns are linearly independent. In MTEX, this matrix
% can be computed as

t = tensor.eye('rank',2)

%%
% *Random tensors*
%
% One can also define a tensor in which the components are pseudorandom, by
% using the function |<tensor.rand.html tensor.rand>|

t = tensor.rand('rank',2)

%%
% *The Levi Civita tensor*
%
% The Levi-Civita symbol $\epsilon_{ijk}$ is a third rank tensor and is
% defined by 0, if $i=j$, $j=k$ or $k=1$, by 1, if $(i,j,k)=(1,2,3)$,
% $(2,3,1)$ or $(3,1,2)$ and by $-1$, if $(i,j,k)=(3,2,1)$, $(1,3,2)$ or
% $(2,1,3)$. The Levi-Civita symbol allows the cross product of two vectors
% in 3D Euclidean space and the determinant of a square matrix to be
% expressed in Einstein's index notation. With MTEX the Levi Civita tensor
% is expressed as

t = tensor.leviCivita


function [yUpdate, PInvUpdate]=infoFilterUpdate(yPred,PInvPred,z,R,H)
%%INFOFILTERUPDATE Perform the measurement update step in the standard
%                  linear information filter. Using an information filter
%                  means that instead of propagating a state x and its
%                  covariance matrix P, one propagates inv(P) and
%                  y=inv(P)*x.
%
%INPUTS: yPred The xDimX1 predicted information state. The information
%              state is the inverse covariance matrix times the target
%              state.
%     PInvPred The xDimXxDim inverse of the predicted state covariance
%              matrix.
%            z The zDimX1 vector measurement.
%            R The zDimXzDim measurement covariance matrix. This must be
%              positive definite.
%            H The zDimXxDim measurement matrix for a linear measurement
%              model. That is z=H*x+w, where w is measurement noise having
%              covariance matrix R.
%
%OUTPUTS: yUpdate The xDimX1 updated (posterior) information state vector.
%      PInvUpdate The updated xDimXxDim inverse state covariance matrix.
%
%The information filter is algebraically equivalent to the standard linear
%Kalman filter, but allows for track propagation with very uncertain
%states, such as when starting tracks. The implementation of the update
%given here is from the flow chart given in Appendix H of [1].
%
%The standard information filter has the implied linear measurement
%equation
%z=H*x+w
%where z is the measurement, w is the zero-mean Gaussian measurement noise
%with covariance matrix R and x is the true state. However, instead of
%propagating the true state x with its covariance matrix P, the information
%filter propagates
%y=inv(P)*x
%and instead of propagating the true covariance matrix P, it propagates
%PInv=inv(P)
%which means that the filter can be used even when PInv is singular.
%
%More information on information filtering is given in Chapter 7.2 of [2].
%
%EXAMPLE:
%In this example, we feed two measurements to the information filter and
%show that the result has a consistent NEES and the RMSE is the same as the
%second measurement.
% T=1;%Sample period.
% numRuns=1000;
% %Parameters for the noise process dynamics and measurement.
% H=[eye(2,2),zeros(2,2)];
% zDim=size(H,1);
% xDim=size(H,2);
% R=diag([40;40]);
% SR=chol(R,'lower');
%
% %Statistics for the initial state.
% x0Mean=[1e3;0;75;-50];
% x0Cov=diag([1e3^2;100^2;25^2;25^2]);
% x0S=chol(x0Cov,'lower');
%
% %Parameters for the state dynamics.
% q=processNoiseSuggest('PolyKal-ROT',9.8,1);
% F=FPolyKal(T,xDim,1);
% Q=QPolyKal(T,xDim,1,q);
% SQ=chol(Q,'lower');
%
% RMSEMeas=0;
% RMSE=0;
% NEES=0;
% for curRun=1:numRuns
%     %Draw the initial state.
%     x1True=x0Mean+x0S*randn(xDim,1);
%     %Get the first measurement.
%     z1=H*x1True+SR*randn(zDim,1);
%
%     x2True=F*x1True+SQ*randn(xDim,1);
%     z2=H*x2True+SR*randn(zDim,1);
%
%     %Two-point initialization.
%     y0=zeros(xDim,1);
%     PInv0=zeros(xDim,xDim);
%     [yUpdate,PInvUpdate]=infoFilterUpdate(y0,PInv0,z1,R,H);
%     [yPred, PInvPred]=infoFilterDiscPred(yUpdate,PInvUpdate,F,Q);
%     [yUpdate,PInvUpdate]=infoFilterUpdate(yPred,PInvPred,z2,R,H);
%     xEst2=PInvUpdate\yUpdate;
%
%     diff=xEst2-x2True;
%     NEES=NEES+diff'*PInvUpdate*diff;
%     RMSE=RMSE+sum(diff(1:2).^2);
%     diff=z2-x2True(1:2);
%     RMSEMeas=RMSEMeas+sum(diff(1:2).^2);
% end
% RMSEMeas=sqrt(RMSEMeas/numRuns)
% RMSE=sqrt(RMSE/numRuns)
% NEES=NEES/(xDim*numRuns)
%One will see that RMSEMeas equal RMSE, because with just two measurements,
%one cannot smmooth the position estimate syet. Additionally, NEES will be
%close to 1 indicating covariance consistency.
%
%REFERENCES:
%[1] David F. Crouse , "Basic tracking using nonlinear 3D monostatic and
%    bistatic measurements," IEEE Aerospace and Electronic Systems
%    Magazine, vol. 29, no. 8, Part II, pp. 4-53, Aug. 2014.
%[2] Y. Bar-Shalom, X. R. Li, and T. Kirubarajan, Estimation with
%    Applications to Tracking and Navigation. New York: John Wiley and
%    Sons, Inc, 2001.
%
%September 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

    yUpdate=yPred+H'/R*z;
    PInvUpdate=PInvPred+H'/R*H;
    %Ensure symmetry
    PInvUpdate=(PInvUpdate+PInvUpdate')/2;
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, odata, opts ] = solver_SLOPE( A, b, lambda, x0, opts )
% SOLVER_SLOPE Sorted l1-regularized least squares problem,
% [ beta, odata, opts ] = solver_SLOPE( X, y, lambda, beta0, opts )
%    Solves the l1-regularized least squares problem, using the sorted/ordered l1 norm,
%        minimize (1/2)*norm( A * x - b )^2 + norm( lasso.*sort(abs(x),'descend'), 1 )
%    using the Auslender/Teboulle variant with restart. X must be a matrix
%    or a linear operator, y must be a vector, and lambda must be a real
%    positive vector in decreasing order.
%    The initial point beta0 and option structure opts are both optional.
%
% SLOPE stands for Sorted L-One Penalized Estimation
%
% Reference:
%   "Statistical Estimation and Testing via the Ordered l1 Norm"
%   by M. Bogdan, E. van den Berg, W. Su, and E. J. Candès, 2013
%   http://www-stat.stanford.edu/~candes/OrderedL1/
%
%   See also solver_L1RLS.m, solver_LASSO.m, prox_Sl1.m

error(nargchk(3,5,nargin));
if nargin < 4, x0 = []; end
if nargin < 5, opts = []; end
if ~isfield( opts, 'restart' ),
    opts.restart = 100;
end

[x,odata,opts] = tfocs( smooth_quad, { A, -b }, prox_Sl1( lambda ), x0, opts );

% TFOCS v1.3 by Stephen Becker, Emmanuel Candes, and Michael Grant.
% Copyright 2013 California Institute of Technology and CVX Research.
% See the file LICENSE for full license information.


function r= cauchyrnd(varargin)

% USAGE:       r= cauchyrnd(a, b, n, ...)
%
% Generate random numbers from the Cauchy distribution, r= a + b*tan(pi*(rand(n)-0.5)).
%
% ARGUMENTS:
% a (default value: 0.0) must be scalars or size(x).
% b (b>0, default value: 1.0) must be scalars or size(x).
% n and onwards (default value: 1) specifies the dimension of the output.
%
% EXAMPLE:
% r= cauchyrnd(0, 1, 10); % A 10 by 10 array of random values, Cauchy distributed.
%
% SEE ALSO:    cauchycdf, cauchyfit, cauchyinv, cauchypdf.
%
% Copyright (C) Peder Axensten <peder at axensten dot se>
%
% HISTORY:
% Version 1.0, 2006-07-10.
% Version 1.1, 2006-07-26.
% - Added cauchyfit to the cauchy package.
% Version 1.2, 2006-07-31:
% - cauchyinv(0, ...) returned a large negative number but should be -Inf.
% - Size comparison in argument check didn't work.
% - Various other improvements to check list.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

	% Default values
	a=	0.0;
	b=	1.0;
	n=	1;

	% Check the arguments
	if(nargin >= 1)
		a=	varargin{1};
		if(nargin >= 2)
			b=			varargin{2};
			b(b <= 0)=	NaN;	% Make NaN of out of range values.
			if(nargin >= 3),	n=	[varargin{3:end}];		end
		end
	end

	% Generate
	r=	cauchyinv(rand(n), a, b);
end


function [xn,dxdf,dxdc,dxdk,dxdalpha] = normalize2(x_kk,fc,cc,kc,alpha_c),

%normalize
%
%[xn] = normalize(x_kk,fc,cc,kc,alpha_c)
%
%Computes the normalized coordinates xn given the pixel coordinates x_kk
%and the intrinsic camera parameters fc, cc and kc.
%
%INPUT: x_kk: Feature locations on the images
%       fc: Camera focal length
%       cc: Principal point coordinates
%       kc: Distortion coefficients
%       alpha_c: Skew coefficient
%
%OUTPUT: xn: Normalized feature locations on the image plane (a 2XN matrix)
%
%Important functions called within that program:

k1 = kc(1);
k2 = kc(2);
k3 = kc(5);
p1 = kc(3);
p2 = kc(4);

N = size(x_kk,2);

% First: Subtract principal point, and divide by the focal length:
x_distort = [(x_kk(1,:) - cc(1))/fc(1);(x_kk(2,:) - cc(2))/fc(2)];

v1 = - x_distort(1,:) / fc(1);
v2 = - x_distort(2,:) / fc(1);

dx_distortdfc = zeros(2*N,2);
dx_distortdfc(1:2:end,1) = v1';
dx_distortdfc(2:2:end,2) = v2';

v1 = - x_distort(1,:) / fc(1);
v2 = - x_distort(2,:) / fc(1);

dx_distortdcc = zeros(2*N,2);
dx_distortdcc(1:2:end,1) = -(1/fc(1)) * ones(N,1);
dx_distortdcc(2:2:end,2) = -(1/fc(2)) * ones(N,1);

% Second: undo skew
x_distort(1,:) = x_distort(1,:) - alpha_c * x_distort(2,:);

dx_distort2dfc = [ dx_distortdfc(:,1)-alpha_c *dx_distortdfc(:,2)   dx_distortdfc(:,2)];
dx_distort2dcc = [ dx_distortdcc(:,1)-alpha_c *dx_distortdcc(:,2)   dx_distortdcc(:,2)];

dx_distort2dalpha_c = zeros(2*N,1);
dx_distort2dalpha_c(1:2:end) = -x_distort(2,:)';

x = x_distort; 				% initial guess

for kk=1:20,

    r_2 = sum(x.^2);
    k_radial =  1 + k1 * r_2 + k2 * r_2.^2 + k3 * r_2.^3;
    delta_x = [2*p1*x(1,:).*x(2,:) + p2*(r_2 + 2*x(1,:).^2); p1 * (r_2 + 2*x(2,:).^2)+2*p2*x(1,:).*x(2,:)];
    x = (x_distort - delta_x)./(ones(2,1)*k_radial);

end;

xn = x;

dxdk = zeros(2*N,5); % Approximation (no time)
dxdf = dx_distort2dfc;
dxdc = dx_distort2dcc;
dxdalpha = dx_distort2dalpha_c;


function [x,w,P]=GaussRadau(kk)
N = kk-1; % Compute for the number of points kk

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% lgrnodes.m
%
% Computes the Legendre-Gauss-Radau nodes, weights and the LGR Vandermonde
% matrix. The LGR nodes are the zeros of P_N(x)+P_{N+1}(x).
%
% References on LGR nodes and weights:
%   C. Canuto, M. Y. Hussaini, A. Quarteroni, T. A. Tang, "Spectral Methods
%   in Fluid Dynamics," Section 2.3. Springer-Verlag 1987
%
%   F. B. Hildebrand , "Introduction to Numerical Analysis," Section 8.11
%   Dover 1987
%
% Written by Greg von Winckel - 05/02/2004
% Contact: gregvw@chtm.unm.edu
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Truncation + 1
N1=N+1;

% Use Chebyshev-Gauss-Radau nodes as initial guess for LGR nodes
x=-cos(2*pi*(0:N)/(2*N+1))';

% The Legendre Vandermonde Matrix
P=zeros(N1,N1+1);

% Compute P_(N) using the recursion relation
% Compute its first and second derivatives and
% update x using the Newton-Raphson method.

xold=2;

% Free abscissae
free=2:N1;

while max(abs(x-xold))>eps

    xold=x;

    P(1,:)=(-1).^(0:N1);

    P(free,1)=1;    P(free,2)=x(free);

    for k=2:N1
        P(free,k+1)=( (2*k-1)*x(free).*P(free,k)-(k-1)*P(free,k-1) )/k;
    end

    x(free)=xold(free)-((1-xold(free))/N1).*(P(free,N1)+P(free,N1+1))...
        ./(P(free,N1)-P(free,N1+1));
end

% The Legendre-Gauss-Radau Vandermonde
P=P(1:N1,1:N1);

% Compute the weights
w=zeros(N1,1);
w(1)=2/N1^2;
w(free)=(1-x(free))./(N1*P(free,N1)).^2;


function p = polyfitweighted2(x,y,z,n,w)
% polyfitweighted2.m
% -------------------
%
% Find a least-squares fit of 2D data z(x,y) with an nth order
% polynomial, weighted by w(x,y) .
%
% By S.S. Rogers (2006)
%
% Usage
% ------
%
% P = polyfitweighted2(X,Y,Z,N,W) finds the coefficients of a polynomial
% P(X,Y) of degree N that fits the data Z best in a least-squares
% sense. P is a row vector of length (N+1)*(N+2)/2 containing the
% polynomial coefficients in ascending powers, 0th order first.
%
%   P = [p00 p10 p01 p20 p11 p02 p30 p21 p12 p03...]
%
% e.g. For a 3rd order fit,
% the regression problem is formulated in matrix format as:
%
%   wZ = V*P    or
%
%                      2       2   3   2     2      3
%   wZ = [w  wx  wy  wx  xy  wy  wx  wx y  wx y   wy ]  [p00
%                                                        p10
%                                                        p01
%                                                        p20
%                                                        p11
%                                                        p02
%                                                        p30
%                                                        p21
%                                                        p12
%                                                        p03]
%
% *Note:* P is not in the format of standard Matlab 1D polynomials. Use
% polval2.m to evaluate the polynomial in this format, at given values of
% x,y.
%
% X,Y must be vectors
% Z,W must be 2D arrays of size [length(X) length(Y)]
%
% based on polyfit.m by The Mathworks Inc. - see doc polyfit for more details
%
% Class support for inputs X,Y,Z,W:
%      float: double, single

x = x(:);
y = y(:);

lx=length(x);
ly=length(y);

if ~isequal(size(z),size(w),[ly lx])
    error('polyfitweighted2:XYSizeMismatch',...
         [' X,Y *must* be vectors' ...
          '  Z,W *must* be 2D arrays of size [length(X) length(Y)]'])
end

y=y*ones(1,lx);
x=ones(ly,1)*x';
x = x(:);
y = y(:);
z = z(:);
w = w(:);

pts=length(z);

% Construct weighted Vandermonde matrix.
V=zeros(pts,(n+1)*(n+2)/2);
V(:,1) = w;
%V(:,1) = ones(pts,1);
ordercolumn=1;
for order = 1:n
    for ordercolumn=ordercolumn+(1:order)
        V(:,ordercolumn) = x.*V(:,ordercolumn-order);
    end
    ordercolumn=ordercolumn+1;
    V(:,ordercolumn) = y.*V(:,ordercolumn-order-1);
end

% Solve least squares problem.
[Q,R] = qr(V,0);
ws = warning('off','all');
p = R\(Q'*(w.*z));    % Same as p = V\(w.*z);
warning(ws);
if size(R,2) > size(R,1)
   warning('polyfitweighted2:PolyNotUnique', ...
       'Polynomial is not unique; degree >= number of data points.')
elseif condest(R) > 1.0e10
        warning('polyfitweighted2:RepeatedPointsOrRescale', ...
            ['Polynomial is badly conditioned. Remove repeated data points\n' ...
            '         or try centering and scaling as described in HELP POLYFIT.'])
end
%r = z - (V*p)./w;
p = p.';          % Polynomial coefficients are row vectors by convention.


function[upa,upb,upc,upd,upe,upf]=ellband(varargin)
%ELLBAND  Bandwidth of modulated elliptical signals in two or three dimensions.
%
%   [A,B,C]=ELLBAND(KAPPA,LAMBDA,THETA,PHI) computes the instantaneous
%   bandwidth of the elliptical signal characterized by RMS amplitude
%   KAPPA, linearity LAMBDA, orientation THETA, and orbital phase PHI.
%
%   The three output arguments are
%
%          A  - Amplitude modulation bandwidth
%          B  - Deformation bandwidth
%          C  - Precession bandwidth.
%
%   and these satisfy UPSILON^2=A^2+B^2+C^2 where UPSILON is the joint
%   instantaneous bandwidth of the bivariate signal.
%
%   The form of these terms is as follows:
%
%         A = 1/KAPPA d/dt KAPPA
%         B = 1/2 * 1/SQRT(1-LAMBDA^2) *  d/dt LAMBDA
%         C = LAMBDA d/dt THETA
%
%   [A,B,C,UPSILON]=ELLBAND(KAPPA,LAMBDA,THETA,PHI) also returns the total
%   instantaneous bandwith UPSILON=SQRT(A^2+B^2+C^2).
%
%   ELLBAND(...,DIM) performs the analysis with time running along
%   dimension DIM, as opposed to the default behavior of DIM=1.
%
%   For details see Lilly and Olhede (2010).
%
%   ELLBAND also works if the input arguments are cell arrays of numerical
%   arrays, in which case the output will be similarly sized cell arrays.
%   __________________________________________________________________
%
%   Three dimensions
%
%   ELLBAND can also compute the instantaneous bandwidth of modulated
%   elliptical signals in three dimensions.
%
%   [A,B,C,D,E]=ELLBAND(KAPPA,LAMBDA,THETA,PHI,ALPHA,BETA) returns the
%   terms in the bandwidth from a modulated ellipical signal in a plane
%   with a normal vector having azimuth angle ALPHA and zenith angle BETA.
%
%   The five output arguments are
%
%          A   - Amplitude modulation bandwidth, as in 2D
%          B   - Deformation bandwidth, as in 2D
%          C   - Precession bandwidth, as in 2D
%          D   - Precession bandwidth with full 3D effects
%          E   - Bandwidth due to motion of the normal to the plane
%
%   and these, in principle, satisfy UPSILON^2=A^2+B^2+C^2+D^2+|E|^2 where
%   UPSILON is the joint instantaneous bandwidth of the trivariate signal.
%   See below for a caveat on this statement.
%
%   Terms A--C are just as in the bivariate case.  The new terms are:
%
%         D = LAMBDA [d/dt THETA + COS(BETA) * d/dt ALPHA]
%         E = N^T X_+ / |X_+^H X_+|
%
%   where N is the trivariate normal vector, X_+ is the trivariate analytic
%   signal vector, and "T" denotes the matrix transpose, and "H" the
%   Hermitian transpose.  Note that term E may be complex-valued.
%
%   Note that term C does not contribute to the full bandwidth, but is
%   output in order to compare the two-dimensional and three-dimensional
%   effects in the full precession bandwidth, term D.
%
%   An important point is that the trivariate ellipse parameters can be
%   ill-defined for a nearly linear signal, and the elliptical bandwidth
%   terms can give erroneously large values at isolated points.  To check
%   for this, compare with the joint bandwidth from INSTMOM.
%
%   [A,B,C,D,E,UPSILON]=ELLBAND(KAPPA,LAMBDA,THETA,PHI,ALPHA,BETA) also
%   returns the total bandwith UPSILON=SQRT(A^2+B^2+C^2+D^2+|E|^2).
%
%   For details see Lilly (2011).
%   __________________________________________________________________
%
%   ELLBAND(DT,...) sets the sample interval DT, which defaults to DT=1.
%   DT may be a scalar, or if the input fields are cell arrays having
%   length N, DT may be a numerical array of length N.
%
%   See also ANATRANS, WAVETRANS, INSTMOM.
%
%   'ellband --t' runs a test.
%   'ellband --f' generates a figure from Lilly and Olhede (2010).
%
%   Usage:  [a,b,c]=ellband(kappa,lambda,theta,phi);
%           [a,b,c]=ellband(dt,kappa,lambda,theta,phi);
%           [a,b,c]=ellband(dt,kappa,lambda,theta,phi,dim);
%           [a,b,c,upsilon]=ellband(dt,kappa,lambda,theta,phi,dim);
%           [a,b,c,d,e]=ellband(kappa,lambda,theta,phi,alpha,beta);
%           [a,b,c,d,e]=ellband(dt,kappa,lambda,theta,phi,alpha,beta);
%   __________________________________________________________________
%   This is part of JLAB --- type 'help jlab' for more information
%   (C) 2006--2020 J.M. Lilly --- type 'help jlab_license' for details

if strcmpi(varargin{1}, '--t')
    ellband_test,return
end

if strcmpi(varargin{1}, '--f')
    type makefigs_ellband
    makefigs_ellband;
    return
end

if ~iscell(varargin{end})&&length(varargin{end})==1
    dim=varargin{end};
    varargin=varargin(1:end-1);
else
    dim=1;
end

if length(varargin)==5||length(varargin)==7
    dt=varargin{1};
    varargin=varargin(2:end);
else
    dt=1;
end

kappa=varargin{1};
lambda=varargin{2};
theta=varargin{3};
phi=varargin{4};
if length(varargin)>4
    alpha=varargin{5};
    beta=varargin{6};
    dims=3;
else
    alpha=[];
    beta=[];
    dims=2;
end

%Redo some initializing in case of cell arrays
if iscell(kappa)
    if isempty(alpha)
        for i=1:length(kappa)
            alpha{i,1}=[];
            beta{i,1}=[];
        end
    end
    if length(dt)==1
        dto=dt;
        for i=1:length(kappa)
            dt(i)=dto;
        end
    end
end

if ~isempty(kappa)
    if iscell(kappa)
        [upa,upb,upc,upd,upe,upf]=vempty;
        for i=1:length(kappa)
            [upa{i,1},upb{i,1},upc{i,1},upd{i,1},upe{i,1},upf{i,1}]=...
                ellband_one(dt(i),kappa{i,1},lambda{i,1},theta{i,1},phi{i,1},alpha{i,1},beta{i,1},dim,dims);
        end
    else
        [upa,upb,upc,upd,upe,upf]=ellband_one(dt,kappa,lambda,theta,phi,alpha,beta,dim,dims);
    end
else
    upa=kappa;upb=kappa;upc=kappa;
    upd=kappa;upe=kappa;upf=kappa;
end

function[upa,upb,upc,upd,upe,upf]=ellband_one(dt,kappa,lambda,theta,phi,alpha,beta,dim,dims)

omtheta=frac(vdiff(unwrap(theta,[],dim),dim),dt);
zeta=sign(lambda).*sqrt(1-lambda.^2);

%These do not seem much different
%upa=frac((vdiff(kappa,dim)),kappa*dt);
upa=frac((vdiff(log(kappa),dim)),dt);
upb=frac(sqrt(frac(1,1-lambda.^2)).*(frac(1,2)*vdiff(lambda,dim)),dt);

upb2=frac(sqrt(frac(1,1-zeta.^2)).*(frac(1,2)*vdiff(zeta,dim)),dt);
upb(abs(lambda)>sqrt(1/2))=-upb2(abs(lambda)>sqrt(1/2));
%To correct numerical instability when lambda is close to unity

%This is the same
%[a,b]=kl2ab(kappa,lambda);
%upc=abs(frac(a.*b,a.^2+b.^2).*vdiff(log(abs(b./a)),1))./dt;
upc=(lambda.*omtheta);

%Replace isolated nans that may have been lost
bool=isnan(kappa);
upa(bool)=nan;
upb(bool)=nan;
upc(bool)=nan;

if dims==3
     omalpha=frac(vdiff(unwrap(alpha,[],dim),dim),dt);
     ombeta=frac(vdiff(unwrap(beta,[],dim),dim),dt);

     upd=lambda.*(omtheta+omalpha.*cos(beta));

     [x,y,z]=ellsig(kappa,lambda,theta,phi,alpha,beta);

     %Now find x-tilde, 2-d vector
     [x,y,z]=vectmult(jmat3(-alpha,3),x,y,z);
     [x,y,z]=vectmult(jmat3(-beta,1),x,y,z);

     numer=-omalpha.*sin(beta).*x+ombeta.*y;
     denom=sqrt(abs(x).^2+abs(y).^2);
     upe=frac(numer,denom);
     upf=sqrt(upa.^2+upb.^2+upc.^2+upd.^2+abs(upe).^2);

     %Replace isolated nans that may have been lost
     upd(bool)=nan;
     upe(bool)=nan;
     upf(bool)=nan;

else
    upd=sqrt(upa.^2+upb.^2+upc.^2);
    upe=[];
    upf=[];
end

function[]=ellband_test
ellband_test1;
ellband_test2;
ellband_test3;
ellband_test4;

function[]=ellband_test1

load npg2006
use npg2006

%Decide on frequencies
fs=2*pi./(logspace(log10(10),log10(100),50)');

%Compute wavelet transforms using generalized Morse wavelets
[wx,wy]=wavetrans(real(cx),imag(cx),{1,2,4,fs,'bandpass'},'mirror');
[wxr,wyr,ir,jr]=ridgewalk(dt,wx,wy,fs,sqrt(2*4),1);

[kappa,lambda,theta,phi]=ellparams(wxr,wyr);
[ba,bd,bp]=ellband(kappa,lambda,theta,phi);
[a,om,upbar]=instmom([wxr wyr],1,2);

wxr2=permute(wxr,[3 2 1]);
wyr2=permute(wyr,[3 2 1]);
clear wr2
wr2(1,1,:)=wxr2;wr2(1,2,:)=wyr2;
[kappa2,lambda2,theta2,phi2]=ellparams(wxr2,wyr2,3);
[ba2,bd2,bp2]=ellband(kappa2,lambda2,theta2,phi2,3);
[a2,om2,upbar2]=instmom(wr2,3,2);
vcolon(kappa2,lambda2,theta2,phi2,ba2,bd2,bp2,a2,om2,upbar2);
bool=aresame([kappa2 lambda2 theta2 phi2 ba2 bd2 bp2 a2 om2 upbar2],...
    [kappa lambda theta phi ba bd bp a om upbar]);

reporttest('ELLBAND time running in pages', bool);

vindex(upbar,ba,bd,bp,2:length(upbar)-1,1);
err=vsum((upbar-sqrt(ba.^2+bd.^2+bp.^2)).^2,1)./vsum(upbar.^2,1);
reporttest('ELLBAND terms sum to bandwidth from INSTMOM', err<1e-5);

function[]=ellband_test2
t=(0:1:925)';
cxe=zeros(length(t),3);

kappa=3*exp(2*0.393*(t/1000-1));
lambda=0.4+0*t;
phi=(t/1000*5)*2*pi;
theta=pi/4+0*t;

om=vdiff(phi,1);  %Since theta is constant

[x,y]=ellsig(kappa,lambda,theta,phi);
[a,om,up]=instmom([x y],1,2);

ups=sqrt(vsum(abs(up).^2,2));
[upsa,upsb,upsc]=ellband(kappa,lambda,theta,phi);
b1=aresame(vmean([ups upsa upsb upsc]./[om om om om],1),[1 1 0 0]*0.025,1e-4);

om=vdiff(phi,1);  %Since theta is constant

kappa=2.5+0*t;
lambda=zeros(size(t));
for i=2:length(t)
    lambda(i)=real(lambda(i-1)+2*sqrt(1-lambda(i-1).^2)*0.025.*om(i));
end
lambda(1)=nan;
lambda(lambda>1)=1;

[x,y]=ellsig(kappa,lambda,theta,phi);
[a,om,ups]=instmom([x y],1,2);

[upsa,upsb,upsc]=ellband(kappa,lambda,theta,phi);
b2=aresame(vmean([ups upsa upsb upsc]./[om om om om],1),[1 0 1 0]*0.025,1e-4);

[kappa,lambda]=ab2kl(3+zeros(size(t)),2+zeros(size(t)));

theta=phi/14.45;

[x,y]=ellsig(kappa,lambda,theta,phi);
[a,om,ups]=instmom([x y],1,2);

[upsa,upsb,upsc]=ellband(kappa,lambda,theta,phi);
b3=aresame(vmean([ups upsa upsb upsc]./[om om om om],1),[1 0 0 1]*0.025,1e-4);

reporttest('ELLBAND three panels of figure each have bandwidth 0.025',b1&&b2&&b3)

function[]=ellband_test3

kappao=10;
lambdao=2/3;
omtheta=linspace(-2,2,100);
omphi=1;

dt=0.1;
t=[0:dt:100]';

zo=ellsig(kappao,lambdao,0,omphi.*t);
psi=sleptap(length(zo));

z=vzeros(length(zo),length(omtheta));
for i=1:length(omtheta)
    z(:,i)=rot(omtheta(i).*t).*zo;
end

%Taper for better computation
z=z.*vrep(psi(:,1),size(z,2),2);
[zp,zn]=anatrans(z,conj(z));

[ap,omp,upp]=instmom(dt,zp);
[an,omn,upn]=instmom(dt,zn);

kappa=sqrt(abs(zp).^2+abs(zn).^2);
om=frac(omp.*abs(zp).^2+omn.*abs(zn).^2,kappa.^2);
ombar=vmean(om,1,squared(kappa));

sig=sqrt(frac((upp.^2+(omp-vrep(ombar,size(om,1),1)).^2).*abs(zp).^2+...
    (upn.^2+(omn-vrep(ombar,size(om,1),1)).^2).*abs(zn).^2,kappa.^2));
sigbar=sqrt(vmean(sig.^2,1,squared(kappa)));

[x,y]=vectmult(sqrt(2)*tmat',zp,zn);
[kappa,lambda,theta,phi]=ellparams(x,y);
[ba,bd,bp]=ellband(dt,kappa,lambda,theta,phi);
bom=om-vrep(ombar,size(om,1),1);

sig2=sqrt(ba.^2+bd.^2+bp.^2+bom.^2);
sigbar2=sqrt(vmean(sig2.^2,1,squared(kappa)));

%plot(sig,'b'),hold on,plot(sig2,'r')  %Visually identical
err=sum(squared(sigbar-sigbar2))./sum(squared(sigbar));
reporttest('ELLBAND rotary and elliptical bandwidths match for shifted ellipse, non-unit sample rate',err<1e-5)

function[]=ellband_test4

load solomon
use solomon

%Choose central portion where polarization is elliptical
vindex(x,y,z,420:580,1);
vfilt(x,y,z,10,'zeros');
[x,y,z]=anatrans(x,y,z,'mirror');

[kappa,lambda,theta,phi,alpha,beta]=ellparams(x,y,z);
[a,ombar,upbar]=instmom([x,y,z],1,2);
[a,b,c,d,e]=ellband(kappa,lambda,theta,phi,alpha,beta);
upbar1=sqrt(a.^2+b.^2+d.^2+abs(e).^2);

err=abs(upbar-upbar1).^2./abs(upbar).^2;
err=sort(err,'descend');
err=err(20:end);
%figure, plot([upbar upbar1])
reporttest('ELLBAND trivariate case, sum of bandwith terms for Solomon Islands (removing worst outliers)',allall(err<0.05))


function [ seed, xd, td ] = triangulation_order3_sample ( node_num, node_xy, ...
  triangle_num, triangle_node, num_ran, seed )

%*****************************************************************************80
%
%% TRIANGULATION_ORDER3_SAMPLE returns random points in a triangulation.
%
%  Discussion:
%
%    It is assumed that the triangulation consists of a set of non-overlapping
%    triangles.
%
%    The point is chosen uniformly in the area covered by the triangulation.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 December 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NODE_NUM, the number of nodes.
%
%    Input, real NODE_XY(2,NODE_NUM), the node coordinates.
%
%    Input, integer TRIANGLE_NUM, the number of triangles.
%
%    Input, integer TRIANGLE_NODE(3,TRIANGLE_NUM), the nodes that make up the triangles.
%
%    Input, integer NUM_RAN, the number of points to sample.
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, integer SEED, a seed for the random number generator.
%
%    Output, real XD(2,NUM_RAN), the sample points.
%
%    Output, integer TD(NUM_RAN), the triangle to which each sample point
%    belongs.
%
  dim_num = 2;
%
%  Compute the areas of the triangles.
%  Build a cumulative area vector.
%  Convert it to a relative cumulative area vector.
%
  area_cum(0+1) = 0.0;

  for i = 1 : triangle_num

    i1 = triangle_node(1,i);
    i2 = triangle_node(2,i);
    i3 = triangle_node(3,i);

    t(1:dim_num,1) = node_xy(1:dim_num,i1);
    t(1:dim_num,2) = node_xy(1:dim_num,i2);
    t(1:dim_num,3) = node_xy(1:dim_num,i3);

    area = triangle_area_2d ( t );

    area_cum(i+1) = area_cum(i-1+1) + area;

  end

  area_total = area_cum(triangle_num+1);

  area_cum(0+1:triangle_num+1) = area_cum(0+1:triangle_num+1) / area_total;
%
%  Pick random values.  A random value R indicates the corresponding triangle
%  whose cumulative relative area contains R.
%
%  Bracket the random value in the cumulative relative areas,
%  indicating a triangle.
%
%  Pick a random point in the triangle.
%
  for i = 1 : num_ran

    [ r, seed ] = r8_uniform_01 ( seed );

    [ left, right ] = r8vec_bracket ( triangle_num+1, area_cum, r );

    td(i) = right - 1;

    i1 = triangle_node(1,td(i));
    i2 = triangle_node(2,td(i));
    i3 = triangle_node(3,td(i));

    t(1:dim_num,1) = node_xy(1:dim_num,i1);
    t(1:dim_num,2) = node_xy(1:dim_num,i2);
    t(1:dim_num,3) = node_xy(1:dim_num,i3);

    [ xd(1:dim_num,i), seed ] = triangle_sample ( t, 1, seed );

  end

  return
end


function val=intPowSinPow(u,n,m)
%%INTPOWSINPOW Evaluate the integral of u^n*sin(u)^m du.  A definite
%           integral can be evaluated, or an indefinite integral (with a
%           particular additive constant).
%
%INPUTS: u A 2XN (for definite integral) or a 1XN (for indefinite
%          integrals) set of N points. For definite integrals, u(1,:) are
%          the real lower bounds and u(2,:) are the real upper bounds. For
%          indefinite integrals, the integral is evaluated at the points in
%          u. The values in u should be real.
%        n The positive integer exponent of u.
%        m The positive integer exponent of the sine term.
%
%OUTPUTS: val The 1XN set of values of the integral of u^n*sin(u)^m.
%
%This function implements formulas 4 and 5 of Section 2.631 of [1].
%
%REFERENCES:
%[1] I. S. Gradshteyn and I. M. Ryzhik, Tables of Integrals, Series, and
%    Products, Corrected and Enlarged Edition. New York: Academic Press,
%    1980, translated from Russian.
%
%October 2016 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

numDim=size(u,1);

if(isempty(u))
   val=[];
   return;
end

if(numDim==1)%An indefinite integral
    val=indefIntPowSinPow(u,n,m);
else%A definite integral
    val=indefIntPowSinPow(u(2,:),n,m)-indefIntPowSinPow(u(1,:),n,m);
end

end

function val=indefIntPowSinPow(u,n,m)

if(mod(m,2)==0)%Use formula 4.
    m=m/2;

    val=0;
    for k=0:(m-1)
        a=2*(m-k);
        x=a*u;
        val=val+(-1)^k*binomial(2*m,k)*(1/a)^(n+1)*intPowCos(x,n);
    end

    val=(-1)^m/(2^(2*m-1))*val;
    val=val+binomial(2*m,m)*u.^(n+1)/(2^(2*m)*(n+1));
else%Use formula 5.
    m=(m-1)/2;

    val=0;
    for k=0:m
        a=2*(m-k)+1;
        x=a*u;
        val=val+(-1)^k*binomial(2*m+1,k)*(1/a)^(n+1)*intPowSin(x,n);
    end

    val=val*(-1)^m/(2^(2*m));
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.


% book : Signals and Systems Laboratory with MATLAB
% authors : Alex Palamides & Anastasia Veloni
%
%
% problem 6 - Computation of linear convolution

% a)
x1=[1 2 3 4];
x2=[ 5 4 3 2 1];
y=conv(x1,x2)

% b)
N1=length(x1);
N2=length(x2);
M=N1+N2-1;
x1(M)=0;
x2(M)=0;
y=circonv(x1,x2)

% c)
X1=fft(x1,M);
X2=fft(x2,M);
y=ifft(X1.*X2)


function R = rand_rotation(n)
  % Uniformly sample SO(n)
  %
  % R = rand_rotation(n)
  %
  % Inputs:
  %   n  dimension
  % Output:
  %   R  n by n rotation matrix
  %

  % "How to generate a random unitary matrix" [Maris Ozols 2006]
  % http://home.lu.lv/~sd20008/papers/essays/Random%20unitary%20[paper].pdf
  [Q,~] = qr(randn(n));
  s = (2*(rand>0.5)-1);
  Q(:,1) = Q(:,1)*s;
  %b = det(Q);
  b = s*(2*mod(n,2)-1);
  Q(:,2) = b*Q(:,2);
  assert(abs(det(Q)-1)<1e-10)
  % rename as R
  R = Q;
end


function angles = tetrahedron_face_angles_3d ( tetra )

%*****************************************************************************80
%
%% TETRAHEDRON_FACE_ANGLES_3D returns the 12 face angles of a tetrahedron 3D.
%
%  Discussion:
%
%    The tetrahedron has 4 triangular faces.  This routine computes the
%    3 planar angles associated with each face.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 July 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real TETRA(3,4) the tetrahedron vertices.
%
%    Output, real ANGLES(3,4), the face angles.
%

%
%  Face 123
%
  tri(1:3,1:3) = tetra(1:3,1:3);
  angles(1:3,1) = triangle_angles_3d ( tri );
%
%  Face 124
%
  tri(1:3,1:2) = tetra(1:3,1:2);
  tri(1:3,3) = tetra(1:3,4);
  angles(1:3,2) = triangle_angles_3d ( tri );
%
%  Face 134
%
  tri(1:3,1) = tetra(1:3,1);
  tri(1:3,2:3) = tetra(1:3,3:4);
  angles(1:3,3) = triangle_angles_3d ( tri );
%
%  Face 234
%
  tri(1:3,1:3) = tetra(1:3,2:4);
  angles(1:3,4) = triangle_angles_3d ( tri );

  return
end


% Author: Jai Juneja (adapted from video lecture series by Joan Sola)
% Date: 12/02/2013
%
% Given a range-bearing measurement y, obtain position p of observed
% landmark in the robot's frame.
%
% Inputs:
%   y = [d a]       :   Laser range and bearing measurement
%
% Outputs:
%   p_r = [p_x p_y]':   Cartesian position of observation in robot's frame
%   Optional:
%   P_y             :   Jacobian of p wrt. y

function [p_r, P_y] = getInvMeasurement(y)
    d = y(1, :);           % Range measurement
    a = y(2, :);           % Bearing measurement

    p_x = d .* cos(a);   % x-position of observation in robot's frame
    p_y = d .* sin(a);   % y-position of observation in robot's frame

    p_r = [p_x; p_y];

    if nargout > 1      % Compute Jacobian (only works for single measurement)
        P_y = [...
            cos(a)  -d*sin(a)
            sin(a)  d*cos(a)];
    end
end


%% Example 2.5: Viscous Conservation Law
% In this example, we consider the viscous conservation law
%
% $$ u_t + f(u)_x = \mu u_{xx}, \qquad u(x,0)=u_0(x).$$
%
% A common method to derive efficient methods for this equation is to split
% the evolution into two different operators
%
% $$ S(t): \quad v_t + f(v)_x = 0, \qquad v(x,0)=v_0(x)$$
%
% $$ H(t): \quad w_t = \mu w_{xx}, \qquad w(y,0)=w_0(y)$$
%
% The approximate solution is constructed from the formula
%
% $$u(x,t)\approx [H(\Delta t)\circ S(\Delta t) ]^n u_0(x)$$
%
% This way, one can utilize highly efficient solvers for each of the two
% subequations. Here, we use a spectral difference method for the heat
% equation. Moreover, because the purpose here is simply to demonstrate the
% operator splitting, we use the simple first-order Lax-Friedrichs scheme
% for the hyperbolic conservation law ($r=\Delta t/\Delta x$)
%
% $$u_i^{n+1} = \frac12\bigl(u_{i-1}^n + u_{i+1}^n\bigr)
%     - \frac12 r \bigl[ f(u_{i+1}^n) - f(u_{i-1}^n)\bigr]$$

%% Initial setup
N  = 512; h = 2*pi/N;
T  = 6;
x  = -pi+(0:N)*h; x = 0.5*(x(1:end-1)+x(2:end));
u0 = exp(-4*sin((x+2)/2).^2);

%% Burgers' equation
% A classical example of a viscous conservation law is the so-called
% Burgers' equation
%
% $$ u_t + ( 0.5 u^2)_x = \mu u_{xx}
%
% which can be considered a simplified model of the momentum equation from
% the Navier-Stokes equations. Here, we will consider the evolution of a
% smooth profile for \mu=0.01 using periodic boundary conditions.
nsplit = 50;
u = consheat('flux', u0, x, T, nsplit, 0.01);
subplot(2,1,1), plot(x,u(:,1:10:nsplit+1))
subplot(2,1,2), surf(x,linspace(0,T,nsplit+1),u')
shading interp; view(-10,50), axis tight

%%
% As we see from the figure, the nonlinear convective forces sharpen the
% smooth profile into a shock layer whose width is determined by the
% balance of the diffusion term and the self-sharpening mechanisms in the
% nonlinear flux function.

%% Compare different time steps
% Next, we increase \mu to 0.1 and consider the effect of different number
% of splitting steps.
for n=1:4,
	nsplit = 4*3.^(n-1);
	u = consheat('flux', u0, x, T, nsplit);
   subplot(2,2,n); contourf(x, linspace(0,T,nsplit+1), u');
   xlabel('x'), ylabel('t'), colorbar, title([num2str(nsplit) ' steps']);
end;
%%
% The approximations computed with four and twelve splitting steps clearly
% capture the dominant evolutionary behavior of the solution, but also
% contain nonphysical wiggles caused by splitting errors. As we increase
% the number of splitting steps, the splitting errors decrease and the
% approximate solutions are able to capture the whole evolution without
% significant artifacts created by our splitting strategy.

%% Comparing different shock widths
% As a last example, we will look at the evolution of the smooth profile
% for different balances between the convective and diffusive forces.
mu = 4; nsplit = 50;
t  = linspace(0,T,nsplit+1);
for n=1:4,
   mu = 0.25*mu;
	u  = consheat('flux',u0,x,T,nsplit,mu);
	subplot(2,2,n); contourf(x,t,u');
   xlabel('x'), ylabel('t'), colorbar, title(['mu = ', num2str(mu)]);
end;
%%
% For large values of \mu, the diffusive forces dominate and the solution
% profile evolves toward a flat surface. As \mu decreases, the convective
% forces take over and the solution develops into a viscous shock that
% travels to the right.


function idx = findClosestCentroids(X, centroids)
%FINDCLOSESTCENTROIDS computes the centroid memberships for every example
%   idx = FINDCLOSESTCENTROIDS (X, centroids) returns the closest centroids
%   in idx for a dataset X where each row is a single example. idx = m x 1
%   vector of centroid assignments (i.e. each entry in range [1..K])
%

% Set K
K = size(centroids, 1);

% You need to return the following variables correctly.
idx = zeros(size(X,1), 1);

% ====================== YOUR CODE HERE ======================
% Instructions: Go over every example, find its closest centroid, and store
%               the index inside idx at the appropriate location.
%               Concretely, idx(i) should contain the index of the centroid
%               closest to example i. Hence, it should be a value in the
%               range 1..K
%
% Note: You can use a for-loop over the examples to compute this.
%

m = size(X,1)

for i = 1:m
    distance_array = zeros(1,K);
    for j = 1:K
        distance_array(1,j) = sqrt(sum(power((X(i,:)-centroids(j,:)),2)));
    end
    [d, d_idx] = min(distance_array);
    idx(i,1) = d_idx;
end

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

end


function [R,W] = rodrigues(N,phi)
  % RODRIGUES Construct the rotation matrix for rotating vectors around each
  % given vector.
  %
  % Inputs:
  %   N  #N by 3 list of 3D vectors
  %   phi  angle to rotate by.
  % Outputs:
  %   R  3#N by 3#N "rotation" matrix
  %   W  3#N by 3#N cross product matrix
  %
  % Example:
  %   % Given a scalar function Z on a mesh (V,F)
  %   G = grad(V,F);
  %   N = normalizerow(normals(V,F));
  %   X = reshape(G*Z,[],3);
  %   Y = reshape(rodrigues(N,pi/2)*G*Z,[],3);
  %   BC = barycenter(V,F);
  %   tsurf(F,V,'CData',Z,fphong,'EdgeColor','none');
  %   hold on;
  %   quiver3(BC(:,1),BC(:,2),BC(:,3),X(:,1),X(:,2),X(:,3))
  %   quiver3(BC(:,1),BC(:,2),BC(:,3),Y(:,1),Y(:,2),Y(:,3))
  %   hold off;
  %

  m = size(N,1);
  % Either uniform angle or per-vector
  if numel(phi)>1
    assert(numel(phi) == m);
    phi = diag(sparse(phi));
  end
  % https://math.stackexchange.com/a/142831
  %
  W = sparse( ...
    [1 2 0 2 0 1]*m+(1:m)', ...
    [0 0 1 1 2 2]*m+(1:m)', ...
    [N(:,3) -N(:,2) -N(:,3) N(:,1) N(:,2) -N(:,1)], ...
    3*m,3*m);
  if isempty(phi)
    R = [];
  else
    R = speye(3*m,3*m) + sin(phi)*W + (2*sin(phi/2)^2)*W*W;
  end
end


function result = ball_unit_07_3d ( func )

%*****************************************************************************80
%
%% BALL_UNIT_07_3D approximates an integral inside the unit ball in 3D.
%
%  Integration region:
%
%    Points (X,Y,Z) such that:
%
%      X**2 + Y**2 + Z**2 <= 1.
%
%  Discussion:
%
%    A 64 point 7-th degree formula is used.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 May 2004
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Arthur H Stroud,
%    Approximate Calculation of Multiple Integrals,
%    Prentice Hall, 1971.
%
%  Parameters:
%
%    Input, external FUNC, the name of the user supplied
%    function which evaluates F(X,Y,Z), of the form
%      function value = func ( x, y, z )
%
%    Output, real RESULT, the approximate integral of the function.
%
  norder = 4;
%
%  This is the 5 point Gauss-Legendre rule,
%  but with the midpoint deleted, and with different weights.
%
  xtab1(1:4) = [...
    -0.906179845938663992797626878299E+00, ...
    -0.538469310105683091036314420700E+00, ...
     0.538469310105683091036314420700E+00, ...
     0.906179845938663992797626878299E+00 ];

  weight1(1:4) = [ ...
    0.19455533421780251826E+00, ...
    0.13877799911553081506E+00, ...
    0.13877799911553081506E+00, ...
    0.19455533421780251826E+00 ];
%
%  Set XTAB2 and WEIGHT2.
%
  for j = 1 : norder
    angle = pi * ( 2 * j - 1 ) / ( 2 * norder );
    xtab2(j) = cos ( angle );
  end

  weight2(1:norder) = 1.0E+00;
%
%  Set XTAB3 and WEIGHT3 for the interval [-1,1].
%
  [ xtab3, weight3 ] = legendre_set ( norder );

  w = 3.0E+00 / 16.0E+00;

  quad = 0.0E+00;

  for i = 1 : norder
    for j = 1 : norder
      for k = 1 : norder

        x = xtab1(i) * sqrt ( 1.0E+00 - xtab2(j)^2 ) ...
                     * sqrt ( 1.0E+00 - xtab3(k)^2 );
        y = xtab1(i) * xtab2(j) * sqrt ( 1.0E+00 - xtab3(k)^2 );
        z = xtab1(i) * xtab3(k);

        quad = quad + w * weight1(i) * weight2(j) * weight3(k) ...
          * feval ( func, x, y, z );

      end
    end
  end

  volume = ball_unit_volume_3d ( );
  result = quad * volume;

  return
end


function [theta, J_history] = gradientDescent(X, y, theta, alpha, num_iters)
    %% GRADIENTDESCENT Performs gradient descent to learn theta
    %   theta = GRADIENTDESENT(X, y, theta, alpha, num_iters) updates theta by
    %   taking num_iters gradient steps with learning rate alpha

    % Initialize some useful values
    n = length(y); % number of training examples
    J_history = zeros(num_iters, 1);

    for iter = 1 : num_iters

        % ====================== YOUR CODE HERE ======================
        % Instructions: Perform a single gradient step on the parameter vector
        %               theta.
        %
        % Hint: While debugging, it can be useful to print out the values
        %       of the cost function (computeCost) and gradient here.
        %

        % compute the gradient
        grad = zeros(size(theta));
        error = X * theta - y;
        for j = 1 : length(grad)
            grad(j) = alpha * sum(error .* X(:, j)) / n;
        end

        % simultaneously update all theta entries
        theta = theta - grad;

        % Save the cost J in every iteration
        J_history(iter) = computeCost(X, y, theta);
    end
end


function value = r4_acosh ( x )

%*****************************************************************************80
%
%% R4_ACOSH evaluates the arc-hyperbolic cosine of an R4 argument.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 September 2011
%
%  Author:
%
%    Original FORTRAN77 version by Wayne Fullerton.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Wayne Fullerton,
%    Portable Special Function Routines,
%    in Portability of Numerical Software,
%    edited by Wayne Cowell,
%    Lecture Notes in Computer Science, Volume 57,
%    Springer 1977,
%    ISBN: 978-3-540-08446-4,
%    LC: QA297.W65.
%
%  Parameters:
%
%    Input, real X, the argument.
%
%    Output, real VALUE, the arc-hyperbolic cosine of X.
%
  persistent dln2;
  persistent xmax;

  if ( isempty ( xmax ) )
    dln2 = 0.69314718055994530941723212145818;
    xmax = 1.0 / sqrt ( r4_tiny ( ) );
  end

  if ( x < 1.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R4_ACOSH - Fatal error!\n' );
    fprintf ( 1, '  X < 1.0\n' );
    error ( 'R4_ACOSH - Fatal error!' )
  elseif ( x < xmax )
    value = log ( x + sqrt ( x * x - 1.0 ) );
  else
    value = dln2 + log ( x );
  end

  return
end


function value = stirling2_value ( n, m )

%*****************************************************************************80
%
%% STIRLING2_VALUE computes a Stirling number of the second kind.
%
%  Discussion:
%
%    S2(N,M) represents the number of distinct partitions of N elements
%    into M nonempty sets.  For a fixed N, the sum of the Stirling
%    numbers S2(N,M) is represented by B(N), called "Bell's number",
%    and represents the number of distinct partitions of N elements.
%
%    For example, with 4 objects, there are:
%
%    1 partition into 1 set:
%
%      (A,B,C,D)
%
%    7 partitions into 2 sets:
%
%      (A,B,C) (D)
%      (A,B,D) (C)
%      (A,C,D) (B)
%      (A) (B,C,D)
%      (A,B) (C,D)
%      (A,C) (B,D)
%      (A,D) (B,C)
%
%    6 partitions into 3 sets:
%
%      (A,B) (C) (D)
%      (A) (B,C) (D)
%      (A) (B) (C,D)
%      (A,C) (B) (D)
%      (A,D) (B) (C)
%      (A) (B,D) (C)
%
%    1 partition into 4 sets:
%
%      (A) (B) (C) (D)
%
%    So S2(4,1) = 1, S2(4,2) = 7, S2(4,3) = 6, S2(4,4) = 1, and B(4) = 15.
%
%
%  First terms:
%
%    N/M: 1    2    3    4    5    6    7    8
%
%    1    1    0    0    0    0    0    0    0
%    2    1    1    0    0    0    0    0    0
%    3    1    3    1    0    0    0    0    0
%    4    1    7    6    1    0    0    0    0
%    5    1   15   25   10    1    0    0    0
%    6    1   31   90   65   15    1    0    0
%    7    1   63  301  350  140   21    1    0
%    8    1  127  966 1701 1050  266   28    1
%
%  Recursion:
%
%    S2(N,1) = 1 for all N.
%    S2(I,I) = 1 for all I.
%    S2(I,J) = 0 if I < J.
%
%    S2(N,M) = M * S2(N-1,M) + S2(N-1,M-1)
%
%  Properties:
%
%    sum ( 1 <= K <= M ) S2(I,K) * S1(K,J) = Delta(I,J)
%
%    X**N = sum ( 0 <= K <= N ) S2(N,K) X_K
%    where X_K is the falling factorial function.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 August 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of rows of the table.
%
%    Input, integer M, the number of columns of the table.
%
%    Output, integer VALUE, the value of S2(N,M).
%
  if ( n <= 0 )
    value = 0;
    return
  end

  if ( m <= 0 )
    value = 0;
    return
  end

  s2(1,1) = 1;
  s2(1,2:m) = 0;

  for i = 2 : n

    s2(i,1) = 1;

    for j = 2 : m
      s2(i,j) = j * s2(i-1,j) + s2(i-1,j-1);
    end

  end

  value = s2(n,m);

  return
end


function trans = createScaling3d(varargin)
%CREATESCALING3D Create the 4x4 matrix of a 3D scaling
%
%   TRANS = createScaling3d(S);
%   returns the scaling transform corresponding to a scaling factor S in
%   each direction. S can be a scalar, or a 1x3 vector containing the
%   scaling factor in each direction.
%
%   TRANS = createScaling3d(SX, SY, SZ);
%   returns the scaling transform corresponding to a different scaling
%   factor in each direction.
%
%   The returned matrix has the form :
%   [SX  0  0  0]
%   [ 0 SY  0  0]
%   [ 0  0 SZ  0]
%   [ 0  0  0  0]
%
%   See also:
%   transforms3d, transformPoint3d, transformVector3d, createTranslation3d,
%   createRotationOx, createRotationOy, createRotationOz
%
%   ---------
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 20/04/2006.
%

%   HISTORY
%   25/11/2008 rename from scale3d to scaling3d
%   30/04/2009 rename to createScaling3d

% process input parameters
if isempty(varargin)
    % assert uniform scaling in each direction
    sx = 1;
    sy = 1;
    sz = 1;
elseif length(varargin)==1
    % only one argument
    var = varargin{1};
    if length(var)==1
        % same scaling factor in each direction
        sx = var;
        sy = var;
        sz = var;
    elseif length(var)==3
        % scaling is a vector, giving different scaling in each direction
        sx = var(1);
        sy = var(2);
        sz = var(3);
    else
        error('wrong size for first parameter of "createScaling3d"');
    end
elseif length(varargin)==3
    % 3 arguments, giving scaling in each direction
    sx = varargin{1};
    sy = varargin{2};
    sz = varargin{3};
else
    error('wrong number of arguments for "createScaling3d"');
end

% create the scaling matrix
trans = [sx 0 0 0;0 sy 0 0;0 0 sz 0;0 0 0 1];


function dy_dx = parabolderiv( x, y, k, graphics_flag )

% function dy_dx = parabolderiv( x, y, k, graphics_flag )
%
% default: example from Ref. [1], p. 322
%
% or try: parabolderiv( ( -2 : 0.01 : 2 )', exp( -2 : 0.01 : 2 )', 2 )
%         parabolderiv( ( -2 : 0.01 : 2 )', ( exp( -2 : 0.01 : 2 ) + 0.01 * rand( 1, 401 ) )', 25 )
%         parabolderiv( ( ( 0 : 200 * pi ) / 100 )', ( sin( 0 : 0.01 : 2 * pi ) + 0.01 * rand( 1, 629 ) )', 40 )
%           for demonstration of noisy input and edge deviations
%
% graphics_flag = 1: show result in graph (default)
%                    (includes comparison to 2 other methods)
%                 0: show no graph
%
% This program differentiates an empirical function (an array of evenly
% spaced values), for instance periodically sampled data from an
% experiment.
%
% Method used: basically, a parabola is fit from k points to the left to k
% points to the right of the point where the derivative is required, this
% is then analytically differentiated. These calculations are not actually
% performed, but the method as given by Lanczos in Ref. [1] is used: only
% 1 fit parameter of the parabola is needed, which is calculated directly
% from the data; at the edges a parabola is fit through the first (last)
% 2k points, from which the derivative is calculated directly. Additional
% information (on accuracy, &c.) in Ref. [2]. Noise is handled well. An
% example from Ref. [1] is included.
%
% The datapoints need to be equidistant. If the graphics_flag is set, the
% result of this method is compared to applying the standard Matlab diff()
% function on the raw data as well as on adjacent averaged filtered data
% (zero phase delay). The example commands given above illustrate this.
%
% References:
%
% [1] Title                    : Applied analysis / by Cornelius Lanczos
%     Author                   : Cornelius Lanczos
%     Edition                  : 3rd print.
%     Publisher                : London : Pitman, 1964
%     Pages                    : 539 p.
%     Bibliographic annotation : 1st print: 1957
%     see pp. 321 - 324
%
% [2] Title                    : Digital filters / by Richard W. Hamming
%     Author                   : Richard W. Hamming
%     Edition                  : 3rd ed.
%     Publisher                : Englewoood Cliffs : Prentice-Hall, 1989
%     Pages                    : XIV, 284 p.
%     Bibliographic annotation : 1st print: 1977
%     ISBN                     : 0-13-212812-8
%     see p. 137
%
% Last update 24-03-2004 by Robert Klein-Douwel
%
% mail: robertkdkd@yahoo.co.uk, R.J.H.Klein-Douwel@tue.nl
% web:  http://www.sci.kun.nl/mlf/robertkd/

linewidth = 1; % can be changed for plotting purposes

% fill in some default values

if nargin < 4
    graphics_flag = 1;
end

if nargin < 3 % take example from Ref. [1], p. 322
    y = [ 0
          4
         25
         50
         67.4
        124.9
        172.0
        201.4
        288.1
        321.3
        387.1 ];

    x = ( 0 : 1 : length( y ) - 1 )';

    k = 2;
end

% initialise stuff

if length( x ) ~= length( y )
    error( 'x and y vectors have different lengths (RKD)' );
end

n = length( y ); %number of points

if 2 * k + 1 > n
    error( 'k too large or too few datapoints (RKD)' );
end

dy = zeros( n, 1 );

% check equidistancy of x
dx = diff( x );
d2x = diff( dx );

rel_error_d2x = max( abs( d2x ) ) / min( dx );
if rel_error_d2x > 401 * eps
    % vectors like ( -2 : 0.01 : 2 ) are not equidistant, but have a small variation
    % in dx and d2x; if this is not more than 401 * eps, then it is neglected
    disp( [ 'relative error in d2x = ' num2str( rel_error_d2x ) ] );
    disp( [ 'relative error / eps = ' num2str( rel_error_d2x / eps ) ] );
    disp( [ 'min( d2x ) = ' num2str( min( d2x ) ) ] );
    disp( [ 'max( d2x ) = ' num2str( max( d2x ) ) ] );
    error( 'non-equidistant data points (RKD)' );
end
h = dx( 1 ); % stepsize

% start calculations
dy_denominator = 2 * h * sum( ( 1 : k ).^2 );

a = - k : + k;
for i = k + 1 : n - k
    dy( i ) = sum( a .* y( i + a )' );
end

dy = dy / dy_denominator;

% first and last k points:
% fit parabola over first and last 2k points
kk = 2 * k;

% first kk points
p1 = polyfit( x( 1 : kk ), y( 1 : kk ), 2 );
q1 = polyder( p1 ); % take derivative
dy( 1 : k ) = polyval( q1, x( 1 : k ) );

% last kk points
p2 = polyfit( x( n - kk + 1 : n ), y( n - kk + 1 : n ), 2 );
q2 = polyder( p2 ); % take derivative
dy( n - k + 1 : n ) = polyval( q2, x( n - k + 1 : n ) );

if nargout ~= 0 % output
    dy_dx = dy;
end

% end of calculations (everything below is just for making comparisons and nice output)

if graphics_flag == 1 % make some comparisons
    xdiff_range = x( 1 : n - 1 ) + h / 2; % x coordinates for results of diff() function

    % compare with diff()
    dydx_matlab = diff( y ) / h;

    if nargin >=3 % not enough data points in example to apply this filter

        % another test: try first adjacent averaging and then diff()
        % use k points to left and to right
        kkk = 2 * k + 1;
        b = ones( 1, kkk ) / kkk; % k point averaging filter
        y_filt = filtfilt( b, 1, y ); % noncausal filtering, zero phase distortion

        % calculate dy_filt/dx
        dy_filtdx = diff( y_filt ) / h;
    end

    if nargin < 3 % show results of example
        disp( '      x         y         dy' );
        disp( [ x, y, dy ] );
    end
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% show results
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

if graphics_flag == 1
    if nargout == 0
        close all;
    end

    dx_idx_range = k + 1 : n - k;

    % parabolic fits to first (last) kk points
    x1_fit = min( x( 1 : kk ) ) : h / 100 : max( x( 1 : kk ) );
    y1_fit = polyval( p1, x1_fit );

    x2_fit = min( x( n - kk + 1 : n ) ) : h / 100 : max( x( n - kk + 1 : n ) );
    y2_fit = polyval( p2, x2_fit );

    figure;
    hold on;
    plot( x, y, '-ob', 'LineWidth', linewidth );
    plot( x( dx_idx_range ), dy( dx_idx_range ), '-or', 'LineWidth', linewidth );
    plot( x( 1 : k ), dy( 1 : k ), '-sr', 'LineWidth', linewidth );
    plot( x1_fit, y1_fit, '--r', 'LineWidth', linewidth );
    plot( xdiff_range, dydx_matlab, ':sg', 'LineWidth', linewidth );

    if nargin >= 3
        plot( xdiff_range, dy_filtdx, ':dm', 'LineWidth', linewidth );
    end

    legend( 'y', [ 'dy/dx_{Lanczos: {\pm}' num2str( k ) '}' ], ...
        [ 'first/last ' num2str( k ) ' pts' ], ...
        [ 'y_{fit, parabola, ' num2str( kk ) ' pts}' ], 'diff( y )/ dx', ...
        [ 'diff( y_{adj avg ({\pm}' num2str( k ) ')} )/ dx' ], 0 );

    plot( x( n - k + 1 : n ), dy( n - k + 1 : n ), '-sr', 'LineWidth', linewidth );
    plot( x2_fit, y2_fit, '--r', 'LineWidth', linewidth );

    plot( x, dy, '-r', 'LineWidth', linewidth );

    % replot to put these on top again
    plot( x( dx_idx_range ), dy( dx_idx_range ), '-or', 'LineWidth', linewidth );
    plot( x( 1 : k ), dy( 1 : k ), '-sr', 'LineWidth', linewidth );
    hold off;

    xlabel( 'x' );
    ylabel( 'y' );

    title( 'parabolic derivative (Lanczos)' );
end