function [x1,lun,err] = perform_douglas_rachford(A,y,options)

% perform_douglas_rachford -
%
%   [x1,lun,err] = perform_douglas_rachford(A,y,options);
%
%   Solve noiseless Basis Pursuit
%       x1 = argmin_x \sum |x[k]|   subj.to A*x=y
%
%   A can be a matrix or an operator A(x,dir,options)
%   with dir=1 for A*x and dir=-2 for A^{+}*x (pseudo inverse).
%
%   options.x can be an initial guess
%
%   options.niter is the number of iterations
%
%   Special thanks to Jalal Fadili for useful helps on this algorithm.
%
%   Copyright (c) 2008 Gabirel Peyre

niter = getoptions(options, 'niter', 100);
mu = getoptions(options, 'mu', 3);

if isnumeric(A)
    pA = getoptions(options, 'pseudo_inv', []);
    if isempty(pA)
        pA = pinv(A);
    end
end

% initial guess
x = getoptions(options, 'x', [] );
if isempty(x)
    if isnumeric(A)
        x = pA*y;
    else
        x = feval(A, y, -2, options);
    end
end

% target result (error monitoring
x0 = getoptions(options, 'x0', [] );

drawiter = getoptions(options, 'drawiter',0);
verb = getoptions(options, 'verb', 1);
isreal = getoptions(options, 'isreal', 0);

solve_tv = getoptions(options, 'solve_tv', 0);

lun = [];
err = [];
if isnumeric(A)
    x1 = x + pA*(y-A*x);
else
    x1 = y - feval(A,x, +1, options);
    x1 = x + feval( A, x1, -2, options);
end
if drawiter
    clf;
end

nrefresh = 100;
ndisp = max(2, ceil(niter/nrefresh) );

for i=1:niter
    if verb
        progressbar(i,niter);
    end
    % compute the intermediary point
    %   x <- x-x1 + thresh( 2*x1-x )

    u = 2*x1-x;

%    sparse_weight = getoptions(options, 'sparse_weight', 0,1);
%    u = u./sparse_weight;
    if not(solve_tv)
        % L1 regularization
        u = perform_thresholding( u, mu, 'soft' );
    else
        % TV regularization
        opts.method = 'gradient';
        opts.lambda = mu;
        opts.niter = getoptions(options, 'niter_tv', 50);
        opts.niter_inner = 1;
        opts.display = 0;
        opts.verb = 0;
        [u1,err,tv,lalist,Err] = perform_tv_denoising(u,opts);
        % u1 = u - mu*div( xi, options )
%        opts.xi = grad( (u1-u)/mu, options );
        u = u1;
    end
%    u = u.*sparse_weight;

    x = x-x1 + u;

    % perform projection step on the constraints
    if isnumeric(A)
        x1 = x + pA*(y-A*x);
    else
        x1 = y - feval(A,x,+1, options);
        x1 = x + feval( A, x1, -2, options);
    end

    if isreal
 %       x1 = real(x1); % force real signal
    end
    % check for error decay
    if nargout>=3 && not(isempty(x0))
        err(end+1) = norm(x0-x1, 'fro')^2;
    end
    if nargout>=2
        if not(solve_tv)
            lun(end+1) = sum(abs(x1(:))); % mean(sum(abs(x1)));
        else
            lun(end+1) = compute_total_variation(x1,options);
        end
    end
    if drawiter && mod(i,ndisp)==1  % && ntest==1
        if nb_dims(x1)==1
            n = size(x1,1);
            plot( real(x1(:,1:min(end,3))) );
            axis([1,n,-1,1]);
            axis tight;
            drawnow;
        else
            clf; imageplot(x1); drawnow;
        end
    end
end
% x1 = real(x1);


function [ result, seed ] = sphere01_triangle_quad_00 ( n, v1, v2, v3, f, ...
  seed )

%*****************************************************************************80
%
%% SPHERE01_TRIANGLE_QUAD_00: quadrature over a triangle on the unit sphere.
%
%  Discussion:
%
%    This is a Monte Carlo approach.
%
%    The integral is approximated by averaging the values at N random points,
%    multiplied by the area.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 April 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of sample points.
%
%    Input, real V1(3), V2(3), V3(3), the XYZ coordinates of
%    the vertices of the triangle.
%
%    Input, function v = f ( x ), evaluates the integrand at X.
%
%    Input/output, integer SEED, a seed for the random
%    number generator.
%
%    Output, real RESULT, the approximate integral.
%
  area = sphere01_triangle_vertices_to_area ( v1, v2, v3  );

  [ vc, seed ] = sphere01_triangle_sample ( n, v1, v2, v3, seed );

  quad = 0.0;
  for j = 1 : n
    quad = quad + f ( vc(1:3,j) );
  end

  result = quad * area / n;

  return
end


function [Rz] = R_z(psi)

% Input sanity check
if nargin < 1
    psi = sym('psi_t','real');
end
assert(numel(psi) == 1,'Angle must be a scalar [1x1].');
assert(isa(psi,'sym') || isnumeric(psi),'Angle must be a symbolic or numeric value.');

% Rotate about the z-axis (yaw)
Rz = [cos(psi) -sin(psi) 0;
      sin(psi)  cos(psi) 0;
             0         0 1];
end


%% ALS optimization for CP and Tucker tensor decompositions

%% Alternating least squares for PARAFAC/CANDECOMP
% The function |parafac_als| computes an estimate of the best rank-R
% PARAFAC model of a tensor X using an alternating least-squares
% algorithm.  The input X can be a tensor, sptensor, ktensor, or
% ttensor. The result P is a ktensor.
rand('state',0);
X = sptenrand([5 4 3], 10)
%%
P = parafac_als(X,2)
%%
P = parafac_als(X,2,struct('dimorder',[3 2 1]))
%%
P = parafac_als(X,2,struct('dimorder',[3 2 1],'init','nvecs'))
%%
U0 = {rand(5,2),rand(4,2),[]}; %<-- Initial guess for factors of P
P = parafac_als(X,2,struct('dimorder',[3 2 1],'init',{U0}))
%% Alternating least squares for Tucker model
% The function |tucker_als| computes the best rank(R1,R2,..,Rn)
% approximation of tensor X, according to the specified dimensions in
% vector R.  The input X can be a tensor, sptensor, ktensor, or
% ttensor.  The result returned in T is a ttensor.
X = sptenrand([5 4 3], 10)
%%
T = tucker_als(X,2)        %<-- best rank(2,2,2) approximation
%%
T = tucker_als(X,[2 2 1])  %<-- best rank(2,2,1) approximation
%%
T = tucker_als(X,2,struct('dimorder',[3 2 1]))
%%
T = tucker_als(X,2,struct('dimorder',[3 2 1],'init','eigs'))
%%
U0 = {rand(5,2),rand(4,2),[]}; %<-- Initial guess for factors of T
T = tucker_als(X,2,struct('dimorder',[3 2 1],'init',{U0}))


function [coeff,variance]=OLSPriorTheta(y,timetrend,f)
% OLSPriorTheta(y,timetrend,f) computes the prior for coefficients of the
% deterministic trends in the TVE-VAR approach. The prior is normally
% distributed with mean "coeff" and variance "variance". The prior
% parameters are computed regressing the data y on timetrend. Then, coeff is
% the OLS estimate, and variance is the asymptotic variance of the OLS,
% scaled up of a factor f.

%% Prepare data: make sure the dimensions of the inputs are correct
if size(y,1)>size(y,2)
    y=y';
end
if size(timetrend,1)>size(timetrend,2)
    timetrend=timetrend';
end

%% Run OLS
T=size(y,2); % number of data available
k=size(timetrend,1); % number of regressors

P=y*timetrend'*inv(timetrend*timetrend'); % the OLS coefficients
resid=y-P*timetrend; % the residuals from the regression
O=1/(T-k)*resid*resid';
Q=timetrend*timetrend';
varpar=kron(O,Q^-1); % the asymptotic variance

%% The output
variance=varpar*f;
coeff=P';

end


function [pos vel alt] = RadarUKF(z, dt)
%
%
persistent Q R
persistent x P
persistent n m
persistent firstRun

if isempty(firstRun)
  Q = [ 0.01  0      0;
        0  0.01  0;
        0  0      0.01 ];

  R = 100;

  x = [0 90 1100]';
  P = 100*eye(3);

  n = 3;
  m = 1;

  firstRun = 1;
end

[Xi W] = SigmaPoints(x, P, 0);

fXi = zeros(n, 2*n+1);
for k = 1:2*n+1
  fXi(:, k) = fx(Xi(:,k), dt);
end

[xp Pp] = UT(fXi, W, Q);
%norm(xp - fx(x, dt))

hXi = zeros(m, 2*n+1);
for k = 1:2*n+1
  hXi(:, k) = hx(fXi(:,k));
end

[zp Pz] = UT(hXi, W, R);
%norm(zp - hx(xp))

Pxz = zeros(n, m);
for k = 1:2*n+1
  Pxz = Pxz + W(k)*(fXi(:, k) - xp)*(hXi(:, k) - zp)';
end

K = Pxz*inv(Pz);

x = xp + K*(z - zp);
P = Pp - K*Pz*K';

pos = x(1);
vel = x(2);
alt = x(3);

%------------------------------
function xp = fx(x, dt)
%
%
A = eye(3) + dt*[ 0 1 0;
                  0 0 0;
                  0 0 0 ];

xp = A*x;

%------------------------------
function yp = hx(x)
%
%
yp = sqrt(x(1)^2 + x(3)^2);


% Build a spectrum-like harmonic model using a given sinusoids set
%
% Input
%  sins     : Nx3 (or Nx4), frequency[Hz], amplitude, phase, (harmnb)
%             Only positive frequencies, with DC.
%  opt      : Options (see below)
%
% Output
%  M        : Spectrum built from sins.
%             (With same format as a DFT result)
%
% Copyright (c) 2012 University of Crete - Computer Science Department
%
% License
%  This file is under the LGPL license,  you can
%  redistribute it and/or modify it under the terms of the GNU Lesser General
%  Public License as published by the Free Software Foundation, either version 3
%  of the License, or (at your option) any later version. This file is
%  distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
%  without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
%  PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
%  details.
%
% This function is part of the Covarep project: http://covarep.github.io/covarep
%
% Author
%  Gilles Degottex <degottex@csd.uoc.gr>
%
% TODO
%  Manage holes in the harmonic structure
%

function [M, opt] = sin2shm(sins, opt)

    if nargin<2
        opt.nbh             = Inf; % Number of harmonics to be use in the model
                                   % (excluding DC).
        opt.max_freq        = 8000;% Consider harmonics up to this limit.

        opt.prevpow2        = false;% if true, reduce the harmonic model to the
                                  % biggest possible power of 2 length
        opt.dc_extrap       = 1;  % 0: Use the current value
                                  % 1; Copy the amplitude of the first harm
                                  % 2: Use linear extrapolation on log scale
                                  % both 1 and 2 assume angle(DC)=0
    end
    if nargin==0; M=opt; return; end

    % TODO manage holes in the harmonic structure

    if ~isempty(opt.max_freq) && opt.max_freq>0
        inds = sins(1,:)<=opt.max_freq;
        sins = sins(:,inds);
    end
    sins = sins(:,1:min(size(sins,2), 1+opt.nbh));
    if opt.prevpow2
        % in order to speed up the computation of the min phase
        harmsize = 2^(nextpow2(size(sins,2))-1) + 1;
        sins = sins(:,1:min(size(sins,2), harmsize));
    end

    M = [sins(2,:).'.*exp(1i*sins(3,:).')];

    M(1) = sign(real(M(1)))*abs(M(1));        % set DC phase to 0 or pi

    M(end) = sign(real(M(end)))*abs(M(end));  % set the Nyquist phase to 0 or pi

    M = hspec2spec(M);

    % Extrapolate the model DC
    % Doesn't keep the DC phase !
    % Angle(DC) never makes sens in acoustic measurement
    % For technical reasons, we can therefore assume any constant (here zero).
    if opt.dc_extrap==1;     M(1) = abs(M(2));
    elseif opt.dc_extrap==2; M(1) = db2lin(lin2db(M(2))+(lin2db(M(2))-lin2db(M(3))));   end

return


function writeDoublePendulumDynamics(f,M)

%This function writes the dynamics file for the double pendulum.

filename = 'doublePendulumDynamics';

comments{1} = ['DZ = ' upper(filename) '(T,Z,P)'];
comments{2} = ' ';
comments{3} = 'FUNCTION:  This function computes the dynamics of a double';
comments{4} = '    pendulum, and is designed to be called from ode45. The';
comments{5} = '    model allows for arbitrary mass and inertia for each';
comments{6} = '    link, but no friction or actuation';
comments{7} = ' ';
comments{8}  = 'INPUTS: ';
comments{9}  = '    t = time. Dummy input for ode45. Not used.';
comments{10} = '    z = [4xn] matrix of states.';
comments{12} = '    P = struct of parameters';
comments{13} = 'OUTPUTS: ';
comments{14} = '    dz = [4xn] matrix of state derivatives';
comments{15} = ' ';
comments{16} = 'NOTES:';
comments{17} = ['    This file was automatically generated by ' mfilename];

params{1} = {'m1','link one mass'};
params{2} = {'m2','link two mass'};
params{3} = {'g ','gravity'};
params{4} = {'l1','link one length'};
params{5} = {'l2','link two length'};
params{6} = {'I1','link one moment of inertia about its center of mass'};
params{7} = {'I2','link two moment of inertia about its center of mass'};
params{8} = {'d1','distance between link one center of mass and parent joint'};
params{9} = {'d2','distance between link two center of mass and parent joint'};

states{1} = {'th1','link one absolute angle'};
states{2} = {'dth1','link one angular rate'};
states{3} = {'th2','link two absolute angle'};
states{4} = {'dth2','link two angular rate'};

dstates{1} = {'dth1','derivative of link one absolute angle'};
dstates{2} = {'ddth1','derivative of link one angular rate'};
dstates{3} = {'dth2','derivative of link two absolute angle'};
dstates{4} = {'ddth2','derivative of link two angular rate'};

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                               write file                                %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
fid = fopen([filename '.m'],'w');

fprintf(fid, ['function dz = ' filename '(~,z,P) \n']);

