function [J, grad] = lrCostFunction(theta, X, y, lambda)
    %% LRCOSTFUNCTION Compute cost and gradient for logistic regression with
    %regularization
    %   J = LRCOSTFUNCTION(theta, X, y, lambda) computes the cost of using
    %   theta as the parameter for regularized logistic regression and the
    %   gradient of the cost w.r.t. to the parameters.

    n = length(y); % number of training examples

    % cost: J, this time with the penalty for the magnitude of theta
    J = -1/n * sum(...
        y      .* log(sigmoid(X * theta)) + ...
        (1 - y) .* log(1 - sigmoid(X * theta)) ...
    ) + lambda / (2 * n) * sum(theta(2:end) .* theta(2:end));

    % gradient: compute as the derivative of the cost function
    grad = 1/n * X' * ((sigmoid(X * theta)) - y) + theta * lambda / n;

    % we do not regularize the constant offset term,
    % undo the gradient penalization for the constant x_0 term
    grad(1) = grad(1) - lambda / n * theta(1);
end


function e = imEntropy(img, varargin)
%IMENTROPY Compute entropy of an image
%
%   H = imEntropy(IMG)
%   Computes the entropy of the image IMG.
%   The entropy is computed as:
%     H  = -sum(P .* log2(P));
%   where P is the density probability that image takes a given value.
%
%   Note that the computation of the entropy depends on the way the
%   histogram is computed. This should not be a concern for uint8 images,
%   but can give results inconsistent with imMutualInformation for double
%   images.
%
%   H = imEntropy(IMG, N)
%   H = imEntropy(IMG, BINS)
%   Computes the histogram using N bins, or the bins specified by BINS.
%
%
%   Example
%     % compute entropy on a sample image
%     img = imread('rice.png');
%     H = imEntropy(img)
%     H =
%         7.0115
%
%     % entropy is independent of pixel ordering
%     img2 = circshift(img, [30 40]);
%     H = imEntropy(img2)
%     H =
%         7.0115
%
%     % entropy computed on an histogram with fewer bins
%     imEntropy(img, 16)
%     ans =
%         3.1158
%
%   See also
%     imJointEntropy, imMutualInformation, imHistogram
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2010-08-26,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2010 INRA - Cepia Software Platform.

% first compute histogram
h = imHistogram(img, varargin{:});

% keep only positive values, and normalize by 1 to have density
h(h==0) = [];
h = h / sum(h);

% compute entropy
e = -sum(h .* log2(h));


function [d, xd, bw] = kernelDensity(x, bins, h, kernel)
%KERNELDENSITY Calculate the kernel density of a data set
%
%   [D, XD] = IOSR.STATISTICS.KERNELDENSITY(X) calculate the kernel density
%   D of a dataset X for query points XD. The kernel density is calculated
%   for 100 query points equally spaced between the minimum and maximum of
%   the data X. The density is estimated using a gaussian kernel with a
%   width that is optimal for normal data. X may be a vector, matrix, or
%   multi-dimensional array; the entire array is treated as the sample. D
%   and XD are 100-point column vectors. NaN are excluded from the
%   calculations.
%
%   ... = IOSR.STATISTICS.KERNELDENSITY(X, BINS) calculates the density for
%   the query points specified by BINS. If BINS is a scalar, then BINS
%   points are queried between MIN(X(:)) and MAX(X(:)); if BINS is a
%   vector, then the values are used as the query points directly. D and XD
%   are column vectors.
%
%   ... = IOSR.STATISTICS.KERNELDENSITY(X, BINS, H) uses the bandwidth H to
%   calculate the kernel density. H must be a scalar. BINS may be an empty
%   array in order to use the default described above.
%
%   ... = IOSR.STATISTICS.KERNELDENSITY(X, BINS, [], KERNEL) uses the
%   kernel function specified by KERNEL to calculate the density. The
%   kernel may be:
%       - 'normal' (default),
%       - 'uniform',
%       - 'triangular',
%       - 'epanechnikov',
%       - 'quartic',
%       - 'triweight',
%       - 'tricube',
%       - 'cosine',
%       - 'logistic',
%       - 'sigmoid', or
%       - 'silverman'.
%   For the uniform case the bandwidth is set to 15% of the range of the
%   data [1]. Otherwise the bandwidth is chosen to be optimal for normal
%   data assuming a gaussian kernel.
%
%   ... = IOSR.STATISTICS.KERNELDENSITY(X, BINS, H, KERNEL) allows the
%   bins BINS and bandwidth H to be specified directly.
%
%   [D, XD, BW] = IOSR.STATISTICS.KERNELDENSITY(...) returns the badwidth
%   BW.
%
%   Examples
%
%     Example 1: Plot the kernel density of gaussian data
%       figure
%       % gaussian random numbers
%       y = randn(100000, 1);
%       % density
%       [d, xd] = iosr.statistics.kernelDensity(y);
%       % plot
%       plot(xd, d);
%
%     Example 2: Density trace with 200 bins of width of 10% of data range
%       figure
%       % random numbers
%       y = randn(100000, 1);
%       % y range
%       range = max(y(:)) - min(y(:));
%       % density trace
%       [d, xd] = iosr.statistics.kernelDensity(y, 200, 0.1*range, 'uniform');
%       % plot
%       plot(xd, d);
%
%   References
%
%   [1] Hintze, Jerry L.; Nelson, Ray D. (1998). "Violin Plots: A Box
%       Plot-Density Trace Synergism". The American Statistician. 52 (2):
%       181?4.

    %% input check

    x = x(:);
    x = x(~isnan(x));
    assert(numel(x) > 1, 'X must be a vector, matrix, or array.')

    % x bins
    if nargin < 2
        bins = [];
    end
    if isempty(bins)
        bins = 100;
    end
    if isscalar(bins)
        bins = linspace(min(x), max(x), round(bins));
    end
    bins = bins(:);

    % bin width
    if nargin < 3
        h = [];
    end

    % kernel
    if nargin < 4
        kernel = [];
    end
    if isempty(kernel)
        kernel = 'normal';
    end
    % return kernel function
    switch lower(kernel)
        case 'uniform'
            K = @(u) 0.5*(abs(u) <= 1);
            if isempty(h)
                h = 0.15 * (max(x) - min(x));
            end
        case 'normal'
            K = @(u) ((1/sqrt(2*pi)) * exp(-0.5*(u.^2)));
        case 'triangular'
            K = @(u) ((1-abs(u)) .* (abs(u) <= 1));
        case 'epanechnikov'
            K = @(u) ((0.75*(1-(u.^2))) .* (abs(u) <= 1));
        case 'quartic'
            K = @(u) (((15/16)*(1-(u.^2)).^2) .* (abs(u) <= 1));
        case 'triweight'
            K = @(u) (((35/32)*(1-(u.^2)).^3) .* (abs(u) <= 1));
        case 'tricube'
            K = @(u) (((70/81)*(1-(abs(u).^3)).^3) .* (abs(u) <= 1));
        case 'cosine'
            K = @(u) (((pi/4)*cos((pi/2)*u)) .* (abs(u) <= 1));
        case 'logistic'
            K = @(u) (1 / (exp(u) + 2 + exp(-u)));
        case 'sigmoid'
            K = @(u) ((2/pi) * (1 / (exp(u) + exp(-u))));
        case 'silverman'
            K = @(u) (0.5 * exp((-abs(u))/(sqrt(2))) .* sin((abs(u))/(sqrt(2)) + (pi/4)));
        otherwise
            error('Unknown kernel specified');
    end
    if isempty(h)
        h = ((4*(std(x).^5))/(3*numel(x))).^(1/5);
    end

    assert(isscalar(h), 'h must be a scalar')

    %% calculate kernel density

    xd = sort(bins);
    d = zeros(size(xd));

    for i = 1:numel(xd)
        d(i) = sum(K((x-xd(i))/h))./(numel(x)*h);
    end
    d(isnan(d)) = 0;
    bw = h;

end


function p = predict(theta, X)
%PREDICT Predict whether the label is 0 or 1 using learned logistic
%regression parameters theta
%   p = PREDICT(theta, X) computes the predictions for X using a
%   threshold at 0.5 (i.e., if sigmoid(theta'*x) >= 0.5, predict 1)

m = size(X, 1); % Number of training examples

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

% ====================== YOUR CODE HERE ======================
% Instructions: Complete the following code to make predictions using
%               your learned logistic regression parameters.
%               You should set p to a vector of 0's and 1's
%

h_theta = sigmoid(X*theta);
p = ceil(2*h_theta)-1;

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

end


function y = sinc(x, c)
%SINC   The function SIN(PI*X)/(PI*X).
%
%   SINC(X) returns the SIN(PI*X)/(PI*X) function which is defined as
%
%      1                 if x = 0
%      sin(pi*x)/(pi*x)  if 0 < |x| < infinity
%      0                 if |x| = infinity
%
%   SINC(X, C) returns the SIN(C*X)/(C*X) function, where C > 0, which is a
%   generalization of the above and is defined as
%
%      1                 if x = 0
%      sin(c*x)/(c*x)    if 0 < |x| < infinity
%      0                 if |x| = infinity
%
%   See also SIN.

%   Author:      Peter J. Acklam
%   Time-stamp:  2003-10-20 08:45:14 +0200
%   E-mail:      pjacklam@online.no
%   URL:         http://home.online.no/~pjacklam

   nargsin = nargin;

   % check number of input arguments
   error(nargchk(1, 2, narsgin));

   if nargsin < 2
      c = pi;
   else
      if any(size(c) ~= 0) | (c <= 0)
         error('Second argument must be a positive scalar.');
      end
   end

   y = ones(size(x));           % initialize output
   i = x ~= 0;                  % find non-zero elements
   t = c * x(i);                % precompute C*X
   y(i) = sin(t) ./ t;          % compute SIN(C*X)/(C*X)
   y(isinf(x)) = 0;             % 0 not NaN when X = +/-Inf


function line_monte_carlo_test01 ( )

%*****************************************************************************80
%
%% LINE_MONTE_CARLO_TEST01 estimates integrals in 1D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 January 2014
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'LINE_MONTE_CARLO_TEST01\n' );
  fprintf ( 1, '  Use LINE01_SAMPLE to estimate integrals\n' );
  fprintf ( 1, '  along the length of the unit line in 1D.\n' );

  seed = 123456789;

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

  n = 1;

  while ( n <= 65536 )

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

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

    for j = 1 : 7

      e = j - 1;

      value = monomial_value_1d ( n, e, x );

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

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

    end

    fprintf ( 1, '\n' );

    n = 2 * n;

  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '     Exact' );

  for j = 1 : 7

    e = j - 1;

    result = line01_monomial_integral ( e );
    fprintf ( 1, '  %14.6g', result );

  end

  fprintf ( 1, '\n' );

  return
