function [C,S]=getEarth2014TerrainCoeffs(M,coeffType)
%%GETEARTH2014TERRAINCOEFFS Get fully normalized spherical harmonic
%                   coefficients for the radial distance offset of the
%                   terrain out from the surface of the GRS80 reference
%                   ellipsoid in meters under various parts of the degree
%                   2160 Earth2014 model.
%
%INPUTS: M The integer maximum order of the spherical harmonic
%          coefficients obtained. This is a value between 0 and 2160. If
%          this parameter is omitted, the default value is 2160.
% coeffType An optional parameter selecting the type of coefficients to
%          load. There are five variants of this model. Possible values
%          are
%          0 (The default if omitted or an empty matrix is passed) The
%            Earth's surface including water in lakes and major ice
%            sheets. oceans are placed on the GRS80 ellipsoid surface.
%          1 The Earth's bedrock.
%          2 The Earth's toporgraphy, bedrock plus major ice sheets.
%          3 The rock-equivalent topography of the Earth (ice and water
%            masses are condensed to layers of rock)
%          4 Major ice sheets. Everything else is set to the surface of the
%            GRS80 ellipsoid.
%
%OUTPUTS: C An array holding the coefficient terms that are multiplied by
%           cosines in the spherical harmonic expansion. If given to a
%           CountingClusterSet class, C(n+1,m+1) is the coefficient of
%           degree n and order m. When a maximum degree of M is used, all C
%           have values for all n from 0 to M and for all m from 0 to n for
%           each n. The coefficients are unitless.
%         S An array holding the coefficient terms that are multiplied by
%           sines in the spherical harmonic expansion. The format of S is
%           the same as that of C.
%
%The model is described in [1]. This function is only for values up to
%degree and order 2160 as the spherical harmonic synthesis routine in the
%Tracker Component Library cannot currently handle the full degree 10,800
%model, because it does not use extended precision arithmetic.
%
%The Earth 2014 data can be downloaded from
%http://ddfe.curtin.edu.au/models/Earth2014/
%To use the data for all of the models, place the files
%Earth2014.SUR2014.degree2160.bshc, Earth2014.BED2014.degree2160.bshc
%Earth2014.TBI2014.degree2160.bshc, Earth2014.RET2014.degree2160.bshc
%and Earth2014.ICE2014.degree2160.bshc in the data folder that is in the
%same folder as this function.
%
%EXAMPLE:
%The fact that this only stores radial distance offsets can be a little
%confusing when one wants to find the location of a point on the Earth's
%surface in the WGS-84 coordinate system/ International Terrestrial
%Reference System (ITRS). Here, we give an example:
% %WGS-84 latitude and longitude in radians.
% latLon=[19.4721;-155.5922]*(pi/180);
% [C,S]=getEarth2014TerrainCoeffs();
% %To use the terrain coefficients, the latitude must be changed from
% %geodetic to geocentric. We will assume that the latitude is given in
% %WGS-84 coordinates, as is standard with GPS.
% latSpher=ellipsLat2SpherLat(latLon(1));
% azElSphere=[latLon(2);latSpher];
% terHeight=spherHarmonicEval(C,S,azElSphere);
% %Add the ellipsoidal radius of the GRS80 reference ellipsoid at the
% %point. Note that this actually changes the ellipsoidal latitude
% %slightly.
% a=Constants.GRS80SemiMajorAxis;
% f=Constants.GRS80Flattening;
% terRad=terHeight+ellipsoidalRadius(latSpher,0,a,f);
% %Convert from spherical to Cartesian coordinates to find the point.
% CartLoc=spher2Cart([terRad;azElSphere])
%
%REFERENCES:
%[1] C. Hirt and M. Rexer, "Earth2014: 1arc-min shape, topography,
%    bedrock and ice-sheet models - available as gridded data and degree-
%    10,800 spherical harmonics," International Journal of Applied Earth
%    Observation and Geoinformation, vol. 39, pp. 103-112, Jul. 2015.
%
%June 2016 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<2||isempty(coeffType))
     coeffType=0;
end

%The Earth2014 terrain coefficient data file, should be located in a data
%folder that is in the same folder as this file. This finds the path to
%this file.
ScriptPath=mfilename('fullpath');
ScriptFolder = fileparts(ScriptPath);

switch(coeffType)
    case 0
        fileID=fopen([ScriptFolder,'/data/Earth2014.SUR2014.degree2160.bshc'],'rb');
    case 1
        fileID=fopen([ScriptFolder,'/data/Earth2014.BED2014.degree2160.bshc'],'rb');
    case 2
        fileID=fopen([ScriptFolder,'/data/Earth2014.TBI2014.degree2160.bshc'],'rb');
    case 3
        fileID=fopen([ScriptFolder,'/data/Earth2014.RET2014.degree2160.bshc'],'rb');
    case 4
        fileID=fopen([ScriptFolder,'/data/Earth2014.ICE2014.degree2160.bshc'],'rb');
    otherwise
        error('Unknown coefficient type specified.')
end

data = fread(fileID,Inf,'double');
fclose(fileID);

%Note that data(1) should be zero. data(1) is the degree of the lowest
%degree coefficient. data(2) is the degree of the maximum degree
%coefficient.
maxDeg = data(2);
maxNumCoeffs=(maxDeg+1)*(maxDeg+2)/2;

if(nargin<1||isempty(M))
    M=maxDeg;
end

totalNumCoeffs=(M+1)*(M+2)/2;

%Extract the data. The additive 2 skips the first two entries, which
%indicate the lowest and highest coefficient degrees.
C=data((2+1):(2+totalNumCoeffs));
S=data((2+maxNumCoeffs+1):(2+maxNumCoeffs+totalNumCoeffs));
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 [simulatedata,ht,pseudorc]=matrix_garch_simulate(t,k,parameters,p,o,q,m)
% Simulation of symmetric and asymmetric MATRIX multivariate GARCH models
%
% USAGE:
%   [SIMULATEDATA, HT, PSEUDORC] = matrix_garch_simulate(T, K, PARAMETERS, P, O, Q, M)
%
% INPUTS:
%   T            - Length of the time series to be simulated
%   K            - Cross-sectional dimension
%   PARAMETERS   - A 3-D matrix of K by K matrices where
%                    CC' = PARAMETERS(:,:,1), AA'(j)=PARAMETERS(:,:,1+j),
%                    GG'(j) = PARAMETERS(:,:,1+P+j), BB'(j)=PARAMETERS(:,:,1+P+Q+j)
%                    -OR- a K(K+1)/2*(1+P+O+Q) by 1 vector of parameters of
%                    the form returned my calling matrix_garch
%   P            - Positive, scalar integer representing the number of lags of the innovation process
%   O            - Non-negative scalar integer representing the number of asymmetric lags to include
%   Q            - Non-negative scalar integer representing the number of lags of conditional covariance
%   M            - [OPTIONAL] Number of ``intradaily'' returns to simulate to pseudo-Realized
%                    Covariance. If omitted, set to 72.
%
% OUTPUTS:
%   SIMULATEDATA - A time series with constant conditional correlation covariance
%   HT           - A [k k t] matrix of simulated conditional covariances
%   PSEUDORC     - A [k k t] matrix of pseudo-Realized Covariances
%
% COMMENTS:
%    The conditional variance, H(t), of a MATRIX GARCH is modeled as follows:
%
%      H(t) = CC' + AA'(1).*r_{t-1}'*r_{t-1} + ... + AA'(P).*r_{t-P}'*r_{t-P}
%                 + GG(1)'.*n_{t-1}'*n_{t-1} + ... + GG(O)'.*n_{t-P}'*n_{t-P}
%                  + BB(1)'.*H(t-1) +...+ BB(Q)'.*H(t-q)
%
%    where n_{t} = r_{t} .* (r_{t}<0).  If using realized measures, the
%    RM_{t-1} replaces r_{t-1}'*r_{t-1}, and the asymmetric version
%    replaces n_{t-1}'*n_{t-1}
%
%   Pseudo Realized Covariances are simulated by generating m-intra daily returns from a N(0,1/m)
%   and computing the Realized Covariance of these. These were used in Patton and Sheppard (2009)
%   when evaluating variance and covariance specifications in a Monte Carlo.  If M=1, then PSEUDORC
%   is just the outer product of the SIMULATEDATA.
%
%   NOTE: This program generates 2000 more than required to minimize any start-up bias
%
% See also TARCH_SIMULATE, CCC_GARCH_SIMULATE, MATRIX_GARCH

% Copyright: Kevin Sheppard
% kevin.sheppard@economics.ox.ac.uk
% Revision: 1    Date: 3/10/2011

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
switch nargin
    case 6
        m=[];
    case 7
        % nothing
    otherwise
        error('MFEToolbox:Input','6 or 7 inputs required.');
end

% t
if ~isscalar(t) || t<0 || floor(t)~=t
    error('T must be a positive integer.');
end
% k
if ~isscalar(k) || k<2 || floor(k)~=k
    error('K must be a positive integer greater than or equal to 2.');
end
% p
if ~isscalar(p) || p<1 || floor(p)~=p
    error('P must be a positive integer if scalar.');
end

% o

if ~isscalar(o) || o<0 || floor(o)~=o
    error('O must be a non-neagative integer if scalar.');
end
% q

if ~isscalar(q) || q<0 || floor(q)~=q
    error('Q must be a non-neagative integer if scalar.');
end
% m
if isempty(m)
    m = 72;
end
if ~isscalar(m) || floor(m)~=m || m<1
    error('M must be a positive integer.')
end
% parameters
k2 = k*(k+1)/2;
if ismatrix(parameters)
    parameterCount = k*(k+1)/2 *(1+ p + o + q);
    if size(parameters,2)>size(parameters,1)
        parameters = parameters';
    end
    if length(parameters)~=parameterCount
        error('PARAMETERS must be K(K+1)/28(1+P+O+Q) when using a vector.')
    end
    parameterMatrices= zeros(k,k,1+p+o+q);
    index = 0;
    for i=1:(1+p+o+1)
        temp = vec2chol(parameters(index+1:index+k2));
        parameterMatrices(:,:,i) = (temp*temp'+temp*temp')/2;
        index=index+k2;
    end
    parameters = parameterMatrices;
elseif ndims(parameters)==3
    if any(size(parameters)~=[k k 1+p+o+q])
        error('PARAMETERS must be K by K by (1+P+O+Q) when using a 3-D matrix.')
    end
    parameterMatrices = parameters;
else
    error('The size of PARAMETERS is not compatible with this function.')
end

% Check stationarity
sumParameters = zeros(k);
for i=2:size(parameterMatrices,3)
    if i<=(p+1) || i>(1+p+o)
        w=1;
    else
        w=0.5;
    end
    sumParameters = sumParameters + w*parameterMatrices(:,:,i);