for i=1:length(comments)
    fprintf(fid,['%%' comments{i} '\n']);
end
fprintf(fid,'\n');

for i=1:length(params)
    fprintf(fid,[params{i}{1} ' = P.' params{i}{1} '; %%' params{i}{2} '\n']);
end
fprintf(fid,'\n');

for i=1:length(states)
    fprintf(fid,[states{i}{1} ' = z(' num2str(i) ',:); %%' states{i}{2} '\n']);
end
fprintf(fid,'\n');

for i=1:2;
    fprintf(fid,['f' num2str(i) ' = ' vectorize(char(f(i))) ';\n']);
end
fprintf(fid,'\n');

for i=1:2
    for j=1:2
        fprintf(fid,['M' num2str(i) num2str(j) ' = ' vectorize(char(M(i,j))) ';\n']);
    end
end
fprintf(fid,'\n');

fprintf(fid,'D = M11.*M22 - M12.*M21;\n\n');

fprintf(fid,'ddth1 = (f2.*M12 - f1.*M22)./D;\n');
fprintf(fid,'ddth2 = -(f2.*M11 - f1.*M21)./D;\n');
fprintf(fid,'\n');

fprintf(fid,'dz = [...\n');
for i=1:length(dstates)
    fprintf(fid,['    ' dstates{i}{1} '; %%' dstates{i}{2} '\n']);
end
fprintf(fid,'];\n');

fprintf(fid,'end \n');

fclose(fid);
end


function result = cube_unit_3d ( func )

%*****************************************************************************80
%
%% CUBE_UNIT_3D approximates an integral inside the unit cube in 3D.
%
%  Integration region:
%
%    Points (X,Y,Z) such that:
%
%      -1 <= X <= 1,
%      -1 <= Y <= 1,
%      -1 <= Z <= 1.
%
%  Discussion:
%
%    An 8 point third degree formula is used.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 May 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Arthur H Stroud,
%    Approximate Calculation of Multiple Integrals,
%    Prentice Hall, 1971.
%
%  Parameters:
%
%    Input, external FUNC, the name of the user supplied
%    function which evaluates F(X,Y,Z), of the form
%      function value = func ( x, y, z )
%
%    Output, real RESULT, the approximate integral of the function.
%
  s = 1.0E+00 / sqrt ( 3.0E+00 );
  w = 1.0E+00 / 8.0E+00;

  x = s;
  y = s;
  z = s;

  quad = w * ( ...
      feval ( func,  x,  y,  z ) ...
    + feval ( func,  x,  y, -z ) ...
    + feval ( func,  x, -y,  z ) ...
    + feval ( func,  x, -y, -z ) ...
    + feval ( func, -x,  y,  z ) ...
    + feval ( func, -x,  y, -z ) ...
    + feval ( func, -x, -y,  z ) ...
    + feval ( func, -x, -y, -z ) );

  volume = cube_unit_volume_nd ( 3 );
  result = quad * volume;

  return
end


function y = inverseGammaPdf(x,alpha,beta)
% The inverse Gamma PDF.
% INPUT
% x  (>0)
% alpha - shape parameter
% beta  - scale parameter
% OUTPUT
% y = the pdf [nrX nrBeta]
% BK - 2018

[nrX,nrBetaInX] = size(x);
[nrBeta,nrXInBeta] = size(beta);
if nrXInBeta<nrBetaInX
    beta =repmat(beta,[1 nrBetaInX]);
end

if nrX==1 && nrBeta>1
    x = repmat(x,[1 nrBeta]);
end

z = x<0;
x(z) =NaN;
y = (beta.^alpha)./gamma(alpha).*(1./x).^(alpha+1).*exp(-beta./x);
y(z) = 0;
end


%% RPCA demo using TFOCS
% This is a demo of Robust PCA (RPCA) using <http://tfocs.stanford.edu TFOCS>. Since 2009,
% there has been much interest in this specific RPCA formulation
% (RPCA can refer to many different formulations; we will state
% our formulation precisely below), but most solvers
% assume equality constraints. One great feature of TFOCS
% is how easy it is to prototype new formulations, so we show
% here a formulation using inequality constraints.
%
% The basic (equality constraint) RPCA formulation is:
% ( see <http://perception.csl.uiuc.edu/matrix-rank/references.html here>
% for references )
%
% $$ \min_{S,L} \|L\|_* + \lambda \|S\|_{\ell_1} \quad\textrm{subject to}\;L+S = X $$
%
% The idea is that some object $X$ can be broken into two components $X=L+S$,
% where $L$ is low-rank and $S$ is sparse.  To tease out this separation,
% RPCA penalizes $\|L\|_*$ (the nuclear norm of $L$), which is a proxy for
% the rank of $L$.  We also penalizes $\|S\|_{\ell_1}$ which is just the sum
% of the absolute value of all the entries of the matrix $S$; this is a proxy
% for the number of non-zero entries of $S$.
%
% The parameter $\lambda$ is important, as it controls how much weight
% to put on $S$ relative to $L$.  This parameter is not overly sensitive,
% and we picked the value used in this demo by a few quick rounds of trial-and-error.
%
% _Code by Stephen Becker, May 2011_

%% Inequality constraints
% In this demo, $X$ is a video. In particular, each column of $X$ is the set
% of $m n$ pixels from a $m \times n$ pixel frame of the video, reshaped
% so that it is now a vector instead of a matrix.
%
% The particular video we use is not compressed using a video codec, so
% each frame of the video is stored. This leads to huge files, so to save
% a bit of space, the value of each pixel is stored as an 8 bit number,
% which means we can think of it as a value from $0,1,2,\ldots,255$.  Now,
% we may imagine that there is some "true" video where each pixel
% is a _real_ number between $[0,255]$, and that we see a quantized version of it.
% It would be nice to apply RPCA to this true version instead of applying
% it to the quantized version.  In particular, it is unlikely that the quantized
% version can be split nicely into a low-rank and sparse component.
% To account for this quantization, we will ask that $S+L$ is not exactly equal
% to $X$, but rather that $S+L$ agrees with $X$ up to the precision
% of the quantization.  The quantization can induce at most an error of .5 in
% the pixel value (e.g. true value is 5.6, which is rounded to 6.0, so an error of .4;
% or a true value of 6.4, which is also rounded to 6.0, so also an error of 0.4).
% A nice way to capture this is via the $\ell_\infty$ norm.
%
% So, the inequality constrained version of RPCA uses the same objective
% function, but instead of the constraints $L+S=X$, the constraints are
%
% $$ \|L+S-X\|_{\ell_\infty} \le 0.5. $$

%% Numerical demo
% We demonstrate this on a video clip taken from a surveillance
% camera in a subway station.  Not only is there a background
% and a foreground (i.e. people), but there is an escalator
% with periodic motion.  Conventional background subtraction
% algorithms would have difficulty with the escalator, but
% this RPCA formulation easily captures it as part of the low-rank
%   structure.
% The clip is taken from the data at this website (after
% a bit of processing to convert it to grayscale):
% http://perception.i2r.a-star.edu.sg/bk_model/bk_index.html

% Load the data:
load escalator_data % contains X (data), m and n (height and width)
X = double(X);

% addpath ~/Dropbox/TFOCS/   % add TFOCS to your path, so change this to suit your computer

%%
nFrames     = size(X,2);

lambda  = 1e-2;

opts = [];
opts.stopCrit       = 4;
opts.printEvery     = 1;
opts.tol            = 1e-4;

opts.maxIts         = 25;

opts.errFcn{1}      = @(f,d,p) norm(p{1}+p{2}-X,'fro')/norm(X,'fro');

largescale      = false;

for inequality_constraints = 0:1

    if inequality_constraints
        % if we already have equality constraint solution,
        %   it would make sense to "warm-start":
%         x0      = { LL_0, SS_0 };
        % but it's more fair to start all over:
        x0      = { X, zeros(size(X))   };
        z0      = [];
    else
        x0      = { X, zeros(size(X))   };
        z0      = [];
    end

    obj    = { prox_nuclear(1,largescale), prox_l1(lambda) };
    affine = { 1, 1, -X };

    mu = 1e-4;
    if inequality_constraints
        epsilon  = 0.5;
        dualProx = prox_l1(epsilon);
    else
        dualProx = proj_Rn;
    end

    tic
    % call the TFOCS solver:
    [x,out,optsOut] = tfocs_SCD( obj, affine, dualProx, mu, x0, z0, opts);
    toc

    % save the variables
    LL = x{1};
    SS = x{2};
    if ~inequality_constraints
        z0      = out.dual;
        LL_0    = LL;
        SS_0    = SS;
    end

end % end loop over "inequality_constriants" variable

%% show all together in movie format
% If you run this in your own computer, you can see the movie. On the webpage,
%   we have a youtube version of the video.
% The top row is using equality constraints, and the bottom row
% is using inequality constraints.
%  The first column of both rows is the same (i.e. it is the original image).
mat  = @(x) reshape( x, m, n );
figure();
colormap( 'Gray' );
k = 1;
for k = 1:nFrames

    imagesc( [mat(X(:,k)), mat(LL_0(:,k)),  mat(SS_0(:,k)); ...
              mat(X(:,k)), mat(LL(:,k)),    mat(SS(:,k))  ] );

    axis off
    axis image

    drawnow;
    pause(.05);

    if k == round(nFrames/2)
        snapnow; % Take a single still snapshot for publishing the m file to html format
    end
end

%% Is there a difference between the two versions?
% Compare the equality constrained version and the inequality
%   constrained version.  To do this, we can check whether
%   the "L" components are really low rank
%   and whether the "S" components are really sparse.
% Even if the two versions appear visually similar, the variables
%   may behave quite differently with respect to low-rankness
%   and sparsity.

fprintf('"S" from equality constrained version has  \t%.1f%% nonzero entries\n',...
    100*nnz(SS_0)/numel(SS_0) );
fprintf('"S" from inequality constrained version has  \t%.1f%% nonzero entries\n',...
    100*nnz(SS)/numel(SS) );

s  = svd(LL);    % inequality constraints
s0 = svd(LL_0);  % equality constraints

fprintf('"L" from equality constrained version has numerical rank\t %d (of %d possible)\n',...
    sum( s0>1e-6), min( m*n, nFrames)  );
fprintf('"L" from inequality constrained version has numerical rank\t %d (of %d possible)\n',...
    sum( s > 1e-6), min( m*n, nFrames)  );

figure();
semilogy( s0 ,'o-')
hold all;
semilogy( s ,'o-')
legend('Equality constraints','Inequality constraints');
xlabel('sorted singular value location');
ylabel('value of singular value');
title('Comparison of RPCA using equality and inequality constraints');

%%


function hpp_test03 ( )

%*****************************************************************************80
%
%% HPP_TEST03 tests HEPP_VALUE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 October 2014
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'HPP_TEST03:\n' );
  fprintf ( 1, '  HePP_VALUE evaluates a Hermite product polynomial.\n' );
  fprintf ( 1, '  POLYNOMIAL_VALUE evaluates a polynomial.\n' );

  m = 3;
  n = 1;
  seed = 123456789;
  [ x, seed ] = r8vec_uniform_ab ( m, -1.0, +1.0, seed );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Evaluate at X = ( %g, %g, %g )\n', x(1:3) )
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Rank  I1  I2  I3:  He(I1,X1)*He(I2,X2)*He(I3,X3)  P(X1,X2,X3)\n' );
  fprintf ( 1, '\n' );

  for rank = 1 : 20

    l = comp_unrank_grlex ( m, rank );
%
%  Evaluate the HePP directly.
%
    v1 = hepp_value ( m, n, l, x );
%
%  Convert the HePP to a polynomial, and reevaluate.
%
    o_max = prod ( floor ( ( l(1:m) + 2 ) / 2 ) );

    [ o, c, e ] = hepp_to_polynomial ( m, l, o_max );

    v2 = polynomial_value ( m, o, c, e, n, x );
%
%  Compare results.
%
    fprintf ( 1, '  %4d  %2d  %2d  %2d  %12g  %12g\n', rank, l(1:m), v1, v2 );

  end

  return
end


function point_num = levels_index_size_own ( dim_num, level_max )

%*****************************************************************************80
%
%% LEVELS_INDEX_SIZE_OWN sizes a sparse grid made from OWN 1D rules.
%
%  Discussion:
%
%    The sparse grid is presumed to have been created from products
%    of OPEN WEAKLY NESTED 1D quadrature rules.
%
%    OWN rules include Gauss Hermite and Gauss Legendre.
%
%    The sparse grid is the logical sum of product grids with total LEVEL
%    between LEVEL_MIN and LEVEL_MAX.
%
%    Oddly enough, in order to count the number of points, we will
%    behave as though LEVEL_MIN was zero.  This is because our computation
%    concentrates on throwing away all points generated at lower levels,
%    but, in fact, if we start at a nonzero level, we need to include
%    on that level all the points that would have been generated on lower
%    levels.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 July 2008
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Fabio Nobile, Raul Tempone, Clayton Webster,
%    A Sparse Grid Stochastic Collocation Method for Partial Differential
%    Equations with Random Input Data,
%    SIAM Journal on Numerical Analysis,
%    Volume 46, Number 5, 2008, pages 2309-2345.
%
%  Parameters:
%
%    Input, integer DIM_NUM, the spatial dimension.
%
%    Input, integer LEVEL_MAX, the maximum value of LEVEL.
%
%    Output, integer POINT_NUM, the number of points in the grid.
%

%  Special case.
%
  if ( level_max == 0 )
    point_num = 1;
    return
  end
%
%  The outer loop generates LEVELs from LEVEL_MIN to LEVEL_MAX.
%
%  The normal definition of LEVEL_MIN:
%
%   level_min = max ( 0, level_max + 1 - dim_num )
%
%  Our somewhat artificial temporary local definition of LEVEL_MIN:
%
  if ( dim_num == 1 )
    level_min = level_max;
    point_num = 1;
  else
    level_min = 0;
    point_num = 0;
  end

  for level = level_min : level_max
%
%  The middle loop generates the next partition that adds up to LEVEL.
%
    level_1d = [];
    more = 0;
    h = 0;
    t = 0;

    while ( 1 )

      [ level_1d, more, h, t ] = comp_next ( level, dim_num, level_1d, more, h, t );