end


function b = isPointInEllipse(point, ellipse, varargin)
%ISPOINTINELLIPSE Check if a point is located inside a given ellipse
%
%   B = isPointInEllipse(POINT, ELLIPSE)
%   Returns true if point is located inside the given ellipse.
%
%   B = isPointInEllipse(POINT, ELLIPSE, TOL)
%   Specifies the tolerance value
%
%   Example:
%   isPointInEllipse([1 0], [0 0 2 1 0])
%   ans =
%       1
%   isPointInEllipse([0 0], [0 0 2 1 0])
%   ans =
%       1
%   isPointInEllipse([1 1], [0 0 2 1 0])
%   ans =
%       0
%   isPointInEllipse([1 1], [0 0 2 1 30])
%   ans =
%       1
%
%   See also:
%   ellipses2d, isPointInCircle
%
%   ---------
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 11/03/2011
%

%   HISTORY

% extract computation tolerance
tol = 1e-14;
if ~isempty(varargin)
    tol = varargin{1};
end

% compute ellipse to unit circle transform
rot = createRotation(-deg2rad(ellipse(5)));
sca = createScaling(1./ellipse(3:4));
trans = sca * rot;

% transform points to unit circle basis
pTrans = bsxfun(@minus, point, ellipse(:,1:2));
pTrans = transformPoint(pTrans, trans);

% test if distance to origin smaller than 1
b = sqrt(sum(power(pTrans, 2), 2)) - 1 <= tol;


function [ area, radin, side ] = polygon_outrad_data_2d ( n, radout )

%*****************************************************************************80
%
%% POLYGON_OUTRAD_DATA_2D determines polygonal data from its outer radius in 2D.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    09 September 2003
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of sides of the polygon.
%    N must be at least 3.
%
%    Input, real RADOUT, the outer radius of the polygon, that is,
%    the radius of the smallest circle that can be described
%    around the polygon.
%
%    Output, real AREA, the area of the regular polygon.
%
%    Output, real RADIN, the inner radius of the polygon, that is,
%    the radius of the largest circle that can be inscribed
%    within the polygon.
%
%    Output, real SIDE, the length of one side of the polygon.
%
  if ( n < 3 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'POLYGON_OUTRAD_DATA_2D - Fatal error!\n' );
    fprintf ( 1, '  Input value of N must be at least 3.\n' );
    fprintf ( 1, '  but your input value was N = %d\n', n );
    error ( 'POLYGON_OUTRAD_DATA_2D - Fatal error!' );
  end

  angle = pi / n;
  area = 0.5 * n * radout * radout * sin ( 2.0 * angle );
  side = 2.0 * radout * sin ( angle );
  radin = 0.5 * side / tan ( angle );

  return
end


function [v] = spm_mar_gen (w,A,C,n,ndisc)
% Generate data from MAR model
% FORMAT [v] = spm_mar_gen (w,A,C,n,ndisc)
%
% Generates n time steps of the MAR(p) process
%
%     v(k,:)' = w' + A1*v(k-1,:)' +...+ Ap*v(k-p,:)' + eta(k,:)',
%
%  where A=[A1 ... Ap] is the coefficient matrix, and w is a vector of
%  intercept terms that is included to allow for a nonzero mean of the
%  process. The vectors eta(k,:) are independent Gaussian noise
%  vectors with mean zero and covariance matrix C.
%
%  This function is adapted from the ARFIT toolbox by Neumaier and
%  Schneider
%___________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Will Penny
% $Id: spm_mar_gen.m 1143 2008-02-07 19:33:33Z spm $

m       = size(C,1);                  % dimension of state vectors
p       = size(A,2)/m;                % order of process

if (p ~= round(p))
    error('Bad arguments.');
end

if (length(w) ~= m | min(size(w)) ~= 1)
    error('Dimensions of arguments are mutually incompatible.')
end
w       = w(:)';                      % force w to be row vector

% Discard the first ndisc time steps; if ndisc is not given as input
% argument, use default
if (nargin < 5)
    ndisc = 10^3;
end

% Compute Cholesky factor of covariance matrix C
R       = chol(C);                    % R is upper triangular

% Get ndisc+n independent Gaussian pseudo-random vectors with
% covariance matrix C=R'*R
randvec = randn([ndisc+n,m])*R;

% Add intercept vector to random vectors
randvec = randvec + ones(ndisc+n,1)*w;

% Get transpose of system matrix A (use transpose in simulation because
% we want to obtain the states as row vectors)
AT      = A';

% Take the p initial values of the simulation to equal the process mean,
% which is calculated from the parameters A and w
if any(w)
    %  Process has nonzero mean    mval = inv(B)*w'    where
    %             B = eye(m) - A1 -... - Ap;
    %  Assemble B
    B    = eye(m);
    for j=1:p
        B = B - A(:, (j-1)*m+1:j*m);
    end
    %  Get mean value of process
    mval = w / B';

    %  The optimal forecast of the next state given the p previous
    %  states is stored in the vector x. The vector x is initialized
    %  with the process mean.
    x    = ones(p,1)*mval;
else
    %  Process has zero mean
    x    = zeros(p,m);
end

% Initialize state vectors
u      = [x; zeros(ndisc+n,m)];

% Simulate n+ndisc observations. In order to make use of Matlab's
% vectorization capabilities, the cases p=1 and p>1 must be treated
% separately.
if p==1
    for k=2:ndisc+n+1;
        x(1,:) = u(k-1,:)*AT;
        u(k,:) = x + randvec(k-1,:);
    end
else
    for k=p+1:ndisc+n+p;
        for j=1:p;
            x(j,:) = u(k-j,:)*AT((j-1)*m+1:j*m,:);
        end
        u(k,:) = sum(x)+randvec(k-p,:);
    end
end

% return only the last n simulated state vectors
v = u(ndisc+p+1:ndisc+n+p,:);


function [ax_mps2] = calcPathAx(s_m, v_mps)
%_________________________________________________________________
%% Documentation
%
% Authors:      Alexander Wischnewski (alexander.wischnewski@tum.de)
%
% Start Date:   08.02.2018
%
% Description:  calculates the acceleration profile corresponding to a
%               velocity profile for a given path length. Constant
%               acceleration is assumed between the discretization points.
%
% Inputs:
%   s_m         Vector with path length values
%   v_mps       Vector with velocity at these path points
%
% Outputs:
%   ax_mps2     Vector with accelerations valid from the current to the
%               next point.

% initialize variables
ax_mps2 = zeros(length(v_mps), 1);
% calculate acceleration based on the assumption that it is constant
% between two discretization points. Last point can't be calculated as no
% further velocity information is available for the point behind.
dS = diff(s_m);
dV = diff(v_mps);
ax_mps2(1:(end-1)) = (dV.^2 + 2.*dV.*v_mps(1:(end-1)))./(2.*dS);
% copy last point which could be calculated, as no better information is
% available
ax_mps2(end) = ax_mps2(end-1);