end
sumParameters = (sumParameters+sumParameters')/2;
if max(diag(sumParameters))>1
    warning('MFE::Nonstationary','The parameters do not correspond to a stationary solution.  Check for overflow.')
    uncond = parameters(:,:,1)/.005;
else
    uncond = parameters(:,:,1)./(ones(k)-sumParameters);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Input checking
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Set the burnin amount, 2000 is probably reasonable
burnin=2000;
%Up t by the burnin amount
t=t+burnin;

%Draw some normal random numbers
intraRandomNums=randn(m*t,k)*sqrt(1/m);
randomNums = cumsum(intraRandomNums);
randomNums = diff([zeros(1,k);randomNums(m:m:m*t,:)]);

%Perform the recursion
backCast = uncond;
backCastAsym = uncond;
T = t;
Ht = zeros(k,k,T);
simulatedData = zeros(T,k);
for t=1:T;
    Ht(:,:,t)=parameterMatrices(:,:,1);
    for j=1:p
        if (t-j)<1
            Ht(:,:,t)=Ht(:,:,t)+parameterMatrices(:,:,j+1).*backCast;
        else
            r = simulatedData(t-j,:);
            Ht(:,:,t)=Ht(:,:,t)+parameterMatrices(:,:,j+1).*(r'*r);
        end
    end
    for j=1:o
        if (t-j)<1
            Ht(:,:,t)=Ht(:,:,t)+parameterMatrices(:,:,p+j+1).*backCastAsym;
        else
            n = simulatedData(t-j,:).*(simulatedData(t-j,:)<0);
            Ht(:,:,t)=Ht(:,:,t)+parameterMatrices(:,:,p+j+1).*(n'*n);
        end
    end
    for j=1:q
        if (t-j)<1
            Ht(:,:,t)=Ht(:,:,t)+parameterMatrices(:,:,p+o+j+1).*backCast;
        else
            Ht(:,:,t)=Ht(:,:,t)+parameterMatrices(:,:,p+o+j+1).*Ht(:,:,t-j);
        end
    end
    simulatedData(t,:) = randomNums(t,:)*Ht(:,:,t)^(0.5);
end

%Initialize the covariance
pseudorc=zeros(k,k,t);
for t=1:T
    r = intraRandomNums((t-1)*m+1:t*m,:)*Ht(:,:,t)^(0.5);
    pseudorc(:,:,t) = r'*r;
end

%Truncate the data and the covariance
simulatedata=simulatedData(burnin+1:t,:);
pseudorc = pseudorc(:,:,burnin+1:t);
ht=Ht(:,:,burnin+1:t);


%% POWER_WRITE writes a power table to a file.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 October 2007
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'POWER_WRITE\n' );
  fprintf ( 1, '  Write a table of integer powers to a file.\n' );

  file_name = 'power_write.txt';

  file_unit = fopen ( file_name, 'w' );

  fprintf ( file_unit, '\n' );
  fprintf ( file_unit, '           N   N-squared     N-cubed\n' );
  fprintf ( file_unit, '\n' );
  for i = 0 : 10
    fprintf ( file_unit, '  %10d  %10d  %10d\n', i, i^2, i^3 );
  end

  fclose ( file_unit );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  POWER_WRITE wrote the file %s\n', file_name );

  exit


function [Nm] = eV2Nm(eV)
% Convert energy or work from electron volts to newton-meters.
% Chad A. Greene 2012
Nm = eV*1.6021773e-19;


function I=montec(f,a,b,n)

%I=montec('f',a,b,k);
%Calculates integral using montecarlo method
for k=1:n
    s=2.^k;
    t=rand(1,s);
    x=a+t.*(b-a);

    for i=1:s
        y(i)=feval(f,x(i));
    end

    I(k)=((b-a)./s)*sum(y);
end
plot(I);


function linpack_s_test30 ( )

%*****************************************************************************80
%
%% TEST30 tests STRDI.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 June 2009
%
%  Author:
%
%    John Burkardt
%
  n = 5;
  lda = n;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST30\n' );
  fprintf ( 1, '  For a triangular matrix,\n' );
  fprintf ( 1, '  STRDI computes the determinant or inverse.\n' );
  fprintf ( 1, '  The matrix size is N = %d\n', n );
%
%  Lower triangular matrix A.
%
  [ a, seed ] = r4mat_uniform_01 ( n, n, seed );

  for i = 1 : n
    for j = i+1 : n
      a(i,j) = 0.0;
    end
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Lower triangular matrix A:\n' );
  fprintf ( 1, '\n' );

  for i = 1 : n
    for j = 1 : n
      fprintf ( 1, '  %14f', a(i,j) );
    end
    fprintf ( 1, '\n' );
  end

  job = 110;

  [ a, det, info ] = strdi ( a, lda, n, job );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The determinant = %f * 10 ^ %f\n ', det(1), det(2) );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The inverse matrix:\n' );
  fprintf ( 1, '\n' );

  for i = 1 : n
    for j = 1 : n
      fprintf ( 1, '  %14f', a(i,j) );
    end
    fprintf ( 1, '\n' );
  end

%
%  Upper triangular matrix A.
%
  [ a, seed ] = r4mat_uniform_01 ( n, n, seed );

  for i = 1 : n
    for j = 1 : i - 1
      a(i,j) = 0.0;
    end
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Upper triangular matrix A:\n' );
  fprintf ( 1, '\n' );

  for i = 1 : n
    for j = 1 : n
      fprintf ( 1, '  %14f', a(i,j) );
    end
    fprintf ( 1, '\n' );
  end

  job = 111;

  [ a, det, info ] = strdi ( a, lda, n, job );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The determinant = %f * 10 ^ %f\n ', det(1), det(2) );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The inverse matrix:\n' );
  fprintf ( 1, '\n' );

  for i = 1 : n
    for j = 1 : n
      fprintf ( 1, '  %14f', a(i,j) );
    end
    fprintf ( 1, '\n' );
  end

  return
end


function r = subsref(p,s)
%SUBSREF      Implements subscripted references for polynomials
%
%  r = p(i)
%
%For univariate polynomials p, p(i) is the coefficient of x^i, 0<=i<=degree(p).
%  For i>degree(p), p(i):=0. Similarly, p(i:j) is the vector of coefficients
%  [ p(i) p(i+1) ... p(j) ], or, p(:) is the (row) vector of all coefficients
%  of p, the same as vector(p) for univariate polynomials.
%
%For multivariate polynomials p in k variables x_1..x_k, p(i) is the
%  coefficient of x_1^i, i.e. a polynomial in k-1 variables.
%  Similarly, p(i,[],j) or p(i,:,j) is the coefficient polynomial of
%  x1^i*x3^j. Indices i,j,... must be single indices, no range.
%Note that access to coefficients refers to the current order p.v of
%  variables of p. To change this order, see permvars.
%For example, for a polynomial in three variables p.v={'x','y','z'},
%  p(1,0,3) is the coefficient of x*z^3 (a constant), where p(1,[],3)
%  if the coefficient of x*z^3, a univariate polynomial in y.
%
%Polynomial evaluation is denoted by p{x} or p{x1,...,xn}, computing the value
%  of p at x. This is the same as polyval(p,x) of polyval(p,x1,...,xn).
%For univariate polynomials, x may be a vector or matrix yielding the vector
%  or matrix of polynomial values evaluated at the corresponding coefficients.
%For multivariate polynomials, x is a vector of values of the variables.
%  For x being a matrix, the result is the (column) vector of p{x(i,:)}.
%
%Moreover, p.mid, p.rad, p.inf, p.sup give access to the midpoint, radius,
%  infimum and supremum of p, respectively.
%
%Finally, p.e, p.c and p.v give access to the arrays of exponents, coefficients
%  and variables of p, such that polynom(p.e,p.c,p.v) is again p. Single variables
%  are accessed by p.v{i}.
%
%In the univariate case,  p == polynom(p.c,p.v)  [ degree is length(p.c)+1 ].
%In the multivariate case,  p == polynom(p.e,p.c,p.v)  [ p.e is (sparse) exponent set ].
%

% written  11/20/97     S.M. Rump
% modified 04/04/04     S.M. Rump  set round to nearest for safety
% modified 04/06/05     S.M. Rump  rounding unchanged
%

  while 1
    if ~isa(p,'polynom')
      r = subsref(p,s(1));
    elseif strcmp(s(1).type,'()')     % index reference p(i)
      if size(p.e,2)==1               % univariate polynomial
        if length(s(1).subs)>1
          error('invalid call: more than one index')
        end
        if isequal(s(1).subs{1},':')
          r = p.c;
        else
          n1 = length(p.c);
          index = n1-s(1).subs{1};    % index vector, p(0) constant term, etc.
          if ~isreal(index) | ~isequal(index,round(index))
            error('index must be integer')
          end
          if any(index>n1)            % index negative
            error('index out of range')
          end
          if any(index<1)             % index greater than degree
            indexgt0 = ( index>0 );
            r = typeadj( zeros(1,length(index)) , typeof(p.c) );
            if any(indexgt0)
              r(indexgt0) = p.c(index(indexgt0));
            end
          else
            r = p.c(index);
          end
        end
      else                            % multivariate polynomial
        k = size(p.e,2);              % number of variables
        if length(s(1).subs)>k
          error('too many indices')
        end
        index = logical(zeros(1,k));
        exponents = [];
        for i=1:length(s(1).subs)
          if isempty(s(1).subs{i}) | isequal(s(1).subs{i},':')
            index(i) = 0;
          else
            index(i) = 1;
            exponents = [ exponents s(1).subs{i} ];
          end
        end
        if isempty(exponents)
          r = p;
          return
        end
        I = all( ( p.e(:,index) == repmat(exponents,size(p.e,1),1) ) , 2 );
        if ~any(I)                    % coefficients do not occur
          r = typeadj( 0 , typeof(p.c) );
          return
        end
        r.e = p.e(I,~index);
        if isempty(r.e)               % coefficient is single constant
          r = p.c(I);
          return
        end
        r.c = p.c(I);
        r.v = p.v(~index);
        if size(r.e,2)==1             % coefficient polynomial univariate
          if iscell(r.v)              % be sure r.v is char for univ. pol.
            r.v = r.v{1};
          end
          n = max(r.e);               % r.e is vector
          c = r.c;
          r.c = typeadj( zeros(1,n+1) , typeof(c) );
          r.c(n+1-r.e) = c;
          r.e = n;
          if r.e==0
            r = r.c;
            return
          end
        else
          r = normalize(r);
        end
        r = class(r,'polynom');
      end
    elseif strcmp(s(1).type,'{}')     % polynomial evaluation p{x}
      r = polyval(p,s(1).subs{:});
    elseif strcmp(s(1).type,'.')      % polynomial access to inf, sup, ...
      if     strcmp(s(1).subs,'mid'), r = mid(p);
      elseif strcmp(s(1).subs,'rad'), r = rad(p);
      elseif strcmp(s(1).subs,'inf'), r = inf(p);
      elseif strcmp(s(1).subs,'sup'), r = sup(p);
      elseif strcmp(s(1).subs,'e')
        if iscell(p.v)                  % multivariate
          r = flipud(sortrows(p.e));
        else
          r = p.e;                      % univariate
        end
      elseif strcmp(s(1).subs,'c')
        if iscell(p.v)                  % multivariate
          [ dummy index ] = sortrows(p.e); r = flipud(p.c(index));
        else
          r = p.c;
        end
      elseif strcmp(s(1).subs,'v'), r = p.v;
      else
        error('invalid reference for polynomial')
      end
    else
      error('invalid index reference for polynomial')
    end
    if length(s)==1
      return
    end
    s = s(2:end);
    p = r;
  end


function transfo = createBasisTransform(source, target)
%CREATEBASISTRANSFORM Compute matrix for transforming a basis into another basis
%
%   TRANSFO = createBasisTransform(SOURCE, TARGET)
%   Both SOURCE and TARGET represent basis, in the following form:
%   [x0 y0  ex1 ey1  ex2 ey2]
%   [y0 y0] is the origin of the basis, [ex1 ey1] is the first direction
%   vector, and [ex2 ey2] is the second direction vector.
%
%   The result TRANSFO is a 3-by-3 matrix such that a point expressed with
%   coordinates of the first basis will be represented by new coordinates
%   P2 = transformPoint(P1, TRANSFO) in the target basis.
%
%   TRANSFO = createBasisTransform(TARGET)
%   Assumes the source is the standard (Oij) basis, with origin at (0,0),
%   first direction vector equal to (1,0) and second direction  vector
%   equal to (0,1).
%
%
%   Example
%     % define source and target bases
%     src = [ 0 0   1  0    0  1];
%     tgt = [20 0  .5 .5  -.5 .5];
%     trans = createBasisTransform(src, tgt);
%     % create a polygon in source basis
%     poly = [10 10;30 10; 30 20; 20 20;20 40; 10 40];
%     figure;
%     subplot(121); drawPolygon(poly, 'b'); axis equal; axis([-10 50 -10 50]);
%     hold on; drawLine([0 0 1 0], 'k'); drawLine([0 0 0 1], 'k');
%     drawLine([20 0 1 1], 'r'); drawLine([20 0 -1 1], 'r');
%     t = -1:5; plot(t*5+20, t*5, 'r.'); plot(-t*5+20, t*5, 'r.');
%     % transform the polygon in target basis
%     poly2 = transformPoint(poly, trans);
%     subplot(122); drawPolygon(poly2, 'b'); axis equal; axis([-10 50 -10 50]);
%     hold on; drawLine([0 0 1 0], 'r'); drawLine([0 0 0 1], 'r');
%     t = -1:5; plot(t*10, zeros(size(t)), 'r.'); plot(zeros(size(t)), t*10, 'r.');
%
%   See also
%   transforms2d
%

% ------
% Author: David Legland
% e-mail: david.legland@inra.fr
% Created: 2010-12-03,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2010 INRA - Cepia Software Platform.

% init basis transform to identity
t1 = eye(3);
t2 = eye(3);

if nargin == 2
    % from source to reference basis
    t1(1:2, 1) = source(3:4);
    t1(1:2, 2) = source(5:6);
    t1(1:2, 3) = source(1:2);
else
    % if only one input, use first input as target basis, and leave the
    % first matrix to identity
    target = source;
end

% from reference to target basis
t2(1:2, 1) = target(3:4);
t2(1:2, 2) = target(5:6);
t2(1:2, 3) = target(1:2);

