function [U S V info] = truncated_svd(A, p)
% Returns an SVD decomposition of A truncated to rank p.
%
% function [U S V] = truncated_svd(A, p)
%
% Input: A real matrix A of size mxn and an integer p <= min(m, n).
% Output: An orthonormal matrix U of size mxp, an orthonormal matrix Y of
%         size nxp and a diagonal matrix S of size pxp with nonnegative and
%         decreasing diagonal entries such that USV.' is the best rank p
%         approximation of A according to the Frobenius norm. All real.
%         This function produces an output akin to svds.
%
% The decomposition is obtained by maximizing
%   f(U, V) = .5*norm(U'*A*V, 'fro')^2
% where U, V are orthonormal. Notice that f(U*Q, V*R) = f(U, V) for all
% Q, R orthogonal pxp matrices. Hence, only the column spaces of U and V
% matter and we may perform the optimization over a product of two
% Grassmannian manifolds.
%
% It is easy to show that maximizing f is equivalent to minimizing g with
%   g(U, V) = min_S norm(U*S*V' - A, 'fro')^2,
% which confirms that we are going for a best low-rank approximation of A.
%
% The inner workings of the Grassmann manifold use the built-in svd
% function of Matlab but only for matrices of size mxp and nxp to
% re-orthonormalize them.
%
% Notice that we are actually chasing a best fixed-rank approximation of a
% matrix, which is best obtained by working directly over a manifold of
% fixed-rank matrices. This is simply an example script to demonstrate some
% functionalities of the toolbox.
%
% The code can be modified to accept a function handle for A(x) = A*x
% instead of a matrix A, which is often useful. This would further require
% a function handle At for the transpose of A, such that At(x) = A.'*x.

% This file is part of Manopt and is copyrighted. See the license file.
%
% Main author: Nicolas Boumal, July 5, 2013
% Contributors:
%
% Change log:
%

    % Generate some random data to test the function if none is given.
    if ~exist('A', 'var') || isempty(A)
        A = randn(42, 60);
    end
    if ~exist('p', 'var') || isempty(p)
        p = 5;
    end

    % Retrieve the size of the problem and make sure the requested
    % approximation rank is at most the maximum possible rank.
    [m n] = size(A);
    assert(p <= min(m, n), 'p must be smaller than the smallest dimension of A.');

    % Define the cost and its derivatives on the Grassmann manifold
    tuple.U = grassmannfactory(m, p);
    tuple.V = grassmannfactory(n, p);
    % All of the code will work just as well if we ignore the invariance
    % property of the cost function indicated above and thus place U and V
    % on the Stiefel manifold (orthonormal matrices) instead of the
    % Grassmann manifold. Working on Stiefel is expected to be slower
    % though, partly because de search space is higher dimensional and
    % partly because the optimizers are not isolated.
    % tuple.U = stiefelfactory(m, p);
    % tuple.V = stiefelfactory(n, p);
    M = productmanifold(tuple);

    % Define a problem structure, specifying the manifold M, the cost
    % function and its derivatives. Here, to demonstrate the rapid
    % prototyping capabilities of Manopt, we directly define the Euclidean
    % gradient and the Euclidean Hessian egrad and ehess instead of the
    % Riemannian gradient and Hessian grad and hess. Manopt will take care
    % of the conversion. This automatic conversion is usually not
    % computationally optimal though, because much of the computations
    % involved in obtaining the gradient could be reused to obtain the
    % Hessian. After the prototyping stage, when efficiency becomes
    % important, it makes sense to define grad and hess rather than egrad
    % an ehess, and to use the caching system (the store structure).
    problem.M = M;
    problem.cost  = @cost;
    problem.egrad = @egrad;
    problem.ehess = @ehess;

    % The functions below make many redundant computations. This
    % performance hit can be alleviated by using the caching system.

    % Cost function
    function f = cost(X)
        U = X.U;
        V = X.V;
        f = -.5*norm(U'*A*V, 'fro')^2;
    end
    % Euclidean gradient of the cost function
    function g = egrad(X)
        U = X.U;
        V = X.V;
        AV = A*V;
        AtU = A'*U;
        g.U = -AV*(AV'*U);
        g.V = -AtU*(AtU'*V);
    end
    % Euclidean Hessian of the cost function
    function h = ehess(X, H)
        U = X.U;
        V = X.V;
        Udot = H.U;
        Vdot = H.V;
        AV = A*V;
        AtU = A'*U;
        AVdot = A*Vdot;
        AtUdot = A'*Udot;
        h.U = -(AVdot*AV'*U + AV*AVdot'*U + AV*AV'*Udot);
        h.V = -(AtUdot*AtU'*V + AtU*AtUdot'*V + AtU*AtU'*Vdot);
    end

    % Execute some checks on the derivatives for early debugging.
    % These things can be commented out of course.
    checkgradient(problem);
    pause;
    checkhessian(problem);
    pause;

    % Issue a call to a solver. A random initial guess will be chosen and
    % default options are selected. Here, we specify a maximum trust
    % region radius (which in turn induces an initial trust region radius).
    % Note that this is not required: default values are used if we omit
    % this. The diameter of the manifold scales like sqrt(2*p), hence the
    % form of our (empirical) choice.
    options.Delta_bar = 4*sqrt(2*p);
    [X Xcost info] = trustregions(problem, [], options); %#ok<ASGLU>
    U = X.U;
    V = X.V;

    % Finish the job by rotating U and V such that the middle matrix S can
    % be diagonal with nonnegative, decreasing entries. This requires a
    % small svd of size pxp.
    Spp = U'*A*V;
    [Upp Spp Vpp] = svd(Spp);
    U = U*Upp;
    S = Spp;
    V = V*Vpp;

    % For our information, Manopt can also compute the spectrum of the
    % Riemannian Hessian on the tangent space at (any) X. Computing the
    % spectrum at the solution gives us some idea of the conditioning of
    % the problem. If we were to implement a preconditioner for the
    % Hessian, this would also inform us on its performance.
    %
    % Notice that if the optimization is performed on a product of Stiefel
    % manifolds instead of a product of Grassmannians, the double
    % invariance under the orthogonal group O(p) will appear as twice
    % p*(p-1)/2, thus p*(p-1) zero eigenvalues in the spectrum of the
    % Hessian. This means that the minimizers are not isolated, which
    % typically hinders convergence of second order algorithms.
    if M.dim() < 512
        evs = hessianspectrum(problem, X);
        stairs(sort(evs));
        title(['Eigenvalues of the Hessian of the cost function ' ...
               'at the solution']);
    end

end


function variance = chi_square_noncentral_variance ( a, b )

%*****************************************************************************80
%
%% CHI_SQUARE_NONCENTRAL_VARIANCE returns the variance of the noncentral Chi squared PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, the parameter of the PDF.
%    1 <= A.
%
%    Input, real B, the noncentrality parameter of the PDF.
%    0.0 <= B.
%
%    Output, real VARIANCE, the variance value.
%
  variance = 2.0 * ( a + 2.0 * b );

  return
end


% DIAGPROD - Computes the diagonal elements of a matrix product.
%
% D = DIAGPROD(A,B)
%
% Evaluates diag(A*B') efficiently. A and B must have the same size.
%
% D = DIAGPROD(A)
%
% Evaluates diag(A*A') efficiently.

% Last modified 2010-06-24
% Copyright (c) Jaakko Luttinen (jaakko.luttinen@tkk.fi)

function d = diagprod(A,B)

if nargin < 2
  d = dot(A,A,2);
else
  d = dot(A,B,2);
end


function variance = gumbel_variance ( )

%*****************************************************************************80
%
%% GUMBEL_VARIANCE returns the variance of the Gumbel PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real VARIANCE, the variance of the PDF.
%
  variance = pi * pi / 6.0;

  return
end


function [v,g] = ctransform(x, u, y, p, epsilon)

% ctransform - c-transform and gradient of semi-discrete energy
%
%   [v,g] = ctransform(x,u, y, epsilon);
%
%   u is sampled at points x.
%   v is u^c sampled at points y.
%
%   x must be of size (dim,n)
%   y must be of size (dim,m)
%
% If epsilon==0
%   u^c(y) = min_x  |x-y|^p-u(x)
% else
%   u^c(y) = - epsilon * log( sum_x  exp( (-|x-y|^p + u(x))/epsilon )
%
%   The semi discrete energy between continuous measure a and discrete
%   measure b is
%       E(u) = int u^c(y) da(y) - <u,b>
%   g(:,j) is the gradient of u -> u^c(y_j).
%
%   Copyright (c) 2017 Gabriel Peyre

if nargin<5
    epsilon = 0;
end

n = size(x,2);
m = size(y,2);

c = distmat(x,y).^p; % cost
S = c -  repmat(u(:), [1 m]);
[v,I] = min(S);
if epsilon>0
    % use soft-min, stabilized
    if 1
        S = S - repmat(v, [size(S,1) 1]);
        v = - epsilon * log( sum(  exp( -S/epsilon ) ) ) + v;
    else
        v = - epsilon * log( sum(  exp( -S/epsilon ) ) );
    end
end
v = v(:);

if nargout>1
    % compute also the gradient
    if epsilon==0
        % simply indicator function of the Laguerre cells, indicated by the index I
        g = sparse( I, 1:m, ones(m,1),n, m );
    else
        % smoothed-indicator
        g = exp( -S/epsilon );
        g = g ./ repmat( sum(g), [n 1] );
    end

end

end


function h = p41_h ( n, x )

%*****************************************************************************80
%
%% P41_H evaluates the Hessian for problem 41.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 October 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of variables.
%
%    Input, real X(N), the values of the variables.
%
%    Output, real H(N,N), the N by N Hessian matrix.
%
  h = zeros ( n, n );

  h(1,1) = 1200.0 * x(1)^2 - 400.0 * x(2) + 2.0;
  h(1,2) = - 400.0 * x(1);

  h(2,1) = -400.0 * x(1);
  h(2,2) = 220.2;
  h(2,4) = 19.8;

  h(3,3) = -360.0 * x(4) + 1080.0 * x(3)^2 + 2.0;
  h(3,4) = - 360.0 * x(3);

  h(4,2) = 19.8;
  h(4,3) = - 360.0 * x(3);
  h(4,4) = 200.2;

  return
end


function [mse, rmse] = RMSE2(signal1, signal2)

originalRowSize = size(signal1,1);
originalColSize = size(signal1,2);

signal1 = signal1(:);
signal2 = signal2(:);

mse = sum((signal1 - signal2).^2)./(originalRowSize*originalColSize);
rmse = sqrt(mse);

end


function rms_x=rms(x)

%Calculates RMS for x, NaN's are ignored
x=x(~isnan(x)); %Removing nans
rms_x=sqrt( sum(x.^2)./numel(x) );

end

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% 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 3 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, see <http://www.gnu.org/licenses/>.


function [beta_gibbs,sigma_gibbs]=ndgibbs(It,Bu,beta0,omega0,X,Y,y,Bhat,n,T,q)

% function [beta_gibbs sigma_gibbs]=ndgibbs(It,Bu,beta0,omega0,X,Y,y,Bhat,n,T,q)
% performs the Gibbs algorithm 1.5.2 for the normal-diffuse prior, and returns draws from posterior distribution
% inputs:  - integer 'It': total number of iterations of the Gibbs sampler (defined p 28 of technical guide)
%          - integer 'Bu': number of burn-in iterations of the Gibbs sampler (defined p 28 of technical guide)
%          - vector 'beta0': vector of prior values for beta (defined in 1.3.4)
%          - matrix 'omega0': prior covariance matrix for the VAR coefficients (defined in 1.3.8)
%          - matrix 'X': matrix of regressors for the VAR model (defined in 1.1.8)
%          - matrix 'Y': matrix of regressands for the VAR model (defined in 1.1.8)
%          - vector 'y': vectorised regressands for the VAR model (defined in 1.1.12)
%          - matrix 'Bhat': OLS VAR coefficients, in non vectorised form (defined in 1.1.9)
%          - integer 'n': number of endogenous variables in the BVAR model (defined p 7 of technical guide)
%          - integer 'T': number of sample time periods (defined p 7 of technical guide)
%          - integer 'q': total number of coefficients to estimate for the BVAR model (defined p 7 of technical guide)
% outputs: - matrix 'beta_gibbs': record of the gibbs sampler draws for the beta vector
%          - matrix'sigma_gibbs': record of the gibbs sampler draws for the sigma matrix (vectorised)

% preliminary tasks

% invert omega0, as it will be used repeatedly during step 4
invomega0=diag(1./diag(omega0));

% set initial values for B (step 2); use OLS estimates
B=Bhat;

% create the progress bar
hbar = bear.parfor_progressbar(It,'Progress of the Gibbs sampler.');

% start iterations
for ii=1:It

% Step 3: at iteration ii, first draw sigma from IW, conditional on beta from previous iteration
% obtain first Shat, defined in (1.6.10)
Shat=(Y-X*B)'*(Y-X*B);
% Correct potential asymmetries due to rounding errors from Matlab
C=chol(bear.nspd(Shat));
Shat=C'*C;

% next draw from IW(Shat,T)
sigma=bear.iwdraw(Shat,T);

% step 4: with sigma drawn, continue iteration ii by drawing beta from a multivariate Normal, conditional on sigma obtained in current iteration
% first invert sigma
C=chol(bear.nspd(sigma));
invC=C\speye(n);
invsigma=invC*invC';

% then obtain the omegabar matrix
invomegabar=invomega0+kron(invsigma,X'*X);
C=chol(bear.nspd(invomegabar));
invC=C\speye(q);
omegabar=invC*invC';

% following, obtain betabar
betabar=omegabar*(invomega0*beta0+kron(invsigma,X')*y);

% draw from N(betabar,omegabar);
beta=betabar+chol(bear.nspd(omegabar),'lower')*mvnrnd(zeros(q,1),eye(q))';

% update matrix B with each draw
B=reshape(beta,size(B));

% update progress by one iteration
hbar.iterate(1);

% record the values if the number of burn-in iterations is exceeded
if ii>Bu
% values of vector beta
beta_gibbs(:,ii-Bu)=beta;
% values of sigma (in vectorized form)
sigma_gibbs(:,ii-Bu)=sigma(:);
% if current iteration is still a burn iteration, do not record the result
else
end

% go for next iteration
end
% close progress bar
close(hbar);


function [mWalkout, fR95, fProbR95, PHI, R] = calc_Schusterwalk(mCatalog)
% function  [mWalkout, fR95, fProbR95, PHI, R] = calc_Schusterwalk(mCatalog)
% ------------------------------------------------------------------------
% Calculate random walkout (Schuster's method). See Rydelek & Sacks, Nature
% 1989. Shows phazor plot for one catalog.
%
% Incoming variables:
% mCatalog : EQ catalog
%
% Outgoing variables:
% mWalkout : Random walkout phases
% fR95     : Critical radius at 95% level
% fProbR95 : Probability to obtain vector of length >= R
% PHI      : Sum of phase angles
% R        : Possible sum of vector length
%
% J. Woessner, woessner@seismo.ifg.ethz.ch
% last update: 12.02.03

% Get hours and minutes, calculate minutes
vH = mCatalog(:,8);
vMin = mCatalog(:,9);
vTime = vH*60+vMin;

% Calculate degrees and radians
vThetaDegree = vTime*180/720;
vThetaRad = deg2rad(vThetaDegree);

mWalkout(1,1)=0;
mWalkout(1,2)=0;
for i=1:length(mCatalog(:,8))
    mWalkout(i+1,1)=mWalkout(i,1)+sin(vThetaRad(i));
    mWalkout(i+1,2)=mWalkout(i,2)+cos(vThetaRad(i));
end

A=sum(sin(vThetaRad));
B=sum(cos(vThetaRad));
R=sqrt(A^2+B^2);
PHI=atan(B/A);
if mWalkout(length(mCatalog(:,8)),2)<0
    PHI=PHI+pi;
end

N=length(mCatalog(:,8));
% Critical radius at 95% level
fR95=1.37*sqrt(N); %Nature 1989
% Probability to obtain vector of length >= R
fProbR95=exp(-R^2/N);

figure_w_normalized_uicontrolunits('tag','schuster')
% Plot Schuster walkout
plot(mWalkout(:,1),mWalkout(:,2));
hold on;
polar([0:360]*pi/180,ones(1,361)*fR95,'r');
[x, y] =pol2cart(PHI,R);
plot([0,x],[0 y],'g');
plot ([0 0],[-1/5*fR95,1/5*fR95],'k');
plot ([-1/5*fR95,1/5*fR95],[0 0],'k');
hold off
text (0,2/5*fR95,'0:00','HorizontalAlignment','center');
text (2/5*fR95,0,'6:00','HorizontalAlignment','left');
text (0,-2/5*fR95,'12:00','HorizontalAlignment','center');
text (-2/5*fR95,0,'18:00','HorizontalAlignment','right');
text (0,fR95 ,'95KI','VerticalAlignment','bottom','HorizontalAlignment','center');
axis equal;
%xlim([-15 15])
%ylim([-15 15])
text (-fR95,-fR95,num2str(length(mCatalog(:,8))));

figure_w_normalized_uicontrolunits('tag','schuster2')
% Plot hourly histogram
subplot(2,1,1);
vTime = (mCatalog(:,8)*60+mCatalog(:,9))/60; % Calculate decimal hour
histogram(vTime,0:1:24)
% Plot FMD
subplot(2,1,2);
[mFMDC, mFMD] = calc_FMD(mCatalog);
plot(mFMDC(1,:),mFMDC(2,:),'ks',mFMD(1,:),mFMD(2,:),'k^');


% Data File DTXY3
% Motion of a particle on a horizontal
% plane under action of elastic and
% resistance forces and force of
% dry friction
 m    =  'm';   % mass of the particle
        % Projections of forces on axis x and y
 Fx   = '-c*x-k*xt - mu*m*9.81*xt/sqrt(xt^2+yt^2)';
 Fy   = '-c*y-k*yt - mu*m*9.81*yt/sqrt(xt^2+yt^2)';
 x0   = '1';    % Initial
 y0   = '0';    % coordinates
 v0   = 'v0';   % Initial velocity
 alfa = 'alfa'; % angle between v0 and axis x
 Tend = 7;      % upper bound of integration
 eps  = 1e-10;  % desirable accuracy
 np   = 4;      % number of parameters
 P{1} = 'm';    % mass of the particle
 P{2} = 'c';    % spring stiffness
 P{3} = 'k';    % coefficient of resistance
 P{4} = 'mu';   % coefficient of dry friction


function d = sine_taper_scaled(n, k)

% Compute Riedel & Sidorenko sine tapers.
% sine_taper_scaled(n, k) produces the first 2*k tapers of length n,
% returned as the columns of d. The norm of the tapers will not be 1. The
% norm is a function of the number of the taper in the sequence. This is to
% mimick behavior of the scaling of the resulting powerspectra prior to
% april 29, 2011. Before april 29, 2011, equivalent scaling was applied to
% the powerspectra of the tapered data segments, prior to averaging.

% Copyright (C) 2006, Tom Holroyd
%
% This file is part of FieldTrip, see http://www.fieldtriptoolbox.org
% for the documentation and details.
%
%    FieldTrip is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.
%
%    FieldTrip 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 FieldTrip. If not, see <http://www.gnu.org/licenses/>.
%
% $Id$

if nargin < 2
  ft_error('usage: sine_taper_scaled(n, k)');
end

k = round(k * 2);
if k <= 0 || k > n
  ft_error('sine_taper_scaled: k is %g, must be in (1:n)/2', k)
end

x = (1:k) .* (pi / (n + 1));
d = sqrt(2 / (n + 1)) .* sin((1:n)' * x);

scale = zeros(k);
for i = 1:k
  scale(i,i) = sqrt(1 - ((i-1)./(k-1)).^2);
end
d = d * scale;

return


function qUnNormalised = unnormaliseImagePoints(qNormalised,K,kc)
%function to unnormalise a set of image points according to the perspective
%intrinsic calibration matrix K and distortion parameters kc. This is the model
%from Oulu university and described clearly in Bouguet's Matlab camera calibration toolbox
% (http://www.vision.caltech.edu/bouguetj/calib_doc/). This is done using
% code from Bouguet's toolbox.
%
%inputs:
%q: a 2*N matrix of normalised points in an image.
%
%K: a 3x3 perspective intrinsic calibration matrix
%
%kc: a 5x1 vector of radial and tangential distortion coefficients.
%
%outputs:
%qUnNormalised: the un-normalised points

%first compensate for radial and tangential distortion:
xn = qNormalised;
x = xn(1,:);
y = xn(2,:);

r = sqrt(xn(1,:).^2 + xn(2,:).^2);

dx(1,:) = 2*kc(3)*x.*y + kc(4)*(r.^2 + 2*x.^2);
dx(2,:) = kc(3)*(r.^2 + 2*y.^2) + 2*kc(4)*x.*y;

s = (1+kc(1)*r.^2 + kc(2)*r.^4 + kc(5)*r.^6);
xd = [s;s].*xn + dx;
xd(3,:) = 1;

%now apply the intrinsic matrix
qUnNormalised = K*xd;
qUnNormalised(1,:) = qUnNormalised(1,:)./qUnNormalised(3,:);
qUnNormalised(2,:) = qUnNormalised(2,:)./qUnNormalised(3,:);
qUnNormalised = qUnNormalised(1:2,:);


function [z phi]=sim_rbf(Xc,X,W,k_i,basisfunction)
%simulates a radial basis function
%Xc is a N_r by N_dim matrix of rbf centres
%X is a N_p by N_dim matrix of the points to simulate
%W is the weight vector
%basisfunction may be 'gaussian' or 'polyharmonicspline'
%k_i is a prescaler for 'gaussian' rbf and function order for
%'polyharmonicspline'. See k_i(i)=0 for constant bias

%    Copyright Travis Wiens 2008
%
%    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 3 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, see <http://www.gnu.org/licenses/>.
%
%    If you would like to request that this software be licensed under a less
%    restrictive license (i.e. for commercial closed-source use) please
%    contact Travis at travis.mlfx@nutaksas.com

if nargin<4
    k_i=1;
end

if nargin<5
    basisfunction='gaussian';
end

N_r=size(Xc,1);%number of rbf centres
N_p=size(X,1);%number of points

if numel(k_i)==1
    k_i=k_i*ones(N_r);
end

phi=zeros(N_p,N_r);%rbf outputs

for i=1:N_r
    if k_i(i)==0
        phi(:,i)=1;
    else
        r=sqrt(sum((repmat(Xc(i,:),N_p,1)-X(:,:)).^2,2));%distance from point Xc to X

        switch basisfunction
            case {'gaussian','Gaussian'}
                phi(:,i)=exp(-k_i(i).*r.^2);
            case {'phs','polyharmonicspline'}
                if r==0
                    phi(:,i)=0;
                else
                    if round(k_i(i)/2)==k_i(i)/2%even
                        phi(:,i)=r.^k_i(i).*log(r);
                        phi(r==0,i)=0;%avoid log(0)
                    else
                        phi(:,i)=r.^k_i(i);
                    end
                end
            otherwise
                error('unknown basis function')
        end
    end
end

z=phi*W;


function [mappedX, mapping] = landmark_isomap(X, no_dims, k, percentage)
%ISOMAP Runs the Isomap algorithm
%
%   [mappedX, mapping] = landmark_isomap(X, no_dims, k, percentage);
%
% The functions runs the Landmark Isomap algorithm on dataset X to reduce the
% dimensionality of the dataset to no_dims. The number of neighbors used in
% the compuations is set by k (default = 12). The variable percentage has to be
% between 0 and 1, and determines the number of landmarks that is used (default = 0.1).
%
% If the neighborhood graph that is constructed is not completely
% connected, only the largest connected component is embedded. The indices
% of this component are returned in mapping.conn_comp.
%
%

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

    if ~exist('no_dims', 'var')
        no_dims = 2;
    end
    if ~exist('k', 'var')
        k = 12;
    end
	if ~exist('percentage', 'var')
		percentage = 0.2;
    end

    % Construct neighborhood graph
    disp('Constructing neighborhood graph...');
    D = find_nn(X, k);

    % Select largest connected component
    blocks = components(D)';
    count = zeros(1, max(blocks));
    for i=1:max(blocks)
        count(i) = length(find(blocks == i));
    end
    [count, block_no] = max(count);
    conn_comp = find(blocks == block_no);
    D = D(conn_comp, conn_comp);
    mapping.D = D;
    n = size(D, 1);

    % Compute shortest paths
    disp('Computing shortest paths...');
    landmarks = randperm(n);
	landmarks = landmarks(1:round(percentage * n));
    nl = length(landmarks);
    D = dijkstra(D, landmarks);
	D = full(D)';
    mapping.DD = D;
    mapping.landmarks = landmarks;

	% Do not embed in more dimensions than (nl - 1)
	if no_dims > nl - 1
		no_dims = nl - 1;
		warning(['Target dimensionality reduced to ' num2str(no_dims) '...']);
	end

    % Performing MDS using eigenvector implementation
    disp('Constructing low-dimensional embedding...');
    D = D .^ 2;
    subB = -.5 .* (bsxfun(@minus, bsxfun(@minus, D, sum(D, 2) ./ nl), sum(D, 1) ./ n) + sum(D(:)) ./ (n * nl));
    subB2 = subB' * subB;
	subB2(isnan(subB2)) = 0;
	subB2(isinf(subB2)) = 0;
    [alpha, beta] = eig(subB2);
    val = beta .^ (1 / 2);
    mapping.invVal = inv(val);
    vec = subB * alpha * mapping.invVal;
	if size(vec, 2) < no_dims
		no_dims = size(vec, 2);
		warning(['Target dimensionality reduced to ' num2str(no_dims) '...']);
	end

    % Computing final embedding
    [val, ind] = sort(real(diag(val)), 'descend');
    vec = vec(:,ind(1:no_dims));
    val = val(1:no_dims);
    mappedX = real(bsxfun(@times, vec, sqrt(val)'));

    % Store data for out-of-sample extension
    mapping.conn_comp = conn_comp;
    mapping.k = k;
    mapping.X = X(conn_comp,:);
    mapping.alpha = alpha;
    mapping.beta = beta;
    mapping.no_dims = no_dims;


function [p,f,p1,f1]=optimitaestep(n)
%OPTIMITAESTEP Calculate the optimal ITAE trasnfer function coefficient for a step input
%
% Usage:
% [p,itae,p0,itae0] = optimitaestep(n) returns the optimal coefficients of a
% nth-order transfer function based on the ITAE criterion for a step input.
% Input:  n - the order of transfer function
% Output: p - the optimal coefficients of the nth-order transfer function
%         itae - the optimal ITAE criterion.
%         p0 - original coefficients derivided by Graham and Lathrop.
%         itae0 - the ITAE citerion correcponding to p0.
%
% Background:
% The original ITAE (stands for Integral of Time multiplied by Absolute
% Error) coefficient table was drived by D. Graham and R.C. Lathrop through
% an analog computer. The table has been widely adopted as a standard in
% most undergraduate level control engineering text books. However, the
% optimality of these coefficients has been questioned by several researchers.
% This code provides a means to re-calculate these coefficient using more
% advanced numerical optimization techniques in digital computers.
%
% References:
% 1. D. Graham and R.C. Lathrop, "The Synthesis of Optimum Response: Criteria
%    ans Standard Forms, Part 2", Transactions of the AIEE 72, Nov. 1953, pp. 273-288
% 2. Y. Cao, "Correcting the minimum ITAE standard forms of
%    zero-displaceemnt-error systems", Journal of Zhejiang University
%    (Natural Science) Vol. 23, N0o.4, pp. 550-559, 1989.
%
% Example, the 7th-order optimal and original ITAE coefficients
%{
format compact
format short g
[p,f,p1,f1] = optimitaestep(7)
%}
% p =
%    1  2.2172  6.7447  9.3493  11.58   8.6799  4.3233  1
% f =
%    10.585
% p1 =
%    1  4.475   10.42   15.08   15.54   10.64   4.58    1
% f1 =
%    14.953
%

%Input check
error(nargchk(1,2,nargin));
if n<2 || n>8
    error('n must be within range of [2 8].')
end
itaeOrg={ 1.4, [1.75 2.15], [2.1 3.4 2.7],...
       [2.8 5.0 5.5 3.4], [3.25 6.60 8.60 7.45 3.95],...
       [4.475 10.42 15.08 15.54 10.64 4.58],...
       [5.2 12.8 21.6 25.75 22.2 13.3 5.15]};
p1=itaeOrg{n-1};
p0=fliplr(p1);
opt=optimset('display','off','TolX',1e-9,'TolFun',1e-9,'LargeScale','off');
dt=0.01;
tf=50;
flag=0;
while ~flag
    cost=@(x)itaecost(x,dt,tf);
    [p0,f,flag]=fminunc(cost,p0,opt);
    p=[1 fliplr(p0) 1];
    if any(real(roots(p))>0)
        tf=tf*2;
        flag=0;
        p0=fliplr(itaeOrg{n-1});
    end
end
if nargout>2
    f1=cost(fliplr(p1));
    p1=[1 p1 1];
end

function E=itaecost(p,dt,tf)
n=numel(p);
A=[zeros(n,1) eye(n);-1 -p];
B=[zeros(n,1);1];
A=expm([A B;zeros(1,n+2)]*dt);
x=[zeros(n+1,1);1];
E=0;
for t=0:dt:tf
    tdt=t*dt;
    x=A*x;
    e=1-x(1);
    E=E+abs(e)*tdt;
end


function [fTmax, mRes] = calc_ChTime(mCatalog, fTimeWindow)
% [fTmax, mRes] = calc_ChTime(mCatalog, fTimeWindow)
% -------------------------------------------------------
% Function to identify times of maximum changes of seismicity in time using
% the absolute difference of the non-cumulative FMD in successive time windows
%
% Incoming:
% mCatalog    : EQ catalog
% fTimeWindow : Length of moving time window in dec. years
%
% Outgoing variable:
% fTmax : Time of maximum change
% mRes : Matrix containing time and absoulte changes
%
% J. Woessner
% last update: 21.01.2004

mRes = [];

% Initialize
fMinTime = min(mCatalog(:,3));
fMaxTime = max(mCatalog(:,3));
fTimePeriod = fMaxTime-fMinTime;

% Minimum bin
fMinBin = roundn(min(mCatalog(:,6)),-1);
fMaxBin = roundn(max(mCatalog(:,6)),-1);

% Seismicity of catalog normalized to time period
nNumevents = max(length(mCatalog(:,1)));
fNum = nNumevents/fTimePeriod;

% Starttime
fTime = fMinTime;

while fTime < fMaxTime-2*fTimeWindow
    vSel = (mCatalog(:,3) >= fTime & mCatalog(:,3) < fTime+fTimeWindow);
    nNumper = max(length(mCatalog(vSel,1)));
    fNumper = nNumper/fTimeWindow;
    fChTime = fNumper/fNum; % Relative change of entire data

    % Change in FMD
    [vFMD1, vBin1] = hist(mCatalog(vSel,6),fMinBin:0.1:fMaxBin);
    vSel2 = (mCatalog(:,3) >= fTime+fTimeWindow & mCatalog(:,3) < fTime+2*fTimeWindow);
    [vFMD2, vBin2] = hist(mCatalog(vSel2,6),fMinBin:0.1:fMaxBin);
    fChFMD = max(cumsum(abs(vFMD2-vFMD1)));

    mRes = [mRes; fTime fNumper fChTime fChFMD];

    % Shift time by half window width
    fTime = fTime+0.1; %fTimeWindow/10;
end

% Maximum positive change
vSel = (mRes(:,4) == max(mRes(:,4)));
mMax = mRes(vSel,:);
fTmax = mRes(vSel,1);

figure
hPlot1=plot(mRes(:,1),mRes(:,4)/mean(mRes(:,4)),'Color',[0 0 0],'Linewidth',2)
% sTitle = ['Time of maximum change: ' num2str(fTmax)]
% hTit=title(sTitle)
xlabel('Time [dec. year]','FontSize',12,'Fontweight','bold')
ylabel('Normalized \Delta_{FMD}','FontSize',12,'Fontweight','bold')
set(gca,'FontSize',12,'Fontweight','bold','Linewidth',2)
%set(hTit,'FontSize',12,'Fontweight','bold')


function [LS,LD,I] = isolines(V,F,S,iso)
  % ISOLINES compute a list of isolines for a scalar field S defined on the
  % mesh (V,F)
  %
  % [LS,LD,I] = isolines(V,F,S,iso)
  %
  % Inputs:
  %   V  #V by dim list of vertex positions
  %   F  #F by 3 list of triangle indices
  %   S  #V list of scalar values defined on V
  %   iso #iso list of iso values
  % Outputs:
  %   LS  #L by dim list of isoline start positions
  %   LD  #L by dim list of isoline destination positions
  %   I  #L list of indices into iso revealing corresponding iso value
  %
  % alternative: tracing isolines should result in smaller plot data (every
  % vertex only appears once
  %

  % make sure iso is a ROW vector
  iso = iso(:)';
  % number of isolines
  niso = numel(iso);

  % number of domain positions
  n = size(V,1);
  % number of dimensions
  dim = size(V,2);

  % number of faces
  m = size(F,1);

  % Rename for convenience
  S1 = S(F(:,1),:);
  S2 = S(F(:,2),:);
  S3 = S(F(:,3),:);

  % t12(i,j) reveals parameter t where isovalue j falls on the line from
  % corner 1 to corner 2 of face i
  t12 = bsxfun(@rdivide,bsxfun(@minus,iso,S1),S2-S1);
  t23 = bsxfun(@rdivide,bsxfun(@minus,iso,S2),S3-S2);
  t31 = bsxfun(@rdivide,bsxfun(@minus,iso,S3),S1-S3);

  % replace values outside [0,1] with NaNs
  t12( (t12<-eps)|(t12>(1+eps)) ) = NaN;
  t23( (t23<-eps)|(t23>(1+eps)) ) = NaN;
  t31( (t31<-eps)|(t31>(1+eps)) ) = NaN;

  % masks for line "parallel" to 12
  l12 = ~isnan(t23) & ~isnan(t31);
  l23 = ~isnan(t31) & ~isnan(t12);
  l31 = ~isnan(t12) & ~isnan(t23);

  % find non-zeros (lines) from t23 to t31
  [F12,I12,~] = find(l12);
  [F23,I23,~] = find(l23);
  [F31,I31,~] = find(l31);
  % indices directly into t23 and t31 corresponding to F12 and I12
  %ti12 = sub2ind(size(l12),F12,I12);
  %ti23 = sub2ind(size(l23),F23,I23);
  %ti31 = sub2ind(size(l31),F31,I31);
  % faster sub2ind
  ti12 = F12+(I12-1)*size(l12,1);
  ti23 = F23+(I23-1)*size(l23,1);
  ti31 = F31+(I31-1)*size(l31,1);

  % compute actual position values
  LS = [ ...
    ... % average of vertex positions between 2 and 3
    bsxfun(@times,1-t23(ti12), V(F(F12,2),:)) + ...
    bsxfun(@times,  t23(ti12), V(F(F12,3),:)) ...
    ; ... % average of vertex positions between 2 and 3
    bsxfun(@times,1-t31(ti23), V(F(F23,3),:)) + ...
    bsxfun(@times,  t31(ti23), V(F(F23,1),:)) ...
    ;... % average of vertex positions between 2 and 3
    bsxfun(@times,1-t12(ti31), V(F(F31,1),:)) + ...
    bsxfun(@times,  t12(ti31), V(F(F31,2),:)) ...
    ];
  LD = [...
    ... % hcat with average of vertex positions between 3 and 1
    bsxfun(@times,1-t31(ti12), V(F(F12,3),:)) + ...
    bsxfun(@times,  t31(ti12), V(F(F12,1),:)) ...
    ;... % hcat with average of vertex positions between 3 and 1
    bsxfun(@times,1-t12(ti23), V(F(F23,1),:)) + ...
    bsxfun(@times,  t12(ti23), V(F(F23,2),:)) ...
    ;... % hcat with average of vertex positions between 3 and 1
    bsxfun(@times,1-t23(ti31), V(F(F31,2),:)) + ...
    bsxfun(@times,  t23(ti31), V(F(F31,3),:)) ...
    ];

  % I is just concatenation of each I12
  I = [I12;I23;I31];

end


function [sbc, fpe, logdp, np] = arord(R, m, mcor, ne, pmin, pmax)
%ARORD	Evaluates criteria for selecting the order of an AR model.
%
%  [SBC,FPE]=ARORD(R,m,mcor,ne,pmin,pmax) returns approximate values
%  of the order selection criteria SBC and FPE for models of order
%  pmin:pmax. The input matrix R is the upper triangular factor in the
%  QR factorization of the AR model; m is the dimension of the state
%  vectors; the flag mcor indicates whether or not an intercept vector
%  is being fitted; and ne is the number of block equations of size m
%  used in the estimation. The returned values of the order selection
%  criteria are approximate in that in evaluating a selection
%  criterion for an AR model of order p < pmax, pmax-p initial values
%  of the given time series are ignored.
%
%  ARORD is called by ARFIT.
%
%  See also ARFIT, ARQR.

%  For testing purposes, ARORD also returns the vectors logdp and np,
%  containing the logarithms of the determinants of the (scaled)
%  covariance matrix estimates and the number of parameter vectors at
%  each order pmin:pmax.

%  Modified 17-Dec-99
%  Author: Tapio Schneider
%          tapio@gps.caltech.edu

  imax 	  = pmax-pmin+1;        % maximum index of output vectors

  % initialize output vectors
  sbc     = zeros(1, imax);     % Schwarz's Bayesian Criterion
  fpe     = zeros(1, imax);     % log of Akaike's Final Prediction Error
  logdp   = zeros(1, imax);     % determinant of (scaled) covariance matrix
  np      = zeros(1, imax);     % number of parameter vectors of length m
  np(imax)= m*pmax+mcor;

  % Get lower right triangle R22 of R:
  %
  %   | R11  R12 |
  % R=|          |
  %   | 0    R22 |
  %
  R22     = R(np(imax)+1 : np(imax)+m, np(imax)+1 : np(imax)+m);

  % From R22, get inverse of residual cross-product matrix for model
  % of order pmax
  invR22  = inv(R22);
  Mp      = invR22*invR22';

  % For order selection, get determinant of residual cross-product matrix
  %       logdp = log det(residual cross-product matrix)
  logdp(imax) = 2.*log(abs(prod(diag(R22))));

  % Compute approximate order selection criteria for models of
  % order pmin:pmax
  i = imax;
  for p = pmax:-1:pmin
    np(i)      = m*p + mcor;	% number of parameter vectors of length m
   if p < pmax
      % Downdate determinant of residual cross-product matrix
      % Rp: Part of R to be added to Cholesky factor of covariance matrix
      Rp       = R(np(i)+1:np(i)+m, np(imax)+1:np(imax)+m);

      % Get Mp, the downdated inverse of the residual cross-product
      % matrix, using the Woodbury formula
      L        = chol(eye(m) + Rp*Mp*Rp')';
      N        = L \ Rp*Mp;
      Mp       = Mp - N'*N;

      % Get downdated logarithm of determinant
      logdp(i) = logdp(i+1) + 2.* log(abs(prod(diag(L))));
   end

   % Schwarz's Bayesian Criterion
   sbc(i) = logdp(i)/m - log(ne) * (ne-np(i))/ne;

   % logarithm of Akaike's Final Prediction Error
   fpe(i) = logdp(i)/m - log(ne*(ne-np(i))/(ne+np(i)));

   % Modified Schwarz criterion (MSC):
   % msc(i) = logdp(i)/m - (log(ne) - 2.5) * (1 - 2.5*np(i)/(ne-np(i)));

   i      = i-1;                % go to next lower order
end


function cor_n = coriolis(lat, vel, h)
% coriolis: calculates Coriolis forces in the navigation frame.
%
% INPUT
%       lat, Mx1 latitude (radians).
%       vel, Mx3 NED velocities (m/s).
%         h, Mx1 altitude (m).
%
% OUTPUT
%		cor_n, Mx3 Coriolis forces vector in the nav-frame (m/s^2).
%
%   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.
%
%   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/>.
%
% Reference:
%
%	R. Gonzalez, J. Giribet, and H. Patiño. NaveGo: a
% simulation framework for low-cost integrated navigation systems,
% Journal of Control Engineering and Applied Informatics}, vol. 17,
% issue 2, pp. 110-120, 2015. Eq. 11.
%
% Version: 002
% Date:    2022/01/25
% Author:  Rodrigo Gonzalez <rodralez@frm.utn.edu.ar>
% URL:     https://github.com/rodralez/navego

M = max(size(lat));

cor_n = zeros(M, 3);

for i = 1:M

    omega_en_n = transport_rate(lat(i), vel(i,1), vel(i,2), h(i));
    omega_ie_n = earth_rate(lat(i));

    cor_n(i,:) = (omega_en_n + 2*omega_ie_n) * vel(i,:)';
end


function [ v, lambda ] = l1nd_eigen ( n, h )

%*****************************************************************************80
%
%% L1ND_EIGEN returns eigeninformation for the 1D ND Laplacian.
%
%  Discussion:
%
%    The grid points are assumed to be evenly spaced by H.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 October 2013
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of points.
%
%    Input, real H, the spacing between points.
%
%    Output, real V(N,N), the eigenvectors.
%
%    Output, real LAMBDA(N), the eigenvalues.
%
  v = zeros ( n, n );
  lambda = zeros ( n, 1 );

  for j = 1 : n
    theta = pi * ( j - 0.5 ) / ( 2.0 * n + 1.0 );
    lambda(j) = 4.0 * ( sin ( theta ) / h ) ^ 2;
    for i = 1 : n
      theta = pi * ( i - 0.5 ) * ( 2.0 * j - 1.0 ) / ( 2.0 * n + 1.0 );
      v(i,j) = sqrt ( 2.0 / ( n + 0.5 ) ) * cos ( theta );
    end
  end

  return
end


function btv_test01 ( )

%*****************************************************************************80
%
%% BTV_TEST01 tests BURGERS_TIME_VISCOUS with the gaussian initial condition.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 April 2012
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'BTV_TEST01\n' );
  fprintf ( 1, '  Test BURGERS_TIME_VISCOUS with the gaussian initial condition.\n' );

  nx = 81;
  nt = 200;
  t_max = 2.0;
  nu = 0.01;
  bc = 4;

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Initial condition: gaussian\n' );
  fprintf ( 1, '  Number of space nodes = %d\n', nx );
  fprintf ( 1, '  Number of time steps = %d\n', nt );
  fprintf ( 1, '  Final time T_MAX = %g\n', t_max );
  fprintf ( 1, '  Viscosity = %g\n', nu );
  fprintf ( 1, '  Boundary condition = %d\n', bc );

  U = burgers_time_viscous ( @ic_gaussian, nx, nt, t_max, nu, bc );

  x = linspace ( -1.0, +1.0, nx );

  figure ( 1 )

  plot ( x, U(1:50:(nt+1),:), 'Linewidth', 3 )
  grid on
  xlabel ( '<-- X -->' )
  ylabel ( '<-- U(X,T) -->' )
  title ( 'Burgers equation solutions over time, initial condition gaussian' )

  filename = 'btv_test01.png';
  print ( '-dpng', filename )
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Saved plot as "%s"\n', filename );

  return
end


function a = givens_llt ( n )

%*****************************************************************************80
%
%% GIVENS_LLT returns the Cholesky factor of the GIVENS matrix.
%
%  Example:
%
%    N = 5
%
%    1    0        0        0       0
%    1  sqrt(2)    0        0       0
%    1  sqrt(2)  sqrt(2)    0       0
%    1  sqrt(2)  sqrt(2)  sqrt(2)   0
%    1  sqrt(2)  sqrt(2)  sqrt(2) sqrt(2)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 March 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Output, real A(N,N), the matrix.
%
  a = zeros ( n, n );

  a(1:n,1) = 1.0;

  for i = 1 : n
    a(i,2:i) = sqrt ( 2.0 );
  end

  return
end


function [MATreordered,MATindices,MATcost] = reorderMAT(MAT,H,cost)
%REORDERMAT         Reorder matrix for visualization
%
%   [MATreordered,MATindices,MATcost] = reorderMAT(MAT,H,cost);
%
%   This function reorders the connectivity matrix in order to place more
%   edges closer to the diagonal. This often helps in displaying community
%   structure, clusters, etc.
%
%   Inputs:     MAT,            connection matrix
%               H,              number of reordering attempts
%               cost,           'line' or 'circ', for shape of lattice
%                               (linear or ring lattice)
%
%               MATreordered    reordered connection matrix
%               MATindices      reordered indices
%               MATcost         cost of reordered matrix
%
%
%   Olaf Sporns, Indiana University

N = length(MAT);
diagMAT = diag(diag(MAT));
MAT = MAT-diagMAT;

% generate cost function
if strcmp(cost,'line')
    profil = fliplr(normpdf(1:N,0,N/2));
end;
if strcmp(cost,'circ')
    profil = fliplr(normpdf(1:N,N/2,N/4));
end;
COST = toeplitz(profil,profil);

% initialize lowCOST
lowMATcost = sum(sum(COST.*MAT));

% keep track of starting configuration
MATstart = MAT;
starta = 1:N;

% reorder
for h=1:H
    a = 1:N;
    % choose two positions at random and flip them
    r = randperm(N);
    a(r(1)) = r(2);
    a(r(2)) = r(1);
    MATcostnew = sum(sum(MAT(a,a).*COST));
    if (MATcostnew < lowMATcost)
        MAT = MAT(a,a);
        r2 = starta(r(2));
        r1 = starta(r(1));
        starta(r(1)) = r2;
        starta(r(2)) = r1;
        lowMATcost = MATcostnew;
    end;
end;	% h

MATreordered = MATstart(starta,starta) + diagMAT(starta,starta);
MATindices = starta;
MATcost = lowMATcost;


function [RMAOV1MS] = RMAOV1MS(X,alpha)
%RMAOV1MS Repeated measures single-factor analysis of variance multiple samples
% test.
% This m-file deals with repeated measurements at t time points obtained from
% s groups of subjects or multiple samples. With or without same size. The
% simplest model is:
%
%           x_hij = m + g_h +t_j + (gt)_hj + p_i(h) + e_hij
%
% where: x_hij = response at time j from the ith subject in group h; m =
%        overall mean; g_h = fixed effect of group h; t_j = fixed effect of
%        time j; (gt)_hj = fixed effect for the interaction of the hth group
%        with the jth time; p_i(h) = random effects for the ith subject in
%        the hth group, and e_hij = independent random error terms.
%
% For we are interested test for differences among groups, test differences
% among time points, and test the significance of the group x time interaction.
% The first test requires the assumption that the within-group covariance
% matrices are equal. The two last-ones tests require this same assumption
% and that the sphericity condition is satisfied. In general, these assumtions
% are required for all tests of within-subjects effects (repeated measures).
%
% Syntax: RMAOV1MS(X,alpha)
%
% Inputs:
%     X - data matrix (Size of matrix must be n-by-4;dependent variable=
%         column 1;group=column 2;independent variable=column 3;subject=
%         column 4).
% alpha - significance level (default = 0.05).
%
% Outputs:
%       - Complete Analysis of Variance Table.
%
% Example: From the example 5.4.2 of Charles S. Davis (2002). from an experiment
%          comparing dissolution times of pills. We use the logarithms of the
%          times, in seconds, for each pill to reach fractions remaining of
%          0.9,0.7,0.5,0.3,0.25, and 0.10. The four groups correspond to
%          different storage conditions. Under the assumption thet the
%          logarithms of the times are normally distributed, repeated measures
%          ANOVA can be used to test for effects due to group, fraction
%          remaining, and interaction effect between group and fraction
%          remaining. Tha data are shown on the below table.
%
%          -----------------------------------------------------------
%                                        Fraction remainig
%                           ------------------------------------------
%          Group    Tablet    0.90   0.70   0.50   0.30   0.25   0.10
%          -----------------------------------------------------------
%            1         1       2.56  2.77   2.94   3.14   3.18   3.33
%                      2       2.64  2.89   3.09   3.26   3.33   3.47
%                      3       2.94  3.18   3.33   3.50   3.50   3.66
%                      4       2.56  2.83   3.04   3.22   3.26   3.37
%                      5       2.64  2.77   2.94   3.14   3.22   3.30
%                      6       2.56  2.77   2.94   3.14   3.18   3.26
%            2         1       2.56  2.83   3.07   3.26   3.33   3.51
%                      2       2.44  2.74   3.02   3.20   3.28   3.44
%                      3       2.34  2.67   2.91   3.16   3.22   3.39
%                      4       2.41  2.71   2.94   3.16   3.21   3.36
%            3         1       2.46  2.83   3.09   3.32   3.37   3.54
%                      2       2.60  2.93   3.21   3.40   3.46   3.62
%                      3       2.48  2.84   3.12   3.35   3.41   3.58
%                      4       2.49  2.82   3.05   3.29   3.37   3.52
%            4         1       2.40  2.67   2.94   3.20   3.26   3.47
%                      2       2.64  2.94   3.18   3.40   3.45   3.66
%                      3       2.40  2.64   2.86   3.09   3.16   3.38
%          -----------------------------------------------------------
%
%     Data matrix must be:
% X=[2.56 1 1 1;2.77 1 1 2;2.94 1 1 3;3.14 1 1 4;3.18 1 1 5;3.33 1 1 6;2.64 1 2 1;2.89 1 2 2;3.09 1 2 3;3.26 1 2 4;3.33 1 2 5;3.47 1 2 6;
% 2.94 1 3 1;3.18 1 3 2;3.33 1 3 3;3.50 1 3 4;3.50 1 3 5;3.66 1 3 6;2.56 1 4 1;2.83 1 4 2;3.04 1 4 3;3.22 1 4 4;3.26 1 4 5;3.37 1 4 6;
% 2.64 1 5 1;2.77 1 5 2;2.94 1 5 3;3.14 1 5 4;3.22 1 5 5;3.30 1 5 6;2.56 1 6 1;2.77 1 6 2;2.94 1 6 3;3.14 1 6 4;3.18 1 6 5;3.26 1 6 6;
% 2.56 2 1 1;2.83 2 1 2;3.07 2 1 3;3.26 2 1 4;3.33 2 1 5;3.51 2 1 6;2.44 2 2 1;2.74 2 2 2;3.02 2 2 3;3.20 2 2 4;3.28 2 2 5;3.44 2 2 6;
% 2.34 2 3 1;2.67 2 3 2;2.91 2 3 3;3.16 2 3 4;3.22 2 3 5;3.39 2 3 6;2.41 2 4 1;2.71 2 4 2;2.94 2 4 3;3.16 2 4 4;3.21 2 4 5;3.36 2 4 6;
% 2.46 3 1 1;2.83 3 1 2;3.09 3 1 3;3.32 3 1 4;3.37 3 1 5;3.54 3 1 6;2.60 3 2 1;2.93 3 2 2;3.21 3 2 3;3.40 3 2 4;3.46 3 2 5;3.62 3 2 6;
% 2.48 3 3 1;2.84 3 3 2;3.12 3 3 3;3.35 3 3 4;3.41 3 3 5;3.58 3 3 6;2.49 3 4 1;2.82 3 4 2;3.05 3 4 3;3.29 3 4 4;3.37 3 4 5;3.52 3 4 6;
% 2.40 4 1 1;2.67 4 1 2;2.94 4 1 3;3.20 4 1 4;3.26 4 1 5;3.47 4 1 6;2.64 4 2 1;2.94 4 2 2;3.18 4 2 3;3.40 4 2 4;3.45 4 2 5;3.66 4 2 6;
% 2.40 4 3 1;2.64 4 3 2;2.86 4 3 3;3.09 4 3 4;3.16 4 3 5;3.38 4 3 6];
%
% Calling on Matlab the function:
%             RMAOV1MS(X)
%
% Answer is:
%
% The number of groups are: 4
% The maximum number of subjects in a group are: 6
% The minimum number of subjects in a group are: 3
% The number of IV levels are: 6
%
% ANOVA table for repeated measures single-factor multiple samples.
% ------------------------------------------------------------------------------
% SOV                        SS          df          MS            F        P
% ------------------------------------------------------------------------------
% Groups                   0.2038         3        0.0679         0.87   0.4798
% Subjts(Gps.)             1.0108        13        0.0778
% IV                      10.0745         5        2.0149      3198.78   0.0000
% Group x IV               0.2045        15        0.0136        21.64   0.0000
% Residual                 0.0409        65        0.0006
% ------------------------------------------------------------------------------
% Total                   11.5344       101
% ------------------------------------------------------------------------------
% If the P results are smaller than 0.05
% the corresponding Ho's tested result statistically significant. Otherwise,
% are not significative.
%
% Created by A. Trujillo-Ortiz, R. Hernandez-Walls, K. Barba-Rojo and
%            A. Castro-Perez
%            Facultad de Ciencias Marinas
%            Universidad Autonoma de Baja California
%            Apdo. Postal 453
%            Ensenada, Baja California
%            Mexico.
%            atrujo@uabc.mx
%
% Copyright.December 13, 2006.
%
% To cite this file, this would be an appropriate format:
% Trujillo-Ortiz, A., R. Hernandez-Walls, K. Barba-Rojo and A. Castro-Perez. (2006).
%   RMAOV1MS:Repeated measures single-factor analysis of variance multiple samples test.
%   A MATLAB file. [WWW document]. URL http://www.mathworks.com/matlabcentral/
%   fileexchange/loadFile.do?objectId=13380
%
% Reference:
%    Davis, S. Ch. (2002), Statistical Methods for the Analysis of Repeated
%             Measurements. Springer-Verlag:New York. Chapter 5.
%

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

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

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

a = max(X(:,2)); %Number of groups.
b = max(X(:,3)); %Maximum number of subjects in a group.
t = max(X(:,4)); %Number of levels of independent variable.

indice = X(:,2);
for i = 1:a
   Xe = find(indice==i);
   eval(['G' num2str(i) ' = X(Xe,1);']);
end
indice = X(:,4);
for k = 1:t
    Xe = find(indice==k);
    eval(['IV' num2str(k) '=X(Xe,1);']);
end

C = (sum(X(:,1)))^2/length(X(:,1));  %Correction term.
SSTO = sum(X(:,1).^2)-C;  %Total sum of squares.
dfTO = length(X(:,1))-1;  %Total degrees of freedom.

for i = 1:a
   for j = 1:b
      Xe = find((X(:,2)==i) & (X(:,3)==j));
      eval(['T' num2str(i) num2str(j) ' = X(Xe,1);']);
   end
end

for i = 1:a
    for k = 1:t
        Xe = find((X(:,2)==i) & (X(:,4)==k));
        eval(['GI' num2str(i) num2str(k) '=X(Xe,1);']);
     end
end

%Procedure related to the subjects in groups.
NSG = [];
for i = 1:a
   for j = 1:b
   eval(['n' num2str(i) num2str(j) ' = length(T' num2str(i) num2str(j) ');'])
   eval(['x =  n' num2str(i) num2str(j) ';'])
   NSG = [NSG,x];
   end
end

%Due it is an unequal sample sizes design, some subgroups arithmetic operations
%could been not-a-number (like 0.0/0.0). So it is necessary to make the estimations
%ignoring NaNs.
warning off;
SG = [];
for i = 1:a
   for j = 1:b
   eval(['x = ((sum(T' num2str(i) num2str(j) ').^2)/length(T' num2str(i) num2str(j) '));'])
   SG = [SG,x];
   nans = isnan(SG);
   r = find(nans);
   SG(r) = zeros(size(r));
   end
end
warning on;
SSSG = sum(SG)-C;
dfSG = (a*b)-1;

%Procedure related to the groups.
G = [];
for i = 1:a
   eval(['x = ((sum(G' num2str(i) ').^2)/length(G' num2str(i) '));'])
   G = [G,x];
end
SSG = sum(G)-C;
dfG = a-1;
MSG = SSG/dfG;

%Procedure related to the IV (independent variable).
IV = [];
for k = 1:t
    eval(['x =((sum(IV' num2str(k) ').^2)/length(IV' num2str(k) '));']);
    IV = [IV,x];
 end
SSIV = sum(IV)-C;  %sum of squares for the IV
dfIV = t-1;  %degrees of freedom for the IV
MSIV = SSIV/dfIV;  %mean square for the IV

%Procedure related to the interaction group-independent variable.
GI = [];
for i = 1:a
    for k = 1:t
        eval(['x =((sum(GI' num2str(i) num2str(k) ').^2)/length(GI' num2str(i) num2str(k) '));']);
        GI = [GI,x];
     end
 end
SSGI = sum(GI)-sum(G)-sum(IV)+C;  %sum of squares of the Group x IV
dfGI = dfG*dfIV;  %degrees of freedom of the Group x IV
MSGI = SSGI/dfGI;  %mean square for the Group x IV

%Procedure for estimation of sample size for unequal sample sizes.
no1 = []; no2 = [];
for i = 1:a
    for j = 1:b
        eval(['x = length(T' num2str(i) num2str(j) ');']);
        no1 = [no1,x];
        eval(['x = length(T' num2str(i) num2str(j) ').^2;']);
        no2 = [no2,x];
    end
end

%Procedure related to the within subgroups (error).
SSE = SSTO-SSSG;
nneg = length(find((no1-1)<0));
dfE = sum(no1-1)+nneg;
MSE = SSE/dfE;

%Procedure related to the among subgroups within groups.
SSSGG = SSTO-SSG-SSE;
dfSGG = dfTO-dfG-dfE;
MSSGG = SSSGG/dfSGG;

%Procedure related to the residual.
SSR = SSTO-SSG-SSSGG-SSIV-SSGI;
dfR = dfTO-dfG-dfSGG-dfIV-dfGI;
MSR = SSR/dfR;

fprintf('The number of groups are:%2i\n', a);
fprintf('The maximum number of subjects in a group are:%2i\n', b);
c = min(full(sum(spones(reshape(no2,b,a))))); %Minimum number of subjects in a group.
fprintf('The minimum number of subjects in a group are:%2i\n', c);
fprintf('The number of IV levels are:%2i\n\n', t);
FG = MSG/MSSGG;
FIV = MSIV/MSR;
FGI = MSGI/MSR;
PrG = 1 - fcdf(FG,dfG,dfSGG);  %Probability associated to the groups F statistic.
PrIV = 1 - fcdf(FIV,dfIV,dfR);  %Probability associated to the IV F statistic.
PrGI = 1 - fcdf(FGI,dfGI,dfR); %Probability associated to the interaction group-IV F statistic.
disp('ANOVA table for repeated measures single-factor multiple samples.')
fprintf('------------------------------------------------------------------------------\n');
disp('SOV                        SS          df          MS            F        P       ')
fprintf('------------------------------------------------------------------------------\n');
fprintf('Groups       %18.4f%10i%14.4f%13.2f%9.4f\n',SSG,dfG,MSG,FG,PrG);
fprintf('Subjts(Gps.)%19.4f%10i%14.4f\n',SSSGG,dfSGG,MSSGG);
fprintf('IV    %25.4f%10i%14.4f%13.2f%9.4f\n',SSIV,dfIV,MSIV,FIV,PrIV);
fprintf('Group x IV %20.4f%10i%14.4f%13.2f%9.4f\n',SSGI,dfGI,MSGI,FGI,PrGI);
fprintf('Residual %22.4f%10i%14.4f\n',SSR,dfR,MSR);
fprintf('------------------------------------------------------------------------------\n');
fprintf('Total %25.4f%10i\n',SSTO,dfTO);
fprintf('------------------------------------------------------------------------------\n');
fprintf('If the P results are smaller than% 3.2f\n', alpha );
disp('the corresponding Ho''s tested result statistically significant. Otherwise,');
disp('are not significative.');
disp('     ')

return,


function stateVec=orbEls2State(orbEls,deltaT,elType,GM,epsVal)
%%ORBELS2STATE Obtain a velocity and position from a set of orbital
%              elements at a desired time in seconds since the epoch time.
%
%INPUTS: orbEls A 6XnumVec vector of orbital numVec elements to convert.
%               The elements are either Gooding's universal orbital
%               elements or equinoctial orbital elements, as specified by
%               the parameter elType. The components of the elements
%               depends on the type and are explained below.
%        deltaT The time in seconds forward that the trajectory should be
%               propagated from the epoch time of the orbital elements. If
%               this parameter is omitted, a value of 0 is used. For
%               example, if one wishes to propagate the state forward an
%               hour from the time associated with the elements, one can
%               use deltaT=60*60.
%        elType A value indicating the type of orbital elements. Possible
%               values are:
%               0 (The default if omitted) The elements are Gooding's
%                 universal orbital elements.
%               1 The elements are direct equinoctial orbital elements.
%               2 The elements are retrograde equnoctial orbital
%                 elements.
%            GM An optional value of the universal gravitational constant
%               times the mass of the Earth. If omitted, the value
%               Constants.WGS84GMWithAtmosphere is used. The units are
%               m^3/sec^2.
%        epsVal If universal orbital elements are chosen, this is a
%               precision bound used on the alpha term (GM divided by the
%               semi-major axis) for determining whether the trajectory is
%               parabolic. If omitted, a default value of eps is used.
%
%OUTPUTS: stateVec A 6XnumVec matrix of numVec state vectors consisting of
%                 position and velocity in a Cartesian inertial coordinate
%                 system where the gravitating body is at the origin. Units
%                 are meters, and then meters per second.
%
%Gooding's universal orbital elements are consist of (in order)
%alpha=GM/a where a is the semi-major axis in meters
%q=a*(1-e)  where e is the eccentricity (unitless). This is the perifocal
%           distance.
%i          inclination in radians
%Omega      longitude of the ascending node in radians
%omega      argument of periapsis in radians
%tau        the time at pericenter (seconds)
%
%The equinoctial orbital elements (for both direct and retrograde elements)
%are
%a      semi-major axis in meters
%h      first eccentricity vector component (unitless)
%k      second eccentricity vector component (unitless)
%p      first ascending node vector component (unitless)
%q      second ascending node vector component (unitless)
%lambda mean longitude in radians.
%
%The algorithm using Gooding's universal orbital elements is very robust to
%numerical errors. It is implemented using the algorithm of [1], where the
%elements are also described.
%
%Equinoctial orbital elements are discussed in Section 2 of [2] and in [3].
%The diffference between direct and retrograde elements is essentially a
%matter of handedness.
%
%Orbital elements are discussed in general in Chapter 2.2 of [4].
%
%REFERENCES:
%[1] R. H. Gooding, "On universal elements, and conversion procedures to
%    and from position and velocity," Celestial mechanics, vol. 44, no. 3,
%    pp. 283-298, 1988.
%[2] D. A. Danielson, C. P. Sagovac, B. Neta, and L. W. Early,
%    "Semianalytic satellite theory," Mathematics Department, Naval
%    Postgraduate School, Monterey, CA, Tech. Rep., 1995. [Online].
%    Available: http://oai.dtic.mil/oai/oai?verb=getRecord&metadataPrefix= html&identifier=ADA531136
%[3] R. A. Broucke and P. J. Cefola, "On the equinoctial orbit elements,"
%    Celestial Mechanics, vol. 5, no. 3, pp. 303-310, 1972.
%[4] O. Montenbruck and E. Gill, Satellite Orbits: Models, Methods
%    Applications. Berlin: Springer, 2000.
%
%January 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<5)
    epsVal=eps;
end

if(nargin<4)
   GM=Constants.WGS84GMWithAtmosphere;
end

if(nargin<3)
    elType=0;
end

if(nargin<2)
    deltaT=0;
end

numVecs=size(orbEls,2);
stateVec=zeros(6,numVecs);

switch(elType)
    case 0
        for curVec=1:numVecs
            stateVec(:,curVec)=orbEls2StateUniv(orbEls(:,curVec),deltaT,GM,epsVal);
        end
    case 1
        for curVec=1:numVecs
            stateVec(:,curVec)=orbEls2StateEquinoctial(orbEls(:,curVec),deltaT,1,GM);
        end
    case 2
        for curVec=1:numVecs
            stateVec(:,curVec)=orbEls2StateEquinoctial(orbEls(:,curVec),deltaT,-1,GM);
        end
    otherwise
        error('Invalid element type provided.')
end
end

function state=orbEls2StateUniv(theEls,deltaT,GM,epsVal)
%%ORBELS2STATEUNIV  An implementation of Gooding's conversion from
%                   universal orbital elements to state vectors. This
%                   implements the ELS3PV function.
%
%The implementation is taken from Appendix B of
%R. H. Gooding, "On universal elements, and conversion procedures to
%and from position and velocity," Celestial mechanics, vol. 44, no. 3,
%pp. 283-298, 1988.
%
%October 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.

%If we are given classical elements...
alpha=theEls(1);%alpha=GM/a where a is the semi-major axis in meters
q=theEls(2);%q=a*(1-e), where e=eccentricity. q is the perifocal distance
i=theEls(3);%Inclination
Omega=theEls(4);%Longitude of the ascending node
omega=theEls(5);%Argument of periapsis
tau=theEls(6)+deltaT;%The time at pericenter.

[r,u,VR,VT]=ELS2PV(alpha,q,omega,tau,GM,epsVal);

c=cos(u);
s=sin(u);
x1=r*c;
y1=r*s;
x2=VR*c-VT*s;
y2=VR*s+VT*c;

c=cos(i);
s=sin(i);
z=y1*s;
y1=y1*c;
zDot=y2*s;
y2=y2*c;
c=cos(Omega);
s=sin(Omega);
x=x1*c-y1*s;
y=x1*s+y1*c;
xDot=x2*c-y2*s;
yDot=x2*s+y2*c;

state=[x;y;z;xDot;yDot;zDot];
end

function state=orbEls2StateEquinoctial(theEls,deltaT,I,GM)
%%ORBELS2STATEEQUINOCTIAL An algorithm to convert from equinoctial orbital
%                         elements to a target state vector.
%
%The input I is the retrograde factor ond is +1 for direct equinoctial
%elements and -1 for retrograde equinoctial elements.
%
%The algorithm is taken from Section 2.1.4 of
%D. A. Danielson, C. P. Sagovac, B. Neta, and L. W. Early, "Semianalytic
%satellite theory," Mathematics Department, Naval Postgraduate School,
%Monterey, CA, Tech. Rep., 1995. [Online]. Available:
%http://oai.dtic.mil/oai/oai?verb=getRecord&metadataPrefix= html&identifier=ADA531136
%
%The equation for propagating the elements forward in time is taken from
%R. A. Broucke and P. J. Cefola, "On the equinoctial orbit elements,"
%Celestial Mechanics, vol. 5, no. 3, pp. 303-310, 1972.
%
%January 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.

a=theEls(1);%Semi-major axis
h=theEls(2);%First eccentricity vector component
k=theEls(3);%Second eccentricity vector component
p=theEls(4);%First ascending node vector component
q=theEls(5);%Second ascending node vector component
lambda=theEls(6);%Mean longitude (at epoch)

n=sqrt(GM/a^3);%The mean motion
%The propagation of the mean longitude is from Equation 15 in Brouke and
%Cefola.
lambda=lambda+n*deltaT;

%Section 2.1.4 conversion from equinoctial elements to position and
%velocity.
coeff=(1/(1+p^2+q^2));
f=coeff*[1-p^2+q^2;
         2*p*q;
         -2*I*p];
g=coeff*[2*I*p*q;
         (1+p^2-q^2)*I;
         2*q];

%Solve the Equinoctial form of Kepler's Equation. to get F.
F=solveEquinoctialKeplersEq(lambda,h,k);

n=sqrt(GM/a^3);
b=1/(1+sqrt(1-h^2-k^2));
denom=1-h*sin(F)-k*cos(F);
sinL=((1-k^2*b)*sin(F)+h*k*b*cos(F)-h)/denom;
cosL=((1-h^2*b)*cos(F)+h*k*b*sin(F)-k)/denom;

r=a*(1-h^2-k^2)/(1+h*sinL+k*cosL);
X=r*cosL;
Y=r*sinL;
XDot=-n*a*(h+sinL)/sqrt(1-h^2-k^2);
YDot=n*a*(k+cosL)/sqrt(1-h^2-k^2);

r=X*f+Y*g;
rDot=XDot*f+YDot*g;

state=[r;rDot];

end

function [r,u,VR,VT]=ELS2PV(alpha,q,omega,tau,GM,epsVal)
%%ELS2PV An implementation of the ELS2PV subroutine of Gooding's algorithm
%        for universal orbital element conversion. This is a
%        two-dimensional conversion suroutine.
%
%The algorithm is taken from Appendix A of
%R. H. Gooding, "On universal elements, and conversion procedures to
%and from position and velocity," Celestial mechanics, vol. 44, no. 3,
%pp. 283-298, 1988.
%with minor changes so that alpha is compared to an epsilon value rather
%than zero for determining the parabolic case.
%
%October 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.

if(abs(alpha)<=epsVal) %Parabolic case, GM cannot be zero.
    d=solveCubicEqGoodin(0.5/GM,q,1.5*GM*tau);
    r=q+(1/2)*d^2/GM;
    h=sqrt(2*GM*q);
    v=2*atan2(d,h);
else%Ellipse or hyperbola
    e1=alpha*q;
    e=GM-e1;
    ep1=GM+e;
    h=sqrt(q*ep1);

    em=tau*abs(alpha)^(3/2);
    if(alpha>0)%ellipse, GM cannot be zero
        ee2=(1/2)*solveKeplersEq(em/GM,e1/GM,2);
        s2=sin(ee2);
        c2=cos(ee2);
        r=q+2*e*s2^2/alpha;
        d=2*e*s2*c2/sqrt(abs(alpha));
        v=2*atan2(ep1*s2,h*sqrt(abs(alpha))*c2);
        emv=em/GM-v;
        v=v+4*pi*fix(abs(emv/(4*pi))+(1/2))*sign(emv);
    else%hyperbola
        s=solveKeplersEq(em/e,-e1/e,3);
        c=sqrt(1+s^2);
        s2=s^2/(c+1);
        r=q-e*s2/alpha;
        d=e*s/sqrt(abs(alpha));
        v=atan2(s*h*sqrt(abs(alpha)),-GM*s2-e1);
    end
end

%All orbits
u=omega+v;
VR=d/r;
VT=h/r;
end

function x=solveCubicEqGoodin(a,b,c)
%SOLVECUBICEQGOODING  This solves the equation a*x^3+3*b*x-2*c=0, where
%                     a>=0 and b^3+a*c^2>=0 for the real root of x.
%                     Additionally, if a and b are both zero, then zero is
%                     generated in lieu of an indeterminate solution.
%
%INPUTS: a,b,c The coefficients in the equation a*x^3+3*b*x-2*c=0 where
%              a>=0 and b^3+a*c^2>=0.
%
%OUTPUTS: x The one real solution to a*x^3+3*b*x-2*c=0.
%
%In the report
%R. H. Gooding and A. W. Odell, "The hyperbolic Kepler's equation,
%and the elliptic equations revisited," Royal Aerospace Executive,
%Procurement Executive, Ministry of Defence, Farnborough, Hants, United
%Kingdom, Tech. Rep. 369, Jul. 1989.
%the source of the algorithm is listed as being in
%R. H. Gooding, "Solution of the hyperbolic Kepler's equation," Royal
%Aerospace Executive, Procurement Executive, Ministry of Defence,
%Farnborough, Hants, United Kingdom, Tech. Rep. 87042, 1987.
%However, the 1987 report appears to be unobtainable. Nonetheless, the
%solution is given in Equation 4.47 in
%G. Beutler, Methods of Celestial Mechanics: Physical, Mathematical and
%Numerical Principles. Berlin: Springer, 2005, vol. 1.
%The modification to return zero instead of infinity is consistent with how
%Gooding used the algorithm.
%
%October 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.

if(a==0&&b==0||c==0)
    x=0;
else
    d=nthroot(sqrt(a)*abs(c)+sqrt(b^3+a*c^2),3)^2;
    x=2*c/(d+b+b^2/d);
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.