function phi=pt2MaxEntropyCoords(x,v,algorithm,maxIter,epsScal)
%%PT2MAXENTROPYCOORDS Convert a point in a polygon to maximum entropy
%       coordinates. This is a type of barycentric coordinate system in 2D
%       that applies to points in polygons and is described in Section 7 of
%       [1]. These coordinates can be useful for Barycentric mapping and
%       for interpolation.
%
%INPUTS: x The 2XnumPts set of points to convert into maximum entropy
%          coordinates.
%        v The 2XnumBases set of vertices of the polygon defining the
%          coordinate system. The vertices can be given in clockwise or
%          counterclockwise order. It is assumed that the first vertex is
%          not repeated at the end.
% algorithm An optional parameter specifying the edge weight function to
%          use. Possible values are:
%          0 (The default if omitted or an empty matrix is passed) Use the
%            edge weight of Hormann and Sukumar, which is the first one
%            given in [1].
%          1 Use the second edge weight given in [1].
%  maxIter The maximum number of iterations of Newton's method to use. The
%          default if omitted or an empty matrix is passed is 100 and
%          generally, far fewer iterations are actually needed.
%  epsScal Convergence is determined when
%          abs(stepSize)<=epsScal*eps(lambda), where lambda is the 2X1
%          vector parameter being estimated from which the weights phi are
%          derived in [1]. The default if omitted or an empty matrix is
%          passed is 1.
%
%OUTPUTS: phi A numBasesXnumPts set of the maximum entropy coordinates of
%             the points given in x. Note that sum(phi(:,k))=1 for any k.
%             Points on the edge and outside of the polygon will typically
%             return a vector of NaNs.
%
%EXAMPLE 1:
%In this example, the coordinates of two points are found and the points
%are recreated from the phi values. The residual error of the reverse
%conversion is seen to be on the order of finite precision errors.
% numVertices=5;
% v=zeros(2,numVertices);
% v(:,1)=[-2;3];
% v(:,2)=[11;0];
% v(:,3)=[9;10];
% v(:,4)=[7;11];
% v(:,5)=[0;10];
% x=[[6;5],[8;2]];
% phi=pt2MaxEntropyCoords(x,v);
% xBack=barycentricCoords2Pt(phi,v);
% ResidualErr=max(max(abs(xBack-x)))
%
%EXAMPLE 2:
%This draws a convex polygon and a set of horizontal and vertical lines in
%the polygon. Then, the points of the lines are converted to maximum
%entropy coordinates and the vertices of the polygon are moved. After
%conversion back to Cartesian coordinates, one can see how the original
%grid in the polygon has been warped.
% numVertices=5;
% v=zeros(2,numVertices);
% v(:,1)=[-2;3];
% v(:,2)=[11;0];
% v(:,3)=[9;10];
% v(:,4)=[7;11];
% v(:,5)=[0;10];
%
% vertLines=-1:1:10;
% numVertLines=length(vertLines);
% horizLines=1:10;
% numHorizLines=length(horizLines);
% numLinPts=99;
% xVert=zeros(2,numLinPts,numVertLines);
% xHoriz=zeros(2,numLinPts,numHorizLines);
% for k=1:numVertLines
%     xVert(1,:,k)=vertLines(k);
%     xVert(2,:,k)=linspace(0,11,numLinPts);
%     sel=pointIsInPolygon(v,xVert(:,:,k),false)==0;
%     xVert(:,sel,k)=NaN;
% end
% for k=1:numHorizLines
%     xHoriz(1,:,k)=linspace(-2,11,numLinPts);
%     xHoriz(2,:,k)=horizLines(k);
%     sel=pointIsInPolygon(v,xHoriz(:,:,k),false)==0;
%     xHoriz(:,sel,k)=NaN;
% end
%
% figure(1)
% clf
% hold on
% plot([v(1,:),v(1,1)],[v(2,:),v(2,1)],'-c','linewidth',2)
% axis([-2, 12, 0, 12])
% for k=1:numVertLines
%     plot(xVert(1,:,k),xVert(2,:,k),'-r')
% end
% for k=1:numHorizLines
%     plot(xHoriz(1,:,k),xHoriz(2,:,k),'-b')
% end
% phiVert=pt2MaxEntropyCoords(xVert(:,:),v);
% phiHoriz=pt2MaxEntropyCoords(xHoriz(:,:),v);
%
% %Move the vertices, but keep the shape convex and the vertices still in
% %counterclockwise order.
% v(:,1)=[3;2];
% v(:,2)=[8;0];
% v(:,3)=[10;10];
% v(:,4)=[9;11];
% v(:,5)=[3;10];
%
% figure(2)
% clf
% hold on
% plot([v(1,:),v(1,1)],[v(2,:),v(2,1)],'-c','linewidth',2)
% axis([-2, 12, 0, 12])
% %Synthesize the corresponding points in the transformed coordinate
% %system.
% xTransVert=reshape(barycentricCoords2Pt(phiVert,v),[2,numLinPts,numVertLines]);
% xTransHoriz=reshape(barycentricCoords2Pt(phiHoriz,v),[2,numLinPts,numHorizLines]);
% for k=1:numVertLines
%     plot(xTransVert(1,:,k),xTransVert(2,:,k),'-r')
% end
% for k=1:numHorizLines
%     plot(xTransHoriz(1,:,k),xTransHoriz(2,:,k),'-b')
% end
%
%REFERENCES:
%[1] M. S. Floater, "Generalized barycentric coordinates and applications,"
%    Acta Numerica, vol. 24, pp. 161-214, 1 May 2015.
%
%September 2021 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

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

if(nargin<4||isempty(maxIter))
    maxIter=100;
end

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

numPts=size(x,2);
numBases=size(v,2);
phi=zeros(numBases,numPts);
for curPt=1:numPts
    if(any(isnan(x(:,curPt))))
       phi(:,curPt)=NaN;
       continue;
    end

    diffs=bsxfun(@minus,x(:,curPt),v);
    mags=sqrt(sum(diffs.*diffs,1));

    rhoVals=zeros(numBases,1);
    if(algorithm==0)
        for k=1:(numBases-1)
            rhoVals(k)=mags(k)+mags(k+1)-norm(v(:,k)-v(:,k+1));
        end
        rhoVals(numBases)=mags(numBases)+mags(1)-norm(v(:,numBases)-v(:,1));
    elseif(algorithm==1)
        for k=1:(numBases-1)
            rhoVals(k)=mags(k)*mags(k+1)+dot(diffs(:,k),diffs(:,k+1));
        end
        rhoVals(numBases)=mags(numBases)*mags(1)+dot(diffs(:,numBases),diffs(:,1));
    else
       error('Unknown algorithm specified.')
    end

    piVals=zeros(1,numBases);
    piVals(1)=prod(rhoVals(2:(numBases-1)));
    for k=2:(numBases-1)
        piVals(k)=prod(rhoVals([1:(k-2),(k+1):numBases]));
    end
    piVals(numBases)=prod(rhoVals(1:(numBases-2)));

    mi=piVals/sum(piVals);
    d=-diffs;

    %lambda can start uniform.
    lambda=[1/2;1/2];
    %Use Newton's method.
    for curIter=1:maxIter
        [F,H]=getFAndH(lambda,d,mi);

        if(any(~isfinite(H(:))))
           lambda=[NaN;NaN];
           break;
        end

        stepVal=pinv(H)*F;

        if(all(abs(stepVal)<=epsScal*eps(lambda)))
            %Convergence to some multiple of finite precision limits.
            break;
        end
        lambda=lambda-stepVal;
    end

    wi=mi.*exp(sum(bsxfun(@times,lambda,d),1));
    phi(:,curPt)=wi/sum(wi);
end
end

function [gradF,H]=getFAndH(lambda,d,mi)
%%GETFANDH Get the the gradient and the Hessian of the cost function.

mExpdTerms=bsxfun(@times,mi.*exp(sum(bsxfun(@times,lambda,d),1)),d);