%
%  Transform each 1D level to a corresponding 1D order.
%
      order_1d = level_to_order_open ( dim_num, level_1d );

      for dim = 1 : dim_num
%
%  Account for the repetition of the center point.
%
        if ( 1 < order_1d(dim) )
          order_1d(dim) = order_1d(dim) - 1;
        end

      end

      point_num = point_num + prod ( order_1d(1:dim_num) );

      if ( ~more )
        break
      end

    end
  end

  return
end


function a = chow_inverse ( alpha, beta, n )

%*****************************************************************************80
%
%% CHOW_INVERSE returns the inverse of the CHOW matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 September 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real ALPHA, the ALPHA value.  A typical value is 1.0.
%
%    Input, real BETA, the BETA value.  A typical value is 0.0.
%
%    Input, integer N, the order of A.
%
%    Output, real A(N,N), the matrix.
%
  a = zeros ( n, n );

  if ( 0.0 == alpha & beta == 0.0 )

    fprintf ( 1, '\n' );
    fprintf ( 1, 'CHOW_INVERSE - Fatal error!\n' );
    fprintf ( 1, '  The Chow matrix is not invertible, because\n' );
    fprintf ( 1, '  ALPHA = 0 and BETA = 0.\n' );
    error ( 'CHOW_INVERSE - Fatal error!' );

  elseif ( 0.0 == alpha & beta ~= 0.0 )

    for i = 1 : n
      for j = 1 : n

        if ( i <= j )
          a(i,j) = (-1)^(j-i) / beta^(j+1-i);
        else
          a(i,j) = 0.0;
        end

      end
    end

    return

  elseif ( 0.0 ~= alpha & beta == 0.0 )

    if ( 1 < n )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'CHOW_INVERSE - Fatal error!\n' );
      fprintf ( 1, '  The Chow matrix is not invertible, because\n' );
      fprintf ( 1, '  BETA = 0 and 1 < N.\n' );
      error ( 'CHOW_INVERSE - Fatal error!' );
    end

    a(1,1) = 1.0 / alpha;

    return

  end

  d(0+1) = 1.0;
  d(1+1) = beta;
  for i = 2 : n
    d(i+1) = beta * d(i-1+1) + alpha * beta * d(i-2+1);
  end

  dp(-1+2) = 1.0 / beta;
  dp(0+2) = 1.0;
  dp(1+2) = alpha + beta;
  for i = 2 : n
    dp(i+2) = d(i+1) + alpha * d(i-1+1);
  end

  for i = 1 : n
    for j = 1 : i-1
      a(i,j) = - alpha * ( alpha * beta )^(i-j) * dp(j-2+2) * d(n-i+1) ...
        / dp(n+2);
    end
    for j = i : n
      a(i,j) = (-1)^(i+j) * dp(i-1+2) * d(n+1-j+1) / ( beta * dp(n+2) );
    end
  end

  return
end


function det = r8plu_det ( n, pivot, lu )

%*****************************************************************************80
%
%% R8PLU_DET computes the determinant of an R8PLU matrix.
%
%  Discussion:
%
%    The matrix should have been factored by R8MAT_TO_R8PLU.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 September 2006
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Jack Dongarra, Jim Bunch, Cleve Moler, Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, 1979,
%    ISBN13: 978-0-898711-72-1.
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%    N must be positive.
%
%    Input, integer PIVOT(N), the pivot vector computed by R8MAT_TO_R8PLU.
%
%    Input, real LU(N,N), the LU factors computed by R8MAT_TO_R8PLU.
%
%    Output, real DET, the determinant of the matrix.
%
  det = 1.0;

  for i = 1 : n
    det = det * lu(i,i);
    if ( pivot(i) ~= i )
      det = -det;
    end
  end

  return
end