% compute transform matrix
transfo = zeros(3, 3);
maxSz = 1;
for i = 1:maxSz
    % coordinate of three reference points in source basis
    po = t1(1:2, 3, i)';
    px = po + t1(1:2, 1, i)';
    py = po + t1(1:2, 2, i)';

    % express coordinates of reference points in the new basis
    t2i = inv(t2(:,:,i));
    pot = transformPoint(po, t2i);
    pxt = transformPoint(px, t2i);
    pyt = transformPoint(py, t2i);

    % compute direction vectors in new basis
    vx = pxt - pot;
    vy = pyt - pot;

    % concatenate result in a 3-by-3 affine transform matrix
    transfo(:,:,i) = [vx' vy' pot' ; 0 0 1];
end


function x = IWT2_PO(wc,L,qmf)
% IWT2_PO -- Inverse 2-d MRA wavelet transform (periodized, orthogonal)
%  Usage
%    x = IWT2_PO(wc,L,qmf)
%  Inputs
%    wc    2-d wavelet transform [n by n array, n dyadic]
%    L     coarse level
%    qmf   quadrature mirror filter
%  Outputs
%    x     2-d signal reconstructed from wc
%
%  Description
%    If wc is the result of a forward 2d wavelet transform, with
%    wc = FWT2_PO(x,L,qmf), then x = IWT2_PO(wc,L,qmf) reconstructs x
%    exactly if qmf is a nice qmf, e.g. one made by MakeONFilter.
%
%  See Also
%    FWT2_PO, MakeONFilter
%
	[n,J] = quadlength(wc);
	x = wc;
	nc = 2^(L+1);
	for jscal=L:J-1, % from coarse to fine
		top = (nc/2+1):nc; bot = 1:(nc/2); all = 1:nc;
		for iy=1:nc,
			x(all,iy) =  UpDyadLo(x(bot,iy)',qmf)'  ...
					   + UpDyadHi(x(top,iy)',qmf)';
		end
		for ix=1:nc,
			x(ix,all) = UpDyadLo(x(ix,bot),qmf)  ...
					  + UpDyadHi(x(ix,top),qmf);
		end
		nc = 2*nc;
	end

%
% Copyright (c) 1993. David L. Donoho
%

%
%  Part of Wavelab Version 850
%  Built Tue Jan  3 13:20:40 EST 2006
%  This is Copyrighted Material
%  For Copying permissions see COPYING.m
%  Comments? e-mail wavelab@stat.stanford.edu


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% ldldown.m %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% function [L,d]=ldldown(L,d,j)
% downdates LDL^T factorization when j-th row and column are replaced
% by j-th unit vector
%
% d contains diag(D) and is assumed positive
%
function [L,d]=ldldown(L,d,j);

n=size(d,1);

test=0;
if test,
  disp('enter ldldown')
  A=L*diag(d)*L';A(:,j)=zeros(n,1);A(j,:)=zeros(1,n);A(j,j)=1;
end;

if j<n,
  I=1:j-1;K=j+1:n;
  [LKK,d(K)]=ldlrk1(L(K,K),d(K),d(j),L(K,j));
  % work around expensive sparse L(K,K)=LKK
  L=[L(I,:);
     sparse(1,n);
     L(K,I),sparse(n-j,1),LKK];
  L(j,j)=1;
else
  L(n,1:n-1)=sparse(1,n-1);
end;
d(j)=1;

if test,
  A1=L*diag(d)*L',A
  quot=norm(A1-A,1)/norm(A,1),
  disp('leave ldldown')
end;


function A = calc_area(I)
% The derivative uses central difference and only applies to the interior
% points
der_x = calc_der_x(I(2:end-1,:,:));
der_y = calc_der_y(I(:,2:end-1,:));
G11 = sum(der_x.^2, 3) + 1;
G12 = sum(der_x.*der_y, 3);
G22 = sum(der_y.^2, 3) + 1;
A = sum(sum(sqrt(G11.*G22 - G12.^2)));
% A = sum(sum(G11.*G22 - G12.^2));

function der_x = calc_der_x(I)
der_x = (1/2) * (I(:,3:end,:) - I(:,1:end-2,:));

function der_y = calc_der_y(I)
der_y = (1/2) * (I(3:end,:,:) - I(1:end-2,:,:));


function  [h, compUpJP] =  lfmComputeH4JP(gamma1_p, gamma1_m, sigma2, t1, ...
    preFactor, preExp, mode)

% LFMCOMPUTEH4JP Helper function for computing part of the LFMJP kernel.
% FORMAT
% DESC computes a portion of the LFMAP kernel.
% ARG gamma1 : Gamma value for first system.
% ARG gamma2 : Gamma value for second system.
% ARG sigma2 : length scale of latent process.
% ARG t1 : first time input (number of time points x 1).
% ARG t2 : second time input (number of time points x 1).
% ARG mode: indicates in which way the vectors t1 and t2 must be transposed
% RETURN h : result of this subcomponent of the kernel for the given values.
%
% COPYRIGHT : Mauricio Alvarez, 2010
%
% SEEALSO : lfmComputeH4.m, lfmComputeH4AP.m

% KERN

if mode==0
    if nargout > 1
        compUpJP{1} = lfmjpComputeUpsilonVector(gamma1_p,sigma2, t1);
        compUpJP{2} = lfmjpComputeUpsilonVector(gamma1_m,sigma2, t1);
        h =  compUpJP{1}*( preExp(:,1)/preFactor(1) - preExp(:,2)/preFactor(2)).' ...
            + compUpJP{2}*( preExp(:,2)/preFactor(3) - preExp(:,1)/preFactor(4)).';
    else
        h =  lfmjpComputeUpsilonVector(gamma1_p,sigma2, t1)*( preExp(:,1)/preFactor(1) - preExp(:,2)/preFactor(2)).' ...
            + lfmjpComputeUpsilonVector(gamma1_m,sigma2, t1)*( preExp(:,2)/preFactor(3) - preExp(:,1)/preFactor(4)).';
    end
else
    if nargout > 1
        compUp{1} = lfmComputeUpsilonVector(gamma1_p,sigma2, t1);
        compUp{2} = lfmComputeUpsilonVector(gamma1_m,sigma2, t1);
        h =  compUp{1}*(preExp(:,2)/preFactor(2) - preExp(:,1)/preFactor(1)).' ...
            + compUp{2}*(preExp(:,1)/preFactor(4) - preExp(:,2)/preFactor(3)).';
        compUpJP = compUp;
    else
        h =  lfmComputeUpsilonVector(gamma1_p,sigma2, t1)*(preExp(:,2)/preFactor(2) - preExp(:,1)/preFactor(1) ).' ...
            + lfmComputeUpsilonVector(gamma1_m,sigma2, t1)*(preExp(:,1)/preFactor(4) - preExp(:,2)/preFactor(3)).';
    end
end


% Calculate adaptive smoothed rate map
%
% Calculates an adaptive smoothed rate map as described in "Skaggs et al 1996 -
% Theta Phase Precession in Hippocampal Neuronal Population and the Compression of Temporal Sequences"
%
%  USAGE
%   [map, posPdf] = analyses.ratemapAdaptiveSmoothing(pos, spkPos, <options>)
%   pos         Position samples (either Nx3 or Nx2 matrix). X and Y coordinates are used.
%   spkPos      Spike position samples (either Nx2 or Nx2 matrix). X and Y coordinates are used.
%   <options>   optional list of property-value pairs (see table below)
%
%   =========================================================================
%    Properties     Values
%   -------------------------------------------------------------------------
%    'binWidth'     width (and height) of bins in firing rate map. Value units
%                   are [cm]. binWidth has priority over parameter nBins. See
%                   description of nBins.
%
%    'nBins'        number of horizontal and vertical bins (default = [50 50]).
%                   If single value is provided, it is used for both horizontal
%                   and vertical number of bins (i.e. nBins = 50 => [50 50]).
%                   Either binWidth or nBins should be provided. If non is provided,
%                   then the default value of binWidth is used. If both are provided,
%                   then the value of binWidth is used.
%
%    'minTime'      minimum time spent in each bin (in s, default = 0).
%    'alphaValue'   scaling parameter. Default value is 10000. See original paper
%                   for the details.
%    'shape'        shape of the arena. Possible values are: 1 for square box,
%                   2 for cylinder. Only square box is currently supported!
%    =========================================================================
%
%  OUTPUT
%
%   map.x       x bins
%   map.y       y bins
%   map.z       adaptively smoothed firing rate map
%   posPdf      position probability density function

function [map, posPdf] = mapAdaptiveSmoothing(pos, spkPos, varargin)
    % Check number of parameters
    if nargin < 2 || mod(length(varargin), 2) ~= 0,
        error('BNT:numArgs', 'Incorrect number of parameters (type ''help analyses.mapAdaptiveSmoothing'' for details).');
    end

    % Check parameter sizes
    if size(pos, 2) < 2
        error('Parameter ''pos'' should have at least 2 columns (type ''help analyses.mapAdaptiveSmoothing'' for details).');
    end
    if size(spkPos, 2) < 2
        error('Parameter ''spkPos'' should have 1 or 2 columns (type ''help analyses.mapAdaptiveSmoothing'' for details).');
    end

    if isempty(pos)
        return;
    end

    % Default values
    alphaValue = 10000;
    binWidth = 5;
    minTime = 0;
    limits = [];
    shape = 1;
    sampleTime = helpers.sampleTimeFromData(pos);
    nBins = [50 50];

    haveBinWidth = false;
    haveNBins = false;

    % Parse parameter list
    i = 1;
    while i < length(varargin)
        if ~ischar(varargin{i}),
            error(['Parameter ' num2str(i+2) ' is not a property (type ''help analyses.mapAdaptiveSmoothing'' for details).']);
        end

        switch(lower(varargin{i})),
            case 'binwidth',
                binWidth = varargin{i+1};
                if ~helpers.isdscalar(binWidth, '>0')
                    error('Incorrect value for property ''nBins'' (type ''help analyses.mapAdaptiveSmoothing'' for details).');
                end
                haveBinWidth = true;
                i = i + 2;

            case 'nbins'
                nBins = varargin{i+1};
                if ~helpers.isivector(nBins, '>0') || length(nBins) > 2,
                    error('Incorrect value for property ''nBins'' (type ''help analyses.mapAdaptiveSmoothing'' for details).');
                end
                haveNBins = true;
                i = i + 2;

            case 'mintime',
                minTime = varargin{i+1};
                if ~helpers.isdscalar(minTime, '>=0'),
                    error('Incorrect value for property ''minTime'' (type ''help analyses.mapAdaptiveSmoothing'' for details).');
                end
                i = i + 2;

            case 'alphavalue'
                alphaValue = varargin{i+1};
                if ~helpers.isiscalar(alphaValue, '>0'),
                    error('Incorrect value for property ''alphaValue'' (type ''help analyses.mapAdaptiveSmoothing'' for details).');
                end
                i = i + 2;

            case 'limits'
                limits = varargin{i+1};
                if ~isvector(limits)
                    error('Incorrect value for property ''limits'' (type ''help analyses.mapAdaptiveSmoothing'' for details).');
                end
                i = i + 2;

            otherwise,
                error(['Unknown property ''' num2str(varargin{i}) ''' (type ''help analyses.mapAdaptiveSmoothing'' for details).']);
        end
    end

    % Some info about x, y and z
    if size(pos, 2) > 2
        posx = pos(:, 2);
        posy = pos(:, 3);
    else
        posx = pos(:, 1);
        posy = pos(:, 2);
    end

    if size(spkPos, 2) > 2
        spkx = spkPos(:, 2);
        spky = spkPos(:, 3);
    else
        spkx = spkPos(:, 1);
        spky = spkPos(:, 2);
    end

    if length(nBins) == 1
        nBins(2) = nBins(1);
    end

    % Calculate the border coordinates
    % xStart        Minimum x-coordinate for the path
    %
    % yStart        Minimum y-coordinate for the path
    %
    % xLength       Length of the arena in the x-direction [cm](for cylinder
    %               this equals the diameter)
    % yLength       Length of the arena in the y-direction [cm] (for cylinder
    %               this equals the diameter)
    maxX = nanmax(posx);
    maxY = nanmax(posy);
    xStart = nanmin(posx);
    yStart = nanmin(posy);
    xLength = maxX - xStart;
    yLength = maxY - yStart;

    if haveNBins && ~haveBinWidth
        binWidth = ceil(xLength / nBins(1));
        numColBins = nBins(1);
        numRowBins = nBins(2);
    else
        % Number of bins in each direction of the map
        numColBins = ceil(xLength / binWidth);
        numRowBins = ceil(yLength / binWidth);
    end

    halfBin = binWidth / 2;

    rowAxis = halfBin:binWidth:(numRowBins*binWidth)-halfBin;
    rowAxis = yStart + rowAxis';
    colAxis = halfBin:binWidth:(numColBins*binWidth)-halfBin;
    colAxis = xStart + colAxis';

    maxBins = max([numColBins, numRowBins]);

    map.x = colAxis;
    map.y = rowAxis;
    map.z = zeros(numRowBins, numColBins);
    posPdf = zeros(numRowBins, numColBins);

    if shape(1) == 1
        % Overall clue:
        %     - grow circle from r=1:maxBins (mult. of binWidth), tracking inside
        %     - stop at smallest rad. such that r >= alpha/samples*sqrt(spikes)
        radsqs = ((1:maxBins) * binWidth) .^ 2; % square radius in cm

        for i = 1:numColBins
            binPosX = colAxis(i);

            dist_sample_xdir = (posx - binPosX) .^ 2;
            dist_spike_xdir = (spkx - binPosX) .^ 2;

            for j = 1:numRowBins
                binPosY = rowAxis(j);

                % Calculate sample and spike distances from bin center
                dist_sample = dist_sample_xdir + (posy-binPosY).^2;
                dist_spike = dist_spike_xdir + (spky-binPosY).^2;

                found = 0;
                % Grow circle in increments of binWidth
                for r = 1:maxBins
                   n = nnz(dist_sample <= radsqs(r));
                   s = nnz(dist_spike <= radsqs(r));

                   if r >= alphaValue/(n*sqrt(s))
                       found = 1;
                       break;
                   end
                end

                % Set the rate for this bin
                map.z(j, i) = found * s/(n*sampleTime);
                posPdf(j, i) = found * n*sampleTime;
            end
        end
    else
        for ii = 1:numColBins
            binPosY = (yStart + binWidth/2);
            for jj = 1:numRowBins
                currentPosition = sqrt(binPosX^2 + binPosY^2);
                if currentPosition > shape(2)/2
                    map.z(numRowBins-jj+1, ii) = NaN;
                    posPdf(numRowBins-jj+1, ii) = NaN;
                else
                    n = 0;
                    s = 0;
                    for r = 1:maxBins
                        % Set the current radius of the circle
                        radius = r * binWidth;
                        % Number of samples inside the circle
                        n = insideCircle(binPosX, binPosY, radius, posx, posy);
                        % Number of spikes inside the circle
                        s = insideCircle(binPosX, binPosY, radius, spkx, spky);

                        if r >= alphaValue/(n*sqrt(s))
                            break;
                        end

                    end
                    % Set the rate for this bin
                    map.z(jj,ii) = s/(n*sampleTime);
                    posPdf(jj,ii) = n*sampleTime;

                end
                binPosY = binPosY + binWidth;
            end

            binPosX = binPosX + binWidth;
        end
    end

    map.z(posPdf < minTime) = NaN;
    posPdf = posPdf / nansum(nansum(posPdf));
end


function [ max_adif, max_adif_i, max_adif_j, max_rdif, max_rdif_i, max_rdif_j ] = ...
  p00_jac_check ( problem, option, nvar, x )

%*****************************************************************************80
%
%% P00_JAC_CHECK compares the jacobian with a finite difference estimate.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 September 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer PROBLEM, the problem index.
%
%    Input, integer OPTION, the option index.
%
%    Input, integer NVAR, the number of variables.
%
%    Input, real X(NVAR), the argument of the jacobian.
%
%    Output, real MAX_ADIF, the maximum absolute difference.
%
%    Output, integer MAX_ADIF_I, MAX_ADIF_J, the indices where
%    the maximmum absolute difference was found.
%
%    Output, real MAX_RDIF, the maximum relative difference.
%
%    Output, integer MAX_RDIF_I, MAX_RDIF_J, the indices where
%    the maximmum relative difference was found.
%
  rel = 0.0001;
%
%  Compute the jacobian.
%
  jac = p00_jac ( problem, option, nvar, x );
  jac_norm = max ( max ( abs ( jac ) ) );
%
%  Estimate the jacobian via finite differences.
%
  jac_dif = p00_jac_dif ( problem, option, nvar, x );
%
%  Compare the jacobians.
%
  max_rdif = 0.0;
  max_rdif_i = 0;
  max_rdif_j = 0;

  max_adif = 0.0;
  max_adif_i = 0;
  max_adif_j = 0;

  for i = 1 : nvar - 1
    for j = 1 : nvar

      dif = abs ( jac(i,j) - jac_dif(i,j) );

      if ( max_adif < dif )
        max_adif = dif;
        max_adif_i = i;
        max_adif_j = j;
      end

      if ( rel < abs ( jac(i,j) ) )
        if ( max_rdif * abs ( jac(i,j) ) < dif )
          max_rdif = dif / abs ( jac(i,j) );
          max_rdif_i = i;
          max_rdif_j = j;
        end
      end

    end
  end

  return
end


classdef ConstantHeadingModelX <  DynamicModelX
% ConstantHeadingModelX class
%
% Summary of ConstantHeadingModelX
% This is a class implementation of a time-varying 2D Linear-Gaussian
% Constant Heading Dynamic Model [1].
%
% The model is described by the following SDEs:
%   dx = v*cos(h)*dt                      | Position on X-axis (m)
%   dy = v*sin(h)*dt                      | Position on Y axis (m)
%   dv = q_vel*dW_t,  W_t~N(0,t)          | Absolute velocity  (m/s)
%   dh = q_head*dB_t, B_t~N(0,t)          | Heading            (rad)
%
% Or equivalently:
%   x(t) = f(x(t-1),Dt) + w(t),  w(t)~ N(0,Q)
%
% where: (using Euler discretisation of the SDEs)
%   x = [x; y; v; h]
%   F = [x + v*cos(h)*Dt; y+ v*sin(h)*Dt; v; h]
%   Q = [0, 0, 0, 0;
%        0, 0, 0, 0;
%        0, 0, q_vel*sqrt(Dt), 0;
%        0, 0, 0, q_head*sqrt(Dt)];
%
% ConstantVelocityModelX_2D Properties:
%   - VelocityErrVariance  Value of the velocity noise diffusion coefficient q_vel
%   - HeadingErrVariance   Value of the heading noise diffusion coefficient q_head
%   - TimeVariant          Value of the time variant Dt
%
% ConstantVelocityModelX_2D Methods:
%   - feval(~)         Equivalent to applying the model transition equations
%   - random(~)        Process noise sample generator function
%   - pdf(~)           Function to evaluate the probability p(x_k|x_{k-1}) of
%                       a set of new states, given a set of (particle) state vectors
%                       e.g. eval = @(xk,xkm1) mvnpdf(xk,xkm1,Q);
%   - covariance(~)    Returns the state covariance process Q_k
%
%  February 2018 Lyudmil Vladimirov, University of Liverpool

    properties
        VelocityErrVariance
        HeadingErrVariance
        TimeVariant
    end

    properties (Access = private)
        f = @(xkm1,Dt) [xkm1(1,:)+Dt*xkm1(3,:).*cos(xkm1(4,:));
                        xkm1(2,:)+Dt*xkm1(3,:).*sin(xkm1(4,:));
                        xkm1(3,:);
                        xkm1(4,:)];
        Q = @(Dt, q_vel, q_head) Dt*blkdiag(q_vel^2, q_vel^2, q_vel, q_head);
    end

    methods
        function this = ConstantHeadingModelX(varargin)
        % CONSTANTHEADINGMODELX Constructor method
        %
        % DESCRIPTION:
        % * ConstantHeadingModelX(q_vel,q_head) instantiates aan object handle
        %   configured with the provided velocity and heading process noise
        %   diffusion coefficients q_vel and q_head (scalar).
        % * ConstantVelocityModelX_2D(config) instantiates an object handle
        %   configured with the provided velocity and heading process noise
        %   diffusion coefficients config.VelocityErrVariance and
        %   config.HeadingErrVariance (scalar).
        % * ConstantVelocityModelX_2D(___,Name,Value) instantiates an object
        %   handle, configured with additional options specified by one or
        %   more Name,Value pair arguments.
        %
        % PARAMETERS
        % * VelocityErrVariance - (Required) The VelocityErrVariance is a scalar
        %   value describing the velocity noise diffusion coefficient.
        % * HeadingErrVariance - (Required) The VelocityErrVariance is a scalar
        %   value describing the heading noise diffusion coefficient (in rads/sec).
        %
        %  See also apply, rnd, pdf, covariance.

            % Call SuperClass method
            this@DynamicModelX;

            % Return quickly if no arguments are passed
            if(nargin==0)
                this.TimeVariant = 1;
                this.VelocityErrVariance = 1;
                this.HeadingErrVariance = 1;
            end

            % First check to see if a structure was received
            if(nargin==1)
                if(isstruct(varargin{1}))
                    this.VelocityErrVariance = varargin{1}.VelocityErrVariance;
                    this.HeadingErrVariance = varargin{1}.HeadingErrVariance;
                    this.TimeVariant = varargin{1}.TimeVariant;
                    return;
                end
            end

            % Otherwise, fall back to input parser
            parser = inputParser;
            parser.KeepUnmatched = true;
            parser.addOptional('VelocityErrVariance',[]);
            parser.addOptional('HeadingErrVariance',[]);
            parser.addOptional('TimeVariant',1);
            parser.parse(varargin{:});

            this.VelocityErrVariance = parser.Results.VelocityErrVariance;
            this.HeadingErrVariance = parser.Results.HeadingErrVariance;
            this.TimeVariant = parser.Results.TimeVariant;

            this.NumStateDims = 4;

        end

        function xk = feval(this, xkm1, wk, Dt)
        % FEVAL Propagate a given state through the dynamic model
        %
        % Parameters
        % ----------
        % xkm1: (NumStateDims x Ns) matrix, optional
        %   - A matrix, whose columns correspond to individual state vectors.
        %   - If not provided, then the state transition matrix (F) will be
        %     returned.
        % wk: (NumStateDims x Ns) matrix or boolean, optional
        %   - If wk is a boolean and set True, then the default model noise
        %     generation function (this.random()) will be used to generate the
        %     noise.
        %   - Otherwise, wk should be a matrix of equal dimensions to xk,
        %     where each column corresponds to the random noise which will
        %     be added to each state vector.
        %   - If not provided, then no noise will be added to the state vectors.
        % Dt: scalar, optional
        %   A time variant. (default=this.TimeVariant)
        %
        % Returns
        % -------
        % xk: (NumStateDims x Ns) matrix or function handle
        %   - If no parameters are passed to the function, then xk will be the
        %     state transition function f.
        %   - Else, xk will be a (NumStateDims x Ns) matrix, whose columns
        %     correspond to the columns of xkm1, each propagated through the
        %     dynamic model
        %
        % Usage
        % -----
        % * xk = FEVAL(this,xkm1,wk,Dt) returns the new state xk, produced by
        %   propagating the given state xkm1 through the dynamic model for
        %   time Dt and adding random noise wk. xk, xkm1 and wk must have
        %   the same dimensions, i.e (NumStateDim x Ns), where Ns is the
        %   number of states/columns in xkm1.
        % * xk = FEVAL(this,xkm1,wk) returns the new state xk, produced by
        %   propagating the given state xkm1 through the dynamic model for
        %   time this.TimeVariant and adding random noise wk.
        %   (i.e. Default Dt = this.TimeVariant)
        % * xk = FEVAL(this,xkm1) returns the new state xk, produced by
        %   propagating the given state xkm1 through the dynamic model for
        %   time this.TimeVariant without the addition of noise.
        %   (i.e. Default wk = 0)
        % * xk = FEVAL(this) returns the model's transition matrix resulting
        %   through the application of this.TimeVariant.
        %   (i.e. Default xkm1 = 1)
        %
        % See also PDF, RANDOM, COVARIANCE.

            switch(nargin)
                case 1
                    xk = this.f;
                    return;
%                     xkm1 = 1;
%                     wk   = 0;
%                     Dt = this.TimeVariant;
                case 2
                    wk   = 0;
                    Dt = this.TimeVariant;
                case 3
                    Dt = this.TimeVariant;
                    if(islogical(wk) && wk)
                        wk = this.random(size(xkm1,2),Dt);
                    end
            end

            % Compute result
            xk = this.f(xkm1,Dt) + wk;
        end

        function cov = covariance(this,Dt)
        % COVARIANCE Returns process covariance matrix.
        %
        % Parameters
        % ----------
        % Dt: scalar, optional
        %   A time variant. (default=this.TimeVariant)
        %
        % Returns
        % -------
        % cov: (NumStateDims x NumStateDims) matrix
        %   The process noise covariance matrix
        %
        % Usage
        % -----
        % * Qk = covariance(this,Dt) returns process covariance matrix, upon
        %   application of Dt.
        % * Qk = covariance(this) returns process covariance matrix, upon
        %   application of this.TimeVariant.
        % (i.e. Default Dt = this.TimeVariant)
        %
        % See also FEVAL, RANDOM, PDF.
            switch(nargin)
                case 1
                    Dt = this.TimeVariant;
            end

            % Return process covariance
            cov = this.Q(Dt,this.VelocityErrVariance, this.HeadingErrVariance); % Time variant
        end

        function wk = random(this, Ns, Dt)
        % RANDOM Generates random samples from the dynamic model's noise
        % distribution.
        %
        % Parameters
        % ----------
        % Ns: scalar, optional
        %   The number of samples to be generated.
        %   (default = 1)
        % Dt: scalar, optional
        %   A time variant.
        %   (default=this.TimeVariant)
        %
        % Returns
        % -------
        % wk: (NumStateDims x Ns)
        %   A matrix, whose columns correspond to indivual/independent
        %   noise samples.
        %
        % Usage
        % -----
        % * wk = random(this,Ns,Dt) generates and returns a set of Ns samples
        %   generated from the noise distribution of the dynamic model, i.e.
        %   wk ~ N(0,Q(Dt)), where Q is the noise covariance upon application
        %   of the time variant Dt.
        % * wk = random(this,Ns) generates and returns a set of Ns samples
        %   generated from the noise distribution of the dynamic model, i.e.
        %   wk ~ N(0,Q), where Q is the noise covariance upon application
        %   of the time variant this.TimeIndex.
        % (i.e. Default Dt = this.TimeVariant)
        %
        % See also FEVAL, PDF, COVARIANCE

            switch(nargin)
                case 1
                    Ns = 1;
                    Dt = this.TimeVariant;
                case 2
                    Dt = this.TimeVariant;
            end

            wk = mvnrnd(zeros(this.NumStateDims,1),this.Q(Dt,this.VelocityErrVariance, this.HeadingErrVariance),Ns)';
        end

        function prob = pdf(this, xk, xkm1, Dt)
        % PDF Evaluates the probability/likelihood p(x_k|x_{k-1}) of a
        % (set of) new state vector(s), given a (set of) old state vector(s)
        %
        % Parameters
        % ----------
        % xk: (NumStateDims x Ns) matrix
        %   A matrix, whose columns correspond to individual new state vectors.
        % xkm1: (NumStateDims x Np) matrix
        %   A matrix, whose columns correspond to individual old state vectors
        % Dt: scalar, optional
        %   A time variant.
        %   (default=this.TimeVariant)
        %
        % Returns
        % -------
        % prob: (Np x Ns) matrix
        %   A matrix, where each element (i,j) corresponds to the evaluated
        %   probability p(xk(:,j)|xkm1(:,i))
        %
        % Usage
        % -----
        % * prob = pdf(x_k, x_km1, Dt) evaluates and returns a (a x b)
        %   probability/likelihood matrix given the (NumStatesDim x a) x_k
        %   and (NumStatesDim x b) x_km1 state matrices. Dt is an optional
        %   argument (Default = this.TimeVariant) time variable, which is
        %   used for computing the model's covariance.
        %
        % See also FEVAL, RANDOM, COVARIANCE

            if(nargin<4)
                Dt = this.TimeVariant;
            end

            xk_km1 = this.feval(xkm1);
            prob = zeros(size(xk,2), size(xkm1,2));
            if(size(xkm1,2)>size(xk,2))
                for i=1:size(xk,2)
                    prob(i,:) = gauss_pdf(xk(:,i), xk_km1, this.Q(Dt,this.VelocityErrVariance,this.HeadingErrVariance));
                end
            else
                for i=1:size(xkm1,2)
                    prob(:,i) = gauss_pdf(xk, xk_km1(:,i), this.Q(Dt,this.VelocityErrVariance,this.HeadingErrVariance))';
                end
            end

        end
    end
end


function [fTafseq] = calc_Afseqlengtwomol(fPval1,fCval1,fKval1,fBgrate,nMod,fTsecaf,fPval2,fCval2,fKval2)
    % [fTafseq] = calc_Afseqlengtwomol(fPval1,fCval1,fKval1,fBgrate,nMod,fTsecaf,fPval2,fCval2,fKval2)
    % -------------------------------------------------------------------------
    % Determines the length of an aftershock sequence by determining the
    % intersection with the DAILY backgraund rate; MOL and nested MOL are allowed
    % (only one secondary sequence)
    %
    % Incoming:
    % fPval1   : Modified Omori law p-value
    % fCval1   : Modified Omori law c-value [days]
    % fKval1   : Modified Omori law k-value
    % fBgrate  : Daily background rate of events above Mc
    % nMod     : Aftershock sequence model choice
    %               1. Modified Omori law (MOL)
    %               2. MOL with one secondary sequence
    % fTsecaf
    % fPval2   : Modified Omori law p-value
    % fCval2   : Modified Omori law c-value [days]
    % fKval2   : Modified Omori law k-value

    % Outgoing:
    % fTafseq  : Length of aftershock sequence
    %
    % last update: 08.07.04
    % jochen.woessner@sed.ethz.ch

    % Time vector in days
    vT = [0:0.1:10000];
    vT = vT';

    % Check input
    if nargin < 4; disp('Not enough input parameters'); return; end
    if nargin == 4; nMod=1; end

    % Switch between MOL and nested MOL
    switch nMod
        case 1 % MOL
            vRate = abs(fKval1.*(vT+fCval1).^-fPval1-fBgrate);
            vSel = (min(vRate) == vRate);
            fTafseq = vT(vSel);
        case 2 % Nested Models
            vSelT = (vT >= fTsecaf);
            vRate1 = abs(fKval1.*(vT(~vSel)+fCval1).^-fPval1-fBgrate);
            vRate2 = abs(fKval1.*(vT(vSel)+fCval1).^-fPval1+fKval2.*(vT(vSel,:)-fTsecaf+fCval2).^-fPval2-fBgrate);
            vRate = [vRate1; vRate2];
            vSel = (min(vRate) == vRate);
            fTafseq = vT(vSel);
        otherwise
            disp('Not a valid model!');
    end
    % % Possible plot
    % figure
    % loglog(vT,vRate)
    % xlabel('Time [days after mainshock]')
    % ylabel('Daily rate of earthquakes')


% the Variable Span beamformer
% h = sum( gevdVec * gevdVec^H / (mu + lambda)) * PhiX * refVec
% input:    PhiN, (Nch, Nch, Nbin) the noise covariance matrix
%           PhiX, (Nch, Nch, Nbin) the speech covariance matrix
%           mu, the speech distortion/noise reduction trade-off parameter
%           span, 1 <= span <= Nch, dimension of the assumed source space
%           refMic, the reference microphone, default = 1
% output:   h, (Nch, Nbin)  the beamformer coefficients
% % Ziteng Wang @ 201812

function h = VarSpan(PhiX, PhiN, mu, span, refMic)
if nargin < 3
    mu = 1;             %%% typical value {0, 1}
elseif nargin < 4
    span = 1;           %%% default 1 for single-target
elseif nargin < 5
    refMic = 1;
end

[Nch, ~, Nbin] = size(PhiX);
refVec = zeros(Nch, 1);
refVec(refMic) = 1;
h = zeros(Nch, Nbin);

for bin = 1:Nbin
    if rcond(PhiN(:,:,bin)) < eps
        disp(['bin ' num2str(bin) ': Noise covariance ill-conditioned.']);
        PhiN(:,:,bin) = PhiN(:,:,bin) + 1e-10 * eye(Nch);
    end
    [vv, dd] = eig(PhiX(:,:,bin), PhiN(:,:,bin));
    [~, idx] = sort(diag(dd), 'descend');
    tmp = 0;
    for i = 1:span
        tmp = tmp + vv(:,idx(i)) * vv(:,idx(i))' / (mu + dd(idx(i),idx(i)));
    end
    h(:,bin) = tmp * PhiX(:,:,bin) * refVec;
end


%% Discrete-time reentry dynamics demonstration with non-linear filters
%
%  Description:
%    In this example various different non-linear filters and smoothers are
%    applied to reentry tracking problem . The filters used in this
%    demonstration are:
%      * Extended Kalman filter
%      * Unscented Kalman filter
%      * Gauss-Hermite Kalman filter (degree 3)
%      * Cubature Kalman filter
%    Additionally, the corresponding smoother results are also presented.
%
%  References:
%    Refer to the Toolbox documentation for details on the model.
%
%  See also:
%    ukf_predict1, ukf_update1, urts_smooth1,
%    ekf_predict1, ekf_update1, erts_smooth1, ghkf_predict, ghkf_update,
%    ghrts_smooth, ckf_predict, ckf_update, crts_smooth
%
%  Author:
%    Copyright (C) 2006 Simo Särkkä
%                  2007 Jouni Hartikainen
%                  2010 Arno Solin
%
%  Licence:
%    This software is distributed under the GNU General Public
%    Licence (version 2 or later); please refer to the file
%    Licence.txt, included with the software, for details.

%% Set up model parameters

  reentry_param;
  make_reentry_data;

  silent = 0;

  % Handles to dynamic and measurement model functions,
  % and to their derivatives.
  func_f  = @reentry_f;
  func_if = @reentry_if;
  func_df = @reentry_df_dx;
  func_h  = @reentry_h;
  func_dh = @reentry_dh_dx;

  % Initial values and space for EKF
  m = m0;
  P = P0;
  Q = L*Qc*L'*dt;

  MM_EKF = zeros(size(m,1),size(Y,2));
  PP_EKF = zeros(size(m,1),size(m,1),size(Y,2));
  VV_EKF = zeros(size(m,1),size(Y,2));
  EE_EKF = zeros(size(m,1),size(Y,2));

  % Check derivatives (should be OK)
  %der_check(func_a, func_da, 1, m0, {dt,b0,H0,Gm0,R0});
  %der_check(func_h, func_dh, 1, m0, {xr,yr});

  clf; clc;
  disp(['This is a demonstration for tracking a reentry vehicle ',...
        'using 1st order EKF and augmented UKF.'])
  disp(' ');

%% Extended Kalman filter

  fprintf('Running EKF...');
  % Filtering with EKF
  for k=1:size(Y,2)
    [m,P] = ekf_predict1(m,P,func_df,Q,func_f,[],{dt,b0,H0,Gm0,R0});
    [m,P] = ekf_update1(m,P,Y(:,k),func_dh,diag([vr va]),func_h,[],{xr,yr});
    MM_EKF(:,k) = m;
    PP_EKF(:,:,k) = P;
    VV_EKF(:,k) = diag(P);
    EE_EKF(:,k) = (X(:,k) - m).^2;
  end

  %
  % Calculate RMSE
  %
  ekf_rmse = sqrt(mean(sum((X(1:2,:)-MM_EKF(1:2,:)).^2)));
  ME_EKF = squeeze(PP_EKF(1,1,:)+PP_EKF(2,2,:));
  fprintf('Done!\n')

  fprintf('Running smoothers...');
  %
  % Smoother 1
  %
  [SM_ERTS,SP_ERTS] = erts_smooth1(MM_EKF,PP_EKF,func_df,Qc*dt,func_f,L,...
                          {dt,b0,H0,Gm0,R0});
  eks_rmse1 = sqrt(mean(sum((X(1:2,:)-SM_ERTS(1:2,:)).^2)));
  ME_ERTS = squeeze(SP_ERTS(1,1,:)+SP_ERTS(2,2,:));

  SV_ERTS = zeros(size(m,1),size(Y,2));
  SE_ERTS = zeros(size(m,1),size(Y,2));
  for k=1:size(Y,2)
    SV_ERTS(:,k) = diag(SP_ERTS(:,:,k));
    SE_ERTS(:,k) = (X(:,k) - SM_ERTS(:,k)).^2;
  end

  %
  % Smoother 2
  %
  [SM_ETF,SP_ETF] = etf_smooth1(MM_EKF,PP_EKF,Y,...
	func_df,Qc*dt,func_if,L,{dt,b0,H0,Gm0,R0},...
	func_dh,diag([vr va]),func_h,[],{xr,yr});

  eks_rmse2 = sqrt(mean(sum((X(1:2,:)-SM_ETF(1:2,:)).^2)));
  ME_ETF = squeeze(SP_ETF(1,1,:)+SP_ETF(2,2,:));
  fprintf('Done!\n');

%% Unscented Kalman filter

  fprintf('Running UKF...');

  % Initial values and space for (augmented) UKF
  m = m0;
  P = P0;
  MM_UKF = zeros(size(m,1),size(Y,2));
  PP_UKF = zeros(size(m,1),size(m,1),size(Y,2));
  VV_UKF = zeros(size(m,1),size(Y,2));
  EE_UKF = zeros(size(m,1),size(Y,2));

  % Filtering with UKF
  for k=1:size(Y,2)
    % Non-augmented UKF
    %[m,P] = ukf_predict1(m,P,func_a,Q,{dt,b0,H0,Gm0,R0});

    % Augmented UKF with separate sigma points
    %[m,P] = ukf_predict2(m,P,func_f,Qc*dt,{dt,b0,H0,Gm0,R0,L});

    %[m,P] = ukf_update2(m,P,Y(:,k),func_h,diag([vr va]),{xr,yr});

    % Augmented UKF with same sigma points for predict and update steps
    [m,P,X_s,w] = ukf_predict3(m,P,func_f,Qc*dt,diag([vr va]),d_param);
    [m,P] = ukf_update3(m,P,Y(:,k),func_h,diag([vr va]),X_s,w,h_param,h_param);

    MM_UKF(:,k) = m;
    PP_UKF(:,:,k) = P;
    VV_UKF(:,k) = diag(P);
    EE_UKF(:,k) = (X(:,k) - m).^2;
  end

  %
  % Calculate RMSE of UKF
  %
  ukf_rmse = sqrt(mean(sum((X(1:2,:)-MM_UKF(1:2,:)).^2)));
  ME_UKF = squeeze(PP_UKF(1,1,:)+PP_UKF(2,2,:));
  fprintf('Done!\n');

  fprintf('Running smoothers...');
  %
  % Smoother 1
  %
  [SM_URTS,SP_URTS] = urts_smooth1(MM_UKF,PP_UKF,func_f,Q,d_param);
  uks_rmse1 = sqrt(mean(sum((X(1:2,:)-SM_URTS(1:2,:)).^2)));
  ME_URTS = squeeze(SP_URTS(1,1,:)+SP_URTS(2,2,:));

  SV_URTS = zeros(size(m,1),size(Y,2));
  SE_URTS = zeros(size(m,1),size(Y,2));
  for k=1:size(Y,2)
    SV_URTS(:,k) = diag(SP_URTS(:,:,k));
    SE_URTS(:,k) = (X(:,k) - SM_URTS(:,k)).^2;
  end

  [SM_URTSb,SP_URTSb] = urts_smooth2(MM_UKF,PP_UKF,func_f,Qc*dt,d_param);
  uks_rmse1b = sqrt(mean(sum((X(1:2,:)-SM_URTSb(1:2,:)).^2)));
  ME_URTSb = squeeze(SP_URTSb(1,1,:)+SP_URTSb(2,2,:));

  %
  % Smoother 2
  %
  [SM_UTF,SP_UTF] = utf_smooth1(MM_UKF,PP_UKF,Y,...
	func_if,Qc*dt,d_param,...
	func_h,diag([vr va]),h_param);

  uks_rmse2 = sqrt(mean(sum((X(1:2,:)-SM_UTF(1:2,:)).^2)));
  ME_UTF = squeeze(SP_UTF(1,1,:)+SP_UTF(2,2,:));

  fprintf('Done!\n');

%% Gauss-Hermite Kalman filter

  fprintf('Running GHKF...');

  % Initial values and space for GHKF
  m = m0;
  P = P0;
  MM_GHKF = zeros(size(m,1),size(Y,2));
  PP_GHKF = zeros(size(m,1),size(m,1),size(Y,2));
  VV_GHKF = zeros(size(m,1),size(Y,2));
  EE_GHKF = zeros(size(m,1),size(Y,2));

  % Filtering with GHKF
  for k=1:size(Y,2)
    [m,P] = ghkf_predict(m,P,func_f,Q,{dt,b0,H0,Gm0,R0},3);
    [m,P] = ghkf_update(m,P,Y(:,k),func_h,diag([vr va]),{xr,yr},3);
    MM_GHKF(:,k) = m;
    PP_GHKF(:,:,k) = P;
    VV_GHKF(:,k) = diag(P);
    EE_GHKF(:,k) = (X(:,k) - m).^2;
  end

  %
  % Calculate RMSE
  %
  ghkf_rmse = sqrt(mean(sum((X(1:2,:)-MM_GHKF(1:2,:)).^2)));
  ME_GHKF = squeeze(PP_GHKF(1,1,:)+PP_GHKF(2,2,:));
  fprintf('Done!\n')

  fprintf('Running smoother...');
  %
  % Smoother
  %
  [SM_GHRTS,SP_GHRTS] = ghrts_smooth(MM_GHKF,PP_GHKF,func_f,Q,...
                          {dt,b0,H0,Gm0,R0},3);
  ghrts_rmse1 = sqrt(mean(sum((X(1:2,:)-SM_GHRTS(1:2,:)).^2)));
  ME_GHRTS = squeeze(SP_GHRTS(1,1,:)+SP_GHRTS(2,2,:));

  SV_GHRTS = zeros(size(m,1),size(Y,2));
  SE_GHRTS = zeros(size(m,1),size(Y,2));
  for k=1:size(Y,2)
    SV_GHRTS(:,k) = diag(SP_GHRTS(:,:,k));
    SE_GHRTS(:,k) = (X(:,k) - SM_GHRTS(:,k)).^2;
  end

  fprintf('Done!\n');

%% Cubature Kalman filter

  fprintf('Running CKF...');

  % Initial values and space for CKF
  m = m0;
  P = P0;
  MM_CKF = zeros(size(m,1),size(Y,2));
  PP_CKF = zeros(size(m,1),size(m,1),size(Y,2));
  VV_CKF = zeros(size(m,1),size(Y,2));
  EE_CKF = zeros(size(m,1),size(Y,2));

  % Filtering with CKF
  for k=1:size(Y,2)
    [m,P] = ckf_predict(m,P,func_f,Q,{dt,b0,H0,Gm0,R0});
    [m,P] = ckf_update(m,P,Y(:,k),func_h,diag([vr va]),{xr,yr});
    MM_CKF(:,k) = m;
    PP_CKF(:,:,k) = P;
    VV_CKF(:,k) = diag(P);
    EE_CKF(:,k) = (X(:,k) - m).^2;
  end

  %
  % Calculate RMSE
  %
  ckf_rmse = sqrt(mean(sum((X(1:2,:)-MM_CKF(1:2,:)).^2)));
  ME_CKF = squeeze(PP_CKF(1,1,:)+PP_CKF(2,2,:));
  fprintf('Done!\n')

  fprintf('Running smoother...');
  %
  % Smoother
  %
  [SM_CRTS,SP_CRTS] = crts_smooth(MM_CKF,PP_CKF,func_f,Q,...
                          {dt,b0,H0,Gm0,R0});
  crts_rmse1 = sqrt(mean(sum((X(1:2,:)-SM_CRTS(1:2,:)).^2)));
  ME_CRTS = squeeze(SP_CRTS(1,1,:)+SP_CRTS(2,2,:));

  SV_CRTS = zeros(size(m,1),size(Y,2));
  SE_CRTS = zeros(size(m,1),size(Y,2));
  for k=1:size(Y,2)
    SV_CRTS(:,k) = diag(SP_CRTS(:,:,k));
    SE_CRTS(:,k) = (X(:,k) - SM_CRTS(:,k)).^2;
  end

  fprintf('Done!\n');

%% Visualize methods

  if ~silent
    aa = 0.02*(-1:0.1:4);
    cx = R0 * cos(aa);
    cy = R0 * sin(aa);
    % Plot EKF estimate
    plot(xr,yr,'ko',cx,cy,'r-',X(1,:),X(2,:),'g-',...
         MM_EKF(1,:),MM_EKF(2,:),'k--');
    legend('Radar','Earth','True','Estimate');
    title('Filtering result with EKF');
    disp(' ');
    disp('Filtering result with EKF is now displayed.');
    disp(' ');
    disp('<press any key to see the estimation error of x_1>');
    pause;

    % Error for x_1 with EKF
    semilogy(T,EE_EKF(1,:),'g-',T,VV_EKF(1,:),'b--',...
	     T,SE_ERTS(1,:),'r-',T,SV_ERTS(1,:),'k--');
    legend('EKF-RMSE','EKF-STDE',...
	   'ERTS-RMSE1','ERTS-STDE1');
    title('RMSE of estimating x_1 with EKF and ERTS')
    clc;
    disp('RMSE of estimating x_1 with EKF and ERTS is now displayed.');
    disp(' ');
    disp('<press any key to see the estimation error of x_5>');
    pause

    % Error for x_5 with EKF
    semilogy(T,EE_EKF(5,:),'g-',T,VV_EKF(5,:),'b--',...
	     T,SE_ERTS(5,:),'r-',T,SV_ERTS(5,:),'k--');
    legend('EKF-RMSE','EKF-STDE',...
	   'ERTS-RMSE1','ERTS-STDE1');
    title('RMSE of estimating x_5 with EKF and ERTS')
    clc;
    disp('RMSE of estimating x_5 with EKF and ERTS is now displayed.');
    disp(' ');
    disp('<press any key to see the estimation error of x_5>');
    pause

    % Plot UKF estimate
    plot(xr,yr,'ko',cx,cy,'r-',X(1,:),X(2,:),'g-',...
         MM_UKF(1,:),MM_UKF(2,:),'k--');
    legend('Radar','Earth','True','Estimate');
    clc;
    disp('Filtering result with UKF is now displayed.');
    disp(' ');
    disp('<press any key to see the estimation error of x_1>');
    pause;

    % Error for x_1 with UKF
    semilogy(T,EE_UKF(1,:),'g-',T,VV_UKF(1,:),'b--',...
             T,SE_URTS(1,:),'r-',T,SV_URTS(1,:),'k--');
    legend('UKF-RMSE','UKF-STDE','URTS-RMSE','URTS-STDE');
    title('RMSE of estimating x_1 with UKF and URTS')
    clc;
    disp('RMSE of estimating x_1 with EKF and ERTS is now displayed.');
    disp(' ');
    disp('<press any key to see the estimation error of x_5>');
    pause;

    % Error for x_5 with UKF
    semilogy(T,EE_UKF(5,:),'g-',T,VV_UKF(5,:),'b--',...
             T,SE_URTS(5,:),'r-',T,SV_URTS(5,:),'k--');
    legend('UKF-RMSE','UKF-STDE','URTS-RMSE','URTS-STDE');
    title('RMSE of estimating x_5 with UKF and URTS')
    clc;
    disp('RMSE of estimating x_5 with EKF and ERTS is now displayed.');

    % Plot GHKF estimate
    plot(xr,yr,'ko',cx,cy,'r-',X(1,:),X(2,:),'g-',...
         MM_GHKF(1,:),MM_GHKF(2,:),'k--');
    legend('Radar','Earth','True','Estimate');
    clc;
    disp('Filtering result with GHKF is now displayed.');
    disp(' ');
    disp('<press any key to see the estimation error of x_1>');
    pause;

    % Error for x_1 with GHKF
    semilogy(T,EE_UKF(1,:),'g-',T,VV_UKF(1,:),'b--',...
             T,SE_URTS(1,:),'r-',T,SV_URTS(1,:),'k--');
    legend('GHKF-RMSE','GHKF-STDE','GHRTS-RMSE','GHRTS-STDE');
    title('RMSE of estimating x_1 with GHKF and GHRTS')
    clc;
    disp('RMSE of estimating x_1 with GHKF and GHRTS is now displayed.');
    disp(' ');
    disp('<press any key to see the estimation error of x_5>');
    pause;

    % Error for x_5 with GHKF
    semilogy(T,EE_GHKF(5,:),'g-',T,VV_GHKF(5,:),'b--',...
             T,SE_GHRTS(5,:),'r-',T,SV_GHRTS(5,:),'k--');
    legend('GHKF-RMSE','GHKF-STDE','GHRTS-RMSE','GHRTS-STDE');
    title('RMSE of estimating x_5 with GHKF and GHRTS')
    clc;
    disp('RMSE of estimating x_5 with EKF and GHRTS is now displayed.');

    % Plot CKF estimate
    plot(xr,yr,'ko',cx,cy,'r-',X(1,:),X(2,:),'g-',...
         MM_CKF(1,:),MM_CKF(2,:),'k--');
    legend('Radar','Earth','True','Estimate');
    clc;
    disp('Filtering result with CKF is now displayed.');
    disp(' ');
    disp('<press any key to see the estimation error of x_1>');
    pause;

    % Error for x_1 with CKF
    semilogy(T,EE_UKF(1,:),'g-',T,VV_UKF(1,:),'b--',...
             T,SE_URTS(1,:),'r-',T,SV_URTS(1,:),'k--');
    legend('CKF-RMSE','CKF-STDE','CRTS-RMSE','CRTS-STDE');
    title('RMSE of estimating x_1 with CKF and CRTS')
    clc;
    disp('RMSE of estimating x_1 with CKF and CRTS is now displayed.');
    disp(' ');
    disp('<press any key to see the estimation error of x_5>');
    pause;

    % Error for x_5 with CKF
    semilogy(T,EE_CKF(5,:),'g-',T,VV_CKF(5,:),'b--',...
             T,SE_CRTS(5,:),'r-',T,SV_CRTS(5,:),'k--');
    legend('CKF-RMSE','CKF-STDE','CRTS-RMSE','CRTS-STDE');
    title('RMSE of estimating x_5 with CKF and CRTS')
    clc;
    disp('RMSE of estimating x_5 with EKF and CRTS is now displayed.');

  end

%% Show RMSE errors for each method

  disp(' ');
  disp('RMS errors:');
  fprintf('EKF-RMSE   = %.6f [%.6f]\n',ekf_rmse,sqrt(mean(ME_EKF)));
  fprintf('ERTS-RMSE  = %.6f [%.6f]\n',eks_rmse1,sqrt(mean(ME_ERTS)));
  fprintf('ETF-RMSE   = %.6f [%.6f]\n',eks_rmse2,sqrt(mean(ME_ETF)));
  fprintf('UKF-RMSE   = %.6f [%.6f]\n',ukf_rmse,sqrt(mean(ME_UKF)));
  fprintf('URTS1-RMSE = %.6f [%.6f]\n',uks_rmse1,sqrt(mean(ME_URTS)));
  fprintf('URTS2-RMSE = %.6f [%.6f]\n',uks_rmse1b,sqrt(mean(ME_URTSb)));
  fprintf('UTF-RMSE   = %.6f [%.6f]\n',uks_rmse2,sqrt(mean(ME_UTF)));

  % Cubature methods
  fprintf('GHKF-RMSE  = %.6f [%.6f]\n',ghkf_rmse,sqrt(mean(ME_GHKF)));
  fprintf('GHRTS-RMSE = %.6f [%.6f]\n',ghrts_rmse1,sqrt(mean(ME_GHRTS)));
  fprintf('CKF-RMSE   = %.6f [%.6f]\n',ckf_rmse,sqrt(mean(ME_CKF)));
  fprintf('CRTS-RMSE  = %.6f [%.6f]\n',crts_rmse1,sqrt(mean(ME_CRTS)));


function W = weight_conversion(W, wcm)
% WEIGHT_CONVERSION    Conversion of weights in input matrix
%
%   W_bin = weight_conversion(W, 'binarize');
%   W_nrm = weight_conversion(W, 'normalize');
%   L = weight_conversion(W, 'lengths');
%   W_fix = weight_conversion(W, 'autofix');
%
%   This function may either binarize an input weighted connection matrix,
%   normalize an input weighted connection matrix, convert an input
%   weighted connection matrix to a weighted connection-length matrix, or
%   fix common connection problems in binary or weighted connection matrices.
%
%       Binarization converts all present connection weights to 1.
%
%       Normalization rescales all weight magnitudes to the range [0,1] and
%   should be done prior to computing some weighted measures, such as the
%   weighted clustering coefficient.
%
%       Conversion of connection weights to connection lengths is needed
%   prior to computation of weighted distance-based measures, such as
%   distance and betweenness centrality. In a weighted connection network,
%   higher weights are naturally interpreted as shorter lengths. The
%   connection-lengths matrix here is defined as the inverse of the
%   connection-weights matrix.
%
%       Autofix removes all Inf and NaN values, remove all self connections
%   (sets all weights on the main diagonal to 0), ensures that symmetric matrices
%   are exactly symmetric (by correcting for round-off error), and ensures that
%   binary matrices are exactly binary (by correcting for round-off error).
%
%   Inputs: W           binary or weighted connectivity matrix
%           wcm         weight-conversion command - possible values:
%                           'binarize'      binarize weights
%                           'normalize'     normalize weights

%                           'lengths'       convert weights to lengths
%                           'autofix'       fixes common weights problems
%
%   Output: W_          output connectivity matrix
%
%
%   Mika Rubinov, U Cambridge, 2012

%   Modification History:
%   Sep 2012: Original
%   Jan 2015: Added autofix feature.

switch wcm
    case 'binarize'
        W=double(W~=0);         % binarize
    case 'normalize'
        W=W./max(abs(W(:)));    % scale by maximal weight
    case 'lengths'
        E=find(W);
        W(E)=1./W(E);           % invert weights
    case 'autofix'
        % clear diagonal
        n = length(W);
        W(1:n+1:end)=0;

        % remove Infs and NaNs
        idx = isnan(W) | isinf(W);
        if any(any(idx));
            W(idx)=0;
        end

        % ensure exact binariness
        U = unique(W);
        if numel(U)>1
            idx_0 = abs(U)<1e-10;
            idx_1 = abs(U-1)<1e-10;
            if all(idx_0 | idx_1)
                W(idx_0)=0;
                W(idx_1)=1;
            end
        end

        % ensure exact symmetry
        if ~isequal(W,W.');
            if max(max(abs(W-W.'))) < 1e-10;
                W=(W+W).'/2;
            end
        end
    otherwise
        error('Unknown weight-conversion command.')
end


function [ r, seed ] = r8vec_uniform_01 ( n, seed )

%*****************************************************************************80
%
%% R8VEC_UNIFORM_01 returns a unit pseudorandom R8VEC.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 September 2006
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Paul Bratley, Bennett Fox, Linus Schrage,
%    A Guide to Simulation,
%    Springer Verlag, pages 201-202, 1983.
%
%    Bennett Fox,
%    Algorithm 647:
%    Implementation and Relative Efficiency of Quasirandom
%    Sequence Generators,
%    ACM Transactions on Mathematical Software,
%    Volume 12, Number 4, pages 362-376, 1986.
%
%    Peter Lewis, Allen Goodman, James Miller,
%    A Pseudo-Random Number Generator for the System/360,
%    IBM Systems Journal,
%    Volume 8, pages 136-143, 1969.
%
%  Parameters:
%
%    Input, integer N, the number of entries in the vector.
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, real R(N), the vector of pseudorandom values.
%
%    Output, integer SEED, an updated seed for the random number generator.
%
  r = zeros ( n, 1 );

  if ( seed == 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R8VEC_UNIFORM_01 - Fatal error!\n' );
    fprintf ( 1, '  Input SEED = 0!\n' );
    error ( 'R8VEC_UNIFORM_01 - Fatal error!' );
  end

  for i = 1 : n

    k = floor ( seed / 127773 );

    seed = 16807 * ( seed - k * 127773 ) - k * 2836;

    if ( seed < 0 )
      seed = seed + 2147483647;
    end

    r(i) = seed * 4.656612875E-10;

  end

  return
end


function [centroids, idx] = runkMeans(X, initial_centroids, ...
                                      max_iters, plot_progress)
%RUNKMEANS runs the K-Means algorithm on data matrix X, where each row of X
%is a single example
%   [centroids, idx] = RUNKMEANS(X, initial_centroids, max_iters, ...
%   plot_progress) runs the K-Means algorithm on data matrix X, where each
%   row of X is a single example. It uses initial_centroids used as the
%   initial centroids. max_iters specifies the total number of interactions
%   of K-Means to execute. plot_progress is a true/false flag that
%   indicates if the function should also plot its progress as the
%   learning happens. This is set to false by default. runkMeans returns
%   centroids, a Kxn matrix of the computed centroids and idx, a m x 1
%   vector of centroid assignments (i.e. each entry in range [1..K])
%

% Set default value for plot progress
if ~exist('plot_progress', 'var') || isempty(plot_progress)
    plot_progress = false;
end

% Plot the data if we are plotting progress
if plot_progress
    figure;
    hold on;
end

% Initialize values
[m n] = size(X);
K = size(initial_centroids, 1);
centroids = initial_centroids;
previous_centroids = centroids;
idx = zeros(m, 1);

% Run K-Means
for i=1:max_iters

    % Output progress
    fprintf('K-Means iteration %d/%d...\n', i, max_iters);
    if exist('OCTAVE_VERSION')
        fflush(stdout);
    end

    % For each example in X, assign it to the closest centroid
    idx = findClosestCentroids(X, centroids);

    % Optionally, plot progress here
    if plot_progress
        plotProgresskMeans(X, centroids, previous_centroids, idx, K, i);
        previous_centroids = centroids;
        fprintf('Press enter to continue.\n');
        pause;
    end

    % Given the memberships, compute new centroids
    centroids = computeCentroids(X, idx, K);
end

% Hold off if we are plotting progress
if plot_progress
    hold off;
end

end


%--------------------------------------------------------------------------
%   p3(signal,mark)
%--------------------------------------------------------------------------
%   功能：
%   复信号绘制画图工具，用处较多
%   常见于查看时域复信号，傅里叶变换后信号的幅度和相位信息
%--------------------------------------------------------------------------
%   输入：
%           signal              按照列排列复信号
%           mark                绘制标记，例如'r--','k-.'
%--------------------------------------------------------------------------
%   例子：
%   暂无
%--------------------------------------------------------------------------
function p3(signal,mark)
if nargin==2
    plot3(1:size(signal,1),real(signal),imag(signal),mark);grid on
    xlabel('点数');ylabel('实部');zlabel('虚部')
elseif nargin ==1
    plot3(1:size(signal,1),real(signal),imag(signal));grid on
    xlabel('点数');ylabel('实部');zlabel('虚部')
end


% In this version of this function we try to return a (NxNxN) cube, while still
% keeping the aspect ratio

function [voxel_model] = pointcloud_to_voxels_3(model,N,bbox,voxelization_type,binaryMax)

if(isempty(model.x))
    warning('empty model passed to this function!');
    voxel_model = zeros([N N N]);

    return;
end

if(~exist('bbox','var'))
    bbox = [];
end

if(~exist('voxelization_type','var'))
    voxelization_type = 'binary';
end

if(~exist('binaryMax','var'))
    binaryMax = 1; %This is only for backward compatibilty. Otherwise it should be 255 to be compatible with 'intensity' type, in addition to other benefits!
end

[X,Y,Z] = model_to_components(model);

%--- Obtain the boundaries of the object, taking into account the bbox if
%needed
if(isempty(bbox))
    mX = min(X);
    MX = max(X);
    mY = min(Y);
    MY = max(Y);
    mZ = min(Z);
    MZ = max(Z);
else
    %-- in case of a bounding box, we only need to check the 4 corners
    x1 = bbox(1);
    y1 = bbox(2);
    z1 = bbox(3);
    w = bbox(4);
    h = bbox(5);
    d = bbox(6);
    x2 = x1+w;
    y2 = y1+h;
    z2 = z1+d;
    phi = bbox(7);

    cornersX = [x1 x1 x2 x2];
    cornersY = [y1 y2 y1 y2];
    xc = mean([x1,x2]);
    yc = mean([y1,y2]);

    % rotate the coordinates around the center
    cornersX = cornersX-repmat(xc,1,size(cornersX,2));
    cornersY = cornersY-repmat(yc,1,size(cornersX,2));
    rotated = [cosd(phi) -sind(phi); sind(phi) cosd(phi)]*[cornersX;cornersY];
    cornersX = rotated(1,:)+repmat(xc,1,size(cornersX,2));
    cornersY = rotated(2,:)+repmat(yc,1,size(cornersY,2));

    %-- now pick the limits of the corner coordinates
    mX = min(cornersX);
    MX = max(cornersX);
    mY = min(cornersY);
    MY = max(cornersY);
    mZ = min([z1,z2]); %z is interpreted differently, as the rotation is only around z
    MZ = max([z1,z2]);

end

maxmax = max([MX-mX,MY-mY,MZ-mZ]);
if(maxmax == 0) maxmax = 1; end %This happens only when there's a single point in the scene (the empty scene case has been taken care of before);

N1 = round(N*(MX-mX)/maxmax);
N2 = round(N*(MY-mY)/maxmax);
N3 = round(N*(MZ-mZ)/maxmax);

% This happens when the input point cloud is in in fact less than 3D
if(~N1), N1 = 1;end;
if(~N2), N2 = 1;end;
if(~N3), N3 = 1;end;

voxel_model = zeros(N1,N2,N3);

ax = 1/(MX-mX)*(N1-1);
bx = -mX;
ay = 1/(MY-mY)*(N2-1);
by = -mY;
az = 1/(MZ-mZ)*(N3-1);
bz = -mZ;

% This happens when the input point cloud is in fact less than 3D
if isnan(ax), ax = 0; end;
if isnan(ay), ay = 0; end;
if isnan(az), az = 0; end;

X2 = round(ax*(X+bx))+1;
Y2 = round(ay*(Y+by))+1;
Z2 = round(az*(Z+bz))+1;

switch(voxelization_type )
    case 'binary'
        voxel_model(sub2ind(size(voxel_model),X2,Y2,Z2)) = binaryMax ;
    case 'density'
        [C,ia,ic] = unique([X2,Y2,Z2],'rows');
        h = hist(ic,1:max(ic));
        ind = sub2ind(size(voxel_model),C(:,1),C(:,2),C(:,3));
        voxel_model(ind) = h;

        %max normalization
        voxel_model = voxel_model ./ max(voxel_model(:)) * 255;
    otherwise
        error('voxelization type not supported');
end

% Add the required symmetric zero padding to make it a cube
Nzx = floor( (N - N1)/2 );
Nzy = floor( (N - N2)/2 );
Nzz = floor( (N - N3)/2 );
final = zeros(N,N,N);
final(Nzx+1:Nzx+N1 , Nzy+1:Nzy+N2 ,Nzz+1:Nzz+N3) = voxel_model;
voxel_model = final;


function [x, infos] = convex_mu_nmf(V, rank, in_options)
% (Kernel) Convex multiplicative update for non-negative matrix factorization ((Kernel-)Convex-MU-NMF).
%
% The problem of interest is defined as
%
%  (standard)   min || VWH - V ||_F^2,
%               where
%               {W, H} >= 0, and
%
%  (kernel)     min || K(V,V) WH - K(V,V) ||_F^2,
%               where
%               {W, H} >= 0, and K(V,V) is a kernel matrix.
%
%
% Given a V with mixed-signs, factorized non-negative matrices {W, H} are calculated.
%
%
% Inputs:
%       matrix      V
%       rank        rank
%       in_options
%           sub_mode: 'std' (default) or 'kernel'
%           kernel  : kernel type (rbf (default), polynomial, linear, sigmoid)
%
% Output:
%       x           solution of x
%       infos       information
%
% References:
%       C. Ding, T. Li, and M.I. Jordan,
%       "Convex and semi-nonnegative matrix factorizations,"
%       IEEE Transations on Pattern Analysis and Machine Intelligence,
%       vol. 32, no. 1, pp. 45-55, 2010.
%
%       T. Li and C. Ding,
%       "The Relationships Among Various Nonnegative Matrix Factorization Methods for Clustering,"
%       International Conference on Data Mining,
%       2006.
%
%       Y. Li and A. Ngom,
%       "A New Kernel Non-Negative Matrix Factorization and Its Application in Microarray Data Analysis,"
%       CIBCB,
%       2012.
%
%
% This file is part of NMFLibrary
%
% This file has been ported from
% convexnmfrule.m and kernelconvexnmf.m at https://sites.google.com/site/nmftool/home/source-code
% by Yifeng Li.
%
%%%%
% Copyright (C) <2012>  <Yifeng Li>
%
% 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
% 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/>.
%
% Contact Information:
% Yifeng Li
% University of Windsor
% li11112c@uwindsor.ca; yifeng.li.cn@gmail.com
% May 01, 2011
%%%%
%
%
% This file was originally created by Graham Grindla.
%
% 2010-01-14 Graham Grindlay (grindlay@ee.columbia.edu)
%
% Copyright (C) 2008-2028 Graham Grindlay (grindlay@ee.columbia.edu)
%
% 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/>.
%
%
% Ported by H.Kasai on June 30, 2022
%
% Change log:
%

    % set dimensions and samples
    [m, n] = size(V);

    % set local options
    local_options = [];
    local_options.sub_mode = 'std';
    local_options.kernel ='rbf';
    local_options.kernel_param = [];
    local_options.special_nmf_cost = @(V, W, H, R, options) convex_mu_nmf_cost_func(V, W, H, R, options);
    local_options.special_init_factors = @(V, rank, wh_flag, r_flag, options) convex_mu_initialize(V, rank, wh_flag, r_flag, options);

    % check input options
    if ~exist('in_options', 'var') || isempty(in_options)
        in_options = struct();
    end
    % merge options
    options = mergeOptions(get_nmf_default_options(), local_options);
    options = mergeOptions(options, in_options);

    % initialize factors
    init_options = options;
    [init_factors, ~] = generate_init_factors(V, rank, init_options);
    W = init_factors.W;
    H = init_factors.H;

    % initialize
    epoch = 0;
    grad_calc_count = 0;

    % initialize for this algorithm
    if strcmp(options.sub_mode, 'std')
        Ak = V' * V;
        X = V;
        options.name = 'std';
    elseif strcmp(options.sub_mode, 'kernel')
        Ak = computeKernelMatrix(V, V, options);
        X = Ak;
        options.name = sprintf('kernel-%s', options.kernel);
    else
        error('Invalid sub_mode')
    end
    method_name = sprintf('Convex-MU (%s)', options.name);

    if options.verbose > 0
        fprintf('# %s: started ...\n', method_name);
    end

    Ap = (abs(Ak) + Ak) ./ 2;
    An = (abs(Ak) - Ak) ./ 2;

    % store initial info
    clear infos;
    [infos, f_val, optgap] = store_nmf_info(X, W, H, [], options, [], epoch, grad_calc_count, 0);

    if options.verbose > 1
        fprintf('Convex-MU (%s): Epoch = 0000, cost = %.16e, optgap = %.4e\n', options.name, f_val, optgap);
    end

    % set start time
    start_time = tic();

    % main loop
    while true

        % check stop condition
        [stop_flag, reason, max_reached_flag] = check_stop_condition(epoch, infos, options);
        if stop_flag
            display_stop_reason(epoch, infos, options, method_name, reason, max_reached_flag);
            break;
        end

        ApW = Ap * W;
        AnW = An * W;
        WH  = W * H;

        % update H
        H = H .* sqrt((ApW' + AnW' * WH) ./ (AnW' + ApW' * WH));
        H = max(H, eps);
        HHt = H * H';

        % update W
        W = W .* sqrt((Ap * H' + AnW * HHt) ./ (An * H' + ApW * HHt));
        W = max(W, eps);

        % measure gradient calc count
        grad_calc_count = grad_calc_count + m*n;

        % measure elapsed time
        elapsed_time = toc(start_time);

        % update epoch
        epoch = epoch + 1;

        % store info
        infos = store_nmf_info(X, W, H, [], options, infos, epoch, grad_calc_count, elapsed_time);

        % display info
        display_info(method_name, epoch, infos, options);

    end

    x.W = W;
    x.H = H;

end

function res = convex_mu_nmf_cost_func(V, W, H, R, options)

    res = norm(V - V * W * H, 'fro');

end

function [init_factors, init_factors_opts] = convex_mu_initialize(V, rank, wh_flag, r_flag, options)

    init_factors_opts = [];

    [~, n] = size(V);

    if wh_flag
        H = rand(rank, n);
        W = H' * diag(1./sum(H,2)');
    else
        W = options.x_init.W;
        H = options.x_init.H;
    end

    init_factors.W = W;
    init_factors.H = H;
    init_factors.R = zeros(size(V));

end


function BrainNet=tHOFC(BOLD)
% Topographical similarity-based high-order FC network construction
%
% Input:
% BOLD. A cell array with size of N x 1, each cell is a matrix of BOLD signals (#time points x #ROIs) from one of N subject
%       Each subject may have different #time points but should have the same #ROIs
%
% Output:
% BrainNet. High-order brain networks (#ROIs x #ROIs x #Subjects)
%
% By Han Zhang, hanzhang@med.unc.edu
% IDEA lab https://www.med.unc.edu/bric/ideagroup
% Department of Radiology and BRIC, UNC Chapel Hill
%
% Cite: 1. Zhang, H., Chen, X., Shi, F., Li, G., Kim, M., Giannakopoulos, P., Haller, S., Shen, D., Jun 28, 2016.
%          Topographic Information based High-Order Functional Connectivity and its Application in Abnormality Detection
%          for Mild Cognitive Impairment, Journal of Alzheimer's Disease. In press. DOI: 10.3233/JAD-160092.
%       2. Zhang, H., Chen, X., Zhang, Y., Shen, D., Test-retest reliability of �high-order� functional connectivity
%          in young healthy adults. Frontiers in Neuroscience, In Press. DOI: 10.3389/fnins.2017.00439.

[nTime,nROI]=size(BOLD{1});
nSubj=length(BOLD);
BrainNet=zeros(nROI,nROI,nSubj,'single');

% Compute low-order FC network
pc=PC(BOLD);   % Pearson's correlation-based network
pc=atanh(pc);       % Fisher z transformation

% Compute topographical similarity which defines high-order FC
for ns=1:nSubj
    tempNet=zeros(nROI,nROI);
    for i=1:nROI
        for j=(i+1):nROI
            temp1=pc(:,i,ns);
            temp2=pc(:,j,ns);
            temp1([i,j])=[];                    % ignore self connections
            temp2([i,j])=[];                    % ignore self connections
            tempNet(i,j)=corr(temp1,temp2);     % only upper triangle
        end
    end
    BrainNet(:,:,ns)=tempNet+tempNet';          % form symmetric adjancency matrix (main diag are zeros)
end


function wrench = fromParametrizationToForces(xi, Config)

    % delta feet size
    delta_x   =  (Config.feet_size(2,2) - Config.feet_size(2,1))/2;
    delta_x_0 = -(Config.feet_size(2,2) + Config.feet_size(2,1))/2;
    delta_y   =  (Config.feet_size(1,2) - Config.feet_size(1,1))/2;
    delta_y_0 =  (Config.feet_size(1,2) + Config.feet_size(1,1))/2;

    % linear forces
    Fz = exp(xi(3)) + Config.fZmin;
    Fx = sqrt(Config.forceFrictionCoefficient)*tanh(xi(1))*Fz/sqrt(1 + tanh(xi(2))^2);
    Fy = sqrt(Config.forceFrictionCoefficient)*tanh(xi(2))*Fz/sqrt(1 + tanh(xi(1))^2);

    % moments
    Mz = Config.torsionalFrictionCoefficient*tanh(xi(6))*Fz;
    Mx = (delta_x*tanh(xi(4)) + delta_x_0)*Fz;
    My = (delta_y*tanh(xi(5)) + delta_y_0)*Fz;

    % compute the final wrench
    wrench = [Fx; Fy; Fz; Mx; My; Mz];
end


% Image super-resolution using sparse representation
% Example code
%
% Nov. 2, 2007. Jianchao Yang
% IFP @ UIUC
%
% Revised version. April, 2009.
%
% Reference
% Jianchao Yang, John Wright, Thomas Huang and Yi Ma. Image superresolution
% via sparse representation of raw image patches. IEEE Computer Society
% Conference on Computer Vision and Pattern Recognition (CVPR), 2008.
%
% For any questions, email me by jyang29@illinois.edu

clear all;
clc;

addpath('Solver');
addpath('Sparse coding');

% =====================================================================
% specify the parameter settings

patch_size = 3; % patch size for the low resolution input image
overlap = 1; % overlap between adjacent patches
lambda = 0.1; % sparsity parameter
zooming = 3; % zooming factor, if you change this, the dictionary needs to be retrained.

tr_dir = 'Data/training'; % path for training images
skip_smp_training = true; % sample training patches
skip_dictionary_training = true; % train the coupled dictionary
num_patch = 50000; % number of patches to sample as the dictionary
codebook_size = 1024; % size of the dictionary

regres = 'L1'; % 'L1' or 'L2', use the sparse representation directly, or use the supports for L2 regression
% =====================================================================
% training coupled dictionaries for super-resolution

if ~skip_smp_training,
    disp('Sampling image patches...');
    [Xh, Xl] = rnd_smp_dictionary(tr_dir, patch_size, zooming, num_patch);
    save('Data/Dictionary/smp_patches.mat', 'Xh', 'Xl');
    skip_dictionary_training = false;
end;

if ~skip_dictionary_training,
    load('Data/Dictionary/smp_patches.mat');
    [Dh, Dl] = coupled_dic_train(Xh, Xl, codebook_size, lambda);
    save('Data/Dictionary/Dictionary.mat', 'Dh', 'Dl');
else
    load('Data/Dictionary/Dictionary.mat');
end;

% =====================================================================
% Process the test image

fname = 'Data/Test/1.bmp';
testIm = imread(fname); % testIm is a high resolution image, we downsample it and do super-resolution

if rem(size(testIm,1),zooming) ~=0,
    nrow = floor(size(testIm,1)/zooming)*zooming;
    testIm = testIm(1:nrow,:,:);
end;
if rem(size(testIm,2),zooming) ~=0,
    ncol = floor(size(testIm,2)/zooming)*zooming;
    testIm = testIm(:,1:ncol,:);
end;

imwrite(testIm, 'Data/Test/high.bmp', 'BMP');

lowIm = imresize(testIm,1/zooming, 'bicubic');
imwrite(lowIm,'Data/Test/low.bmp','BMP');

interpIm = imresize(lowIm,zooming,'bicubic');
imwrite(uint8(interpIm),'Data/Test/bb.bmp','BMP');

% work with the illuminance domain only
lowIm2 = rgb2ycbcr(lowIm);
lImy = double(lowIm2(:,:,1));

% bicubic interpolation for the other two channels
interpIm2 = rgb2ycbcr(interpIm);
hImcb = interpIm2(:,:,2);
hImcr = interpIm2(:,:,3);

% ======================================================================
% Super-resolution using sparse representation

disp('Start superresolution...');

[hImy] = L1SR(lImy, zooming, patch_size, overlap, Dh, Dl, lambda, regres);

ReconIm(:,:,1) = uint8(hImy);
ReconIm(:,:,2) = hImcb;
ReconIm(:,:,3) = hImcr;

nnIm = imresize(lowIm, zooming, 'nearest');
figure, imshow(nnIm);
title('Input image');
pause(1);
figure, imshow(interpIm);
title('Bicubic interpolation');
pause(1)

ReconIm = ycbcr2rgb(ReconIm);
figure,imshow(ReconIm,[]);
title('Our method');
imwrite(uint8(ReconIm),'Data/Test/L1SR.bmp','BMP');


function [yr] = us2yr(us)
% Convert time from microseconds to Julian years (365.25 days).
% Chad Greene 2012
yr = us*3.168808781403e-14 ;


function error_frobenius = c8mat_is_symmetric ( m, n, a )

%*****************************************************************************80
%
%% C8MAT_IS_SYMMETRIC checks a complex matrix for symmetry.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns of A.
%
%    Input, complex A(M,N), the matrix.
%
%    Output, real ERROR_FROBENIUS, measures the Frobenius norm of ( A - A' ).
%
  if ( m ~= n )
    error_frobenius = r8_huge ( );
    return
  end

  error_frobenius = sqrt ( sum ( sum ( ( abs ( a - a' ) ).^2 ) ) );

  return
end


function value = month_length_julian ( y, m )

%*****************************************************************************80
%
%% MONTH_LENGTH_JULIAN returns the number of days in a Julian month.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 June 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer Y, the year in which the month occurred.
%
%    Input, integer M, the number of the month.
%
%    Output, integer VALUE, the number of days
%    in the month.
%
  mdays = [ 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31 ];
%
%  Copy the input.
%
  m2 = m;
  y2 = y;
%
%  Check the input.
%
  [ y2, m2, ierror ] = ym_check_julian ( y2, m2 );

  if ( ierror ~= 0 )
    value = 0;
    return
  end
%
%  Get the number of days in the month.
%
  value = mdays(m2);
%
%  If necessary, add 1 day for February 29.
%
  if ( m2 == 2 && year_is_leap_julian ( y2 ) )
    value = value + 1;
  end

  return
end


function inside = p05_inside ( m, n, point )

%*****************************************************************************80
%
%% P05_INSIDE reports if a point is inside the region in problem 05.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 April 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer N, the number of points.
%
%    Input, real POINT(M,N), the coordinates of the points.
%
%    Output, logical INSIDE(N), is TRUE if the point is in the region.
%
  center1 = [  0.0, 0.0 ];
  center2 = [ -0.4, 0.0 ];
  r1 =   1.00;
  r2 =   0.55;

  inside(1:n) =                                                   ...
       center1(2) <=   point(2,1:n)                               ...
       &                                                          ...
                     ( point(1,1:n) - center1(1) ).^2             ...
                   + ( point(2,1:n) - center1(2) ).^2  <= r1 * r1 ...
       &                                                          ...
       r2 * r2 <=    ( point(1,1:n) - center2(1) ).^2             ...
                   + ( point(2,1:n) - center2(2) ).^2;

  return
end