%The gradient.
gradF=sum(mExpdTerms,2);
H=zeros(2,2);
H(1,1)=sum(mExpdTerms(1,:).*d(1,:));
H(2,1)=sum(mExpdTerms(1,:).*d(2,:));
H(1,2)=H(2,1);
H(2,2)=sum(mExpdTerms(2,:).*d(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 [C, L, U] = SpectralClustering(W, k, Type)
%SPECTRALCLUSTERING Executes spectral clustering algorithm
%   Executes the spectral clustering algorithm defined by
%   Type on the adjacency matrix W and returns the k cluster
%   indicator vectors as columns in C.
%   If L and U are also called, the (normalized) Laplacian and
%   eigenvectors will also be returned.
%
%   'W' - Adjacency matrix, needs to be square
%   'k' - Number of clusters to look for
%   'Type' - Defines the type of spectral clustering algorithm
%            that should be used. Choices are:
%      1 - Unnormalized
%      2 - Normalized according to Shi and Malik (2000)
%      3 - Normalized according to Jordan and Weiss (2002)
%
%   References:
%   - Ulrike von Luxburg, "A Tutorial on Spectral Clustering",
%     Statistics and Computing 17 (4), 2007
%
%   Author: Ingo Buerk
%   Year  : 2011/2012
%   Bachelor Thesis

% calculate degree matrix
degs = sum(W, 2);
D    = sparse(1:size(W, 1), 1:size(W, 2), degs);

% compute unnormalized Laplacian
L = D - W;

% compute normalized Laplacian if needed
switch Type
    case 2
        % avoid dividing by zero
        degs(degs == 0) = eps;
        % calculate inverse of D
        D = spdiags(1./degs, 0, size(D, 1), size(D, 2));

        % calculate normalized Laplacian
        L = D * L;
    case 3
        % avoid dividing by zero
        degs(degs == 0) = eps;
        % calculate D^(-1/2)
        D = spdiags(1./(degs.^0.5), 0, size(D, 1), size(D, 2));

        % calculate normalized Laplacian
        L = D * L * D;
end

% compute the eigenvectors corresponding to the k smallest
% eigenvalues
diff   = eps;
[U, ~] = eigs(L, k, diff);

% in case of the Jordan-Weiss algorithm, we need to normalize
% the eigenvectors row-wise
if Type == 3
    U = bsxfun(@rdivide, U, sqrt(sum(U.^2, 2)));
end

% now use the k-means algorithm to cluster U row-wise
% C will be a n-by-1 matrix containing the cluster number for
% each data point
C = kmeans(U, k, 'start', 'sample', ...
                 'EmptyAction', 'singleton');

% now convert C to a n-by-k matrix containing the k indicator
% vectors as columns
C = sparse(1:size(D, 1), C, 1);

end


%% 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).
%
% -----------------------------------------------------------------------
%
% Copyright (c) 2016, Manuel Guizar Sicairos, James R. Fienup, University of Rochester
% All rights reserved.
%
% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are
% met:
%
%     * Redistributions of source code must retain the above copyright
%       notice, this list of conditions and the following disclaimer.
%     * Redistributions in binary form must reproduce the above copyright
%       notice, this list of conditions and the following disclaimer in
%       the documentation and/or other materials provided with the distribution
%     * Neither the name of the University of Rochester nor the names
%       of its contributors may be used to endorse or promote products derived
%       from this software without specific prior written permission.
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
% POSSIBILITY OF SUCH DAMAGE.
% --------------------------------------------------------------------------

%% 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(1i*2*pi*(deltar*Nr/nr+deltac*Nc/nc))).*exp(-1i*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
usfac = 100;
[output, Greg] = dftregistration(fft2(f),fft2(g),usfac);
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 and working
% correcntly for their application.
%
% Feel free to e-mail me with questions or comments.


function lambda = minij_eigenvalues ( n )

%*****************************************************************************80
%
%% MINIJ_EIGENVALUES returns the eigenvalues of the MINIJ matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Output, real LAMBDA(N,1), the eigenvalues.
%
  lambda = zeros ( n, 1 );

  for i = 1 : n
    angle = ( 2 * i - 1 ) * pi / ( 2 * n + 1 );
    lambda(i,1) = 0.5 / ( 1.0 - cos ( angle ) );
  end

  return
end


function [rho,theta,R]=findTrigMomentFromSamp(n,x,w)
%%FINDTRIGMOMENTFROMSAMPLE Given a set of samples of a circular
%          distribution, determine a particular trigonometric sample
%          moment.
%
%INPUTS: n The order of the moment desired. This is >=1.
%        x The 1XN or NX1 vector of possibly weighted samples.
%        w The NX1 weights associated with the samples. If this parameter
%          is omitted or an empty matrix is passed, then the samples are
%          uniformly weighted.
%
%OUTPUTS: rho The (complex) mean resultant value for the nth moment. Note
%             that rho=R*exp(1j*theta).
%       theta The (real) trigonometric mean angle in radians for the nth
%             moment. This is between -pi and pi.
%           R The (real) mean resultant length for the nth moment.
%
%An expression for the complex trigonometric moments is given in Equation
%14 of [1]. This is just a weighted version of the definition in Chapter
%2.4 of [2].
%
%REFERENCES:
%[1] G. Kurz, I. Gilitschenski, R. Y. Siegwart, and U. D. Hanebeck,
%    "Methods for deterministic approximation of circular densities,"
%    Journal of Advances in Information Fusion, vol. 11, no. 2, pp.
%    138-156, Dec. 2016.
%[2] K. V. Mardia and P. E. Jupp, Directional Statistics. Chichester: John
%    Wiley and Sons, 2000.
%
%April 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

N=length(x);
if(nargin<3||isempty(w))
    w=ones(N,1);
end

%Normalize.
w=w/sum(w);

rho=sum(exp(1i*n*x(:)).*w(:));
if(nargout>1)
    theta=angle(rho);
    R=abs(rho);
end
end

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


function f = fxy5 ( n, x, y )

%*****************************************************************************80
%
%% FXY5 is the fourth 2D example.
%
%  Discussion:
%
%    This is example 3.1 in the reference.
%
%    It is known as the discontinuous medium wave function.
%
%    Here, we are computing the second component of the solution, U(X,Y).
%
%    It should be plotted on (x,y) in [-1,0]x[0,0.1].
%
%    The second variable y actually represents time.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 September 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Rick Archibald, Anne Gelb, Jungho Yoon,
%    Determining the locations and discontinuities in the derivatives
%    of functions,
%    Applied Numerical Mathematics,
%    Volume 58, 2008, pages 577-592.
%
%  Parameters:
%
%    Input, integer N, the number of points.
%
%    Input, real X(N), Y(N), the arguments.
%
%    Output, real F(N,1), the function values.
%

%
%  Destroy all row vectors!
%
  x = x ( : );
  y = y ( : );

  cl = 0.87879;
  cr = 1.0;
  omega = 12.0;
  rhol = 0.55556;
  rhor = 1.0;

  f = zeros ( n, 1 );

  i = find ( x(1:n) <= -0.5 );
  j = find ( -0.5 < x(1:n) );

  f(i) = sin ( pi * omega * ( y(i) - ( x(i) + 0.5 ) / cl ) ) ...
    + ( rhol * cl - rhor * cr ) / ( rhol * cl + rhor * cr ) ...
    / ( rhol * cl ) ...
    * sin ( pi * omega * ( y(i) + ( x(i) + 0.5 ) / cl ) );

  f(j) = 2.0 / ( rhol * cl + rhor * cr ) ...
    * sin ( pi * omega * ( y(j) - ( x(j) + 0.5 ) / cl ) );

  return
end


function [ l, u ] = l1nn_lu ( n, h )

%*****************************************************************************80
%
%% L1NN_LU computes the LU factors of the 1D NN Laplacian.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 November 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of points.
%    N must be at least 3.
%
%    Input, real H, the spacing between points.
%
%    Output, real L(N,N), U(N,n), the LU factors.
%
  if ( n < 3 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'L1NN_LU - Fatal error!\n' );
    fprintf ( 1, '  N < 3.\n' );
    error ( 'L1NN_LU - Fatal error!' );
  end

  l = zeros ( n, n );

  for i = 1 : n
    l(i,i) = 1.0;
  end

  for i = 2 : n
    l(i,i-1) = - 1.0;
  end

  u = zeros ( n, n );

  for i = 1 : n - 1
    u(i,i) = 1.0;
  end
  u(n,n) = 0.0;

  for i = 1 : n - 1
    u(i,i+1) = - 1.0;
  end

  u(1:n,1:n) = u(1:n,1:n) / h / h;

  return
end


function [X maxdot] = packing_on_the_sphere(d, n, epsilon, X0)
% Return a set of points spread out on the sphere.
%
% function [X maxdot] = packing_on_the_sphere(d, n, epsilon, X0)
%
% Using optimization on the oblique manifold, that is, the product of
% spheres, this function returns a set of n points with unit norm in R^d in
% the form of a matrix X of size nxd, such that the points are spread out
% on the sphere. Ideally, we would minimize the maximum inner product
% between any two points X(i, :) and X(j, :), i~=j, but that is a nonsmooth
% cost function. Instead, we replace the max function by a classical
% log-sum-exp approximation and (attempt to) solve:
%
% min_{X in OB(d, n)} log( .5*sum_{i~=j} exp( xi'*xj/epsilon ) ),
%
% with xi = X(:, i) and epsilon is some "diffusion constant". As epsilon
% goes to zero, the cost function is a sharper approximation of the max
% function (under some assumptions), but the cost function becomes stiffer
% and hence harder to optimize.
%
% The second output, maxdot, is the maximum inner product between any two
% points in the returned X. This number is the one we truly are trying to
% minimize.
%
% Notice that this cost function is invariant under rotation of X:
% f(X) = f(XQ) for all orthogonal Q in O(d).
% This calls for optimization over the set of symmetric positive
% semidefinite matrices of size n and rank d with unit diagonal, which can
% be thought of as the quotient of the oblique manifold OB(d, n) by O(d):
% See elliptopefactory.
%
% This is known as the Thomson or, more specifically, the Tammes problem:
% http://en.wikipedia.org/wiki/Tammes_problem
% An interesting page by Neil Sloane collecting best known packings is
% available here http://neilsloane.com/packings/

% This file is part of Manopt and is copyrighted. See the license file.
%
% Main author: Nicolas Boumal, July 2, 2013
% Contributors:
%
% Change log:
%   Aug. 14, 2013 (NB) : Code now compatible to experiment with both the
%                        obliquefactory and the elliptopefactory.
%
%   Jan.  7, 2014 (NB) : Added reference to Neil Sloane's page and the
%                        maxdot output.
%
%   June 24, 2014 (NB) : Now shifting exponentials to alleviate numerical
%                        trouble when epsilon is too small.
%

    if ~exist('d', 'var') || isempty(d)
        % Dimension of the embedding space: R^d
        d = 3;
    end
    if ~exist('n', 'var') || isempty(n)
        % Number n of points to place of the sphere in R^d.
        % For example, n=12 yields an icosahedron:
        % https://en.wikipedia.org/wiki/Icosahedron
        % Notice though that platonic solids are not always optimal.
        % Try for example n = 8: you don't get a cube.
        n = 24;
    end
    if ~exist('epsilon', 'var') || isempty(epsilon)
        % This value should be as close to 0 as affordable.
        % If it is too close to zero, optimization first becomes much
        % slower, than simply doesn't work anymore becomes of floating
        % point overflow errors (NaN's and Inf's start to appear).
        % If it is too large, then log-sum-exp is a poor approximation of
        % the max function, and the spread will be less uniform.
        % An okay value seems to be 0.01 or 0.001 for example. Note that a
        % better strategy than using a small epsilon straightaway is to
        % reduce epsilon bit by bit and to warm-start subsequent
        % optimization in that way. Trustregions will be more appropriate
        % for these fine tunings.
        epsilon = 0.0015;
    end

    % Pick your manifold (the elliptope factory quotients out the global
    % rotation invariance of the problem, which is more natural but
    % conceptually a bit more complicated --- for usage with the toolbox it
    % is the same though: just uncomment the appropriate line).
    manifold = obliquefactory(d, n, true);
    % manifold = elliptopefactory(n, d);

    % Generate a random initial guess if none was given.
    if ~exist('X0', 'var') || isempty(X0)
        X0 = manifold.rand();
    end

    % Define the cost function with caching system used: the store
    % structure we receive as input is tied to the input point X. Everytime
    % this cost function is called at this point X, we will receive the
    % same store structure back. We may modify the store structure inside
    % the function and return it: the changes will be remembered for next
    % time.
    function [f store] = cost(X, store)
        if ~isfield(store, 'ready')
            XXt = X*X';
            % Shift the exponentials by the maximum value to reduce
            % numerical trouble due to possible overflows.
            s = max(max(triu(XXt, 1)));
            expXXt = exp((XXt-s)/epsilon);
            % Zero out the diagonal
            expXXt(1:(n+1):end) = 0;
            u = sum(sum(triu(expXXt, 1)));
            store.XXt = XXt;
            store.s = s;
            store.expXXt = expXXt;
            store.u = u;
            store.ready = true;
        end
        u = store.u;
        s = store.s;
        f = s + epsilon*log(u);
    end

    % Define the gradient of the cost. When the gradient is called at a
    % point X for which the cost was already called, the store structure we
    % receive remember everything that the cost function stored in it, so
    % we can reuse previously computed elements.
    function [g store] = grad(X, store)
        if ~isfield(store, 'ready')
            [~, store] = cost(X, store);
        end
        % Compute the Euclidean gradient
        eg = store.expXXt*X / store.u;
        % Convert to the Riemannian gradient (by projection)
        g = manifold.egrad2rgrad(X, eg);
    end

    % Setup the problem structure with its manifold M and cost+grad
    % functions.
    problem.M = manifold;
    problem.cost = @cost;
    problem.grad = @grad;

    % For debugging, it's always nice to check the gradient a few times.
    % checkgradient(problem);
    % pause;

    % Call a solver on our problem with a few options defined. We did not
    % specify the Hessian but it is still okay to call trustregion: Manopt
    % will approximate the Hessian with finite differences of the gradient.
    opts.tolgradnorm = 1e-8;
    opts.maxtime = 1200;
    opts.maxiter = 1e5;
    % X = trustregions(problem, X0, opts);
    X = conjugategradient(problem, X0, opts);

    % Evaluate the maximum inner product between any two points of X.
    XXt = X*X';
    dots = XXt(find(triu(ones(n), 1))); %#ok<FNDSB>
    maxdot = max(dots);

    % Similarly, even though we did not specify the Hessian, we may still
    % estimate its spectrum at the solution. It should reflect the
    % invariance of the cost function under a global rotatioon of the
    % sphere, which is an invariance under the group O(d) of dimension
    % d(d-1)/2 : this translates into d(d-1)/2 zero eigenvalues in the
    % spectrum of the Hessian.
    % The approximate Hessian is not a linear operator, and is it a
    % fortiori not symmetric. The result of this computation is thus not
    % reliable. It does display the zero eigenvalues as expected though.
    if manifold.dim() < 300
        evs = real(hessianspectrum(problem, X));
        figure;
        stem(1:length(evs), sort(evs), '.');
        title(['Eigenvalues of the approximate Hessian of the cost ' ...
               'function at the solution']);
    end

    % Show how the inner products X(:, i)'*X(:, j) are distributed.
    figure;
    hist(real(acos(dots)), 20);
    title('Histogram of the geodesic distances');

    % This is the quantity we actually want to minimize.
    fprintf('Maximum inner product between two points: %g\n', maxdot);

    % Give some visualization if the dimension allows
    if d == 2
        % For the circle, the optimal solution consists in spreading the
        % points with angles uniformly sampled in (0, 2pi). This
        % corresponds to the following value for the max inner product:
        fprintf('Optimal value for the max inner product: %g\n', cos(2*pi/n));
        figure;
        t = linspace(-pi, pi, 201);
        plot(cos(t), sin(t), '-', 'LineWidth', 3, 'Color', [152,186,220]/255);
        daspect([1 1 1]);
        box off;
        axis off;
        hold on;
        plot(X(:, 1), X(:, 2), 'r.', 'MarkerSize', 25);
        hold off;
    end
    if d == 3
        figure;
        % Plot the sphere
        [sphere_x sphere_y sphere_z] = sphere(50);
        handle = surf(sphere_x, sphere_y, sphere_z);
        set(handle, 'FaceColor', [152,186,220]/255);
        set(handle, 'FaceAlpha', .5);
        set(handle, 'EdgeColor', [152,186,220]/255);
        set(handle, 'EdgeAlpha', .5);
        daspect([1 1 1]);
        box off;
        axis off;
        hold on;
        % Add the chosen points
        Y = 1.02*X';
        plot3(Y(1, :), Y(2, :), Y(3, :), 'r.', 'MarkerSize', 25);
        % And connect the points which are at minimal distance,
        % within some tolerance.
        min_distance = real(acos(maxdot));
        connected = real(acos(XXt)) <= 1.20*min_distance;
        [Ic Jc] = find(triu(connected, 1));
        for k = 1 : length(Ic)
            i = Ic(k); j = Jc(k);
            plot3(Y(1, [i j]), Y(2, [i j]), Y(3, [i j]), 'k-');
        end
        hold off;
    end

end


function [ x, w ] = cce ( l )

%*****************************************************************************80
%
%% CCE computes a Clenshaw Curtis Exponential quadrature rule based on level.
%
%  Discussion:
%
%    Our convention is that the abscissas are numbered from left to right.
%
%    The rule is defined on [0,1].
%
%    The integral to approximate:
%
%      Integral ( 0 <= X <= 1 ) F(X) dX
%
%    The quadrature rule:
%
%      Sum ( 1 <= I <= N ) W(I) * F ( X(I) )
%
%    The input value of L selects the size of the rule as follows:
%    L = 1, N = 1;
%    1 < L, N = 2^(L-1)+1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 February 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer L, the level of the rule.
%    1 <= L.
%
%    Output, real X(N,1), the abscissas.
%
%    Output, real W(N,1), the weights.
%
  if ( l < 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'CCE - Fatal error!\n' );
    fprintf ( 1, '  Illegal value of L = %d\n', l );
    error ( 'CCE - Fatal error!' );
  end
%
%  Find the value of N according to the level.
%
  n = cce_order ( l );
%
%  Compute the points and weights.
%
  [ x, w ] = cc ( n );

  return
end


%IMMOMENTS PRTools routine for computing central moments of object images
%
%	  M = IMMOMENTS(A,TYPE,MOMENTS)
%
% INPUT
%   A        Dataset with object images dataset
%   TYPE     Desired type of moments
%   MOMENTS  Desired moments
%
% OUTPUT
%   M        Dataset with moments replacing images
%
% DESCRIPTION
% Computes for the image A a (1*N) vector M moments as defined by TYPE
% and MOMENTS. The following types are supported:
%
% TYPE = 'none'     Standard moments as specified in the Nx2 array MOMENTS.
%                   Moments are computed with respect to the image center.
%                   This is the default for TYPE.
%                   Default MOMENTS = [1 0; 0 1];
% TYPE = 'central'  Central moments as specified in the Nx2 array MOMENTS.
%                   Moments are computed with respect to the image mean
%                   Default MOMENTS = [2 0; 1 1; 0 2], which computes
%                   the variance in the x-direction (horizontal), the
%                   covariance between x and y and the variance in the
%                   y-direction (vertical).
% TYPE = 'scaled'   Scale-invariant moments as specified in the Nx2 array
%                   MOMENTS. Default MOMENTS = [2 0; 1 1; 0 2].
%                   After: M. Sonka et al.,
%                   Image processing, analysis and machine vision.
% TYPE = 'hu'       Calculates 7 moments of Hu, invariant to translation,
%                   rotation and scale.
%                   After: M. Sonka et al.,
%                   Image processing, analysis and machine vision.
% TYPE = 'zer'      Calculates the Zernike moments up to the order as
%                   specified in the scalar MOMENTS (1 <= MOMENTS <= 12).
%                   MOMENTS = 12 generates in total 47 moments.
%                   After: A. Khotanzad and Y.H. Hong, Invariant image
%                   recognition by Zernike moments, IEEE-PAMI, vol. 12,
%                   no. 5, 1990, 489-497.
%
% See DATASETS

% Copyright: D. de Ridder, R.P.W. Duin, r.p.w.duin@37steps.com
% Faculty EWI, Delft University of Technology
% P.O. Box 5031, 2600 GA Delft, The Netherlands

function b = immoments(a,type,mom)

	if nargin < 3, mom = []; end
	if nargin < 2 | isempty(type), type = 'none'; end
	isobjim(a);
	[m,k] = size(a);
	im = data2im(a);
	for i = 1:m
		imi = im(:,:,i);
		switch type
		case {'none'}
			if isempty(mom)
				mom = [1 0; 0 1];
			end
			mout = moments(imi,mom(:,1),mom(:,2),0,0);
		case {'central'}
			if isempty(mom)
				mom = [2 0; 1 1; 0 2];
			end
			mout = moments(imi,mom(:,1),mom(:,2),1,0);
		case {'scaled'}
			if isempty(mom)
				mom = [2 0; 1 1; 0 2];
			end
			mout = moments(imi,mom(:,1)',mom(:,2)',1,1);
		case {'hu' 'Hu'}
			mout = hu_moments(imi);
		case {'zer' 'zernike' 'Zernike'}
			if isempty(mom)
				mom = 12;
			end
			mout = zernike_moments(imi,mom);
		otherwise
			error('Moments should be of type none, central, scaled, hu or zer')
		end
		if i==1
			b = zeros(m,length(mout));
		end
		b(i,:) = mout;
	end
	b = setdata(a,b);

% M = MOMENTS (IM, P, Q, CENTRAL, SCALED)
%
% Calculates moments of order (P+Q) (can be arrays of indentical length)
% on image IM. If CENTRAL is set to 1 (default: 0), returns translation-
% invariant moments; if SCALED is set to 1 (default: 0), returns scale-
% invariant moments.
%
% After: M. Sonka et al., Image processing, analysis and machine vision.

function m = moments (im,p,q,central,scaled)

	if (nargin < 5), scaled = 0; 	end;
	if (nargin < 4), central = 0; end;
  if (nargin < 3)
  	error ('Insufficient number of parameters.');
  end;

	if (length(p) ~= length(q))
  	error ('Arrays P and Q should have equal length.');
  end;

  if (scaled & ~central)
  	error ('Scale-invariant moments should always be central.');
  end;

  % xx, yy are grids with co-ordinates
  [xs,ys] = size(im);
  [xx,yy] = meshgrid(-(ys-1)/2:1:(ys-1)/2,-(xs-1)/2:1:(xs-1)/2);

	if (central)

  	% Calculate zeroth and first order moments
	  m00 = sum(sum(im));
	  m10 = sum(sum(im.*xx));
	  m01 = sum(sum(im.*yy));

    % This gives the center of gravity
    xc  = m10/m00;
    yc  = m01/m00;

    % Subtract this from the grids to center the object
    xx  = xx - xc;
    yy  = yy - yc;

  end;

  % Calculate moment(s) (p,q).
  for i = 1:length(p)
		m(i) = sum(sum((xx.^p(i)).*(yy.^q(i)).*im));
  end;

  if (scaled)

  	c = 1 + (p+q)/2;

    % m00 should be known, as scaled moments are always central
    m = m ./ (m00.^c);

	end;

return;

% M = HU_MOMENTS (IM)
%
% Calculates 7 moments of Hu on image IM, invariant to translation,
% rotation and scale.
%
% After: M. Sonka et al., Image processing, analysis and machine vision.

function m = hu_moments (im)

	p = [ 1 0 2 1 2 0 3 ];
	q = [ 1 2 0 2 1 3 0 ];

  n = moments(im,p,q,1,1);

  m(1) = n(2) + n(3);
  m(2) = (n(3) - n(2))^2   + 4*n(1)^2;
  m(3) = (n(7) - 3*n(4))^2 + (3*n(5) - n(6))^2;
  m(4) = (n(7) +   n(4))^2 + (  n(5) + n(6))^2;
  m(5) = (  n(7) - 3*n(4)) * (n(7) + n(4)) * ...
           (  (n(7) + n(4))^2 - 3*(n(5) + n(6))^2) + ...
         (3*n(5) -   n(6)) * (n(5) + n(6)) * ...
           (3*(n(7) + n(4))^2 -   (n(5) + n(6))^2);
  m(6) = (n(3) - n(2)) * ((n(7) + n(4))^2 - (n(5) + n(6))^2) + ...
          4*n(1) * (n(7)+n(4)) * (n(5)+n(6));
  m(7) = (3*n(5) -   n(6)) * (n(7) + n(4)) * ...
           (  (n(7) + n(4))^2 - 3*(n(5) + n(6))^2) - ...
         (  n(7) - 3*n(4)) * (n(5) + n(6)) * ...
           (3*(n(7) + n(4))^2 -   (n(5) + n(6))^2);

return;

% M = ZERNIKE_MOMENTS (IM, ORDER)
%
% Calculates Zernike moments up to and including ORDER (<= 12) on image IM.
% Default: ORDER = 12.

function m = zernike_moments (im, order)

  if (nargin < 2),             order = 12;                      end;
  if (order < 1 | order > 12), error ('order should be 1..12'); end;

  % xx, yy are grids with co-ordinates

  [xs,ys] = size(im);
  [xx,yy] = meshgrid(-(ys-1)/2:1:(ys-1)/2,-(xs-1)/2:1:(xs-1)/2);

  % Calculate center of mass and distance of any pixel to it

  m  = moments (im,[0 1 0],[0 0 1],0,0);
  xc = m(2)/m(1); yc = m(3)/m(1);
  xx = xx - xc; yy = yy - yc;

  len     = sqrt(xx.^2+yy.^2);
  max_len = max(max(len));

  % Map pixels to unit circle; prevent divide by zero.

  rho        = len/max_len;
  rho_tmp    = rho; rho_tmp(find(rho==0)) = 1;
  theta      = acos((xx/max_len)./rho_tmp);

  % Flip angle for pixels above center of mass

  yneg            = length(find(yy(:,1)<0));
  theta(:,1:yneg) = 2*pi - theta(:,1:yneg);

  % Calculate coefficients

  c = zeros(order,order);
  s = zeros(order,order);

  i = 1;
  for n = 2:order
    for l = n:-2:0
      r    = polynomial (n,l,rho);
      c    = sum(sum(r.*cos(l*theta)))*((n+1)/(pi*max_len^2));
      s    = sum(sum(r.*sin(l*theta)))*((n+1)/(pi*max_len^2));
      m(i) = sqrt(c^2+s^2);
      i    = i + 1;
    end;
  end;

return

function p = polynomial (n,l,rho)

  switch (n)
    case 2, switch (l)
        case 0, p = 2*(rho.^2)-1;
        case 2, p =   (rho.^2);
      end;
    case 3, switch (l)
        case 1, p = 3*(rho.^3)-2*rho;
        case 3, p =   (rho.^3);
      end;
    case 4, switch (l)
        case 0, p = 6*(rho.^4)-6*(rho.^2)+1;
        case 2, p = 4*(rho.^4)-3*(rho.^2);
        case 4, p =   (rho.^4);
      end;
    case 5, switch (l)
        case 1, p = 10*(rho.^5)-12*(rho.^3)+3*rho;
        case 3, p =  5*(rho.^5)- 4*(rho.^3);
        case 5, p =    (rho.^5);
      end;
    case 6, switch (l)
        case 0, p = 20*(rho.^6)-30*(rho.^4)+12*(rho.^2)-1;
        case 2, p = 15*(rho.^6)-20*(rho.^4)+ 6*(rho.^2);
        case 4, p =  6*(rho.^6)- 5*(rho.^4);
        case 6, p =    (rho.^6);
      end;
    case 7, switch (l)
        case 1, p = 35*(rho.^7)-60*(rho.^5)+30*(rho.^3)-4*rho;
        case 3, p = 21*(rho.^7)-30*(rho.^5)+10*(rho.^3);
        case 5, p =  7*(rho.^7)- 6*(rho.^5);
        case 7, p =    (rho.^7);
      end;
    case 8, switch (l)
        case 0, p = 70*(rho.^8)-140*(rho.^6)+90*(rho.^4)-20*(rho.^2)+1;
        case 2, p = 56*(rho.^8)-105*(rho.^6)+60*(rho.^4)-10*(rho.^2);
        case 4, p = 28*(rho.^8)- 42*(rho.^6)+15*(rho.^4);
        case 6, p =  8*(rho.^8)-  7*(rho.^6);
        case 8, p =    (rho.^8);
      end;
    case 9, switch (l)
        case 1, p = 126*(rho.^9)-280*(rho.^7)+210*(rho.^5)-60*(rho.^3)+5*rho;
        case 3, p =  84*(rho.^9)-168*(rho.^7)+105*(rho.^5)-20*(rho.^3);
        case 5, p =  36*(rho.^9)- 56*(rho.^7)+ 21*(rho.^5);
        case 7, p =   9*(rho.^9)-  8*(rho.^7);
        case 9, p =     (rho.^9);
      end;
    case 10, switch (l)
        case  0, p = 252*(rho.^10)-630*(rho.^8)+560*(rho.^6)-210*(rho.^4)+30*(rho.^2)-1;
        case  2, p = 210*(rho.^10)-504*(rho.^8)+420*(rho.^6)-140*(rho.^4)+15*(rho.^2);
        case  4, p = 129*(rho.^10)-252*(rho.^8)+168*(rho.^6)- 35*(rho.^4);
        case  6, p =  45*(rho.^10)- 72*(rho.^8)+ 28*(rho.^6);
        case  8, p =  10*(rho.^10)-  9*(rho.^8);
        case 10, p =     (rho.^10);
      end;
    case 11, switch (l)
        case  1, p = 462*(rho.^11)-1260*(rho.^9)+1260*(rho.^7)-560*(rho.^5)+105*(rho.^3)-6*rho;
        case  3, p = 330*(rho.^11)- 840*(rho.^9)+ 756*(rho.^7)-280*(rho.^5)+ 35*(rho.^3);
        case  5, p = 165*(rho.^11)- 360*(rho.^9)+ 252*(rho.^7)- 56*(rho.^5);
        case  7, p =  55*(rho.^11)-  90*(rho.^9)+  36*(rho.^7);
        case  9, p =  11*(rho.^11)-  10*(rho.^9);
        case 11, p =     (rho.^11);
      end;
    case 12, switch (l)
        case  0, p = 924*(rho.^12)-2772*(rho.^10)+3150*(rho.^8)-1680*(rho.^6)+420*(rho.^4)-42*(rho.^2)+1;
        case  2, p = 792*(rho.^12)-2310*(rho.^10)+2520*(rho.^8)-1260*(rho.^6)+280*(rho.^4)-21*(rho.^2);
        case  4, p = 495*(rho.^12)-1320*(rho.^10)+1260*(rho.^8)- 504*(rho.^6)+ 70*(rho.^4);
        case  6, p = 220*(rho.^12)- 495*(rho.^10)+ 360*(rho.^8)-  84*(rho.^6);
        case  8, p =  66*(rho.^12)- 110*(rho.^10)+  45*(rho.^8);
        case 10, p =  12*(rho.^12)-  11*(rho.^10);
        case 12, p =     (rho.^12);
      end;
  end;

return


function [uHessMat,uTriMat,Q,Z]=HessenbergTriangRed(A,B)
%%HESSENBERGTRIANGRED Perform a Hessenberg triangular reduction of the nXn
%               matrices A and B. This finds Q and Z such that Q'*A*Z= an
%               upper Hessenberg matrix and Q'*B*Z is an upper triangular
%               matrix.
%
%INPUTS: A, B Two real nXn matrices.
%
%OUTPUTS: uHessMat The upper Hessenberg matrix obtained from Q'*A*Z.
%          uTriMat The upper triangular matrix obtained with Q'*B*Z.
%
%This function implements algorithm 7.7.1 in [1].
%
%REFERENCES:
%[1] G. H. Golub and C. F. Van Loan, Matrix Computations, 4th ed.
%    Baltimore: Johns Hopkins University Press, 2013.
%
%September 2019 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

n=size(A,1);

if(nargout>2)
    Z=eye(n,n);
    %B=Q*R. Replace B with Q'*B and A with Q'*A
    [Q,R]=qr(B);
    B=R;%Q'*B=R.
    A=Q'*A;

    for j=1:(n-2)
        for i=n:-1:(j+2)
            %Zeros A(i,j)
            [c,s]=GivensCS(A(i-1,j),A(i,j));
            rotMat=[c, s;
                   -s, c];
            A((i-1):i,j:n)=rotMat'*A((i-1):i,j:n);
            B((i-1):i,(i-1):n)=rotMat'*B((i-1):i,(i-1):n);

            Q(:,(i-1):i) = Q(:,(i-1):i)*rotMat;

            %Zeros B(i,i-1).
            [c,s]=GivensCS(-B(i,i),B(i,i-1));
            rotMat=[c, s;
                   -s, c];
            B(1:i,(i-1):i) = B(1:i,(i-1):i)*rotMat;
            A(1:n,(i-1):i) = A(1:n,(i-1):i)*rotMat;

            Z(:,(i-1):i) = Z(:,(i-1):i)*rotMat;
        end
    end
    uHessMat=A;
    uTriMat=B;
else
    %If Q and Z are not desired, don't compute them.

    %B=Q*R. Replace B with Q'*B and A with Q'*A
    [Q,R]=qr(B);
    B=R;%Q'*B=R.
    A=Q'*A;
    for j=1:(n-2)
        for i=n:-1:(j+2)
            %Zeros A(i,j)
            [c,s]=GivensCS(A(i-1,j),A(i,j));
            rotMat=[c, s;
                   -s, c];
            A((i-1):i,j:n)=rotMat'*A((i-1):i,j:n);
            B((i-1):i,(i-1):n)=rotMat'*B((i-1):i,(i-1):n);

            %Zeros B(i,i-1).
            [c,s]=GivensCS(-B(i,i),B(i,i-1));
            rotMat=[c, s;
                   -s, c];
            B(1:i,(i-1):i) = B(1:i,(i-1):i)*rotMat;
            A(1:n,(i-1):i) = A(1:n,(i-1):i)*rotMat;
        end
    end
    uHessMat=A;
    uTriMat=B;
end
end

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


function [f] = betad(z)
%BETAD Dirichlet Beta function
%
%usage: f = betad(z)
%
%tested on version 5.3.1
%
%      This program calculates the Dirichlet Beta function
%      for the elements of Z using the Dirichlet deta function.
%      Z may be complex and any size.
%
%      Note: this is NOT the beta function defined by
%            Gamma(x)*Gamma(y)/Gamma(x+y)
%
%      Has zeros for z=(-odd integers),
%      and infinite number of zeros for z=1/2+i*y
%
%
%see also: Zeta, Deta, Eta, Lambda, Bern, Euler

%Paul Godfrey
%pgodfrey@conexant.com
%3-24-01

f=deta(z,2);

return


function euler = qua2euler(qin)
% qua2euler: transforms quaternion to Euler angles.
%
% INPUT
%   qin: 4x1 quaternion.
%
% OUTPUT
%   euler: 3x1 Euler angles [roll pitch yaw] (rad, rad, rad).
%
%   Copyright (C) 2014, Rodrigo Gonzalez, all rights reserved.
%
%   This file is part of NaveGo, an open-source MATLAB toolbox for
%   simulation of integrated navigation systems.
%
%   NaveGo is free software: you can redistribute it and/or modify
%   it under the terms of the GNU Lesser General Public License (LGPL)
%   version 3 as published by the Free Software Foundation.000000000
%
%   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 Lesser General Public License for more details.
%
%   You should have received a copy of the GNU Lesser General Public
%   License along with this program. If not, see
%   <http://www.gnu.org/licenses/>.
%
% References:
%
%		Dr. Paolo Zoccarato's comments at
% https://github.com/rodralez/NaveGo/pull/9
%
%		Crassidis, J.L. and Junkins, J.L. (2011). Optimal Esti-
% mation of Dynamic Systems, 2nd Ed. Chapman and Hall/CRC, USA.
% Eq. 7.39, p. 458.
%
% Version: 005
% Date:    2017/12/05
% Author:  Rodrigo Gonzalez <rodralez@frm.utn.edu.ar>
% URL:     https://github.com/rodralez/navego

% Quaternion format from Crassidis' book.

DCMbn = qua2dcm(qin);

phi   = atan2( DCMbn(3,2), DCMbn(3,3) );    % roll
theta = asin (-DCMbn(3,1) );                % pitch
psi   = atan2( DCMbn(2,1), DCMbn(1,1) );    % yaw

euler = [phi theta psi];

end


function disk_integrals_test01 ( )

%*****************************************************************************80
%
%% DISK_INTEGRALS_TEST01 uses DISK01_SAMPLE to estimate various integrals.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 January 2014
%
%  Author:
%
%    John Burkardt
%
  m = 2;
  n = 4192;
  test_num = 20;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST01\n' );
  fprintf ( 1, '  Estimate monomial integrals using Monte Carlo\n' );
  fprintf ( 1, '  over the interior of the unit disk in 2D.\n' );
%
%  Get sample points.
%
  seed = 123456789;
  [ x, seed ] = disk01_sample ( n, seed );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of sample points used is %d\n', n );
%
%  Randomly choose X,Y exponents between 0 and 8.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  If any exponent is odd, the integral is zero.\n' );
  fprintf ( 1, '  We will restrict this test to randomly chosen even exponents.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Ex  Ey     MC-Estimate           Exact      Error\n' );
  fprintf ( 1, '\n' );

  for test = 1 : test_num

    [ e, seed ] = i4vec_uniform_ab ( m, 0, 4, seed );

    e(1:m) = e(1:m) * 2;

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

    result = disk01_area ( ) * sum ( value(1:n) ) / n;
    exact = disk01_monomial_integral ( e );
    error = abs ( result - exact );

    fprintf ( 1, '  %2d  %2d  %14.6g  %14.6g  %10.2e\n', ...
      e(1:m), result, exact, error );

  end

  return
end


function p = sigmoid(log_odds)
% SIGMOID: Inverse of the logit function.
%          This is a one-to-one mapping from log odds to probability.
%          i.e. it maps the real line to the interval (0,1).
%
%   p = sigmoid(log_odds)

assert(nargin==1)

p = 1 ./ (1 + exp(-log_odds));


function [qp]=zn3tr(input)
% [qp]=zn3tr(input)
% Ziegler-Nichols PID controller for processes of 3rd order.
% This function computes parameters of the controller (q0, q1, q2, p1, p2).
% Controller is based on trapezoidal method of discretization.
% Transfer function of the controller is as follows:
%
%            q0 + q1*z^-1 + q2*z^-2     q0 + q1*z^-1 + q2*z^-2
% G(z^-1) = ------------------------ = ------------------------
%                  1 - z^-1              1 + p1*z^-1 + p2*z^-2
%
% where p1=-1 and p2=0.
%
% Transfer function of the controlled system is:
%
%               b1*z^-1 + b2*z^-2 + b3*z^-3
% Gs(z^-1) = ---------------------------------
%             1 + a1*z^-1 + a2*z^-2 + a3*z^-3
%
% Input: input ... input parameters
%                  input(1) ... a1
%                  input(2) ... b1
%                  input(3) ... a2
%                  input(4) ... b2
%                  input(5) ... a3
%                  input(6) ... b3
%                  input(7) ... sample time T0
% Output: qp ... controller parameters
%                qp(1) ... q0
%                qp(2) ... q1
%                qp(3) ... q2
%                qp(4) ... p1 (-1)
%                qp(5) ... p2 (0)

a1 = input(1);
b1 = input(2);
a2 = input(3);
b2 = input(4);
a3 = input(5);
b3 = input(6);
T0 = input(7);

% compute ultimate gain and frequency
[Kpu, Tu] =  ultim([b1 b2 b3],[a1 a2 a3],T0);

Kp = 0.6*Kpu;
Ti = Tu/2;
Td = Tu/8;

q0 = Kp*(1 + T0/(2*Ti) + Td/T0);
q1 = -Kp*(1 - T0/(2*Ti) + 2*Td/T0);
q2 = Kp*(Td/T0);
p1 = -1;
p2 = 0;

qp=[q0; q1; q2; p1; p2];


%ANGDIFF Difference of two angles
%
% D = ANGDIFF(TH1, TH2) returns the difference between angles TH1 and TH2 on
% the circle.  The result is in the interval [-pi pi).  If TH1 is a column
% vector, and TH2 a scalar then return a column vector where TH2 is modulo
% subtracted from the corresponding elements of TH1.
%
% D = ANGDIFF(TH) returns the equivalent angle to TH in the interval [-pi pi).
%

% Copyright (C) 1993-2014, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB 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 Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

function d = angdiff(th1, th2)

    if nargin < 2
% THIS IS A BAD IDEA, WHERE IS IT USED?
%         if length(th1) > 1
%             d = th1(1) - th1(2);
%         else
%             d = th1;
%         end
        d = th1;
    else
        d = th1 - th2;
    end

    d = mod(d+pi, 2*pi) - pi;

% Simplistic version of the code, easy to see what it does, but slow...
%
% for very negative angles keep adding 2pi
%     while true
%         k = find(d < -pi);
%         if isempty(k)
%             break;
%         end
%         d(k) = d(k) + 2*pi;
%     end
%
%     % for very positive angles keep subtracting 2pi
%     while true
%         k = find(d > pi);
%         if isempty(k)
%             break;
%         end
%         d(k) = d(k) - 2*pi;
%     end


function prob_test039 ( )

%*****************************************************************************80
%
%% TEST039 tests COSINE_MEAN, COSINE_SAMPLE, COSINE_VARIANCE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 April 2009
%
%  Author:
%
%    John Burkardt
%
  nsample = 1000;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST039\n' );
  fprintf ( 1, '  For the Cosine PDF:\n' );
  fprintf ( 1, '  COSINE_MEAN computes the mean;\n' );
  fprintf ( 1, '  COSINE_SAMPLE samples;\n' );
  fprintf ( 1, '  COSINE_VARIANCE computes the variance.\n' );

  a = 2.0;
  b = 1.0;

  check = cosine_check ( a, b );

  if ( ~check );
    fprintf ( 1, '\n' );
    fprintf ( 1, 'TEST039 - Fatal error!\n' );
    fprintf ( 1, '  The parameters are not legal.\n' );
    return
  end

  mean = cosine_mean ( a, b );
  variance = cosine_variance ( a, b );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  PDF parameter A = %14f\n', a );
  fprintf ( 1, '  PDF parameter B = %14f\n', b );
  fprintf ( 1, '  PDF mean =        %14f\n', mean );
  fprintf ( 1, '  PDF variance =    %14f\n', variance );

  for i = 1 : nsample
    [ x(i), seed ] = cosine_sample ( a, b, seed );
  end

  mean = r8vec_mean ( nsample, x );
  variance = r8vec_variance ( nsample, x );
  xmax = max ( x(1:nsample) );
  xmin = min ( x(1:nsample) );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Sample size =     %6d\n', nsample );
  fprintf ( 1, '  Sample mean =     %14f\n', mean );
  fprintf ( 1, '  Sample variance = %14f\n', variance );
  fprintf ( 1, '  Sample maximum =  %14f\n', xmax );
  fprintf ( 1, '  Sample minimum =  %14f\n', xmin );

  return
end


%
% y = nnormn(x,dim,p)
%
% NNORMN normalizes an array x by its p-vector norms along dimension <dim>.
%
% dim: dimension along which to calculate norm.  Default first nonsingleton
%   p: norm-type.  Default is 2.
%
% Equivalence: normc(x) == nnormn(x,1,2), normr(x) == nnormn(x,2,2)
%
% See also NORMC, NORMR, NNORM

% Created by Bill Winter December 2005
% Based on normc and normr
function x = nnormn(x,dim,p)
siz = size(x);
if nargin < 2, dim = find(siz > 1,1); end
if nargin < 3, p = 2; end
switch p
    case inf,   a = max(x,[],dim);              % max
    case -inf,  a = min(x,[],dim);              % min
    case 1,     a = sum(abs(x),dim);            % manhattan
    case 2,     a = sqrt(sum(abs(x).^2,dim));   % euclidean
    otherwise,  a = sum(abs(x).^p,dim).^(1/p);  % p-norm
end
a(a == 0) = 1;
N(1:length(siz)) = {':'};
N{dim} = ones(1,siz(dim));
x = x./a(N{:});


function [X, W, mu_X] = prewhiten(X)
%PREWHITEN Performs prewhitening of a dataset X
%
%   [X, W, mu_X] = prewhiten(X)
%
% Performs prewhitening of the dataset X. Prewhitening concentrates the main
% variance in the data in a relatively small number of dimensions, and
% removes all first-order structure from the data. In other words, after
% the prewhitening, the covariance matrix of the data is the identity
% matrix. The function returns the subtracted data mean in mu_X, and the
% applied linear mapping in W.
%
%

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

    welcome;

    % Compute and apply the ZCA mapping
    mu_X = mean(X, 1);
    X = bsxfun(@minus, X, mu_X);
    mappedX = X / sqrtm(cov(X));
    if nargout > 1
        W = X \ mappedX;
    end


function X=RiccatiSolveC(A,B,Q,R,S,E)
%%RICCATISOLVEC Solve the general continuous-time Riccati equation.
%
%INPUTS: A An nXn matrix.
%        B An nXm matrix, where m<=n.
%        Q An nXn matrix such that Q=Q' and all eigenvalues are non-
%          negative.
%        R An optional mXm matrix such that R=R' and all eigenvalues are
%          non-negative. If omitted, eye(m,m) is used.
%        S An optional nXm matrix. If omitted, zeros(n,n) is used.
%        E An optional nXn matrix. If omitted, eye(n) is used.
%
%OUTPUTS: X The nXn nonnegative definite solution to the discrete-time
%           algebraic Ricatti equation.
%
%This function finds the nonnegative definite solution to the
%continuous-time Ricatti equation having the form
%A'*X*E+E'*X*A-(E'*X*B+S)*inv(R)*(B'*X*E+S')+Q=0
%or, if R, S, and E are omitted, the equation under consideration becomes
%A'*X+X*A-X*B*B'*X+Q=0
%The algorithm of  [1] is used. Note that this function does not work with
%problems of the form -X*B*B'*X+Q=0 due to numerical issues.
%
%The continuous-time Ricatti equation arises when solving for the  steady-
%state covariance of a continuous-time linear Kalman filter, as described
%in Chapter 9.2.3 of [2].
%
%REFERENCES:
%[1] W. F. Arnold III and A. J. Laub, "Generalized eigenproblem algorithms
%    and software for algebraic Riccati equations," Proceedings of the
%    IEEE, vol. 72, no. 12, pp. 1746-1754, Dec. 1984.
%[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.
%
%October 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

n=size(A,1);
m=size(B,2);

if(nargin<4)
    R=eye(m,m);
end

if(nargin<5)
    S=zeros(n,m);
end

if(nargin<6)
   E=eye(n);
end

L=[E,            zeros(n,n),zeros(n,m);
   zeros(n,n),   E',        zeros(n,m);
   zeros(m,n),   zeros(m,n),zeros(m,m)];
M=[A,   zeros(n,n),  B;
  -Q,  -A',         -S;
   S',  B',          R];

[MHat,LHat,V,U]=qz(M,L,'real');
[~,~,~,U]=ordqz(MHat,LHat,V,U,'lhp');

W=[E,           zeros(n,n),zeros(n,m);
   zeros(n,n),  eye(n),zeros(n,m)]*U;

W11=W(1:n,1:n);
W21=W((n+1):(2*n),1:n);

X=W21/W11;
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 ray = bisector(varargin)
%BISECTOR Return the bisector of two lines, or 3 points
%
%   RAY = bisector(LINE1, LINE2);
%   create the bisector of the two lines, given as [x0 y0 dx dy].
%
%   RAY = bisector(P1, P2, P3);
%   create the bisector of lines (P2 P1) and (P2 P3).
%
%   The result has the form [x0 y0 dx dy], with [x0 y0] being the origin
%   point ans [dx dy] being the direction vector, normalized to have unit
%   norm.
%
%   See also:
%   lines2d, rays2d
%
%   ---------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% created the 31/10/2003.
% Copyright 2010 INRA - Cepia Software Platform.

%   HISTORY
%   2005-07-07 add bisector of 3 points
%   2010-11-05 ode cleanup

if length(varargin)==2
    % two lines
    line1 = varargin{1};
    line2 = varargin{2};

    point = intersectLines(line1, line2);

elseif length(varargin)==3
    % three points
    p1 = varargin{1};
    p2 = varargin{2};
    p3 = varargin{3};

    line1 = createLine(p2, p1);
    line2 = createLine(p2, p3);
    point = p2;

elseif length(varargin)==1
    % three points, given in one array
    var = varargin{1};
    p1 = var(1, :);
    p2 = var(2, :);
    p3 = var(3, :);

    line1 = createLine(p2, p1);
    line2 = createLine(p2, p3);
    point = p2;
end

% compute line angles
a1 = lineAngle(line1);
a2 = lineAngle(line2);

% compute bisector angle (angle of first line + half angle between lines)
angle = mod(a1 + mod(a2-a1+2*pi, 2*pi)/2, pi*2);

% create the resulting ray
ray = [point cos(angle) sin(angle)];


% DERIVEST demo script

% This script file is designed to be used in cell mode
% from the matlab editor, or best of all, use the publish
% to HTML feature from the matlab editor. Older versions
% of matlab can copy and paste entire blocks of code into
% the Matlab command window.

% DERIVEST is property/value is driven for its arguments.
% Properties can be shortened to the

%% derivative of exp(x), at x == 0
[deriv,err] = derivest(@(x) exp(x),0)

%% DERIVEST can also use an inline function
[deriv,err] = derivest(inline('exp(x)'),0)

%% Higher order derivatives (second derivative)
% Truth: 0
[deriv,err] = derivest(@(x) sin(x),pi,'deriv',2)

%% Higher order derivatives (third derivative)
% Truth: 1
[deriv,err] = derivest(@(x) cos(x),pi/2,'der',3)

%% Higher order derivatives (up to the fourth derivative)
% Truth: sqrt(2)/2 = 0.707106781186548
[deriv,err] = derivest(@(x) sin(x),pi/4,'d',4)

%% Evaluate the indicated (default = first) derivative at multiple points
[deriv,err] = derivest(@(x) sin(x),linspace(0,2*pi,13))

%% Specify the step size (default stepsize = 0.1)
deriv = derivest(@(x) polyval(1:5,x),1,'deriv',4,'FixedStep',1)

%% Provide other parameters via an anonymous function
% At a minimizer of a function, its derivative should be
% essentially zero. So, first, find a local minima of a
% first kind bessel function of order nu.
nu = 0;
fun = @(t) besselj(nu,t);
fplot(fun,[0,10])
x0 = fminbnd(fun,0,10,optimset('TolX',1.e-15))
hold on
plot(x0,fun(x0),'ro')
hold off

deriv = derivest(fun,x0,'d',1)

%% The second derivative should be positive at a minimizer.
deriv = derivest(fun,x0,'d',2)

%% Compute the numerical gradient vector of a 2-d function
% Note: the gradient at this point should be [4 6]
fun = @(x,y) x.^2 + y.^2;
xy = [2 3];
gradvec = [derivest(@(x) fun(x,xy(2)),xy(1),'d',1), ...
           derivest(@(y) fun(xy(1),y),xy(2),'d',1)]

%% Compute the numerical Laplacian function of a 2-d function
% Note: The Laplacian of this function should be everywhere == 4
fun = @(x,y) x.^2 + y.^2;
xy = [2 3];
lapval = derivest(@(x) fun(x,xy(2)),xy(1),'d',2) + ...
           derivest(@(y) fun(xy(1),y),xy(2),'d',2)

%% Compute the derivative of a function using a central difference scheme
% Sometimes you may not want your function to be evaluated
% above or below a given point. A 'central' difference scheme will
% look in both directions equally.
[deriv,err] = derivest(@(x) sinh(x),0,'Style','central')

%% Compute the derivative of a function using a forward difference scheme
% But a forward scheme will only look above x0.
[deriv,err] = derivest(@(x) sinh(x),0,'Style','forward')

%% Compute the derivative of a function using a backward difference scheme
% And a backward scheme will only look below x0.
[deriv,err] = derivest(@(x) sinh(x),0,'Style','backward')

%% Although a central rule may put some samples in the wrong places, it may still succeed
[d,e,del]=derivest(@(x) log(x),.001,'style','central')

%% But forcing the use of a one-sided rule may be smart anyway
[d,e,del]=derivest(@(x) log(x),.001,'style','forward')

%% Control the behavior of DERIVEST - forward 2nd order method, with only 1 Romberg term
% Compute the first derivative, also return the final stepsize chosen
[deriv,err,fdelta] = derivest(@(x) tan(x),pi,'deriv',1,'Style','for','MethodOrder',2,'RombergTerms',1)

%% Functions should be vectorized for speed, but its not always easy to do.
[deriv,err] = derivest(@(x) x.^2,0:5,'deriv',1)
[deriv,err] = derivest(@(x) x^2,0:5,'deriv',1,'vectorized','no')