% this function solves the odometry calibration problem
% given a measurement matrix Z.
% We assume that the information matrix is the identity
% for each of the measurements
% Every row of the matrix contains
% z_i = [u'x, u'y, u'theta, ux, uy, ytheta]
% Z:	The measurement matrix
% X:	the calibration matrix
% returns the correction matrix X
function X = ls_calibrate_odometry(Z)
  % initial solution (the identity transformation)
  X = eye(3);

  % TODO: initialize H and b of the linear system
  H = zeros(9);
  b = zeros(9,1);
  omega = eye(3);

  % TODO: loop through the measurements and update H and b
  % You may call the functions error_function and jacobian, see below
  % We assume that the information matrix is the identity.
  for i=1:size(Z,1)
	H = H + jacobian(i,Z)'*omega*jacobian(i,Z);
	b = b + jacobian(i,Z)'*omega'*error_function(i,X,Z);
  endfor
  % TODO: solve and update the solution
  delX = -H\b;
  X = X + [delX(1:3)';delX(4:6)';delX(7:9)'];
end

% this function computes the error of the i^th measurement in Z
% given the calibration parameters
% i:	the number of the measurement
% X:	the actual calibration parameters
% Z:	the measurement matrix, each row contains first the scan-match result
%       and then the motion reported by odometry
% e:	the error of the ith measurement
function e = error_function(i, X, Z)
  % TODO compute the error of each measurement
  e = Z(i,1:3)' - X*Z(i,4:6)';
end

% derivative of the error function for the ith measurement in Z
% i:	the measurement number
% Z:	the measurement matrix
% J:	the jacobian of the ith measurement
function J = jacobian(i, Z)
  % TODO compute the Jacobian
  J = -[Z(i,4) Z(i,5) Z(i,6) zeros(1,6); zeros(1,3) Z(i,4) Z(i,5) Z(i,6) zeros(1,3); zeros(1,6) Z(i,4) Z(i,5) Z(i,6)];
end


function [ aj, bj, zemu ] = class_matrix ( kind, m, alpha, beta )

%*****************************************************************************80
%
%% CLASS_MATRIX computes the Jacobi matrix for a quadrature rule.
%
%  Discussion:
%
%    This routine computes the diagonal AJ and subdiagonal BJ
%    elements of the order M tridiagonal symmetric Jacobi matrix
%    associated with the polynomials orthogonal with respect to
%    the weight function specified by KIND.
%
%    For weight functions 1-7, M elements are defined in BJ even
%    though only M-1 are needed.  For weight function 8, BJ(M) is
%    set to zero.
%
%    The zero-th moment of the weight function is returned in ZEMU.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 February 2010
%
%  Author:
%
%    Original FORTRAN77 version by Sylvan Elhay, Jaroslav Kautsky.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Sylvan Elhay, Jaroslav Kautsky,
%    Algorithm 655: IQPACK, FORTRAN Subroutines for the Weights of
%    Interpolatory Quadrature,
%    ACM Transactions on Mathematical Software,
%    Volume 13, Number 4, December 1987, pages 399-415.
%
%  Parameters:
%
%    Input, integer KIND, the rule.
%    1, Legendre,             (a,b)       1.0
%    2, Chebyshev Type 1,     (a,b)       ((b-x)*(x-a))^(-0.5)
%    3, Gegenbauer,           (a,b)       ((b-x)*(x-a))^alpha
%    4, Jacobi,               (a,b)       (b-x)^alpha*(x-a)^beta
%    5, Generalized Laguerre, (a,+oo)     (x-a)^alpha*exp(-b*(x-a))
%    6, Generalized Hermite,  (-oo,+oo)   |x-a|^alpha*exp(-b*(x-a)^2)
%    7, Exponential,          (a,b)       |x-(a+b)/2.0|^alpha
%    8, Rational,             (a,+oo)     (x-a)^alpha*(x+b)^beta
%    9, Chebyshev Type 2,     (a,b)       ((b-x)*(x-a))^(+0.5)
%
%    Input, integer M, the order of the Jacobi matrix.
%
%    Input, real ALPHA, the value of Alpha, if needed.
%
%    Input, real BETA, the value of Beta, if needed.
%
%    Output, real AJ(M), BJ(M), the diagonal and subdiagonal
%    of the Jacobi matrix.
%
%    Output, real ZEMU, the zero-th moment.
%
  temp = eps;

  parchk ( kind, 2 * m - 1, alpha, beta );

  temp2 = 0.5;

  if ( 500.0 * temp < abs ( ( gamma ( temp2 ) )^2 - pi ) )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'CLASS - Fatal error!\n' );
    fprintf ( 1, '  Gamma function does not match machine parameters.\n' );
    error ( 'CLASS - Fatal error!' );
  end

  bj = zeros(m,1);
  aj = zeros(m,1);

  if ( kind == 1 )

    ab = 0.0;

    zemu = 2.0 / ( ab + 1.0 );

    aj(1:m) = 0.0;

    for i = 1 : m
      abi = i + ab * mod ( i, 2 );
      abj = 2 * i + ab;
      bj(i) = abi * abi / ( abj * abj - 1.0 );
    end
    bj(1:m) =  sqrt ( bj(1:m) );

  elseif ( kind == 2 )

    zemu = pi;

    aj(1:m) = 0.0;

    bj(1) =  sqrt ( 0.5 );
    bj(2:m) = 0.5;

  elseif ( kind == 3 )

    ab = alpha * 2.0;
    zemu = 2.0^( ab + 1.0 ) * gamma ( alpha + 1.0 )^2 ...
      / gamma ( ab + 2.0 );

    aj(1:m) = 0.0;
    bj(1) = 1.0 / ( 2.0 * alpha + 3.0 );
    for i = 2 : m
      bj(i) = i * ( i + ab ) / ( 4.0 * ( i + alpha )^2 - 1.0 );
    end
    bj(1:m) =  sqrt ( bj(1:m) );

  elseif ( kind == 4 )

    ab = alpha + beta;
    abi = 2.0 + ab;
    zemu = 2.0^( ab + 1.0 ) * gamma ( alpha + 1.0 ) ...
      * gamma ( beta + 1.0 ) / gamma ( abi );
    aj(1) = ( beta - alpha ) / abi;
    bj(1) = 4.0 * ( 1.0 + alpha ) * ( 1.0 + beta ) ...
      / ( ( abi + 1.0 ) * abi * abi );
    a2b2 = beta * beta - alpha * alpha;

    for i = 2 : m
      abi = 2.0 * i + ab;
      aj(i) = a2b2 / ( ( abi - 2.0 ) * abi );
      abi = abi^2;
      bj(i) = 4.0 * i * ( i + alpha ) * ( i + beta ) * ( i + ab ) ...
        / ( ( abi - 1.0 ) * abi );
    end
    bj(1:m) =  sqrt ( bj(1:m) );

  elseif ( kind == 5 )

    zemu = gamma ( alpha + 1.0 );

    for i = 1 : m
      aj(i) = 2.0 * i - 1.0 + alpha;
      bj(i) = i * ( i + alpha );
    end
    bj(1:m) =  sqrt ( bj(1:m) );

  elseif ( kind == 6 )

    zemu = gamma ( ( alpha + 1.0 ) / 2.0 );

    aj(1:m) = 0.0;

    for i = 1 : m
      bj(i) = ( i + alpha * mod ( i, 2 ) ) / 2.0;
    end
    bj(1:m) =  sqrt ( bj(1:m) );

  elseif ( kind == 7 )

    ab = alpha;
    zemu = 2.0 / ( ab + 1.0 );

    aj(1:m) = 0.0;

    for i = 1 : m
      abi = i + ab * mod(i,2);
      abj = 2 * i + ab;
      bj(i) = abi * abi / ( abj * abj - 1.0 );
    end
    bj(1:m) =  sqrt ( bj(1:m) );

  elseif ( kind == 8 )

    ab = alpha + beta;
    zemu = gamma ( alpha + 1.0 ) * gamma ( - ( ab + 1.0 ) ) ...
      / gamma ( - beta );
    apone = alpha + 1.0;
    aba = ab * apone;
    aj(1) = - apone / ( ab + 2.0 );
    bj(1) = - aj(1) * ( beta + 1.0 ) / ( ab + 2.0 ) / ( ab + 3.0 );
    for i = 2 : m
      abti = ab + 2.0 * i;
      aj(i) = aba + 2.0 * ( ab + i ) * ( i - 1 );
      aj(i) = - aj(i) / abti / ( abti - 2.0 );
    end

    for i = 2 : m - 1
      abti = ab + 2.0 * i;
      bj(i) = i * ( alpha + i ) / ( abti - 1.0 ) * ( beta + i ) ...
        / ( abti^2 ) * ( ab + i ) / ( abti + 1.0 );
    end

    bj(m) = 0.0;
    bj(1:m) =  sqrt ( bj(1:m) );

  elseif ( kind == 9 )

    zemu = pi /  2.0;

    aj(1:m) = 0.0;

    bj(1:m) = 0.5;

  end

  return
end


% book : Signals and Systems Laboratory with MATLAB
% authors : Alex Palamides & Anastasia Veloni
%
%
% The  command invfreqs

%verification of the result of the command freqs
num=[ -2/j^2 7 0];
den=[3 0 2];
w=0:.1:10;
H=freqs(num,den,w);
freqs(num,den,w)
[num1,den1]=invfreqs(H,w,2,2)

%Equivalent frequency responses
[num2,den2]=invfreqs(H,w,3,3)
freqs(num2,den2,w)

figure
[num3,den3]=invfreqs(H,w,10,7);
freqs(num3,den3,w)


%% Author: epokh
%% Website: www.epokh.org/drupy
%% This software is under GPL

%%Transformation of the end effector frame following the RPY angles in
%%closed form
%%input: rotation angles along y,z,x axis expressed in degrees

function [RPYmat]=RPY(fia,fio,fin)

RPYmat=[cosd(fia)*cosd(fio),cosd(fia)*sind(fin)*sind(fio)-sind(fia)*cosd(fin),cosd(fia)*sind(fio)*cosd(fin)+sind(fia)*sind(fin),0;
        sind(fia)*cosd(fio),sind(fia)*sind(fio)*sind(fin)+cosd(fia)*cosd(fin),sind(fia)*sind(fio)*cosd(fin)-cosd(fia)*sind(fin),0;
        -sind(fio),cosd(fio)*sind(fin),cosd(fio)*cosd(fin),0;
        0,0,0,1];

end


function y = nanvar(x,w,dim)
%Replacement for Matlab NANVAR Variance function, ignoring NaNs.
%
%
%
%

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

sz = size(x);
if nargin < 3 || isempty(dim)
    % The output size for [] is a special case when DIM is not given.
    if isequal(x,[]), y = NaN(class(x)); return; end

    % Figure out which dimension sum will work along.
    dim = find(sz ~= 1, 1);
    if isempty(dim), dim = 1; end
elseif dim > length(sz)
    sz(end+1:dim) = 1;
end

% Need to tile the mean of X to center it.
tile = ones(size(sz));
tile(dim) = sz(dim);

if isequal(w,0) || isequal(w,1)
    % Count up non-NaNs.
    n = sum(~isnan(x),dim);

    if w == 0
        % The unbiased estimator: divide by (n-1).  Can't do this when
        % n == 0 or 1, so n==1 => we'll return zeros
        denom = max(n-1, 1);
    else
        % The biased estimator: divide by n.
        denom = n; % n==1 => we'll return zeros
    end
    denom(n==0) = NaN; % Make all NaNs return NaN, without a divideByZero warning

    x0 = x - repmat(nanmean(x, dim), tile);
    y = nansum(abs(x0).^2, dim) ./ denom; % abs guarantees a real result

% Weighted variance
elseif numel(w) ~= sz(dim)
    error('MATLAB:nanvar:InvalidSizeWgts','The length of W must be compatible with X.');
elseif ~(isvector(w) && all(w(~isnan(w)) >= 0))
    error('MATLAB:nanvar:InvalidWgts','W must be a vector of nonnegative weights, or a scalar 0 or 1.');
else
    % Embed W in the right number of dims.  Then replicate it out along the
    % non-working dims to match X's size.
    wresize = ones(size(sz)); wresize(dim) = sz(dim);
    wtile = sz; wtile(dim) = 1;
    w = repmat(reshape(w, wresize), wtile);

    % Count up non-NaNs.
    n = nansum(~isnan(x).*w,dim);

    x0 = x - repmat(nansum(w.*x, dim) ./ n, tile);
    y = nansum(w .* abs(x0).^2, dim) ./ n; % abs guarantees a real result
end


function [mlfcoeffs, R2] = mlffit2(XNodes, YDataPoints, mlfcoeffs0, Precision)
%
% Fitting data [XNodes, YDataPoints] by Mittag-Leffler function
% y(t) = C*t^(beta-1)*E_{alpha, beta}(a*t^alpha)
%
% Output:   mlfcoeffs(1) = alpha
%           mlfcoeffs(2) = beta
%           mlfcoeffs(3) = C
%           mlfcoeffs(4) = a
%
%           R2 = sum of squares of vertical offsets
%                of the fitting curve from the data points
%
% (C) Igor Podlubny, 2011

% The following is just one-time installation
% of the necessary packages 8738 and 31069
% from Matlab Central FIle Exchange (FEX)

% Check if the function 'mlf' from the FEX package 8738
% (for evaluation of the Mittag-Leffler function)
% is on your MATLAB path, and if it is not there,
% require the FEX packages 31069 ("requireFEXpackage")
% and 8738 ("Mittag-Leffler function"):
if ~(exist('mlf', 'file') == 2)
    P = requireFEXpackage(31069); % "requireFEXpackage"
    P = requireFEXpackage(8738);  % "Mittag-Leffler function"
end

% Also, the function 'fminsearchbnd' (FEX package 8277) is nedeed:
if ~(exist('fminsearchbnd', 'file') == 2)
    P = requireFEXpackage(8277);  % fminsearchbnd is part of 8277
end

% Now, we have MLF.M installed and can do the fitting

c = fminsearchbnd(@(Params) mlfparam(Params, XNodes, Precision, YDataPoints), mlfcoeffs0, ...
            [eps; eps; -Inf; -Inf]);

R2 = sum(c(3)*XNodes.^(c(2)-1).*(mlf(c(1), c(2), c(4)*XNodes.^c(1), Precision) - YDataPoints).^2);
mlfcoeffs = c;
end

function f = mlfparam(Params, XNodes, Precision, YDataPoints)
R2 =  sum((Params(3)* XNodes.^(Params(2)-1).*mlf(Params(1), Params(2), ...
        Params(4)*XNodes.^Params(1), Precision) - YDataPoints).^2);
f = R2;
end


function out = poly(f)
%POLY   Polynomial coefficients of a CHEBTECH.
%   C = POLY(F) returns the polynomial coefficients of F so that
%           F(x) = C(1)*x^N + C(2)*x^(N-1) + ... + C(N)*x + C(N+1)
%
%   Note that unlike the MATLAB POLY command, CHEBTECH/POLY can operate on
%   array-valued CHEBTECH objects, and hence produce a matrix output. In such
%   instances, the rows of C correspond to the columns of F = [F1, F2, ...].
%   That is,
%           F1(x) = C(1,1)*x^N + C(1,2)*x^(N-1) + ... + C(1,N)*x + C(1,N+1)
%           F2(x) = C(2,1)*x^N + C(2,2)*x^(N-1) + ... + C(2,N)*x + C(2,N+1).
%   This strange behaviour is a result of MATLAB's decision to return a row
%   vector from the POLY command, even for column vector input.
%
% See also CHEBCOEFFS, TRIGCOEFFS, LEGCOEFFS.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% [Mathematical reference]: Section 3.3 Mason & Handscomb, "Chebyshev
% Polynomials". Chapman & Hall/CRC (2003).
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Deal with empty case:
if ( isempty(f) )
    out = [];
    return
end

% Flip the Chebyshev coefficients to match Matlab ordering:
coeffs = flipud(f.coeffs);
[n, m] = size(coeffs);

% Coefficients on the unit interval:
if ( n == 1 )
    % Constant case:
    out = coeffs;

elseif ( n == 2 )
    % Linear case:
    out = coeffs.';

else
    % General case:

    % Initialise working vectors:
    tn = zeros(m, n);
    tnold1 = tn;
    tnold1(:,2) = 1;
    tnold2 = tn;
    tnold2(:,1) = 1;
    zer = zeros(m, 1);

    % Transpose coeffs (as output is in this form):
    coeffs = coeffs.';

    % Initialise output:
    out = zeros(m, n);

    % Initial step:
    out(:,[1,2]) = [ zer coeffs(:,end).*tnold2(:,1) ] + ...
        bsxfun(@times, coeffs(:,end-1), tnold1(:,[2,1]));

    % Recurrence:
    for k = 3:n
        tn(:,1:k)  = [ zer 2*tnold1(:,1:k-1) ] - [ tnold2(:,1:k-2), zer, zer ];
        out(:,1:k) = bsxfun(@times, coeffs(:,end-k+1), tn(:,k:-1:1)) + ...
                        [ zer, out(:,1:k-1) ];
        tnold2 = tnold1;
        tnold1 = tn;
    end

end

end


function y_line=runline(y,n,dn)
% Running line fit (local linear regression)
%
% Usage: y_line=runline(y,n,dn);
%
% Inputs:
% y: input 1-d time series (real)
% n: length of running window in samples
% dn: stepsize of window in samples
%
% Outputs:
% y_line: local line fit to data

y=y(:);
nt=length(y);
y_line=zeros(nt,1);
norm=y_line;
nwin=ceil((nt-n)/dn);
yfit=zeros(nwin,n);
xwt=((1:n)-n/2)/(n/2);
wt=(1-abs(xwt).^3).^3;
for j=1:nwin,
	tseg=y(dn*(j-1)+1:dn*(j-1)+n);
	y1=mean(tseg);
	y2=mean((1:n)'.*tseg)*2/(n+1);
	a=(y2-y1)*6/(n-1); b=y1-a*(n+1)/2;
	yfit(j,:)=(1:n)*a+b;
	y_line((j-1)*dn+(1:n))=y_line((j-1)*dn+(1:n))+(yfit(j,:).*wt)';
	norm((j-1)*dn+(1:n))=norm((j-1)*dn+(1:n))+wt';
end
mask=find(norm>0); y_line(mask)=y_line(mask)./norm(mask);
indx=(nwin-1)*dn+n-1;
npts=length(y)-indx+1;
y_line(indx:end)=(n+1:n+npts)'*a+b;


function value = r4_gamr ( x )

%*****************************************************************************80
%
%% R4_GAMR evaluates the reciprocal gamma function of an R4 argument.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 September 2011
%
%  Author:
%
%    Original FORTRAN77 version by Wayne Fullerton.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Wayne Fullerton,
%    Portable Special Function Routines,
%    in Portability of Numerical Software,
%    edited by Wayne Cowell,
%    Lecture Notes in Computer Science, Volume 57,
%    Springer 1977,
%    ISBN: 978-3-540-08446-4,
%    LC: QA297.W65.
%
%  Parameters:
%
%    Input, real X, the argument.
%
%    Output, real VALUE, the value of the reciprocal gamma
%    function at X.
%
  if ( x <= 0.0 && r4_aint ( x ) == x )

    value = 0.0;

  elseif ( abs ( x ) <= 10.0 )

    value = 1.0 / r4_gamma ( x );

  else

    [ alngx, sgngx ] = r4_lgams ( x );
    value = sgngx * exp ( - alngx );

  end

  return
end


function lagrange2D
%% STARTVARIABLES
[X,Y]=meshgrid(-1:1:5,-1:1:5);
Z = sin(Y)*sin(X);

step = 0.2;
%% INTERPOLATION
% erst kurven in x richtung ausrechnen
xCurves={};
for i=1:size(X,1)
    % x vector must be a column vector for the lagrange function
    x = X(i,:)';
    % z vector must be a column vector for the lagrange function
    z = Z(i,:)';
    p=[];
    for j = x(1):step:x(end)
        % interpolate for every parameter value j and add it to p
        p = [p,lagrange(x,z,j)];
    end
    % save curves in x direction
    xCurves{i} = p;
end

y = Y(:,1);
% matrix for the graphical outpu
A=[];
% interpolate in y-direction
for i=1:length(xCurves{1})
    p=[];
    z=[];
    for l=1:length(y)
        z = [z;xCurves{l}(i)];
    end
    for j = y(1):step:y(end)
         % interpolate for every parameter value j and add it to p
        p = [p;lagrange(y,z,j)];
    end
    A = [A,p];
end

%% GRAPHICAL OUTPUT
% plot surface
surf(x(1):(x(end)-x(1))/(size(A,1)-1):x(end),y(1):(y(end)-y(1))/(size(A,1)-1):y(end),A);
hold on;
% plot points
for i=1:size(X,1)
    for j=1:size(Y,1)
        p = plot3(X(i,j),Y(i,j),Z(i,j));
        set(p,'Marker','.');
        set(p,'MarkerSize',30);
    end
end

end
%% nested functions
function res = lagrange(x,z,t)
% lagrange interpolation in 1d
% input: x...vektor, welcher die x (bzw. y) Werte beinhaltet

res = 0;
for i=1:length(x)
    % delete the component not needed
    temp = [x(1:i-1);x(i+1:end)];

    denominator = x(i)-temp;

    numerator = t-temp;
    res = res + z(i)*(prod(numerator))/(prod(denominator));
end
end


% Demonstration for EKF using a random sine signal model.
%
% A Very simple demonstration for extended Kalman filter (EKF), which is
% used to track a random single-component sinusoid signal,
% which is modelled as x_k = a_k*sin(\theta_k), dtheta/dt = omega_k.
% The signal is also filtered with unscented Kalman filter (UKF) for
% comparison.
%
% Copyright (C) 2007 Jouni Hartikainen
%
% 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.

clc;
disp('Filtering the signal with EKF...');

save_plots = 1;

% Measurement model and it's derivative
f_func = @ekf_sine_f;
h_func = @ekf_sine_h;
dh_dx_func = @ekf_sine_dh_dx;
d2h_dx2_func = @ekf_sine_d2h_dx2;

% Initial values for the signal.
f = 0;
w = 10;
a = 1;

% Number of samples and stepsize.
d = 5;
n = 500;
dt = d/n;
x = 1:n;

% Check the derivative of the measurement function.
der_check(h_func, dh_dx_func, 1, [f w a]');

% Dynamic state transition matrix in continous-time domain.
F = [0 1 0;
     0 0 0;
     0 0 0];

% Noise effect matrix in continous-time domain.
L = [0 0;
     1 0;
     0 1];

% Spectral power density of the white noise.
q1 = 0.2;
q2 = 0.1;
Qc = diag([q1 q2]);

% Discretize the plant equation.
[A,Q] = lti_disc(F,L,Qc,dt);

% Generate the real signal.
X = zeros(3, n);
X(:,1) = [f w a]';
for i = 2:n
   X(:,i) = A*X(:,i-1) + gauss_rnd([0 0 0]', Q);
end

% Generate the observations with Gaussian noise.
sd = 1;
R = sd^2;

Y = zeros(1,n);
Y_real = feval(h_func,X);
Y = Y_real + gauss_rnd(0,R,n);

plot(x,Y,'.',x,Y_real)

% Initial guesses for the state mean and covariance.
M = [f w a]';
P = diag([3 3 3]);

% Reserve space for estimates.
MM = zeros(size(M,1),size(Y,2));
PP = zeros(size(M,1),size(M,1),size(Y,2));

% Estimate with EKF
for k=1:size(Y,2)
   [M,P] = ekf_predict1(M,P,A,Q);
   [M,P] = ekf_update1(M,P,Y(:,k),dh_dx_func,R*eye(1),h_func);
   MM(:,k)   = M;
   PP(:,:,k) = P;
end

% Initial guesses for the state mean and covariance.
M = [f w a]';
P = diag([3 3 3]);

% Reserve space for estimates.
MM2 = zeros(size(M,1),size(Y,2));
PP2 = zeros(size(M,1),size(M,1),size(Y,2));

% Estimate with EKF
for k=1:size(Y,2)
   [M,P] = ekf_predict1(M,P,A,Q);
   [M,P] = ekf_update2(M,P,Y(:,k),dh_dx_func,d2h_dx2_func,R*eye(1),h_func);
   MM2(:,k)   = M;
   PP2(:,:,k) = P;
end

clf; clc;
disp('The filtering results using the 1st order EKF is now displayed')

% Project the estimates to measurement space
Y_m = feval(h_func, MM);
Y_m2 = feval(h_func, MM2);
plot(x,Y,'.', x,Y_real,'--',x,Y_m);
legend('Measurements','Real signal', 'Filtered estimate');
xlim([0 ceil(max(x))]);
title('Estimating a random Sine signal with extended Kalman filter.');

if save_plots
    print -dpsc demo2_f1.ps
end

clc;
disp('The filtering result using the 1st order EKF is now displayed.');
fprintf('Smoothing the estimates using the RTS smoother...');

[SM1,SP1] = erts_smooth1(MM,PP,A,Q);

[SM2,SP2] = etf_smooth1(MM,PP,Y,A,Q,[],[],[],...
                         dh_dx_func,R*eye(1),h_func);

[SM1_2,SP1_2] = erts_smooth1(MM2,PP2,A,Q);

[SM2_2,SP2_2] = etf_smooth1(MM2,PP2,Y,A,Q,[],[],[],...
                         dh_dx_func,R*eye(1),h_func);

fprintf('ready.\n');
disp(' ');
disp('Push any button to display the smoothing results.');
pause

Y_s1 = feval(h_func, SM1);
plot(x,Y,'.', x, Y_real,'--',x,Y_s1);
legend('Measurements','Real signal','Smoothed estimate');
xlim([0 ceil(max(x))]);
title(['Smoothing a random Sine signal with extended ',...
       'Kalman (RTS) smoother.']);

if save_plots
    print -dpsc demo2_f2.ps
end

clc;
disp('The smoothing results using the ERTS smoother is now displayed.');
disp(' ');
disp('Push any button to see the smoothing results of a ETF smoother.');
pause

Y_s2 = feval(h_func, SM2);
plot(x,Y,'.', x, Y_real,'--',x,Y_s2);
legend('Measurements','Real signal','Smoothed estimate');
title(['Smoothing a random Sine signal with extended ',...
       'Kalman (Two Filter) smoother.']);
xlim([0 ceil(max(x))]);

if save_plots
    print -dpsc demo2_f3.ps
end

clc;
disp('The smoothing results using the ETF smoother is now displayed.');

Y_s1_2 = feval(h_func, SM1_2);
Y_s2_2 = feval(h_func, SM2_2);
% Errors.
EMM_Y = sum((Y_m-Y_real).^2)/n;
EMM_Y2 = sum((Y_m2-Y_real).^2)/n;
ESM1_Y = sum((Y_s1-Y_real).^2)/n;
ESM2_Y = sum((Y_s2-Y_real).^2)/n;
ESM1_2_Y = sum((Y_s1_2-Y_real).^2)/n;
ESM2_2_Y = sum((Y_s2_2-Y_real).^2)/n;

disp(' ');
fprintf('Filtering now with UKF...');

% In the rest the signal is filtered with UKF for comparison.

% Initial guesses for the state mean and covariance.
M = [f w a]';
P = diag([3 3 3]);

% Reserve space for estimates.
U_MM = zeros(size(M,1),size(Y,2));
U_PP = zeros(size(M,1),size(M,1),size(Y,2));

% Estimate with UKF
for k=1:size(Y,2)
   [M,P,X_s,w] = ukf_predict3(M,P,f_func,Q,R*eye(1),dt);
   [M,P] = ukf_update3(M,P,Y(:,k),h_func,R*eye(1),X_s,w,[]);
   U_MM(:,k)   = M;
   U_PP(:,:,k) = P;
end
[U_SM, U_SP] = urts_smooth1(U_MM,U_PP,f_func,Q,dt);

fprintf('ready.\n')
disp(' ');
disp('Push any button to see the filtering results.');
pause

Y_m_u = feval(h_func, U_MM);
plot(x,Y,'.', x, Y_real,'--',x,Y_m_u);
legend('Measurements','Real signal', 'Filtered estimate');
xlim([0 ceil(max(x))]);
title('Estimating a random Sine signal with unscented Kalman filter.');

clc;
disp('The filtering results of a UKF are now displayed.')
disp(' ');
disp('Push any button to display the smoothing results.');
pause

Y_m_su = feval(h_func, U_SM);
plot(x,Y,'.', x, Y_real,'--',x,Y_m_su);
legend('Measurements','Real signal', 'Filtered estimate');
xlim([0 ceil(max(x))]);
title('Estimating a random Sine signal with unscented Kalman smoother (RTS).');

clc;
disp('The smoothing results of a ERTS smoother are now displayed');

UKF_EMM_Y = sum((Y_m_u-Y_real).^2)/n;
URTS_EMM_Y = sum((Y_m_su-Y_real).^2)/n;

disp(' ');
disp('Mean square errors of all estimates:');
fprintf('EKF1-MSE = %.4f\n',sqrt(EMM_Y));
fprintf('ERTS-MSE = %.4f\n',sqrt(ESM1_Y));
fprintf('ETF-MSE = %.4f\n',sqrt(ESM2_Y));
fprintf('EKF2-MSE = %.4f\n',sqrt(EMM_Y2));
fprintf('ERTS2-MSE = %.4f\n',sqrt(ESM1_2_Y));
fprintf('ETF2-MSE = %.4f\n',sqrt(ESM2_2_Y));
fprintf('UKF-RMSE = %.4f\n',sqrt(UKF_EMM_Y));
fprintf('URTS-RMSE = %.4f\n',sqrt(URTS_EMM_Y));


function [res,fval,it] = muller (f,Z0,itmax,ztol,ftol,option)
% MULLER find a zero of a real or complex function Y = F(Z)
%
% Syntax:
%
% RES = MULLER (F,Z0) find the zero of a complex or real function
% 'F' (either an anonymous function or .m function) using three initial
% guesses contained in the vector Z0. Muller takes the function F and
% evaluetes it at each initial point using feval. F doesn't need to be
% vectorized.
% The initial guesses can be real or complex numbers close to the zero,
% bracketing the zero is not necessary. Parameters ITMAX, ZTOL and
% FTOL are set by default to 1000, 1e-5 and 1e-5, respectively.
%
% RES = MULLER (F,Z0,ITMAX) the maximum number of iterations is set
% equal to ITMAX. ZTOL and FTOL are set by default with the values mentioned
% above.
%
% RES = MULLER (F,Z0,ITMAX,ZTOL) ZTOL is used as a stopping
% criterion. If the absolute difference between the values of Z found in
% the two latest iterations is less than ZTOL, the program is stopped. FTOL
% is set by default with the value mentioned above.
%
% RES = MULLER (F,Z0,ITMAX,ZTOL,FTOL) FTOL is used as a stopping
% criterion. If the value of the function F at the Z found in the last
% iteration is less than FTOL, the program is stopped.
%
% RES = MULLER (F,Z0,ITMAX,ZTOL,FTOL,'both') indicate that both
% criteria ZTOL and FTOL must be satisfied simultaneously. By default,
% MULLER stops if one of the two criteria is fulfilled.
%
% [RES,FVAL] = MULLER (F,Z0,...) return the value of the function
% F at the Z found in the last iteration.
%
% [RES,FVAL,IT] = MULLER (F,Z0,...) return the number of iterations
% used to find the zero.
%
% Example 1:
% myf = @(x) (x-1)^3;
%
% muller(myf,[0 0.1 0.2],[],[],[],'both')
% ans =
%    1.0000 + 0.0000i
%
% Example 2:
%
% [res,fval,it] = muller2('cosh',[0 0.1 0.2],[],[],[],'both')
%
% res =
%   0.0000 + 1.5708i
%
% fval =
%   5.5845e-012 + 3.0132e-012i
%
% it =
%   5
%
% Method taken from:
% Numerical Recipes: The art of scientific computing
% W.H. Press; B.P. Flannery; S.A. Teukolsky; W.T. Vetterling
% 1986
%
% Thanks to John D'Errico for his helpfull review.
%
% Written by Daniel H. Cortes
% MAE Department, West Virginia University
% March, 2008.
%

%=================================================
% Checking proper values of the input parameters
%=================================================

if nargin > 6
    error ('Too many arguments.')
elseif nargin < 2
    error('Too few arguments.')
end

if nargin < 6
    opt = 1;
elseif ischar(option) == 1
    if size(option,2) == 4
        if sum(option == 'both') == 4
            opt = 2;
        else
            error ('Option parameter must be *both*.')
        end
    else
        error ('Option parameter must be *both*.')
    end
else
    error ('Option parameter must be a character array (string).')
end

if nargin < 5
    ftol = 1e-5;
elseif isnumeric(ftol) ~= 1
    error ('FTOL must be a numeric argument.')
elseif isempty(ftol) == 1
    ftol = 1e-5;
elseif  size(ftol,1) ~= 1 || size(ftol,2) ~= 1
    error ('FTOL cannot be an array')
end

if nargin < 4
    ztol = 1e-5;
elseif isnumeric(ztol) ~= 1
    error ('ZTOL must be a numeric argument.')
elseif isempty(ztol) == 1
    ztol = 1e-5;
elseif  size(ztol,1) ~= 1 || size(ztol,2) ~= 1
    error ('ZTOL cannot be an array.')
end

if nargin < 3
    itmax = 1000;
elseif isnumeric(itmax) ~= 1
    error ('ITMAX must be a numeric argument.')
elseif isempty(itmax) == 1
    itmax = 1000;
elseif  size(itmax,1) ~= 1 || size(itmax,2) ~= 1
    error ('ITMAX cannot be an array.')
end

if isnumeric(Z0) ~= 1
    error ('Z0 must be a vector of three numeric arguments.')
elseif isempty(Z0) == 1 || length(Z0) ~= 3 || min(size(Z0)) ~= 1
    error ('Z0 must be a vector of length 3 of either complex or real arguments.')
end

if Z0(1)==Z0(2) || Z0(1)==Z0(3) || Z0(2)==Z0(3)
    error('The initial guesses must be different')
end

%=============================
% Begining of Muller's method
%=============================

z0 = Z0(1);
z1 = Z0(2);
z2 = Z0(3);

y0 = feval ( f, z0);
y1 = feval ( f, z1);
y2 = feval ( f, z2);

for it = 1:itmax
    q = (z2 - z1)/(z1 - z0);
    A = q*y2 - q*(1+q)*y1 + q^2*y0;
    B = (2*q + 1)*y2 - (1 + q)^2*y1 + q^2*y0;
    C = (1 + q)*y2;

    if ( A ~= 0 )

        disc = B^2 - 4*A*C;

        den1 = ( B + sqrt ( disc ) );
        den2 = ( B - sqrt ( disc ) );

        if ( abs ( den1 ) < abs ( den2 ) )
            z3 = z2 - (z2 - z1)*(2*C/den2);
        else
            z3 = z2 - (z2 - z1)*(2*C/den1);
        end

    elseif ( B ~= 0 )
        z3 = z2 - (z2 - z1)*(2*C/B);
    else
        warning('Muller Method failed to find a root. Last iteration result used as an output. Result may not be accurate')
        res = z2;
        fval = y2;
        return
    end

    y3 = feval ( f, z3);

    if opt == 1
        if ( abs (z3 - z2) < ztol || abs ( y3 ) < ftol )
            res = z3;
            fval = y3;
            return
        end
    else
        if ( abs (z3 - z2) < ztol && abs ( y3 ) < ftol )
            res = z3;
            fval = y3;
            return
        end
    end

    z0 = z1;
    z1 = z2;
    z2 = z3;

    y0 = y1;
    y1 = y2;
    y2 = y3;

end

res = z2;
fval = y2;
warning('Maximum number of iterations reached. Result may not be accurate')


function pdf = arcsin_pdf ( x, a )

%*****************************************************************************80
%
%% ARCSIN_PDF evaluates the Arcsin PDF.
%
%  Discussion:
%
%    The LOGISTIC EQUATION has the form:
%
%      X(N+1) = 4.0D+00 * LAMBDA * ( 1.0D+00 - X(N) ).
%
%    where 0 < LAMBDA <= 1.  This nonlinear difference equation maps
%    the unit interval into itself, and is a simple example of a system
%    exhibiting chaotic behavior.  Ulam and von Neumann studied the
%    logistic equation with LAMBDA = 1, and showed that iterates of the
%    function generated a sequence of pseudorandom numbers with
%    the Arcsin probability density function.
%
%    The derived sequence
%
%      Y(N) = ( 2 / PI ) * Arcsin ( SQRT ( X(N) ) )
%
%    is a pseudorandom sequence with the uniform probability density
%    function on [0,1].  For certain starting values, such as X(0) = 0, 0.75,
%    or 1.0D+00, the sequence degenerates into a constant sequence, and for
%    values very near these, the sequence takes a while before becoming
%    chaotic.
%
%    PDF(X) = 1 / ( PI * Sqrt ( A**2 - X**2 ) )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Daniel Zwillinger and Stephen Kokoska,
%    CRC Standard Probability and Statistics Tables and Formulae,
%    Chapman and Hall/CRC, 2000, pages 114-115.
%
%  Parameters:
%
%    Input, real X, the argument of the PDF.
%    -A < X < A.
%
%    Input, real A, the parameter of the CDF.
%    A must be positive.
%
%    Output, real PDF, the value of the PDF.
%
  if ( a <= 0.0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'ARCSIN_PDF - Fatal error!\n' );
    fprintf ( 1, '  Parameter A must be positive.\n' );
    error ( 'ARCSIN_PDF - Fatal error!' );
  end

  if ( x <= -a | a <= x )
    pdf = 0.0;
  else
    pdf = 1.0 / ( pi * sqrt ( a * a - x * x ) );
  end

  return
end


function [kli, set_kli]= KLIndex(ar_est,ma_est,varx_est,ar,ma,varx,n_obs)

%KLIndex Kullback-Leibler index.
%   kli = KLIndex(a_est,b_est,varx_est,a,b,varx,nobs) is the exact Kullbach-Leibler
%   Index between two time series models for nobs observations.
%   The estimated proces a_est, b_est,var_est and the true process a,b,varx are assumed
%   to be normally distributed.
%
%   Possible second output argument: set of KLI's of AR models of increasing autoregressive
%   model order:
%   [kli set_kli] = KLIndex(a_est,b_est,varx_est,a,b,varx,nobs)

%S. de Waele, Februari 2003.

rc = ar_ma2rc(ar,ma,n_obs);
ar_order = length(rc)-1;
rc_est = ar_ma2rc(ar_est,ma_est,n_obs);
ar_est_order = length(rc_est)-1;

%Kullback-Leiber index of the estimated process
%with respect to the true process: kli_fef.
varp_est = varx_est*[1 cumprod(1-rc_est(2:end).^2)];

%Prediction errors (vareta)
set_ar_est = rc2arset(rc_est,0:ar_est_order);
cov = varx*arma2cor(ar,ma,ar_est_order);
covs = [cov(end:-1:2) cov];
for L = 0:ar_est_order,
    par_est = set_ar_est{1+L};
    sh = ar_est_order+L+1;
    vareta(L+1) = convol(par_est,convol(covs,par_est(end:-1:1)),sh,sh);
end

L = 0:ar_est_order;
set_kli_fef = cumsum(log(varp_est)) + cumsum(vareta./varp_est) ...
    + (n_obs-L-1).*(log(varp_est) + vareta./varp_est);

set_kli = set_kli_fef;
kli = set_kli(end);


function varargout = goertzel(x,fs,ft,N,varargin)
% GOERTZEL   Goertzel Algorithm.
%
%  [RE,IM] = GOERTZEL(X,FS,FT,N) calculates the goertzel algorithm
%  for X, considering a sampling frequency FS, the target
%  frequency FT and using a block of size N.
%
%  [MAG,PHASE] = GOERTZEL(...,'mag') returns the magnitude and
%  phase information of the transform.
%
%  [...] = GOERTZEL(...,'overlap',OL) will perform the calculation
%  on blocks that overlap. The amount of overlapping is defined by
%  OL, and can be set from 0 to 1.
%

[mag,OL] = parse_inputs(varargin{:});

if size(x,1)~=1 & size(x,2)~=1
    error('X must be a vector.')
end

flag_rot = 0;
if size(x,2) == 1
    flag_rot = 1;
    x = x';
end

n = length(x);
k = 1*(0.5+N*ft/fs);
w = 2*pi*k/N;
c = cos(w);
s = sin(w);
coef = 2*c;
ADV = max(round((1-OL)*N),1);

re = zeros(1,floor(n/ADV));
im = re;

for j = 1 : floor((n-N)/ADV)
    q1 = 0;
    q2 = 0;
    for i = 1 : N
        q0 = coef*q1 - q2 + x(i+(j-1)*ADV);
        q2 = q1;
        q1 = q0;
    end
    re(j) = q1 - q2*c;
    im(j) = q2*s;
end

if mag == 0
    varargout{1} = re;
    varargout{2} = im;
else
    ma = sqrt(re.^2 + im.^2);
    ph = angle(re+i*im);

    varargout{1} = ma;
    varargout{2} = ph;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [mag,OL] = parse_inputs(varargin)

mag = 0;
OL = 0;
flag_par = 0;

for i = 1 : nargin
    if flag_par == 1
        flag_par = 0;
        continue
    elseif strcmp(varargin{i},'mag')
        mag = 1;
    elseif strcmp(varargin{i},'overlap')
        if nargin == i
            error('OL not defined.')
        end
        flag_par = 1;
        OL = varargin{i+1};
    else
        error('Unknown parameter.')
    end
end

if OL>1 | OL<0
    error('OL should be set inside the interval [0,1].')
end


function [beta_median beta_std beta_lbound beta_ubound sigma_median]=olsestimates(betahat,sigmahat,X,k,q,cband)

% function [beta_median beta_std beta_lbound beta_ubound sigma_median]=olsestimates(betahat,sigmahat,X,k,q,cband)
% estimates the posterior mean, standard deviation and confidence interval for the VAR coefficients of an OLS VAR model
% inputs:  - vector 'betahat': OLS VAR coefficientsm in vectorised form (defined in 1.1.15)
%          - matrix 'sigmahat': OLS VAR variance-covariance matrix of residuals (defined in 1.1.10)
%          - matrix 'X': matrix of regressors for the VAR model (defined in 1.1.8)
%          - integer 'k': number of coefficients to estimate for each equation in the BVAR model (defined p 7 of technical guide)
%          - integer 'q': total number of coefficients to estimate for the BVAR model (defined p 7 of technical guide)
%          - scalar 'cband': confidence level for VAR coefficients
% outputs: - vector 'beta_median': median value of the posterior distribution of beta
%          - vector 'beta_std': standard deviation of the posterior distribution of beta
%          - vector 'beta_lbound': lower bound of the credibility interval of beta
%          - vector 'beta_ubound': upper bound of the credibility interval of beta
%          - vector 'sigma_median': median value of the posterior distribution of sigma (vectorised)

% compute the point estimate of beta
beta_median=betahat;
% compute estimates for the variance of each coefficient
% first obtain omeagahat, the (OLS) variance-covariance matrix for betahat, from (a.9.1)
omegahat=kron(sigmahat,(X'*X)\speye(k));
% consider only the diagonal (variance terms), and take the square root of each element
beta_std=diag(omegahat).^0.5;
% build the confidence intervals
for ii=1:q
beta_lbound(ii,:)=norminv((1-cband)/2,beta_median(ii,1),beta_std(ii,1));
beta_ubound(ii,:)=norminv(1-(1-cband)/2,beta_median(ii,1),beta_std(ii,1));
end

sigma_median=sigmahat;


function r1 = rOneCalculation(fracFreq)
%method for caculating r1 used in identifying noise type for a giving tau
%value. input argument is array of fractrional frequency values for a given
%tau.
%r1 = <(Zi - Zavg)*(Zi+1 - Zavg)> / <(Zi - Zavg)^2>
%Z is fractional freq values. Numerator sum is from 1 to N-1 and demonator
%is from 1 to N

avgFF = mean(fracFreq); %get average value
N = length(fracFreq); %get number of frac freq value in array
n=0; %store numerator of r1
j=0; %store denomenator of r1

%This loop builds the numerator for r1
for i = 1:(N-1)
    if i == 1
        n = (fracFreq(i) - avgFF)*(fracFreq(i+1) - avgFF);
    else
        n = n + (fracFreq(i) - avgFF)*(fracFreq(i+1) - avgFF);
    end
end
%this loop builds the denomentor for r1
for i = 1:N
    if i == 1
        j = (fracFreq(i) - avgFF)^2;
    else
        j = j + (fracFreq(i) - avgFF)^2;
    end
end

%calculate and return r1
r1 = n/j;


function [labels rts] = graph_connected_components(C)
% C - connection matrix
% labels =[1 1 1 2 2 3 3 ...]  lenght(labels)=L, label for each vertex
% labels(i) is order number of connected component, i is vertex number
% rts - roots, numbers of started vertex in each component

L=size(C,1); % number of vertex

% Breadth-first search:
labels=zeros(1,L); % all vertex unexplored at the begining
rts=[];
ccc=0; % connected components counter
while true
    ind=find(labels==0);
    if ~isempty(ind)
        fue=ind(1); % first unexplored vertex
        rts=[rts fue];
        list=[fue];
        ccc=ccc+1;
        labels(fue)=ccc;
        while true
            list_new=[];
            for lc=1:length(list)
                p=list(lc); % point
                cp=find(C(p,:)); % points connected to p
                cp1=cp(labels(cp)==0); % get only unexplored vertecies
                labels(cp1)=ccc;
                list_new=[list_new cp1];
            end
            list=list_new;
            if isempty(list)
                break;
            end
        end
    else
        break;
    end
end


function [T, M, S, DISTR, df] = dtiTTestStat(g1, g2, Y, mask)

% Computes voxel-wise T-test statistics for two groups from a data array.
%
%   [T, M, S, DISTR, df] = dtiTTestStat(g1, g2, DT_ARRAY, [MASK])
%
% Input:
%   g1, g2      List of indices that correspond to each group out of 1:N
%                   E.g: g1 = 1:7, g2 = 8:14, N = 14
%   DT_ARRAY    Data array of size XxYxZxN (or nxN), where X, Y, Z are the volume
%                   dimensions and N is the number of subjects.
%                   (n is the number of voxels).
%   MASK        Optional XxYxZ binary array. Values of M and S are computed
%                   where mask = 1; in other voxels, M and S are set to 0.
%                   Default is entire volume.
%
% Output:
%   T           XxYxZx1 array of test statistics (0 where mask = 0)
%   M           XxYxZx2 array of means for both groups (0 where mask = 0)
%   S           XxYxZx1 array of pooled standard deviations (0 where mask = 0)
%   DISTR       The string 't'
%   df          The number of degrees of freedom = N-2
%
% Utilities:    ndfun.m, dtiSplitTensor.m, dti33to6.m
%
% WARNING: If using Pentium 4, eliminate NaN's from array before running
% (processor bug).
%
% Copyright by Armin Schwartzman, 2005

% HISTORY:
%   2004.06.23 ASH (armins@stanford.edu) wrote it.
%

% Check inputs
if (ndims(Y)==2 | ndims(Y)==3),
    Ind = 1;    % Data in indexed nxN format
    Y = shiftdim(Y, -2);
else
    Ind = 0;    % Data in XxYxZxN format
end
if (ndims(Y)<4 | ndims(Y)>5),
    error('Wrong input format');
end
if (~exist('mask')),
    mask = ones([size(Y,1) size(Y,2) size(Y,3)]);
end

% Computations
N1 = length(g1);
N2 = length(g2);
N  = N1 + N2;

M = cat(4, mean(Y(:,:,:,g1), 4), mean(Y(:,:,:,g2), 4));
Ystd1 = std(Y(:,:,:,g1), 1, 4); Ystd1(~mask) = 1;
Ystd2 = std(Y(:,:,:,g2), 1, 4); Ystd2(~mask) = 1;
S = sqrt((N1*Ystd1.^2 + N2*Ystd2.^2)/(N-2) .* (1/N1 + 1/N2));
T = (M(:,:,:,1) - M(:,:,:,2)) ./ S;

% Adjust output
if Ind,
    T = shiftdim(T, 2);
    M = shiftdim(M, 2);
    S = shiftdim(S, 2);
end

DISTR = 't';
df = [N-2 N-2];     % Second entry is dummy

return


   function [mean_pos,ma_pos] = mean_posit_track(t,s,tau,train_len)
%| function [mean_pos,ma_pos] = mean_posit_track(t,s,tau,train_len)
%| INPUT
%|	t: time indices
%|	s: position sequence (should be same length as t)
%|	tau: delay parameter,
%|		set to be the equivalence index to about 0.5sec for breathing.
%|	train_Len: training length (starting up stage),
%|		approximately one period's equivalence
%|
%| OUTPUT
%|	mean_pos: a N*2 matrix,
%|		for practical purposes, take the first column mean_pos(:,1)
%|	ma_pos: naive moving average.
%|
%| Dan Ruan, University of Michigan, 2007
%|
%| Based on the paper by Dan Ruan, J A Fessler, James M Balter
%| Mean position tracking of respiratory motion
%| Med. Phys. 35(2):782-92, Feb. 2008
%| doi 10.1118/1.2825616

if nargin < 1, help(mfilename), error(mfilename), end
if streq(t, 'test'), self_test, return, end

s_complete = s;
s_d = s(1:end-tau);
s = s(tau+1:end);
for tt = 1:length(t)-train_len-tau;
        s_temp = s(tt:tt+train_len);
        sd_temp = s_d(tt:tt+train_len);
        params = fitellipse(s_temp,sd_temp);
        mean_pos(tt,:) =  [params(1),params(2)];
        ma_pos(tt) = mean(s(tt:tt+train_len));
end

plot(t,s_complete,'linewidth',2);
hold on
plot(t(1+tau+train_len:end),ma_pos,'g:','linewidth',2);
plot(t(1+tau+train_len:end),mean_pos(:,1),'r--','linewidth',2);
hold off
set(gca,'DataAspectRatio',[5 5 1]);
set(gca,'fontsize',14,'FontWeight','demi');
h = axis;
axis(h + [0 0 -1 1]);
xlabel('time (second)');
ylabel('displacement');
legend('Observation','Estimation with MA','Estimate with Ellipse Center','Location','SouthWest');

%
function a = fitellipse(X,Y)
% fit ellipse with sample locations in X,Y
% normalize data
mx = mean(X);
my = mean(Y);
sx = (max(X)-min(X))/2;
sy = (max(Y)-min(Y))/2;

x = (X-mx)/sx;
y = (Y-my)/sy;

% Force to column vectors
x = x(:);
y = y(:);

% Build design matrix
D = [ x.*x  x.*y  y.*y  x  y  ones(size(x)) ];

% Build scatter matrix
S = D'*D;

% Build 6x6 constraint matrix
C(6,6) = 0; C(1,3) = -2; C(2,2) = 1; C(3,1) = -2;

  % Break into blocks
  tmpA = S(1:3,1:3);
  tmpB = S(1:3,4:6);
  tmpC = S(4:6,4:6);
  tmpD = C(1:3,1:3);
  tmpE = inv(tmpC)*tmpB';
  [evec_x, eval_x] = eig(inv(tmpD) * (tmpA - tmpB*tmpE));

  % Find the positive (as det(tmpD) < 0) eigenvalue
  I = find(real(diag(eval_x)) < 1e-8 & ~isinf(diag(eval_x)));

  % Extract eigenvector corresponding to negative eigenvalue
  A = real(evec_x(:,I));

  % Recover the bottom half...
  evec_y = -tmpE * A;
  A = [A; evec_y];

% unnormalize
par = [
  A(1)*sy*sy,   ...
      A(2)*sx*sy,   ...
      A(3)*sx*sx,   ...
      -2*A(1)*sy*sy*mx - A(2)*sx*sy*my + A(4)*sx*sy*sy,   ...
      -A(2)*sx*sy*mx - 2*A(3)*sx*sx*my + A(5)*sx*sx*sy,   ...
      A(1)*sy*sy*mx*mx + A(2)*sx*sy*mx*my + A(3)*sx*sx*my*my   ...
      - A(4)*sx*sy*sy*mx - A(5)*sx*sx*sy*my   ...
      + A(6)*sx*sx*sy*sy   ...
      ]';

% Convert to geometric radii, and centers

thetarad = 0.5*atan2(par(2),par(1) - par(3));
cost = cos(thetarad);
sint = sin(thetarad);
sin_squared = sint.*sint;
cos_squared = cost.*cost;
cos_sin = sint .* cost;

Ao = par(6);
Au =   par(4) .* cost + par(5) .* sint;
Av = - par(4) .* sint + par(5) .* cost;
Auu = par(1) .* cos_squared + par(3) .* sin_squared + par(2) .* cos_sin;
Avv = par(1) .* sin_squared + par(3) .* cos_squared - par(2) .* cos_sin;

% ROTATED = [Ao Au Av Auu Avv]

tuCentre = - Au./(2.*Auu);
tvCentre = - Av./(2.*Avv);
wCentre = Ao - Auu.*tuCentre.*tuCentre - Avv.*tvCentre.*tvCentre;

uCentre = tuCentre .* cost - tvCentre .* sint;
vCentre = tuCentre .* sint + tvCentre .* cost;

Ru = -wCentre./Auu;
Rv = -wCentre./Avv;

Ru = sqrt(abs(Ru)).*sign(Ru);
Rv = sqrt(abs(Rv)).*sign(Rv);

a = [uCentre, vCentre, Ru, Rv, thetarad];

% self_test
function self_test
t = linspace(0, 60, 601);
dt = t(2) - t(1);
s = (8 + cos(2*pi/6*t)).^2 - 8^2;
tau = round(0.5 / dt);
train_len = round(5.1 / dt);

[mean_pos ma_pos] = mean_posit_track(t, s, tau, train_len);

prompt
tt = t(tau+train_len+1:end);
clf, plot(t, s, '-', tt, mean_pos(:,1), '--', tt, ma_pos, ':', 'linewidth', 2)
legend('signal', 'proposed', 'moving avg')
xlabel t


% book : Signals and Systems Laboratory with MATLAB
% authors : Alex Palamides & Anastasia Veloni

%problem 3 - Inverse Fourier Transform of sin(w)/w

syms t w
X=sin(w)/w;
x=ifourier(X,t)
ezplot(x, [-3 3])


function v = sum2(f)
%SUM2   Double integral of a SPHEREFUN over its domain.
%   I = SUM2(F) returns the double definite integral of a SPHEREFUN.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

% Split f into its plus/minus terms.  The minus terms have integral zero
% on the sphere since the rows in this case are anti-periodic with period
% pi. Thus, we only need to integrate the plus terms.

[cols, d, rows] = cdr(f);

% If there are no plus terms then the integral is zero
if ( isempty(f.idxPlus) )
    v = 0;
    return
end

cols = cols(:, f.idxPlus); % Just keep the plus terms
rows = rows(:, f.idxPlus);
d = diag(d(f.idxPlus, f.idxPlus)).';

% Integrate the rows over their domain.
intRows = sum(rows);

% One could use the following code to do the integrals in latitude (theta),
% but this can be slow due to the fact that the integrand (which are
% trigfuns) may first be converted to chebfuns when sum (with an interval
% other than the period of the integrand) is called.  Mathematically, this
% is, of course, unnecessary and I tried arguing that this should not be
% the case.  However, I was not successful in convincing everyone and so we
% are stuck with a potentially slow method to do definite integrals rather
% than a fast one. See ticket numbers #1004 and #1034 for more details.
%
% We are going to do the integral the fast way.

% Slow code: Left here in case someone ever makes sum(f,[a,b]) fast for
% trigfuns.
%
% % Create a trigfun of the measure for the sphere.
% measure = chebfun(@(x) sin(x), f.domain(3:4), 'trig');
%
% % Multiply the columns by the measure
% cols = cols.*(measure*ones(1, size(cols, 2)));
%
% % Integrate each column over the non-doubled up latitude coordinate.
% intCols = sum(cols, [0 pi]);

%
% Fast code: We know the columns are even functions, which means they have
% cosine series expansions:
% col(:, j) = sum_{k=0}^{m} a_k cos(k*t)
% So, in the case of the elevation angle being measured as co-latitude, we
% want to compute the integral
% int_{0}^{pi}col(:,j).*sin(t)dt = sum_{k=0}^{m} a_k int_{0}^{pi}cos(k*t).*sin(t)dt
% This simplifies down to
% int_{0}^{pi}col(:,j).*sin(t)dt = sum_{k=0}^{m} a_k (1+(-1)^k)/(1-k^2)

[a, ignore] = trigcoeffs(cols);

k = (0:size(a, 1)-1).';
intFactor = 2./(1-k(1:2:end).^2);
intCols = sum(bsxfun(@times, a(1:2:end, :), intFactor));

% Put the integrals together to get the final result.
v = sum(d.*intRows.*intCols);

% TODO: Add support for different domains.

end


function [w, infos] = subsamp_svrg(problem, options)
% Sabsampled SVRG algorithm.
%
% Inputs:
%       problem     function (cost/grad/hess)
%       options     options
% Output:
%       w           solution of w
%       infos       information
%
% References:
%       Subsampled SVRG:
%       R. Kolte, M. Erdogdu and A. Ozgur,
%       "Accelerating SVRG via second-order information,"
%       OPT2015, 2015.
%
%
% Created by H.Kasai on Oct. 28, 2016
% Modified by H.Kasai on Mar. 25, 2018

    % set dimensions and samples
    d = problem.dim();
    n = problem.samples();

    % extract options
    if ~isfield(options, 'stepsizefun')
        options.stepsizefun = @stepsize_alg;
    else
    end

    if ~isfield(options, 'tol_optgap')
        tol_optgap = 1.0e-12;
    else
        tol_optgap = options.tol_optgap;
    end

    if ~isfield(options, 'batch_size')
        batch_size = 10;
    else
        batch_size = options.batch_size;
    end

    if batch_size > n
        batch_size = n;
    end
    num_of_bachces = floor(n / batch_size);

    if ~isfield(options, 'max_epoch')
        max_epoch = 100;
    else
        max_epoch = options.max_epoch;
    end

    if ~isfield(options, 'r')
        r = inf;
    else
        r = options.r;
    end

    if r > d + 1
        r = d - 1;
    end

    if ~isfield(options, 'w_init')
        w = randn(d,1);
    else
        w = options.w_init;
    end

    if ~isfield(options, 'f_opt')
        options.f_opt = -Inf;
    end

    if ~isfield(options, 'permute_on')
        permute_on = 1;
    else
        permute_on = options.permute_on;
    end

    if ~isfield(options, 'verbose')
        verbose = false;
    else
        verbose = options.verbose;
    end

    if ~isfield(options, 'store_w')
        options.store_w = false;
    end

    % initialize
    total_iter = 0;
    epoch = 0;
    grad_calc_count = 0;
    w = options.w_init;
    num_of_bachces = floor(n / options.batch_size);

    % store first infos
    clear infos;
    [infos, f_val, optgap] = store_infos(problem, w, options, [], epoch, grad_calc_count, 0);

    %
    sample_size = round(10*r*log(d));

    % set start time
    start_time = tic();

    % display infos
    if verbose > 0
        fprintf('Sub-sampled SVRG: Epoch = %03d, cost = %.16e, optgap = %.4e\n', epoch, f_val, optgap);
    end

    % main loop
    while (optgap > tol_optgap) && (epoch < max_epoch)

        % permute samples
        if permute_on
            perm_idx = randperm(n);
        else
            perm_idx = 1:n;
        end

       % compute full gradient
        full_grad = problem.grad(w,1:n);
        % store w
        w0 = w;
        grad_calc_count = grad_calc_count + n;

        % calculated Hessian using subsamples every outer loop
        sub_indices = datasample((1:n), sample_size);
        H = problem.hess(w, sub_indices);
        [u, sigma,~] = svd(H);
        Q = u(:,1:r);
        gamma = sigma(r+1,r+1);
        sigma = sigma(1:r,1:r);
        Sing_inv_gamma = diag(1./diag(sigma)) - diag(ones(r,1)/gamma);

        for j = 1 : num_of_bachces

            % update step-size
            step = options.stepsizefun(total_iter, options);

            % calculate variance reduced gradient
            start_index = (j-1) * batch_size + 1;
            indice_j = perm_idx(start_index:start_index+batch_size-1);
            grad = problem.grad(w, indice_j);
            grad_0 = problem.grad(w0, indice_j);
            grad_est = full_grad + grad - grad_0;

            % update w
            v = -Q*(Sing_inv_gamma)*(Q' * grad_est) - (1/gamma)*grad_est;
            w = w + step * v;

            % proximal operator
            if ismethod(problem, 'prox')
                w = problem.prox(w, step);
            end

            total_iter = total_iter + 1;
        end

        % measure elapsed time
        elapsed_time = toc(start_time);

        % count gradient evaluations
        grad_calc_count = grad_calc_count + j * batch_size + sample_size;
        epoch = epoch + 1;

        % store infos
        [infos, f_val, optgap] = store_infos(problem, w, options, infos, epoch, grad_calc_count, elapsed_time);

        % display infos
        if verbose > 0
            fprintf('Sub-sampled SVRG: Epoch = %03d, cost = %.16e, optgap = %.4e\n', epoch, f_val, optgap);
        end
    end

    if optgap < tol_optgap
        fprintf('Optimality gap tolerance reached: tol_optgap = %g\n', tol_optgap);
    elseif epoch == max_epoch
        fprintf('Max epoch reached: max_epochr = %g\n', max_epoch);
    end
end


function cvx_optval = sum_largest( x, k, dim )

%SUM_LARGEST Sum of the largest k values of a vector.
%   For a real vector X and an integer k between 1 and length(X) inclusive,
%   y = SUM_LARGEST(X,k) is the sum of the k largest elements of X; e.g.,
%       temp = sort( x )
%       y = sum( temp( 1 : k ) )
%   If k=1, then SUM_LARGEST(X,k) is equivalent to MAX(X); if k=length(X),
%   then SUM_LARGEST(X,k) is equivalent to SUM(X).
%
%   Both X and k must be real, and k must be a scalar. But k is not, in
%   fact, constrained to be an integer between 1 and length(X); the
%   function is extended continuously and logically to all real k. For
%   example, if k <= 0, then SUM_LARGEST(X,k)=0. If k > length(X), then
%   SUM_LARGEST(X,k)=SUM(X). Non-integer values of k interpolate linearly
%   between their integral neighbors.
%
%   For matrices, SUM_LARGEST(X,k) is a row vector containing the
%   application of SUM_LARGEST to each column. For N-D arrays, the
%   SUM_LARGEST operation is applied to the first non-singleton dimension
%   of X.
%
%   SUM_LARGEST(X,k,DIM) performs the operation along dimension DIM of X.
%
%   Disciplined convex programming information:
%       SUM_LARGEST(X,...) is convex and nondecreasing in X. Thus, when
%       used in CVX expressions, X must be convex (or affine). k and DIM
%       must both be constant.

%
% Check arguments
%

narginchk(2,3);
if ~isreal( x ),
    error( 'First argument must be real.' );
elseif ~isnumeric( k ) || ~isreal( k ) || length( k ) ~= 1,
    error( 'Second argument must be a real scalar.' );
elseif nargin < 3 || isempty( dim ),
    dim = cvx_default_dimension( size( x ) );
elseif ~cvx_check_dimension( dim, false ),
    error( 'Third argument, if supplied, must be a positive integer.' );
end

%
% Determine output size
%

sx = size( x );
nd = max( dim, length( sx ) );
sx = [ sx, ones( 1, dim - nd ) ];
sy = sx;
sy( dim ) = 1;

%
% Compute results
%

if k <= 0,

    cvx_optval = zeros( sy );

elseif k <= 1,

    cvx_optval = k * max( x, [], dim );

elseif k >= sx( dim ),

    cvx_optval = sum( x, dim );

else

    ck = ceil( k );
    x = sort( x, dim );
    ndxs = cell( 1, nd );
    [ ndxs{:} ] = deal( ':' );
    ndxs{ dim } = size( x, dim ) - ( 0 : ck - 1 );
    x = x( ndxs{ : } );
    if k ~= ck,
        ndxs{ dim } = ck;
        x( ndxs{ : } ) = ( k - floor( k ) ) * x( ndxs{ : } );
    end
    cvx_optval = sum( x, dim );

end

% Copyright 2005-2016 CVX Research, Inc.
% See the file LICENSE.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function triangle_plots ( )

%*****************************************************************************80
%
%% TRIANGLE_PLOTS plots the surface and the R(Theta) function for a triangle.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 May 2013
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TRIANGLE_PLOTS:\n' );
  fprintf ( 1, '  For a 2D triangle defined by a 0/1 characteristic function,\n' );
  fprintf ( 1, '  plot the surface, and R(Theta),\n' );
  fprintf ( 1, '  using a centered point, and then an offcentered point.\n' );
%
%  1): Plot the surface, using a centered base point.
%
  n = 61;
  theta = linspace ( 0.0, 2.0 * pi, n );

  f = zeros(n,1);
  x0 = [ 2.0 / 3.0, 1.0 / 3.0 ];
  for i = 1 : n
    r(i) = bisect_characteristic ( x0, theta(i), @triangle_characteristic );
  end
  x = x0(1) + r(1:n) .* cos ( theta(1:n) );
  y = x0(2) + r(1:n) .* sin ( theta(1:n) );

  xc = x0(1) + 0.5 .* cos ( theta(1:n) );
  yc = x0(2) + 0.5 .* sin ( theta(1:n) );

  clf
  hold on
  plot ( x0(1), x0(2), 'b.', 'MarkerSize', 50 );
  plot ( xc, yc, 'b-', 'LineWidth', 2 );
  plot ( x, y, 'ro', 'LineWidth', 3 );
  axis equal
  grid on
  xlabel ( '<---X--->', 'FontSize', 24 );
  ylabel ( '<---Y--->', 'FontSize', 24 );
  title ( 'Triangle transition surface', 'FontSize', 24)
  hold off
  filename =  'triangle_centered_surface.png';
  print ( '-dpng', filename );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Created plotfile "%s".\n', filename );
%
%  2): Plot R as a function of Theta for a centered base point.
%
  plot ( theta, r, 'LineWidth', 3 );
  xlabel ( '<---Theta--->', 'FontSize', 24 )
  ylabel ( '<---R(Theta)--->', 'FontSize', 24 )
  title ( 'R(Theta) from base point to surface', 'FontSize', 24 )
  grid on
  filename = 'triangle_centered_plot.png';
  print ( '-dpng', filename );
  fprintf ( 1, '  Created plotfile "%s".\n', filename );
%
%  3): Plot the surface, using an offcentered base point.
%
  n = 61;
  theta = linspace ( 0.0 , 2.0 * pi, n );

  f = zeros(n,1);
  x0 = [ 1.0, 1.50 ];
  for i = 1 : n
    r(i) = bisect_characteristic ( x0, theta(i), @triangle_characteristic );
  end

  x = x0(1) + r(1:n) .* cos ( theta(1:n) );
  y = x0(2) + r(1:n) .* sin ( theta(1:n) );
%
%  Coordinates of nominal triangle around (x0,y0), to
%  suggest coordinate system.
%
  xc = x0(1) + 0.5 .* cos ( theta(1:n) );
  yc = x0(2) + 0.5 .* sin ( theta(1:n) );

  clf
  hold on
  plot ( x0(1), x0(2), 'b.', 'MarkerSize', 50 );
  plot ( xc, yc, 'b-', 'LineWidth', 2 );
  plot ( x, y, 'ro', 'LineWidth', 3 );
  axis equal
  grid on
  xlabel ( '<---X--->', 'FontSize', 24 );
  ylabel ( '<---Y--->', 'FontSize', 24 );
  title ( 'Triangle transition surface', 'FontSize', 24 )
  hold off
  filename = 'triangle_offcentered_surface.png';
  print ( '-dpng', filename );
  fprintf ( 1, '  Created plotfile "%s".\n', filename );
%
%  4): Plot R as a function of Theta for an offcentered base point.
%
  plot ( theta, r, 'LineWidth', 3 );
  xlabel ( '<---Theta--->', 'FontSize', 24 )
  ylabel ( '<---R(Theta)--->', 'FontSize', 24 )
  title ( 'R(Theta) from base point to surface', 'FontSize', 24 )
  grid on
  filename = 'triangle_offcentered_plot.png';
  print ( '-dpng', filename );
  fprintf ( 1, '  Created plotfile "%s".\n', filename );

  return
end


function x = dvand ( n, alpha, b )

%*****************************************************************************80
%
%% DVAND solves a Vandermonde system A' * x = b.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 November 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Ake Bjorck, Victor Pereyra,
%    Solution of Vandermonde Systems of Equations,
%    Mathematics of Computation,
%    Volume 24, Number 112, October 1970, pages 893-903.
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, real ALPHA(N), the parameters that define the matrix.
%    The values should be distinct.
%
%    Input, real B(N), the right hand side of the linear system.
%
%    Output, real X(N), the solution of the linear system.
%
  x(1:n) = b(1:n);

  for k = 1 : n - 1
    for j = n : -1 : k + 1
      x(j) = ( x(j) - x(j-1) ) / ( alpha(j) - alpha(j-k) );
    end
  end

  for k = n - 1 : -1 : 1
    for j = k : n - 1
      x(j) = x(j) - alpha(k) * x(j+1);
    end
  end

  return
end


function points = intersectLineSphere(line, sphere, varargin)
    %INTERSECTLINESPHERE Return intersection points between a line and a sphere
    %
    %   PTS = intersectLineSphere(LINE, SPHERE);
    %   Returns the two points which are the intersection of the given line and
    %   sphere.
    %   LINE   : [x0 y0 z0  dx dy dz]
    %   SPHERE : [xc yc zc  R]
    %   PTS     : [x1 y1 z1 ; x2 y2 z2]
    %   If there is no intersection between the line and the sphere, return a
    %   2-by-3 array containing only NaN.
    %
    %   Example
    %     % draw the intersection between a sphere and a collection of parallel
    %     % lines
    %     sphere = [50.12 50.23 50.34 40];
    %     [x, y] = meshgrid(10:10:90, 10:10:90);
    %     n = numel(x);
    %     lines = [x(:) y(:) zeros(n,1) zeros(n,2) ones(n,1)];
    %     figure; hold on; axis equal;
    %     axis([0 100 0 100 0 100]); view(3);
    %     drawSphere(sphere);
    %     drawLine3d(lines);
    %     pts = intersectLineSphere(lines, sphere);
    %     drawPoint3d(pts, 'ro');
    %
    %   See also
    %   spheres, circles3d, intersectPlaneSphere
    %
    %   ---------
    %   author : David Legland
    %   INRA - TPV URPOI - BIA IMASTE
    %   created the 18/02/2005.
    %
    %   HISTORY
    %   2011-06-21 bug for tangent lines, add tolerance
    % Process input arguments
    % check if user-defined tolerance is given
    tol = 1e-14;
    if ~isempty(varargin)
        tol = varargin{1};
    end
    % difference between centers
    dc = bsxfun(@minus, line(:, 1:3), sphere(:, 1:3));
    % equation coefficients
    a = sum(line(:, 4:6) .* line(:, 4:6), 2);
    b = 2 * sum(bsxfun(@times, dc, line(:, 4:6)), 2);
    c = sum(dc.*dc, 2) - sphere(:,4).*sphere(:,4);
    % solve equation
    delta = b.*b - 4*a.*c;
    % initialize empty results
    points = NaN * ones(2 * size(delta, 1), 3);
    % process couples with two intersection points
    % proces couples with two intersection points
    inds = find(delta > tol);
    if ~isempty(inds)
        % delta positive: find two roots of second order equation
        u1 = (-b(inds) -sqrt(delta(inds))) / 2 ./ a(inds);
        u2 = (-b(inds) +sqrt(delta(inds))) / 2 ./ a(inds);

        % convert into 3D coordinate
        points(inds, :) = line(inds, 1:3) + bsxfun(@times, u1, line(inds, 4:6));
        points(inds+length(delta),:) = line(inds, 1:3) + bsxfun(@times, u2, line(inds, 4:6));
    end
    % process couples with one intersection point
    % proces couples with two intersection points
    inds = find(abs(delta) < tol);
    if ~isempty(inds)
        % delta around zero: find unique root, and convert to 3D coord.
        u = -b(inds) / 2 ./ a(inds);

        % convert into 3D coordinate
        pts = line(inds, 1:3) + bsxfun(@times, u, line(inds, 4:6));
        points(inds, :) = pts;
        points(inds+length(delta),:) = pts;
    end
end


function [cap res] = pll_synth_3rd_order(ipump, vco_sensitivity, fout, fcomp, bandwidth, pm)

% Synthesizes PLL loop components for a 3rd order system using the
% topology shown below.
%
% Input - ipump is the charge pump current in Amperes
%       - vco_sensitivity is the VCO sensitivity in Hertz/Volt
%       - fout is the output frequency in Hertz
%       - fcomp is the comparison frequency in Hertz
%       - bandwidth is the open loop bandwidth in Hertz
%       - pm is the phase margin in degrees
% Output - cap is the capacitors of the loop in Farads [C1, C2, C3, 0]
%        - res is the resistors of the  loop in Ohms [ R2, R3, 0]
%
%
% The method used here is derived from that presented in Dean Banerjee's
% Book "PLL Performance, Simulation, and Design" 4th Ed available at
% National Semiconductors site www.national.com.  Most of the work is
% derived from Chapter 22 pp.178-185.
%
% No closed form solution exists for 3rd
% order systems, therefore some simplification of the model has been made
% to derive the results.  Due to the simplifications the results will
% also not always be 100% accurate.  It is recommended that the
% results are verified using the pll_simulation.m script, which does not
% use a simplified model.
% Loop Topology
%
%        +  _____                                   ______
% fcomp -->|Phase|-----------------R3---------------| VCO |---->fout
%          |Det. |     |      |         |           |     |  |
%           -----      |      |         |            -----   |
%            ^ -      C1     R2        C3                    |
%            |         |      |         |                    |
%            |        GND    C2        GND                   |
%            |                |                              |
%            |               GND         _______             |
%            ----------------------------| 1/N |--------------
%                                        -------
%
%   Author: Ben Gilbert
%   Homepage: http://nicta.com.au/people/gilbertb
%   Email: ben.gilbert (wibble) nicta.com.au
%   (c) 2009 by National ICT Australia (NICTA)
%   %

%% Design Parameters
% The following values will give sensible results but not necessarily
% optimal
T31 = 0.6; % T3/T1 ratio of time constants
gamma = 1.0; % optimization factor p172 and pp220-227 of Banerjee

% Conversion of parameters to more convenient units
Kpd = ipump/2/pi; % phase detector gain
Kvco = vco_sensitivity*2*pi; % vco gain
omega = 2*pi* bandwidth; % open loop bandwidth in radians/sec

%% Synthesis
%% Find T1 using numerical methods
t1 = (sec(pm*pi/180)-tan(pm*pi/180))/(omega*(1+T31)); % approximate value for T1

% T1 is the root of this equation
num1 = @(t) atan(gamma./(omega.*t*(1+T31)))-atan(omega.*t)-atan(omega.*t*T31)- pm*pi/180;
T1 = fzero(num1,t1);% find root numerically

%% Find T2 and T3
T3 = T1*T31;
T2 = gamma/omega^2/(T1+T3);

%% Solving for Loop Components
A0 = Kpd * Kvco * fcomp/fout / omega^2 * sqrt((1+omega^2*T2^2)/(1+omega^2*T1^2)/(1+omega^2*T3^2));
A1 = A0*(T1+T3);
A2 = A0*T1*T3;

C1 = A2/T2^2*(1+sqrt(1+T2/A2*(T2*A0-A1)));
C3 = (-T2^2*C1^2+T2*A1*C1-A2*A0)/(T2^2*C1-A2);
C2 = A0-C1-C3;
R2 = T2/C2;
R3 = A2/C1/C3/T2;

cap = [C1 C2 C3 0];
res = [R2 R3 0];


function [U,s,V] = csvd(A,tst)
%CSVD Compact singular value decomposition.
%
% s = csvd(A)
% [U,s,V] = csvd(A)
% [U,s,V] = csvd(A,'full')
%
% Computes the compact form of the SVD of A:
%    A = U*diag(s)*V',
% where
%    U  is  m-by-min(m,n)
%    s  is  min(m,n)-by-1
%    V  is  n-by-min(m,n).
%
% If a second argument is present, the full U and V are returned.

% Per Christian Hansen, IMM, 06/22/93.

if (nargin==1)
  if (nargout > 1)
    [m,n] = size(A);
    if (m >= n)
      [U,s,V] = svd(full(A),0); s = diag(s);
    else
      [V,s,U] = svd(full(A)',0); s = diag(s);
    end
  else
    U = svd(full(A));
  end
else
  if (nargout > 1)
    [U,s,V] = svd(full(A)); s = diag(s);
  else
    U = svd(full(A));
  end
end


function varargout = ellipsoid( a, b, c )
%ELLIPSOID  Generate an ellipsoid-like surface. (Not necessarily an ellipsoid!)
%   ELLIPSOID(A,B,C), where A, B, and C are CHEBFUN2 objects on the domain [0
%   pi]x[0 2*pi] plots the "ELLIPSOID" of semi axis lengths A(th,phi),
%   B(th,phi), and C(th,phi).
%
%   [X Y Z]=ELLIPSOID(A,B,C) returns X, Y, and Z as CHEBFUN2 objects such that
%   SURF(X,Y,Z) plots an ELLIPSOID of semi axis lengths A(th,phi), B(th,phi),
%   and C(th,phi).
%
%   F = ELLIPSOID(A,B,C) returns the CHEBFUN2V representing the ELLIPSOID
%   SURF(F) plots the ELLIPSOID.
%
%   Omitting output arguments causes the ELLIPSOID command to be displayed with
%   a SURF command and no outputs are returned.
%
% For the ellipsoid:
%   a = chebfun2(@(th,phi) 1+0*th,[0 pi 0 2*pi]);
%   F = ellipsoid(a,2*a,3*a); surf(F)
%
% For a badly shaped ship:
%   a = chebfun2(@(th,phi) th,[0 pi 0 2*pi])
%   F = ellipsoid(a,2*a,cos(2*a)+.25); surf(F)
%
% See also SPHERE, CYLINDER.

% Copyright 2017 by The University of Oxford and The Chebfun Developers.
% See http://www.chebfun.org/ for Chebfun information.

if ( nargin < 3 )
    error('CHEBFUN:CHEBFUN2:ellipsoid:inputs', 'Incorrect input arguments.');
end

% ELLIPSOID with axis lengths a(th,phi), b(th,phi), c(th,phi).
dom = [0 pi 0 2*pi];
th = chebfun2(@(th, phi) th, dom);
phi = chebfun2(@(th, phi) phi, dom);

x = a.*sin(th).*cos(phi);
y = b.*sin(th).*sin(phi);
z = c.*cos(th);

if ( nargout == 0 )
    surf(x, y, z)
    axis equal
elseif ( nargout == 1 )
    varargout = { [x ; y ; z] };
else
    varargout = { x, y, z };
end

end


function [ m, d ] = easter_ds ( y )

%*****************************************************************************80
%
%% EASTER_DS computes the month and day of Easter for a Gregorian year.
%
%  Example:
%
%    Input:
%
%      Y = 2000
%
%    Output:
%
%      M = 4
%      D = 23
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 June 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Peter Duffett-Smith,
%    Practical Astronomy With Your Calculator,
%    Third Edition,
%    Cambridge University Press, 1996,
%    ISBN: 0-521-35699-7,
%    LC: QB62.5.D83.
%
%  Parameters:
%
%    Input, integer Y, the year, which must be 1583 or greater.
%    (The formula is only valid for years after the Gregorian calendar
%    was adopted.)
%
%    Output, integer M, D, the month and day of Easter.
%
  if ( y <= 0 )
    m = -1;
    d = -1;
    return
  end

  a = year_to_golden_number ( y );

  a = a - 1;

  b = floor ( y / 100 );
  c = mod ( y, 100 );

  dd = floor ( b / 4 );
  e = mod ( b, 4 );

  f = floor ( ( b + 8 ) / 25 );
  g = floor ( ( b - f + 1 ) / 3 );
  h = mod ( 19 * a + b - dd - g + 15, 30 );

  i = floor ( c / 4 );
  k = mod ( c, 4 );

  l = mod ( 32 + 2 * e + 2 * i - h - k, 7 );
  mm = floor ( ( a + 11 * h + 22 * l ) / 451 );

  m = floor ( ( h + l - 7 * mm + 114 ) / 31 );
  d = mod ( h + l - 7 * mm + 114, 31 ) + 1;

  return
end