function [w,b,sv,obj] = linear_primal_svm(lambda,wInit,bInit,D,noneg, maxIteration,opt)
% Solves the following SVM optimization problem in the primal (with quatratic
%   penalization of the training errors). Default solved by Newton.
%
% min_{w,e}  lambda/2 * w'w  + 1/2 sum_i out_i^2
%      s.t.  Y_i * (w .* X_i + b) >= D_i - out_i
%            w(nonneg)>=0
%
% A global variable X containing the training inputs
%   should be defined. X is an n x d matrix (n = number of points).
%   X can be either normal matrix or sparse matrix.
% A global variable Y is the target vector of size nx1. Normal SVM will
%   have +1 and -1 value, but it can be actually aribitury value
% A global variable n is the number of elements that you want to use for training.
% LAMBDA is the regularization parameter ( = 1/C)
% wInit is an optional input for the initial value of [w;b]
% dvec is an optional input, usually it is 1 for standard SVM
% maxIteration is the number of iterations allowd
%
% W is the hyperplane w (vector of length d).
% B is the bias
% The outputs on the training points are either X*W+B
% SV is the support vector index number
% OBJ is the objective function value
% OPT is a structure containing the options (in brackets default values):
%   cg: Do not use Newton, but nonlinear conjugate gradients [0]
%   lin_cg: Compute the Newton step with linear CG
%           [0 unless solving sparse linear SVM]
%   iter_max_Newton: Maximum number of Newton steps [20]
%   prec: Stopping criterion
%   cg_prec and cg_it: stopping criteria for the linear CG.

% Original written by Olivier Chapelle @ http://olivier.chapelle.cc/primal/
% Modified by Jianxiong Xiao to have several advance features @ http://mit.edu/jxiao/

if ~exist('maxIteration','var') || maxIteration==Inf     % Assign the options to their default values
    maxIteration = 10000000;
end

if ~exist('opt','var')       % Assign the options to their default values
    opt = [];
end
if ~isfield(opt,'cg'),                opt.cg = 0;                        end;
if ~isfield(opt,'lin_cg'),            opt.lin_cg = 0;                    end;
if ~isfield(opt,'iter_max_Newton'),   opt.iter_max_Newton = 20;          end; % used to be 20
if ~isfield(opt,'prec'),              opt.prec = 1e-6;                   end;
if ~isfield(opt,'cg_prec'),           opt.cg_prec = 1e-4;                end;
if ~isfield(opt,'cg_it'),             opt.cg_it = 20;                    end;

global X;
global Y;

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

if ~exist('dvec','var') || isempty(D)
    D = ones(numel(Y),1);
end

if isempty(X), error('Global variable X undefined'); end;

if ~exist('bInit','var')
    bInit=0;
end
if ~exist('wInit','var')
    d = size(X,2);
    wInit = zeros(d,1);
end
if issparse(X)
    opt.lin_cg = 1;
end;
if ~opt.cg
    [sol,obj, sv] = primal_svm_linear   (lambda,maxIteration,wInit,bInit,D,noneg,opt);
else
    [sol,obj, sv] = primal_svm_linear_cg(lambda,maxIteration,wInit,bInit,D,noneg,opt);
end;

% The last component of the solution is the bias b.
b = sol(end);
w = sol(1:end-1);
fprintf('\n');

% -------------------------------
% Train a linear SVM using Newton
% -------------------------------
function  [w,obj,sv] = primal_svm_linear(lambda,maxIteration,wInit,bInit,D,noneg,opt)

global X;
global Y;
global n;
d = size(X,2);

w = [wInit; bInit]; % The last component of w is b.
w(noneg) = max(w(noneg),0);
%out = ones(n,1); % Vector containing 1-Y.*(X*w)
out = D(1:n) - Y(1:n).*(X(1:n,:)*w(1:end-1)+w(end));

for iter=1:maxIteration
    if iter > opt.iter_max_Newton;
        warning('PrimalSVM:MaxNumNewton','Maximum number of Newton steps reached. Try larger lambda');
        break;
    end;

    [obj, grad, sv] = obj_fun_linear(w,lambda,out);

    % Compute the Newton direction either exactly or by linear CG
    if opt.lin_cg
        % Advantage of linear CG when using sparse input: the Hessian is never computed explicitly.
        [step, foo, relres] = minres(@hess_vect_mult, -grad, opt.cg_prec,opt.cg_it,[],[],[],sv,lambda);
    else
        Xsv = X(sv,:);
        hess = lambda*diag([ones(d,1); 0]) + [[Xsv'*Xsv sum(Xsv,1)']; [sum(Xsv) length(sv)]];   % Hessian
        step  = - hess \ grad;   % Newton direction
    end;

    % Do an exact line search
    [t,out, sv] = line_search_linear(w,step,out, lambda);

    w = w + t*step;
    w(noneg) = max(w(noneg),0);
    fprintf('Iter = %d, Obj = %f, Nb of sv = %d, Newton decr = %.3f,  Line search = %.3f',iter,obj,length(sv),-step'*grad/2,t);
    if opt.lin_cg
        fprintf(', Lin CG acc = %.4f     \n',relres);
    else
        fprintf('      \n');
    end;

    if -step'*grad < opt.prec * obj
        % Stop when the Newton decrement is small enough
        break;
    end;
end;

% -----------------------------------------------------
% Train a linear SVM using nonlinear conjugate gradient
% -----------------------------------------------------
function  [w, obj, sv] = primal_svm_linear_cg(lambda,maxIteration,wInit,bInit, D,noneg,opt)
global X;
global Y;
global n;
d = size(X,2);

w = [wInit; bInit]; % The last component of w is b.
w(noneg) = max(w(noneg),0);
%out = ones(n,1); % Vector containing 1-Y.*(X*w)
out = D(1:n) - Y(1:n).*(X(1:n,:)*w(1:end-1)+w(end));

%go = [X(1:n,:)'*Y(1:n); sum(Y(1:n))];  % -gradient at w=0, need to be change for w!=0 initialization
[~, grad] = obj_fun_linear(w,lambda,out); go = -grad; % -gradient

s = go; % The first search direction is given by the gradient
for iter=1:maxIteration
    if iter > opt.cg_it * min(n,d)
        warning('PrimalSVM:MaxNumCG','Maximum number of CG iterations reached. Try larger lambda');
        break;
    end;

    % Do an exact line search
    [t,out,sv] = line_search_linear(w,s,out,lambda);
    w = w + t*s;
    w(noneg) = max(w(noneg),0);

    % Compute the new gradient
    [obj, gn, sv] = obj_fun_linear(w,lambda,out); gn=-gn;
    fprintf('Iter = %d, Obj = %f, Norm of grad = %.3f     \n',iter,obj,norm(gn));

    % Stop when the relative decrease in the objective function is small
    if t*s'*go < opt.prec*obj, break; end;

    % Flecher-Reeves update. Change 0 in 1 for Polack-Ribiere
    be = (gn'*gn - 0*gn'*go) / (go'*go);
    s = be*s+gn;
    go = gn;
end;

function [obj, grad, sv] = obj_fun_linear(w,lambda,out)
% Compute the objective function, its gradient and the set of support vectors
% Out is supposed to contain 1-Y.*(X*w)
global X;
global Y;
global n;
out = max(0,out);
wb0 = w; wb0(end) = 0;  % Do not penalize b <= Very important for object detection
obj = sum(out.^2)/2 + lambda*(wb0')*wb0/2; % L2 penalization of the errors
grad = lambda*wb0 - [((out.*Y(1:n))'*X(1:n,:))'; sum(out.*Y(1:n))]; % Gradient
sv = find(out>0);

function [t,out,sv] = line_search_linear(w,d,out,lambda)
% From the current solution w, do a line search in the direction d by
% 1D Newton minimization
global X;
global Y;
global n;
t = 0;
% Precompute some dots products
Xd = X(1:n,:)*d(1:end-1)+d(end);
wd = lambda * w(1:end-1)'*d(1:end-1);
dd = lambda * d(1:end-1)'*d(1:end-1);
while 1
    out2 = out - t*(Y(1:n).*Xd); % The new outputs after a step of length t
    sv = find(out2>0);
    g = wd + t*dd - (out2(sv).*Y(sv))'*Xd(sv); % The gradient (along the line)
    h = dd + Xd(sv)'*Xd(sv); % The second derivative (along the line)
    t = t - g/h; % Take the 1D Newton step. Note that if d was an exact Newton
    % direction, t is 1 after the first iteration.
    if g^2/h < 1e-10, break; end;
    %    fprintf('%f %f\n',t,g^2/h)
end;
out = out2;

function y = hess_vect_mult(w,sv,lambda)
% Compute the Hessian times a given vector x.
% hess = lambda*diag([ones(d-1,1); 0]) + (X(sv,:)'*X(sv,:));
global X;
global n;
y = lambda*w;
y(end) = 0;
z = (X(1:n,:)*w(1:end-1)+w(end));  % Computing X(sv,:)*x takes more time in Matlab :-(
zz = zeros(length(z),1);
zz(sv)=z(sv);
y = y + [(zz'*X(1:n,:))'; sum(zz)];


function mapping = calc_projection_from_library(endmembers, reduced_dim)
if 1 % Make the same size for all endmembers and the mean lie at the center
    num_spectra_per_endm = zeros(1,length(endmembers));
    for i = 1:length(endmembers)
        num_spectra_per_endm(i) = size(endmembers{i},1);
    end
    num_spectra = round(mean(num_spectra_per_endm));
    spectra_train = [];
    for i = 1:length(endmembers)
        inds = linspace(1, num_spectra_per_endm(i), num_spectra);
        inds = round(inds);
        spectra_train = cat(1, spectra_train, endmembers{i}(inds,:));
    end
else % The size difference is usually not big for spectra libraries.
    spectra_train = cell2mat(endmembers');
end
[~,mapping] = pca(spectra_train, reduced_dim);

end


function jac = p13_jac2 ( option, m, nvar, lambda, u )

%*****************************************************************************80
%
%% P13_JAC2 computes the jacobian by recasting it on a square grid.
%
%  Discussion:
%
%    Actually, to stave off insanity, we only "recast" the variables into
%    a 2D array that corresponds to the spatial ordering of the grid.
%    We leave the jacobian in its original arrangement, which assumes
%    a linear ordering of variables and equations, and we simply
%    compute the equation and variable indices of the jacobian when
%    we are ready to put entries into it.  This approach seems to produce
%    a smaller amount of cosmic grief than the alternatives.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 September 2008
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer OPTION, the option index.
%
%    Input, integer M, the number of grid points on a side of the square.
%
%    Input, integer NVAR, the number of variables.
%
%    Input, real LAMBDA, the value of the parameter.
%
%    Input, real U(M,M), the value of the grid function.
%
%    Output, real JAC(NVAR-1,NVAR), the jacobian matrix evaluated
%    at X.  The NVAR-th row is not set by this routine.
%
  jac(1:nvar-1,1:nvar) = 0.0;

  h = 1.0 / ( m + 1 );

  ieqn = 0;

  for i = 1 : m
    for j = 1 : m

      ieqn = ( j - 1 ) * m + i;

      uc = u(i + (j-1)*m);

      if ( i < m )
        un = u(i+1+(j-1)*m);
      else
        un = 0.0;
      end

      if ( 1 < i )
        us = u(i-1+(j-1)*m);
      else
        us = 0.0;
      end

      if ( j < m )
        ue = u(i+j*m);
      else
        ue = 0.0;
      end

      if ( 1 < j )
        uw = u(i+(j-2)*m);
      else
        uw = 0.0;
      end

      fc = p13_gx ( option, uc );
      fn = p13_gx ( option, un );
      fs = p13_gx ( option, us );
      fe = p13_gx ( option, ue );
      fw = p13_gx ( option, uw );

      del5f = fc + h * h * ( - 4.0 * fc + fn + fs + fe + fw ) / 12.0;

      fcp = p13_gp ( option, uc );
      fnp = p13_gp ( option, un );
      fsp = p13_gp ( option, us );
      fep = p13_gp ( option, ue );
      fwp = p13_gp ( option, uw );

      ivar = ( j - 1 ) * m + i;
      jac(ieqn,ivar) = - 20.0 / ( 6.0 * h * h ) ...
        + lambda * ( fcp - 4.0 * h * h * fcp / 12.0 );

      if ( i < m )
        ivar = ( j - 1 ) * m + i + 1;
        jac(ieqn,ivar) = 4.0 / ( 6.0 * h * h ) ...
          + lambda * h * h * fnp / 12.0;
      end

      if ( 1 < i )
        ivar = ( j - 1 ) * m + i - 1;
        jac(ieqn,ivar) = 4.0 / ( 6.0 * h * h ) ...
          + lambda * h * h * fsp / 12.0;
      end

      if ( j < m )
        ivar = j * m + i;
        jac(ieqn,ivar) = 4.0 / ( 6.0 * h * h ) ...
          + lambda * h * h * fep / 12.0;
      end

      if ( 1 < j )
        ivar = ( j - 2 ) * m + i;
        jac(ieqn,ivar) = 4.0 / ( 6.0 * h * h ) ...
          + lambda * h * h * fwp / 12.0;
      end

      if ( 1 < i & 1 < j )
        ivar = ( j - 2 ) * m + i - 1;
        jac(ieqn,ivar) = 1.0 / ( 6.0 * h * h );
      end

      if ( 1 < i & j < m )
        ivar = j * m + i - 1;
        jac(ieqn,ivar) = 1.0 / ( 6.0 * h * h );
      end

      if ( i < m & 1 < j )
        ivar = ( j - 2 ) * m + i + 1;
        jac(ieqn,ivar) = 1.0 / ( 6.0 * h * h );
      end

      if ( i < m & j < m )
        ivar = j * m + i + 1;
        jac(ieqn,ivar) = 1.0 / ( 6.0 * h * h );
      end

      ivar = nvar;
      jac(ieqn,nvar) = del5f;

    end
  end

  return
end


function zz=lpcfq2zz(f,q)
%LPCFQ2ZZ Convert frequencies and q factors to z-plane poles ZZ=(F,Q)
%all input values are in normalized Hz
% roots are at exp(2*pi*f*(-1/(2q) +- j)
% if f has more columns than q, remaining columns are real roots at -f

%      Copyright (C) Mike Brookes 1998
%      Version: $Id: lpcfq2zz.m,v 1.4 2007/05/04 07:01:38 dmb Exp $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   This program is free software; you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation; either version 2 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

[nf,pf]=size(f);
if nargin < 2
   pq=0;
else
   pq=size(q,2);
end;
zz=zeros(nf,pf+pq);
if pq
   ii=1:pq;
   zz(:,2*ii-1)=exp(pi*f(:,ii).*(2i-q.^(-1)));
   zz(:,2*ii)=conj(zz(:,2*ii-1));
end
if pf>pq
   ii=1+pq:pf;
   zz(:,ii+pq)= exp(-2*pi*f(:,ii));
end


clear all, close all, clc

s = tf('s');

G = (s+1)/(s-2);
Gtrue = (s+.9)/(s-1.9);

K = 1/G;

L = K*Gtrue;

margin(L)

CL = feedback(L,1);


% ========================================================================
% Classification
% USAGE: [prediction, accuracy, err] = classification(D, W, data, Hlabel,
%                                       sparsity)
% Inputs
%       D               -learned dictionary
%       W               -learned classifier parameters
%       data            -testing features
%       Hlabel          -labels matrix for testing feature
%       iterations      -iterations for KSVD
%       sparsity        -sparsity threshold
% outputs
%       prediction      -predicted labels for testing features
%       accuracy        -classification accuracy
%       err             -misclassfication information
%                       [errid featureid groundtruth-label predicted-label]
%
% Author: Zhuolin Jiang (zhuolin@umiacs.umd.edu)
% Date: 10-16-2011
% ========================================================================

function [prediction, accuracy, err] = classification(D, W, data, Hlabel, sparsity)

% sparse coding
G = D'*D;
% Gamma = omp(D'*data,G,sparsity);
Gamma = myOMP(data, D, sparsity);
% classify process
errnum = 0;
err = [];
prediction = [];
for featureid=1:size(data,2)
    spcode = Gamma(:,featureid);
    score_est =  W * spcode;
    score_gt = Hlabel(:,featureid);
    [maxv_est, maxind_est] = max(score_est);  % classifying
    [maxv_gt, maxind_gt] = max(score_gt);
    prediction = [prediction maxind_est];
    if(maxind_est~=maxind_gt)
        errnum = errnum + 1;
        err = [err;errnum featureid maxind_gt maxind_est];
    end
end
accuracy = (size(data,2)-errnum)/size(data,2);


function Ref = RefSelect(Population,k)
% Reference solutions selection by RSEA strategy

%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    k      = min(k,length(Population));
    PopObj = Population.objs;
	[FrontNO,MaxFNO] = NDSort(PopObj,k);
    Next = find(FrontNO<=MaxFNO);
    Pmin = min(PopObj,[],1) + 1e-6;
    Pmax = max(PopObj,[],1);
    if Pmax > Pmin
        PopObj = (PopObj-repmat(Pmin,size(PopObj,1),1))./repmat(Pmax-Pmin,size(PopObj,1),1);
    end

    %% Environmental selection
    Choose = LastSelection(PopObj(Next,:),ismember(Next,find(FrontNO<MaxFNO)),ceil(sqrt(k)),k);
    Ref    = Population(Next(Choose));
end

function Choose = LastSelection(PopObj,Choose,div,k)
% Select part of the solutions based on the radar grid

    %% Identify the extreme solutions
	[~,Extreme] = min(sqrt(sum(PopObj.^2,2)).*sqrt(1-(1-pdist2(PopObj,ones(1,size(PopObj,2)),'cosine')).^2),[],1); %Calculate the extreme points based on PBI
    Choose      = Choose | ismember(1:size(PopObj,1),Extreme);

    %% Calculate the convergence of each solution
	Con = sum(PopObj.^1,2).^1;
    Con = Con./max(Con);

    %% Calculate the radar grid of each solution
    [Site,RLoc] = RadarGrid(PopObj,div);
    RDis        = pdist2(RLoc,RLoc);
    RDis(logical(eye(length(RDis)))) = inf;
    CrowdG      = zeros(1,max(Site));
    temp        = tabulate(Site(Choose));
    CrowdG(temp(:,1)) = temp(:,2);

    %% Select k solutions
    while sum(Choose) < k
        % Delete outline solutions
        remainS  = find(~Choose);
        remainG  = unique(Site(remainS));
        bestG    = CrowdG(remainG) == min(CrowdG(remainG));
        current  = remainS(ismember(Site(remainS),remainG(bestG)));
        fitness  = 0.1.*size(PopObj,2).*Con(current) - min(RDis(current,Choose),[],2); % - 0.1.* min(Dis(current,Choose),[],2);
        [~,best] = min(fitness);
        Choose(current(best))       = true;
        CrowdG(Site(current(best))) = CrowdG(Site(current(best))) + 1;
    end
end

function [Site,RLoc] = RadarGrid(P,div)

	[N,M] = size(P);

    %% Calculate the radar coordinate of each solution
    theta     = 0 : 2*pi/M : 2*pi/M*(M-1);
    RLoc(:,1) = sum(P.*repmat(cos(theta),N,1),2)./sum(P,2);
    RLoc(:,2) = sum(P.*repmat(sin(theta),N,1),2)./sum(P,2);
    RLoc      = (RLoc+1)/2;
    YL        = min(RLoc,[],1);                             % Lower bounary of the transferred points
    YU        = max(RLoc,[],1);                             % Upper bounary of the transferred points
    NRLoc     = (RLoc-repmat(YL,N,1))./repmat(YU-YL,N,1);	% Normalized points

    %% Identify the index of grid of each solution
    GLoc            = floor(NRLoc.*div);
    GLoc(GLoc>=div) = div - 1;
    UniqueGLoc      = sortrows(unique(GLoc,'rows'));
    [~,Site]        = ismember(GLoc,UniqueGLoc,'rows');
end


%Kalman Demo 2 (Mauna Loa Periodic CO2 Readings)
%
%  In this demo we apply the state space inference methods to the
%  well-known time series data consisting of atmospheric CO2
%  concentration readings in parts per million (ppm) by volume from
%  air samples collected at the Mauna Loa observatory, Hawaii (see
%  [1] for details and further references).
%
%  The benefit from the state space formulation is that the
%  computational complexity is linear with respect to the number
%  of data points. Due to efficient matrix solvers in Matlab
%  (favoring the traditional GP solution over sequential looping),
%  the advantages in speed start to show in datasets with thousands
%  of data points.
%
%  The take-home message from this demo is that the GP can be
%  set up exactly as any other GP regression model in GPstuff,
%  and then solved by Kalman filtering methods by specifying the
%  'type' in the GP structure to be 'KALMAN'.
%
%  The methods in this demo are based on the paper:
%
%  [1] Arno Solin and Simo Sarkka (2014). Explicit link between periodic
%      covariance functions and state space models. Accepted for
%      publication in Proceedings of the Seventeenth International
%      Conference on Artifcial Intelligence and Statistics (AISTATS 2014).
%
% Copyright (c) 2014 Arno Solin and Jukka Koskenranta

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

%% Load the data

  % Load the data
  S = which('demo_kalman2.m');
  L = strrep(S,'demo_kalman2.m','demodata/maunaloa_data.txt');

  % Set training data (x = time in years, y = CO2 observations)
  data = load(L);
  y = data(:, 2:13)';  y=y(:);
  x = (data(1,1):1/12:data(end,1)+11/12)';

  % Remove data points with missing information
  x = x(y>0); y = y(y>0);

  % Make data zero mean
  ymean = mean(y); y = y-ymean;

  % Show original data
  figure(1); clf
    plot(x,y+ymean,'-k')
    xlabel('Time (year)');
    ylabel('CO_2 concentration (PPM)')
    title('Kalman Demo 2  (Mauna Loa Periodic CO2 Readings) - Data')

%% Set up GP model

  % Noise variance prior
  ps2 = prior_logunif();

  % The likelihood model
  lik = lik_gaussian('sigma2', 1, 'sigma2_prior', ps2);

  % Covariance function hyperparameter priors
  pl = prior_logunif();
  pm = prior_logunif();

  % A squared exponential covariance function
  % to deal with the smooth long term effects
  gpcf1 = gpcf_sexp('lengthScale', 100, 'magnSigma2', 5000, ...
                    'lengthScale_prior',pl,'magnSigma2_prior',pl);

  % A quasi-periodic covariance function deals with peridic
  % variation in the data. The quasi-periodic covariance function
  % is a product of a periodic covariance function and a squared
  % exponential.
  gpcf2 = gpcf_periodic('magnSigma2',1,'lengthScale',1,'period',1, ...
                        'decay',1,'lengthScale_sexp',100, ...
                        'lengthScale_prior',pl,'magnSigma2_prior',pl, ...
                        'lengthScale_sexp_prior',pl);

  % A Matern52 covariance function deals with short term
  % non-periodic effects that remain otherwise unexplained
  gpcf3 = gpcf_matern52('lengthScale', 10, 'magnSigma2', 10, ...
                        'lengthScale_prior',pl,'magnSigma2_prior',pl);

  % Finally create the GP structure
  gp = gp_set('lik', lik, 'cf', {gpcf1,gpcf2,gpcf3});

  % Set type to KALMAN
  gp = gp_set(gp,'type','KALMAN');

%% Optimize hyperparameters and predict

  % Optimization parameters
  opt=optimset('TolFun',1e-4,'TolX',1e-4,'Display','iter');

  % Find hyperparameters by optimization (BFGS)
  gp=gp_optim(gp,x,y,'opt',opt,'optimf',@fminlbfgs);

  % Set the test points
  xt = (x(end):1/12:x(end)+10)';

  % Predict values
  [Eft,Varft] =  gp_pred(gp, x, y,'xt',xt);

  % Also predict the latent components separately
  [Eft1, Varft1] = gp_pred(gp, x, y, x, 'predcf', [1 3]);
  [Eft2, Varft2] = gp_pred(gp, x, y, x, 'predcf', [2]);

%% Visualize results

  % Plot
  figure(2); clf; hold on

    % Plot the 95% confidence interval of the predictions
    p=patch([xt; flipud(xt)], ...
       [ymean + Eft + 1.96*sqrt(Varft); ...
        flipud(ymean + Eft - 1.96*sqrt(Varft))],[0.9,0.9,0.9]);
    set(p,'EdgeColor','none')

    % Plot observations
    plot(x,ymean+y,'.k','MarkerSize',5)

    % Labels and legends
    title('Kalman Demo 2 (Mauna Loa Periodic CO2 Readings)')
    xlabel('Time (years)');
    ylabel('CO_2 concentration (PPM)')
    legend('95% confidence region', ...
           'Monthly average measurements',...
           'Location', 'NorthWest');

    % Axis options
    box on; axis tight; set(gca,'Layer','top')

%% Show components separately

  figure(3); clf;
  subplot(211)

    plot(x,Eft1,'-k')

    title('Long-term trend and short-scale variation')
    xlabel('Time (years)');
    ylabel('Effect on CO_2 concentration (PPM)')

    axis tight

  subplot(212)

    plot(x,Eft2,'-k')

    title('Quasi-periodic effect')
    xlabel('Time (years)');
    ylabel('Effect on CO_2 concentration (PPM)')

    axis tight


% ######################################################################################################################################################################################
% We are here talking about spatio-temporal detections, i.e. a set of ground-truth bounding boxes that
%  I will denote by g_t, with t between t_g^b and t_g^e (beginning and end time of the ground-truth)
% versus a detection which is also a set of bounding boxes, denoted by d_t, with t between t_d^e et t_d^e.
%
% a) temporal iou =  T_i / T_u
%  this is the intersection over union between the timing of the the tubes,
% ie mathematically T_i / T_u with
% the intersection T_i = max(0,   max(t_g^b,t_d^b)-min(t_d^e,t_g^e) )
% and the union T_u = min(t_g^b,t_d^b)-max(t_d^e,t_g^e)
%
% b) for each t between max(tgb,tdb)-min(tde,tge), we compute the IoU between g_t and d_t, and average them
%
% Multiplying (a) and (b) is the same as computed the average of the spatial iou over all frames in T_u of the two tubes, with a spatial iou of 0 for frames where only one box exists.
% c) as this is standard in detection problem, if there are multiple detections for the same groundtruth detection, the first one is counted as positive and the other ones as negatives
% ######################################################################################################################################################################################
%{
gt_fnr = 1xn doube
gt_bb = nx4 doubld - [x y w h]
dt_fnr = 1xm double
dt_bb = mx4 double - [x y w h]
%}
% -------------------------------------------------------------------------
function st_iou = compute_spatio_temporal_iou(gt_fnr, gt_bb, dt_fnr, dt_bb)
% -------------------------------------------------------------------------

% time gt begin

tgb = gt_fnr(1);
% time gt end
tge = gt_fnr(end);
%time dt begin
tdb = dt_fnr(1);
tde = dt_fnr(end);
% temporal intersection
T_i = double(max(0, min(tge,tde)-max(tgb,tdb)));

if T_i>0
    T_i = T_i +1;
    % temporal union
    T_u = double(max(tge,tde) - min(tgb,tdb)+1);
    %temporal IoU
    T_iou = T_i/T_u;
    % intersect frame numbers
    int_fnr = max(tgb,tdb):min(tge,tde);

    % find the ind of the intersected frames in the detected frames
    [~,int_find_dt] = ismember(int_fnr, dt_fnr);
    [~,int_find_gt] = ismember(int_fnr, gt_fnr);

    assert(length(int_find_dt)==length(int_find_gt));

    iou = zeros(length(int_find_dt),1);
    for i=1:length(int_find_dt)
        if int_find_gt(i)<1
%             fprintf('error ')
            pf = pf;
        else
            pf = i;
        end

        gt_bound = gt_bb(int_find_gt(pf),:);
        dt_bound = dt_bb(int_find_dt(pf),:)+1;

        % gt_bound = [gt_bound(:,1:2) gt_bound(:,3:4)-gt_bound(:,1:2)];
        % dt_bound = [dt_bound(:,1:2) dt_bound(:,3:4)-dt_bound(:,1:2)];
        iou(i) = inters_union(double(gt_bound),double(dt_bound));
    end
    % finalspatio-temporal IoU threshold
    st_iou = T_iou*mean(iou);
else
    st_iou =0;
end
% % iou_thresh = 0.2,...,0.6 % 'Learing to track paper' takes 0.2 for UCF101 and 0.5 for JHMDB
% if delta >= iou_thresh
%     % consider this tube as valid detection
% end

end

% -------------------------------------------------------------------------
function iou = inters_union(bounds1,bounds2)
% -------------------------------------------------------------------------

inters = rectint(bounds1,bounds2);
ar1 = bounds1(:,3).*bounds1(:,4);
ar2 = bounds2(:,3).*bounds2(:,4);
union = bsxfun(@plus,ar1,ar2')-inters;

iou = inters./(union+eps);

end


% ######################################################################################################################################################################################
% We are here talking about spatio-temporal detections, i.e. a set of ground-truth bounding boxes that
%  I will denote by g_t, with t between t_g^b and t_g^e (beginning and end time of the ground-truth)
% versus a detection which is also a set of bounding boxes, denoted by d_t, with t between t_d^e et t_d^e.
%
% a) temporal iou =  T_i / T_u
%  this is the intersection over union between the timing of the the tubes,
% ie mathematically T_i / T_u with
% the intersection T_i = max(0,   max(t_g^b,t_d^b)-min(t_d^e,t_g^e) )
% and the union T_u = min(t_g^b,t_d^b)-max(t_d^e,t_g^e)
%
% b) for each t between max(tgb,tdb)-min(tde,tge), we compute the IoU between g_t and d_t, and average them
%
% Multiplying (a) and (b) is the same as computed the average of the spatial iou over all frames in T_u of the two tubes, with a spatial iou of 0 for frames where only one box exists.
% c) as this is standard in detection problem, if there are multiple detections for the same groundtruth detection, the first one is counted as positive and the other ones as negatives
% ######################################################################################################################################################################################
%{
gt_fnr = 1xn doube
gt_bb = nx4 doubld - [x y w h]
dt_fnr = 1xm double
dt_bb = mx4 double - [x y w h]
%}
% -------------------------------------------------------------------------
function st_iou = compute_spatio_temporal_iou(gt_fnr, gt_bb, dt_fnr, dt_bb)
% -------------------------------------------------------------------------

% time gt begin
tgb = gt_fnr(1);
% time gt end
tge = gt_fnr(end);
%time dt begin
tdb = dt_fnr(1);
tde = dt_fnr(end);
% temporal intersection
T_i = double(max(0, min(tge,tde)-max(tgb,tdb)));

if T_i>0
    T_i = T_i +1;
    % temporal union
    T_u = double(max(tge,tde) - min(tgb,tdb)+1);
    %temporal IoU
    T_iou = T_i/T_u;
    % intersect frame numbers
    int_fnr = max(tgb,tdb):min(tge,tde);

    % find the ind of the intersected frames in the detected frames
    [~,int_find_dt] = ismember(int_fnr, dt_fnr);
    [~,int_find_gt] = ismember(int_fnr, gt_fnr);

    assert(length(int_find_dt)==length(int_find_gt));

    iou = zeros(length(int_find_dt),1);
    for i=1:length(int_find_dt)
        if int_find_gt(i)<1
%             fprintf('error ')
            pf = pf;
        else
            pf = i;
        end

        gt_bound = gt_bb(int_find_gt(pf),:);
        dt_bound = dt_bb(int_find_dt(pf),:)+1;

        % gt_bound = [gt_bound(:,1:2) gt_bound(:,3:4)-gt_bound(:,1:2)];
        % dt_bound = [dt_bound(:,1:2) dt_bound(:,3:4)-dt_bound(:,1:2)];
        iou(i) = inters_union(double(gt_bound),double(dt_bound));
    end
    % finalspatio-temporal IoU threshold
    st_iou = T_iou*mean(iou);
else
    st_iou =0;
end
% % iou_thresh = 0.2,...,0.6 % 'Learing to track paper' takes 0.2 for UCF101 and 0.5 for JHMDB
% if delta >= iou_thresh
%     % consider this tube as valid detection
% end

end

% -------------------------------------------------------------------------
function iou = inters_union(bounds1,bounds2)
% -------------------------------------------------------------------------

inters = rectint(bounds1,bounds2);
ar1 = bounds1(:,3).*bounds1(:,4);
ar2 = bounds2(:,3).*bounds2(:,4);
union = bsxfun(@plus,ar1,ar2')-inters;

iou = inters./(union+eps);

end


function net = knn(nin, nout, k, tr_in, tr_targets)
%KNN	Creates a K-nearest-neighbour classifier.
%
%	Description
%	NET = KNN(NIN, NOUT, K, TR_IN, TR_TARGETS) creates a KNN model NET
%	with input dimension NIN, output dimension NOUT and K neighbours.
%	The training data is also stored in the data structure and the
%	targets are assumed to be using a 1-of-N coding.
%
%	The fields in NET are
%	  type = 'knn'
%	  nin = number of inputs
%	  nout = number of outputs
%	  tr_in = training input data
%	  tr_targets = training target data
%
%	See also
%	KMEANS, KNNFWD
%

%	Copyright (c) Ian T Nabney (1996-2001)

net.type = 'knn';
net.nin = nin;
net.nout = nout;
net.k = k;
errstring = consist(net, 'knn', tr_in, tr_targets);
if ~isempty(errstring)
  error(errstring);
end
net.tr_in = tr_in;
net.tr_targets = tr_targets;


% A LP example which shows all the potentials of GLPKMEX
clear;

disp('LP problem');
s=-1;
c=[10,6,4]';
a=[1,1,1;...
   10,4,5;...
   2,2,6];
b=[100,600,300]';
ctype=['U','U','U']';
lb=[0,0,0]';
ub=[]';
vartype=['C','C','C']';
% Output all GLPK messages on workspace
param.msglev=3;
% Set save options
param.save=1;
param.savefilename='SimpleLP';
param.savefiletype='fixedmps';
[xmin,fmin,status,extra]=glpk(c,a,b,lb,ub,ctype,vartype,s,param)

% OBSOLETE SYNTAX
%lpsolver = param.lpsolver;
%save_pb = param.save;
%[xmin,fmin,status,extra]=glpkmex(s,c,a,b,ctype,lb,ub,vartype,param,lpsolver,save_pb)


function uI = faceinterpolate(u,node,elem,elemType)
%% FACEINTERPOLATE interpolate to face elements (RT0 or BDM1).
%
% uI = faceinterpolate(u,node,elem,elemType) interpolates a given function u
% into the lowesr order RT0 or BDM1 finite element spaces. The coefficient
% is given by the line integral int_e u*n ds. The input elemType can be 'RT0'
% or 'BDM1'.
%
% Example: RT0
%
%      maxIt = 5;
%      node = [1,0; 1,1; 0,1; -1,1; -1,0; -1,-1; 0,-1; 0,0]; % nodes
%      elem = [1,2,8; 3,8,2; 8,3,5; 4,5,3; 7,8,6; 5,6,8];    % elements
%      bdFlag = setboundary(node,elem,'Dirichlet');
%      pde = mixBCdata;
%      err = zeros(maxIt,2);
%      h = zeros(maxIt,1);
%      for k =1:maxIt
%        [node,elem,bdFlag] = uniformrefine(node,elem,bdFlag);
%        [u,sigma,eqn] = PoissonRT0(node,elem,bdFlag,pde);
%        sigmaI = faceinterpolate(pde.Du,node,elem,'RT0');
%        err(k,1) = getL2errorRT0(node,elem,pde.Du,sigmaI);
%        err(k,2) = sqrt((sigma-sigmaI)'*eqn.M*(sigma-sigmaI));
%        h(k) = 1./(sqrt(size(node,1))-1);
%      end
%      figure;
%      showrateh2(h,err(:,1),2,'r-+','|| \sigma - \sigma_I ||',...
%                 h,err(:,2),2,'b-+','|| \sigma_h - \sigma_I ||');
%
% Example: BDM1
%
%      maxIt = 5;
%      node = [1,0; 1,1; 0,1; -1,1; -1,0; -1,-1; 0,-1; 0,0]; % nodes
%      elem = [1,2,8; 3,8,2; 8,3,5; 4,5,3; 7,8,6; 5,6,8];    % elements
%      bdFlag = setboundary(node,elem,'Dirichlet');
%      pde = mixBCdata;
%      err = zeros(maxIt,2);
%      h = zeros(maxIt,1);
%      for k =1:maxIt
%        [node,elem,bdFlag] = uniformrefine(node,elem,bdFlag);
%        [u,sigma,eqn] = PoissonBDM1(node,elem,bdFlag,pde);
%        sigmaI = faceinterpolate(pde.Du,node,elem,'BDM1');
%        err(k,1) = getL2errorBDM1(node,elem,pde.Du,sigmaI);
%        err(k,2) = sqrt((sigma-sigmaI)'*eqn.M*(sigma-sigmaI));
%        h(k) = 1./(sqrt(size(node,1))-1);
%      end
%      figure;
%      showrateh2(h,err(:,1),2,'r-+','|| \sigma - \sigma_I ||',...
%                 h,err(:,2),2,'b-+','|| \sigma_h - \sigma_I ||');
%
%
% Example: RT1
%
%      maxIt = 5;
%      node = [1,0; 1,1; 0,1; -1,1; -1,0; -1,-1; 0,-1; 0,0]; % nodes
%      elem = [1,2,8; 3,8,2; 8,3,5; 4,5,3; 7,8,6; 5,6,8];    % elements
%      bdFlag = setboundary(node,elem,'Dirichlet');
%      pde = mixBCdata;
%      err = zeros(maxIt,2);
%      h = zeros(maxIt,1);
%      for k = 1:maxIt
%        [node,elem,bdFlag] = uniformrefine(node,elem,bdFlag);
%        sigmaI = faceinterpolate(pde.Du,node,elem,'RT1');
%        err(k,1) = getL2errorRT1(node,elem,pde.Du,sigmaI);
%        h(k) = 1./(sqrt(size(node,1))-1);
%      end
%      figure;
%      showrateh2(h,err(:,1),2,'r-+','|| \sigma - \sigma_I ||',...
%                 h,err(:,2),2,'b-+','|| \sigma_h - \sigma_I ||');
%
%
% See also edgeinterpolate, edgeinterpolate1, edgeinterpolate2
%
% Created by Ming Wang at Mar 29, 2011, M-lint modified at May 14, 2011.
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

if ~exist('elemType','var'), elemType = 'RT0'; end
%% edge
if size(elem,2) == 2 % the input elem is edge
    edge = elem;
else
    [elem2edge,edge] = dofedge(elem);
end
NE = size(edge,1);
NT = size(elem,1);
edgeVec = node(edge(:,2),:) - node(edge(:,1),:);
nVec = zeros(NE,2);
nVec(:,1) = edgeVec(:,2);
nVec(:,2) = -edgeVec(:,1);
clear edgeVec

%% dof for RT0
[lambda,weight] = quadpts1(4);
nQuad = size(lambda,1);
uI = zeros(NE,1);
for i = 1:nQuad
    pxy = lambda(i,1)*node(edge(:,1),:)+lambda(i,2)*node(edge(:,2),:);
    flux = u(pxy);
    uI = uI + weight(i)*dot(flux,nVec,2);
end

%% dof for BDM1
if strcmp(elemType,'BDM1') || strcmp(elemType,'RT1')
   uI(NE+1:2*NE) = zeros(NE,1);
   for i = 1:nQuad
        pxy = lambda(i,1)*node(edge(:,1),:)+lambda(i,2)*node(edge(:,2),:);
        uI(NE+1:2*NE) = uI(NE+1:2*NE)+ ...
                     weight(i)*3*(lambda(i,1)-lambda(i,2))*dot(u(pxy),nVec,2);
   end
end

%% dof for RT1
if strcmp(elemType,'RT1')
    mid = (node(edge(:,1),:) + node(edge(:,2),:))/2;
    umid = u(mid);
    uI(2*NE+2*NT) = 0;
    % Face dof coefficients
    uquadpts = umid(elem2edge(:,1),:)+umid(elem2edge(:,2),:)+umid(elem2edge(:,3),:);
    lf = [4*dot(nVec(elem2edge(:,2),:),uquadpts,2) ...
          4*dot(nVec(elem2edge(:,3),:),uquadpts,2)];
    elem2edgeDofValue = [uI(elem2edge(:,1:3)) uI(elem2edge(:,1:3)+NE)];
    localMatrix = [4 8 4 -4  0 4; ...
                   8 4 -4 0 -4 4]';
    lf = lf - elem2edgeDofValue*localMatrix;
    uI((2*NE+1):end) = [2*lf(:,1) - lf(:,2); ...
                        2*lf(:,2) - lf(:,1)]/3;
end


function varargout = rotation3dToEulerAngles(mat, varargin)
%ROTATION3DTOEULERANGLES Extract Euler angles from a rotation matrix.
%
%   [PHI, THETA, PSI] = rotation3dToEulerAngles(MAT)
%   Computes Euler angles PHI, THETA and PSI (in degrees) from a 3D 4-by-4
%   or 3-by-3 rotation matrix.
%
%   ANGLES = rotation3dToEulerAngles(MAT)
%   Concatenates results in a single 1-by-3 row vector. This format is used
%   for representing some 3D shapes like ellipsoids.
%
%   ... = rotation3dToEulerAngles(MAT, CONVENTION)
%   CONVENTION specifies the axis rotation sequence. Default is 'ZYX'.
%   Supported conventions are:
%       'ZYX','ZXY','YXZ','YZX','XYZ','XZY'
%       'ZYZ','ZXZ','YZY','YXY','XZX','XYX'
%
%   Example
%   rotation3dToEulerAngles
%
%   References
%   Code from '1994 - Shoemake - Graphics Gems IV: Euler Angle Conversion:
%   http://webdocs.cs.ualberta.ca/~graphics/books/GraphicsGems/gemsiv/euler_angle/EulerAngles.c
%   (see rotm2eul, that is part of MATLAB's Robotics System Toolbox)
%   Modified using explanations in:
%   http://www.gregslabaugh.net/publications/euler.pdf
%   https://www.geometrictools.com/Documentation/EulerAngles.pdf
%
%   See also
%   transforms3d, rotation3dAxisAndAngle, createRotation3dLineAngle,
%   eulerAnglesToRotation3d
%

% ------
% Authors: David Legland, oqilipo
% E-mail: david.legland@grignon.inra.fr
% Created: 2010-08-11, using Matlab 7.9.0.529 (R2009b)
% Copyright 2010-2022 INRA - Cepia Software Platform

p = inputParser;
validStrings = {...
    'ZYX','ZXY','YXZ','YZX','XYZ','XZY',...
    'ZYZ','ZXZ','YZY','YXY','XZX','XYX'};
addOptional(p,'convention','ZYX',@(x) any(validatestring(x,validStrings)));
logParValidFunc = @(x) (islogical(x) || isequal(x,1) || isequal(x,0));
addParameter(p,'IsRotation', 1, logParValidFunc);
valTol = @(x) validateattributes(x,{'numeric'},{'scalar', '>=',eps(class(mat)), '<=',1});
addParameter(p,'tolerance', 1e-8, valTol);
parse(p,varargin{:});
convention=p.Results.convention;
isRotation = p.Results.IsRotation;
tolerance = p.Results.tolerance;

if isRotation
    if ~isTransform3d(mat(1:3,1:3), 'rotation', 1, 'tolerance', tolerance)
        warning(['Rotation matrix contains reflection or scaling ' ...
            'tested with a tolerance of ' num2str(tolerance) '.' newline ...
            'Calculation of euler angles might be incorrect.'])
    end
end

switch convention
    case 'ZYX'
        % extract |cos(theta)|
        cy = hypot(mat(1,1), mat(2,1));
        % avoid dividing by 0
        if cy > 16*eps
            % normal case: theta <> 0
            phi   = atan2( mat(2,1), mat(1,1));
            theta = atan2(-mat(3,1), cy);
            psi   = atan2( mat(3,2), mat(3,3));
        else
            phi   = 0;
            theta = atan2(-mat(3,1), cy);
            psi   = atan2(-mat(2,3), mat(2,2));
        end
    case 'ZXY'
        cy = hypot(mat(2,2), mat(1,2));
        if cy > 16*eps
            phi   = -atan2( mat(1,2), mat(2,2));
            theta = -atan2(-mat(3,2), cy);
            psi   = -atan2( mat(3,1), mat(3,3));
        else
            phi   = 0;
            theta = -atan2(-mat(3,2), cy);
            psi   = -atan2(-mat(1,3), mat(1,1));
        end
    case 'YXZ'
        cy = hypot(mat(3,3), mat(1,3));
        if cy > 16*eps
            phi   = atan2( mat(1,3), mat(3,3));
            theta = atan2(-mat(2,3), cy);
            psi   = atan2( mat(2,1), mat(2,2));
        else
            phi   = 0;
            theta = atan2(-mat(2,3), cy);
            psi   = atan2(-mat(1,2), mat(1,1));
        end
    case 'YZX'
        cy = hypot(mat(1,1), mat(3,1));
        if cy > 16*eps
            phi   = -atan2( mat(3,1), mat(1,1));
            theta = -atan2(-mat(2,1), cy);
            psi   = -atan2( mat(2,3), mat(2,2));
        else
            phi   = 0;
            theta = -atan2(-mat(2,1), cy);
            psi   = -atan2(-mat(3,2), mat(3,3));
        end
    case 'XYZ'
        cy = hypot(mat(3,3), mat(2,3));
        if cy > 16*eps
            phi   = -atan2( mat(2,3), mat(3,3));
            theta = -atan2(-mat(1,3), cy);
            psi   = -atan2( mat(1,2), mat(1,1));
        else
            phi   = 0;
            theta = -atan2(-mat(1,3), cy);
            psi   = -atan2(-mat(2,1), mat(2,2));
        end
    case 'XZY'
        cy = hypot(mat(2,2), mat(3,2));
        if cy > 16*eps
            phi   = atan2( mat(3,2), mat(2,2));
            theta = atan2(-mat(1,2), cy);
            psi   = atan2( mat(1,3), mat(1,1));
        else
            phi   = 0;
            theta = atan2(-mat(1,2), cy);
            psi   = atan2(-mat(3,1), mat(3,3));
        end

    case 'ZYZ'
        cy = hypot(mat(3,2), mat(3,1));
        if cy > 16*eps
            phi   = -atan2(mat(2,3), -mat(1,3));
            theta = -atan2(cy, mat(3,3));
            psi   = -atan2(mat(3,2), mat(3,1));
        else
            phi   = 0;
            theta = -atan2(cy, mat(3,3));
            psi   = -atan2(-mat(2,1), mat(2,2));
        end
    case 'ZXZ'
        cy = hypot(mat(3,2), mat(3,1));
        if cy > 16*eps
            phi   = atan2(mat(1,3), -mat(2,3));
            theta = atan2(cy, mat(3,3));
            psi   = atan2(mat(3,1), mat(3,2));
        else
            phi   = 0;
            theta = atan2(cy, mat(3,3));
            psi   = atan2(-mat(1,2), mat(1,1));
        end
    case 'YZY'
        cy = hypot(mat(2,3), mat(2,1));
        if cy > 16*eps
            phi   = atan2(mat(3,2), -mat(1,2));
            theta = atan2(cy, mat(2,2));
            psi   = atan2(mat(2,3), mat(2,1));
        else
            phi   = 0;
            theta = atan2(cy, mat(2,2));
            psi   = atan2(-mat(3,1), mat(3,3));
        end
    case 'YXY'
        cy = hypot(mat(2,3), mat(2,1));
        if cy > 16*eps
            phi   = -atan2(mat(1,2), -mat(3,2));
            theta = -atan2(cy, mat(2,2));
            psi   = -atan2(mat(2,1), mat(2,3));
        else
            phi   = 0;
            theta = -atan2(cy, mat(2,2));
            psi   = -atan2(-mat(1,3), mat(1,1));
        end
    case 'XZX'
        cy = hypot(mat(1,3), mat(1,2));
        if cy > 16*eps
            phi   = -atan2(mat(3,1), -mat(2,1));
            theta = -atan2(cy, mat(1,1));
            psi   = -atan2(mat(1,3), mat(1,2));
        else
            phi   = 0;
            theta = -atan2(cy, mat(1,1));
            psi   = -atan2(-mat(3,2), mat(3,3));
        end
    case 'XYX'
        cy = hypot(mat(1,2), mat(1,3));
        if cy > 16*eps
            phi   = atan2(mat(2,1), -mat(3,1));
            theta = atan2(cy, mat(1,1));
            psi   = atan2(mat(1,2), mat(1,3));
        else
            phi   = 0;
            theta = atan2(cy, mat(1,1));
            psi   = atan2(-mat(2,3), mat(2,2));
        end
end

% format output arguments
if nargout <= 1
    % one array
    varargout{1} = rad2deg([phi theta psi]);
else
    % three separate arrays
    varargout = cellfun(@rad2deg, {phi theta psi},'uni',0);
end


classdef SPX_ConjugateDescent < handle
% Conjugate gradient algorithm implementation

    properties
        % Settable and gettable properties

        % Maximum number of iterations for which the algorithm can run
        MaxIterations
        % Threshold of norm (in terms of percentage)
        NormThreshold
    end

    properties(SetAccess=private)
        % Gettable properties

        % The sparse symmetric positive definite matrix operator
        A
        % The vectors to be solved
        B
        % Problem dimension
        N
        % Number of equations to be solved
        S
        % Number of iterations taken for solving the problem
        Iterations
        % Residual norms at the end
        ResidualNorms
        % The solution vectors
        X
        % Residual vectors
        Residuals
        % Indicates if the problems converged
        % This would be false only if the algorithm crossed max iterations
        Converged
    end

    methods
        % Public methods
        function self = SPX_ConjugateDescent(A, B)
            % Constructor
            if isa(A, 'spx.dict.Operator')
                self.A = A;
            elseif ismatrix(A)
                self.A = spx.dict.MatrixOperator(A);
            else
                error('Unsupported operator.');
            end
            self.B = B;
            [self.N, self.S] = size(B);
            self.MaxIterations = self.N ;
            self.NormThreshold = 1e-6;
        end

        function result = solve(self)
            aa = self.A;
            bb = self.B;
            % Initial estimate vectors are all zeros
            xx = zeros(self.N, self.S);
            result = xx;
            self.Iterations = zeros(self.S, 1);
            self.ResidualNorms = zeros(self.S, 1);
            self.Converged = false(self.S, 1);
            % Initial residual vectors
            rr = bb  - aa * xx;
            self.Residuals  = rr;
            % Norm squared of initial residual vectors
            deltas = spx.norm.inner_product_cw(rr, rr);
            % The factor with which the norm needs to be reduced
            epsilon = self.NormThreshold;
            % Target limits on norm squared of residuals
            limits = epsilon^2 * deltas;
            % Maximum number of iterations for which the algorithm
            % is allowed to run
            imax = self.MaxIterations;
            % Number of problems being solved
            ns = self.S;
            for s=1:ns
                % Initialize iteration counter
                i = 0;
                % The quantities for this problem
                limit = limits(s);
                delta = deltas(s);
                % First residual
                r = rr(:, s);
                % First estimate
                x = xx(:, s);
                % Target b
                b = bb(:, s);
                % First direction
                d = r;
                while i < imax && delta > limit
                    % Compute the intermediate variable
                    q = aa * d;
                    % the line search scale factor in current direction
                    alpha = delta / (d' * q);
                    % Update estimate in current direction
                    x = x + alpha * d;
                    if mod(i , 50) == 0
                        % In order to avoid propagation of floating point
                        % errors, we will recompute the value of residual
                        r = b - aa * x;
                    else
                        % Otherwise we use a shortcut
                        r = r  - alpha * q;
                    end
                    % hold the current residual norm squared
                    delta_old = delta;
                    % Update residual norm squared
                    delta = r' * r;
                    % Compute the ratio
                    beta = delta / delta_old;
                    % choose the new direction
                    d = r + beta * d;
                    % Increase iteration counter
                    i = i + 1;
                end
                % The problem has been solved
                result(:, s) = x;
                % Number of iterations taken to solve this problem
                self.Iterations(s) = i;
                self.ResidualNorms(s) = sqrt(delta);
                self.Residuals(:, s) = r;
                self.Converged(s) = delta <= limit;
            end
            % Maintain the result for reference
            self.X = result;
        end

        function result = hasConverged(self)
            % Returns if all the solutions have converged
            result = all(self.Converged);
        end

        function printResults(self)
            ns = self.S;
            nn = self.N;
            for s = 1:ns
                fprintf('Problem: %d\n', s);
                fprintf('Iterations: %d\n', self.Iterations(s));
                fprintf('Residual norm: %.2f, Converged: %d\n', ...
                    self.ResidualNorms(s), self.Converged(s));
                if nn < 10
                    % We will print the solutions too
                    fprintf('Solution vector: ');
                    fprintf('%.4f ', self.X(:, s));
                    fprintf('\n');
                    fprintf('Residual vector: ');
                    fprintf('%.4f ', self.Residuals(:, s));
                    fprintf('\n');
                end
            end
        end
    end

    methods(Access=private)
        % Private methods

    end

    methods(Static)
        % Public static methods

    end

end


function [trh, trv, tE] = trfd(h, v)
% Compute "time"-reversed horizontal and vertical finite differences on the
% current estimate x
%
% Use periodic boundaries

%% Application of "time"-reversed horizontal finite difference
tS = tic;

trh = [h(:,end) h];
trh = imfilter(trh, [1 -1], 'circular');
trh = trh(:, 1:end-1);

%% Application of "time"-reversed vertical finite difference
trv = [v(end,:); v];
trv = imfilter(trv, [1 -1]', 'circular');
trv = trv(1:end-1, :);

tE = toc(tS);


function [hpb] = W2hpb(W)
% Convert power from watts to boiler horsepower.
% Chad A. Greene 2012
hpb = W*0.000101942;


function [z,a,it,ord,s,fct] = backcor(n,y,ord,s,fct)

% BACKCOR   Background estimation by minimizing a non-quadratic cost function.
%
%   [EST,COEFS,IT] = BACKCOR(N,Y,ORDER,THRESHOLD,FUNCTION) computes and estimation EST
%   of the background (aka. baseline) in a spectroscopic signal Y with wavelength N.
%   The background is estimated by a polynomial with order ORDER using a cost-function
%   FUNCTION with parameter THRESHOLD. FUNCTION can have the four following values:
%       'sh'  - symmetric Huber function :  f(x) = { x^2  if abs(x) < THRESHOLD,
%                                                  { 2*THRESHOLD*abs(x)-THRESHOLD^2  otherwise.
%       'ah'  - asymmetric Huber function :  f(x) = { x^2  if x < THRESHOLD,
%                                                   { 2*THRESHOLD*x-THRESHOLD^2  otherwise.
%       'stq' - symmetric truncated quadratic :  f(x) = { x^2  if abs(x) < THRESHOLD,
%                                                       { THRESHOLD^2  otherwise.
%       'atq' - asymmetric truncated quadratic :  f(x) = { x^2  if x < THRESHOLD,
%                                                        { THRESHOLD^2  otherwise.
%   COEFS returns the ORDER+1 vector of the estimated polynomial coefficients
%   (computed with n sorted and bounded in [-1,1] and y bounded in [0,1]).
%   IT returns the number of iterations.
%
%   [EST,COEFS,IT] = BACKCOR(N,Y) does the same, but run a graphical user interface
%   to help setting ORDER, THRESHOLD and FCT.
%
% For more informations, see:
% - V. Mazet, C. Carteret, D. Brie, J. Idier, B. Humbert. Chemom. Intell. Lab. Syst. 76 (2), 2005.
% - V. Mazet, D. Brie, J. Idier. Proceedings of EUSIPCO, pp. 305-308, 2004.
% - V. Mazet. PhD Thesis, University Henri Poincaré Nancy 1, 2005.
%
% 22-June-2004, Revised 19-June-2006, Revised 30-April-2010,
% Revised 12-November-2012 (thanks E.H.M. Ferreira!)
% Comments and questions to: vincent.mazet@unistra.fr.

% Check arguments
if nargin < 2, error('backcor:NotEnoughInputArguments','Not enough input arguments'); end;
if nargin < 5, [z,a,it,ord,s,fct] = backcorgui(n,y); return; end; % delete this line if you do not need GUI
if ~isequal(fct,'sh') && ~isequal(fct,'ah') && ~isequal(fct,'stq') && ~isequal(fct,'atq'),
    error('backcor:UnknownFunction','Unknown function.');
end;

% Rescaling
N = length(n);
[n,i] = sort(n);
y = y(i);
maxy = max(y);
dely = (maxy-min(y))/2;
n = 2 * (n(:)-n(N)) / (n(N)-n(1)) + 1;
y = (y(:)-maxy)/dely + 1;

% Vandermonde matrix
p = 0:ord;
T = repmat(n,1,ord+1) .^ repmat(p,N,1);
Tinv = pinv(T'*T) * T';

% Initialisation (least-squares estimation)
a = Tinv*y;
z = T*a;

% Other variables
alpha = 0.99 * 1/2;     % Scale parameter alpha
it = 0;                 % Iteration number
zp = ones(N,1);         % Previous estimation

% LEGEND
while sum((z-zp).^2)/sum(zp.^2) > 1e-9,

    it = it + 1;        % Iteration number
    zp = z;             % Previous estimation
    res = y - z;        % Residual

    % Estimate d
    if isequal(fct,'sh'),
        d = (res*(2*alpha-1)) .* (abs(res)<s) + (-alpha*2*s-res) .* (res<=-s) + (alpha*2*s-res) .* (res>=s);
    elseif isequal(fct,'ah'),
        d = (res*(2*alpha-1)) .* (res<s) + (alpha*2*s-res) .* (res>=s);
    elseif isequal(fct,'stq'),
        d = (res*(2*alpha-1)) .* (abs(res)<s) - res .* (abs(res)>=s);
    elseif isequal(fct,'atq'),
        d = (res*(2*alpha-1)) .* (res<s) - res .* (res>=s);
    end;

    % Estimate z
    a = Tinv * (y+d);   % Polynomial coefficients a
    z = T*a;            % Polynomial

end;

% Rescaling
[~,j] = sort(i);
z = (z(j)-1)*dely + maxy;

    a(1) = a(1)-1;
    a = a*dely;% + maxy;

end

% delete lines below if you do not need GUI

function [z,a,it,ord,s,fct] = backcorgui(n,y)

% BACKCORGUI   Graphical User Interface for background estimation.

% Initialization
z = [];
a = [];
it = [];
ord = [];
s = [];
fct = [];

order = 4;
threshold = 0.01;
costfunction = 'atq';

% Main window
hwin = figure('Visible','off','Position',[0 0 750 400],'NumberTitle','off','Name','Background Correction',...
    'MenuBar','none','Toolbar','figure','Resize','on','ResizeFcn',{@WinResizeFcn});
bgclr = get(hwin,'Color');

% Axes
haxes = axes('Units','pixels');

% Buttons OK & Cancel
hok = uicontrol('Style','pushbutton','String','OK','Position',[600,40,80,25],'Callback',{@OKFcn},'BackgroundColor',bgclr);
hcancel = uicontrol('Style','pushbutton','String','Cancel','Position',[510,40,80,25],'Callback',{@CancelFcn},'BackgroundColor',bgclr);

% Cost functions menu
hfctlbl = uicontrol('Style','text','String','Cost function:','HorizontalAlignment','left','BackgroundColor',bgclr);
hfct = uicontrol('Style','popupmenu','Value',4,'BackgroundColor','white','Callback',{@CostFunctionFcn},...
    'String',{'Symmetric Huber function','Asymmetric Huber function','Symmetric truncated quadratic','Asymmetric truncated quadratic'});

% Threshold text
hthresholdlbl = uicontrol('Style','text','String','Threshold:','HorizontalAlignment','left','BackgroundColor',bgclr);
hthreshold = uicontrol('Style','edit','String',num2str(threshold),'BackgroundColor','white','Callback',{@ThresholdFcn});

% Order slider
horderlbl = uicontrol('Style','text','String','Polynomial order:','HorizontalAlignment','left','BackgroundColor',bgclr);
horder = uicontrol('Style','slider','SliderStep',[0.5 0.5],'Min',0,'Max',10,'Value',order,'SliderStep',[0.1 0.1],'Callback',{@OrderFcn});
horderval = uicontrol('Style','text','String',num2str(order),'BackgroundColor',bgclr);

% Move the GUI to the center of the screen
movegui(hwin,'center');

% Plot a first estimation
[ztmp,atmp,ittmp,order,threshold,costfunction] = compute(n,y,order,threshold,costfunction);

% Make the GUI visible
set(hwin,'Visible','on');

% Callback functions

    function CancelFcn(source,eventdata)
        % Just close the window
        uiresume(gcbf);
        close(hwin);
    end

    function OKFcn(source,eventdata)
        % Return the current estimation and close the window
        z = ztmp;
        a = atmp;
        it = ittmp;
        ord = order;
        s = threshold;
        fct = costfunction;
        uiresume(gcbf);
        close(hwin);
    end

    function CostFunctionFcn(source,eventdata)
        % Change cost function
        cf = get(hfct,'Value');
        if cf == 1,
            costfunction = 'sh';
        elseif cf == 2,
            costfunction = 'ah';
        elseif cf == 3,
            costfunction = 'stq';
        elseif cf == 4,
            costfunction = 'atq';
        end
        [ztmp,atmp,ittmp,ord,s,fct] = compute(n,y,order,threshold,costfunction);
    end

    function OrderFcn(source,eventdata)
        % Change order
        order = get(horder,'Value');
        set(horderval,'String',num2str(order));
        [ztmp,atmp,ittmp,ord,s,fct] = compute(n,y,order,threshold,costfunction);
    end

    function ThresholdFcn(source,eventdata)
        % Change threshold
        threshold = get(hthreshold,'String');
        threshold = str2double(threshold);
        [ztmp,atmp,ittmp,ord,s,fct] = compute(n,y,order,threshold,costfunction);
    end

    function [ztmp,atmp,ittmp,order,threshold,costfunction] = compute(n,y,order,threshold,costfunction)
        % Compute and plot an estimation (need to sort the data)
        [ztmp,atmp,ittmp,order,threshold,costfunction] = backcor(n,y,order,threshold,costfunction);
        [~,i] = sort(n);
        plot(n(i),y(i),'b-',n(i),ztmp(i),'r-');
    end

    function WinResizeFcn(source,eventdata)
        % Resize the window
        pos = get(hwin,'Position');
        w = pos(3);
        h = pos(4);
        if w>400 && h>100,
            set(haxes,'Position',[40,40,w-320,h-70]);
        end;
        set(hok,'Position',[w-90,30,80,25]);
        set(hcancel,'Position',[w-180,30,80,25]);
        set(hfctlbl,'Position',[w-240,h-30,220,20]);
        set(hfct,'Position',[w-240,h-50,220,25]);
        set(hthresholdlbl,'Position',[w-240,h-80,220,20]);
        set(hthreshold,'Position',[w-240,h-100,220,20]);
        set(horderlbl,'Position',[w-240,h-130,220,20]);
        set(horder,'Position',[w-210,h-150,190,20]);
        set(horderval,'Position',[w-240,h-150,20,20]);
    end

uiwait(gcf);

end


function kts = mph2kts(mph)
%MPH2KTS Convert speed from miles per hour to knots
%
%  kts = MPH2KTS(mph) convert speeds from miles per hour to knots.
%
%  See also MPH2KMPH, MPH2FTPS, MPH2MPS, KTS2MPH.

% Jonathan Sullivan
% Original: May 2011
% jonathan.sullivan@ll.mit.edu

kts = mph*0.868976;


function f=ref_igdgt(coef,g,a,M,c_t,c_f,c_w)
%REF_GDGT  Reference generalized DGT
%   Usage:  f=ref_igdgt(c,g,a,M,c_t,c_f,c_w);
%
%   Linear algebra version of the algorithm. Create big matrix
%   containing all the basis functions and multiply with the transpose.

N=size(coef,1)/M;
L=N*a;

F=zeros(L,M*N);

l=(0:L-1).';

if length(g)<L
  g=fir2long(g,L);
end;

for n=0:N-1
  for m=0:M-1
    F(:,M*n+m+1)=exp(2*pi*i*(m+c_f)*(l+c_t)/M).*circshift(g,n*a+c_w);
  end;
end;

f=F*coef;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Example code for estimating the HRF using the Inverse-Logit Model, a
% Finte Impluse Response Model and the Canonical HRF with 2 derivatives.
% Also the code illustrates our code for detecting model misspecification.
%
% By Martin Lindquist and Tor Wager
% Created  10/02/09
% Last edited 05/20/13
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Load time course
%

mypath = which('ilogit');
if isempty(mypath), error('Cannot find directory with ilogit.m and other functions. Not on path?'); end
[mydir] = fileparts(mypath)

load(fullfile(mydir,'timecourse'))

tc = (tc- mean(tc))/std(tc);
len = length(tc);

%% Or: create your own
[xBF] = spm_get_bf(struct('dt', .5, 'name', 'hrf (with time and dispersion derivatives)', 'length', 32));
clear Xtrue
for i = 1:1, xx = conv(xBF.bf(:,i), [1 1 1 1 1 1 ]');
    Xtrue(:, i) = xx(1:66);
end
for i = 2:3, xx = conv(xBF.bf(:,i), [1 1]');
    Xtrue(:, i) = xx(1:66);
end
hrf = Xtrue * [1 .3 .2]';
xsecs = 0:.5:32;

hrf = [ 0; 0; hrf];
hrf = hrf(1:length(xsecs));
hrf = hrf ./ max(hrf);
figure; plot(xsecs, hrf, 'k')
%hrf = hrf(1:4:end); % downsample to TR, if TR is > 0.5

R = randperm(640); R = sort(R(1:36));
Run = zeros(640,1);
for i=1:length(R), Run(R(i)) = 1; end;
true_sig = conv(Run, hrf);
true_sig = true_sig(1:640);

% tc_noise = noise_arp(640, [.7 .2]);
% tc = true_sig + 0.1 * tc_noise;
tc = true_sig;
%figure; plot(tc);

Runc{1} = Run;

%%

create_figure; subplot(3,1,1); han = plot(tc);
title('Sample time course'); drawnow

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Settings
%

TR = 0.5;
%T = round(30/TR);
T = 30;
t = 1:TR:T;                        % samples at which to get Logit HRF Estimate
FWHM = 4;                       % FWHM for residual scan
pval = 0.01;
df = 600;
alpha = 0.001;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Create stick function (sample event onsets)
% Variable R contains onset times
% Variable Run contains stick (a.k.a. delta or indicator) function

% R = [3, 21, 56, 65, 109, 126, 163, 171, 216, 232, 269, 282, 323, 341, 376, 385, 429, 446, 483, 491, 536, 552, 589, 602];
% Run = zeros(640,1);
% for i=1:length(R), Run(R(i)) = 1; end;
%

try
    hold on;
    hh = plot_onsets(R,'k',-3,1, 1);
    drawnow
catch
    disp('Couldn''t find function to add onset sticks to plot. Skipping.')
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Fit HRF using IL-function

% Choose mode (deterministic/stochastic)

mode = 0;   % 0 - deterministic aproach
            % 1 - simulated annealing approach
            % Please note that when using simulated annealing approach you
            % may need to perform some tuning before use.

[h1, fit1, e1, param] = Fit_Logit2(tc,TR,Runc,T,mode);
[pv sres sres_ns1] = ResidScan(e1, FWHM);
[PowLoss1] = PowerLoss(e1, fit1, (len-7) , tc, TR, Runc, alpha);

hold on; han(2) = plot(fit1,'r');

disp('Summary: IL_function');

disp('Amplitude:'); disp(param(1));
disp('Time-to-peak:'); disp(param(2)*TR);
disp('Width:'); disp(param(3)*TR);

disp('MSE:'); disp((1/(len-1)*sum(e1.^2)));
disp('Mis-modeling:'); disp(pv);
disp('Power Loss:'); disp(PowLoss1);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Fit HRF using FIR-model

% Choose mode (FIR/sFIR)

mode = 1;   % 0 - FIR
            % 1 - smooth FIR

[h2, fit2, e2, param] = Fit_sFIR(tc,TR,Runc,T,mode);
[pv sres sres_ns2] = ResidScan(e2, FWHM);
[PowLoss2] = PowerLoss(e2, fit2, (len-T) , tc, TR, Runc, alpha);

hold on; han(3) = plot(fit2,'g');

disp('Summary: FIR');

disp('Amplitude'); disp(param(1));
disp('Time-to-peak'); disp(param(2)*TR);
disp('Width'); disp(param(3)*TR);

disp('MSE:'); disp((1/(len-1)*sum(e2.^2)));
disp('Mis-modeling'); disp(pv);
disp('Power Loss:'); disp(PowLoss2);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Fit HRF using Canonical HRF + 2 derivatives

p=1;

[h3, fit3, e3, param, info] = Fit_Canonical_HRF(tc,TR,Runc,30,p);
[pv sres sres_ns3] = ResidScan(e3, FWHM);
[PowLoss3] = PowerLoss(e3, fit3, (len-p) , tc, TR, Runc, alpha);

hold on; han(4) = plot(fit3,'m');

legend(han,{'Data' 'IL' 'sFIR' 'DD'})

disp('Summary: Canonical + 2 derivatives');

disp('Amplitude'); disp(param(1));
disp('Time-to-peak'); disp(param(2)*TR);
disp('Width'); disp(param(3)*TR);

disp('MSE:'); disp((1/(len-1)*sum(e3.^2)));
disp('Mis-modeling'); disp(pv);
disp('Power Loss:'); disp(PowLoss3);

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

%figure;
%%

subplot(3,2,5); hold on;
plot(xsecs, hrf, 'k')
xsecs1 = xsecs(1:length(h1));
han2 = plot(xsecs1, h1,'r');
xsecs2 = xsecs(1:length(h2));
han2(2) = plot(xsecs2, h2,'g');
xsecs3 = xsecs(1:length(h3));
han2(3) = plot(xsecs3, h3,'m');
legend(han2,{'IL' 'sFIR' 'DD'})
title('Estimated HRF');

subplot(3,1,2); hold on;
hh = plot_onsets(R,'k',-3,1);
drawnow

han3 = plot(sres_ns1,'r');
hold on; han3(2) = plot(sres_ns2,'g');
hold on; han3(3) = plot(sres_ns3,'m');
hold on; plot((1:len),zeros(len,1),'--k');
legend(han3,{'IL' 'sFIR' 'DD'})
title('Mis-modeling (time course)');

subplot(3,2,6); hold on;

[s1] = Fit_sFIR(sres_ns1,TR,Runc,T,0);
[s2] = Fit_sFIR(sres_ns2,TR,Runc,T,0);
[s3] = Fit_sFIR(sres_ns3,TR,Runc,T,0);

han4 = plot(s1(1:T),'r');
hold on; han4(2) = plot(s2(1:T),'g');
hold on; han4(3) = plot(s3(1:T),'m');
hold on; plot((1:T),zeros(T,1),'--k');
legend(han4,{'IL' 'sFIR' 'DD'})
title('Mis-modeling (HRF)');


function [fb] = fbCreate(numOrient,startSigma,numScales,scaling,elong)
% function [fb] = fbCreate(numOrient,startSigma,numScales,scaling,elong)
%
% Create a filterbank containing numOrient even and odd-symmetric
% filters and one center-surround filter at numScales scales.
%
% The even-symmetric filter is a Gaussian second derivative.
% The odd-symmetric filter is its Hilbert transform.
%
% See also oeFilter, csFilter, fbRun.
%
% David R. Martin <dmartin@eecs.berkeley.edu>
% March 2003

if nargin<3, numScales = 1; end
if nargin<4, scaling = sqrt(2); end
if nargin<5, elong = 3; end
support = 3;

fb = cell(2*numOrient,numScales);
for scale = 1:numScales,
  sigma = startSigma * scaling^(scale-1);
  for orient = 1:numOrient,
    theta = (orient-1)/numOrient * pi;
    fb{2*orient-1,scale} = oeFilter(sigma*[elong 1],support,theta, 2,0);
    fb{2*orient,scale} = oeFilter(sigma*[elong 1],support,theta,2,1);
  end
  %fb{2*numOrient+1,scale} = csFilter(sigma*[3 1],support);
end


function q = qinv(p)

% QINV  quaternion inverse
%
%   Q = QINV(P) returns a quaternion Q which is the quaternion inverse of
%     quaternion P.
%     - P is a quaternion. It is a 4-vector or a 4*N array (column i
%        represents quaternion i) where N is the number of quaternions.
%     - Q is the quaternion inverse of quaternion P. It is a 4*N array.

sp = size(p);
if sp == [1 4]
    p = p.';
    sp = size(p);
end

% wrong format
if sp(1) ~= 4
    error('DualQuaternion:Qinv:wrongsize',...
        '%d rows in the P array. It should be 4.',sp(1));
end

normp = qnorm(p);
q = qconj(p)./repmat(normp.^2,4,1);


function res = qdist(a,b)
%QDIST        Implements  q(a,b)  metrical distance
%  Name  qdist  is used to avoid ambiguities with variable  q
%  This functions for non-interval input only for completeness
%
%     res = qdist(a,b)
%
% for real input          abs(a-b)
% for complex input       qdist(real(a),real(b)) + qdist(imag(a),imag(b))
%

% written  03/23/00     S.M. Rump
% modified 04/04/04     S.M. Rump  set round to nearest for safety
% modified 04/06/05     S.M. Rump  rounding unchanged
%

  if isreal(a) & isreal(b)
    res = abs(a-b);
  else
    res = abs(real(a)-real(b)) + abs(imag(a)-imag(b));
  end


function res = computeDirectionWeights3d13(varargin)
%COMPUTEDIRECTIONWEIGHTS3D13 Direction weights for 13 directions in 3D
%
%   C = computeDirectionWeights3d13
%   Returns an array of 13-by-1 values, corresponding to directions:
%   C(1)  = [+1  0  0]
%   C(2)  = [ 0 +1  0]
%   C(3)  = [ 0  0 +1]
%   C(4)  = [+1 +1  0]
%   C(5)  = [-1 +1  0]
%   C(6)  = [+1  0 +1]
%   C(7)  = [-1  0 +1]
%   C(8)  = [ 0 +1 +1]
%   C(9)  = [ 0 -1 +1]
%   C(10) = [+1 +1 +1]
%   C(11) = [-1 +1 +1]
%   C(12) = [+1 -1 +1]
%   C(13) = [-1 -1 +1]
%   The sum of the weights in C equals 1.
%   Some values are equal whatever the resolution:
%   C(4)==C(5);
%   C(6)==C(7);
%   C(8)==C(9);
%   C(10)==C(11)==C(12)==C(13);
%
%   C = computeDirectionWeights3d13(DELTA)
%   With DELTA = [DX DY DZ], specifies the resolution of the grid.
%
%   Example
%   c = computeDirectionWeights3d13;
%   sum(c)
%   ans =
%       1.0000
%
%   c = computeDirectionWeights3d13([2.5 2.5 7.5]);
%   sum(c)
%   ans =
%       1.0000
%
%
%   See also
%
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2010-10-18,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2010 INRA - Cepia Software Platform.

%% Initializations

% grid resolution
delta = [1 1 1];
if ~isempty(varargin)
    delta = varargin{1};
end

% If resolution is [1 1 1], return the pre-computed set of weights
if all(delta == [1 1 1])
    area1 = 0.04577789120476 * 2;
    area2 = 0.03698062787608 * 2;
    area3 = 0.03519563978232 * 2;
    res = [...
        area1; area1; area1; ...
        area2; area2; area2; area2; area2; area2;...
        area3; area3; area3; area3 ];
    return;
end

% Define points of interest in the 26 discrete directions
% format is pt[Xpos][Ypos][Zpos], with [X], [Y] or [Z] being one of
% 'N' (for negative), 'P' (for Positive) or 'Z' (for Zero)

% points below the OXY plane
ptPNN = normalizeVector3d([+1 -1 -1].*delta);
ptPZN = normalizeVector3d([+1  0 -1].*delta);
ptNPN = normalizeVector3d([-1 +1 -1].*delta);
ptZPN = normalizeVector3d([ 0 +1 -1].*delta);
ptPPN = normalizeVector3d([+1 +1 -1].*delta);

% points belonging to the OXY plane
ptPNZ = normalizeVector3d([+1 -1  0].*delta);
ptPZZ = normalizeVector3d([+1  0  0].*delta);
ptNPZ = normalizeVector3d([-1 +1  0].*delta);
ptZPZ = normalizeVector3d([ 0 +1  0].*delta);
ptPPZ = normalizeVector3d([+1 +1  0].*delta);

% points above the OXY plane
ptNNP = normalizeVector3d([-1 -1 +1].*delta);
ptZNP = normalizeVector3d([ 0 -1 +1].*delta);
ptPNP = normalizeVector3d([+1 -1 +1].*delta);
ptNZP = normalizeVector3d([-1  0 +1].*delta);
ptZZP = normalizeVector3d([ 0  0 +1].*delta);
ptPZP = normalizeVector3d([+1  0 +1].*delta);
ptNPP = normalizeVector3d([-1 +1 +1].*delta);
ptZPP = normalizeVector3d([ 0 +1 +1].*delta);
ptPPP = normalizeVector3d([+1 +1 +1].*delta);

%% Spherical cap type 1, direction [1 0 0]

% Compute area of voronoi cell for a point on the Ox axis, i.e. a point
% in the 6-neighborhood of the center.
refPoint = ptPZZ;

% neighbours of chosen point, sorted by CCW angle
neighbors = [ptPNN; ptPNZ; ptPNP; ptPZP; ptPPP; ptPPZ; ptPPN; ptPZN];

% compute area of spherical polygon
area1 = sphericalVoronoiDomainArea(refPoint, neighbors);

%% Spherical cap type 1, direction [0 1 0]

% Compute area of voronoi cell for a point on the Oy axis, i.e. a point
% in the 6-neighborhood of the center.
refPoint    = ptZPZ;

% neighbours of chosen point, sorted by angle
neighbors   = [ptPPZ; ptPPP; ptZPP; ptNPP; ptNPZ; ptNPN; ptZPN; ptPPN];

% compute area of spherical polygon
area2 = sphericalVoronoiDomainArea(refPoint, neighbors);

%% Spherical cap type 1, direction [0 0 1]

% Compute area of voronoi cell for a point on the Oz axis, i.e. a point
% in the 6-neighborhood of the center.
refPoint = ptZZP;

% neighbours of chosen point, sorted by angle
neighbors = [ptPZP; ptPPP; ptZPP; ptNPP; ptNZP; ptNNP; ptZNP; ptPNP];

% compute area of spherical polygon
area3 = sphericalVoronoiDomainArea(refPoint, neighbors);

%% Spherical cap type 2, direction [1 1 0]

% Compute area of voronoi cell for a point on the Oxy plane, i.e. a point
% in the 18-neighborhood
refPoint = ptPPZ;

% neighbours of chosen point, sorted by angle
neighbors = [ptPZZ; ptPPP; ptZPZ; ptPPN];

% compute area of spherical polygon
area4 = sphericalVoronoiDomainArea(refPoint, neighbors);

%% Spherical cap type 2, direction [1 0 1]

% Compute area of voronoi cell for a point on the Oxz plane, i.e. a point
% in the 18-neighborhood
refPoint = ptPZP;
% neighbours of chosen point, sorted by angle
neighbors = [ptPZZ; ptPPP; ptZZP; ptPNP];

% compute area of spherical polygon
area5 = sphericalVoronoiDomainArea(refPoint, neighbors);

%% Spherical cap type 2, direction [0 1 1]

% Compute area of voronoi cell for a point on the Oxy plane, i.e. a point
% in the 18-neighborhood
refPoint = ptZPP;
% neighbours of chosen point, sorted by angle
neighbors = [ptZPZ; ptNPP; ptZZP; ptPPP];

% compute area of spherical polygon
area6 = sphericalVoronoiDomainArea(refPoint, neighbors);

%% Spherical cap type 3 (all cubic diagonals)

% Compute area of voronoi cell for a point on the Oxyz diagonal, i.e. a
% point in the 26 neighborhood only
refPoint = ptPPP;
% neighbours of chosen point, sorted by angle
neighbors = [ptPZP; ptZZP; ptZPP; ptZPZ; ptPPZ; ptPZZ];

% compute area of spherical polygon
area7 = sphericalVoronoiDomainArea(refPoint, neighbors);

%% Concatenate results

% return computed areas, formatted as fraction of sphere surface
res = [...
    area1 area2 area3 ...
    area4 area4 area5 area5 area6 area6...
    area7 area7 area7 area7...
    ]/(2*pi);


function wavelet_test07 ( )

%*****************************************************************************80
%
%% WAVELET_TEST07 tests DAUB14_TRANSFORM and DAUB14_TRANSFORM_INVERSE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 July 2011
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'WAVELET_TEST07\n' );
  fprintf ( 1, '  DAUB14_TRANSFORM computes the DAUB14 transform of a vector.\n' );
  fprintf ( 1, '  DAUB14_TRANSFORM_INVERSE inverts it.\n' );
%
%  Random data.
%
  n = 16;
  seed = 123456789;
  [ u, seed ] = r8vec_uniform_01 ( n, seed );

  v = daub14_transform ( n, u );

  w = daub14_transform_inverse ( n, v );

  fprintf ( 1, '\n' );
  fprintf ( 1, '   i      U(i)       D14(U)(i)  D14inv(D14(U))(i)\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %2d  %10.4f  %10.4f  %10.4f\n', i, u(i), v(i), w(i) );
  end
%
%  Constant signal.
%
  n = 8;
  u(1:n) = 1.0;

  v = daub14_transform ( n, u );

  w = daub14_transform_inverse ( n, v );

  fprintf ( 1, '\n' );
  fprintf ( 1, '   i      U(i)        D14(U)(i)  D14inv(D14(U))(i)\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %2d  %10.4f  %10.4f  %10.4f\n', i, u(i), v(i), w(i) );
  end
%
%  Linear signal.
%
  n = 16;
  a_first = 1.0;
  a_last = n;
  u = linspace ( a_first, a_last, n );

  v = daub14_transform ( n, u );

  w = daub14_transform_inverse ( n, v );

  fprintf ( 1, '\n' );
  fprintf ( 1, '   i      U(i)        D14(U)(i)  D14inv(D14(U))(i)\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %2d  %10.4f  %10.4f  %10.4f\n', i, u(i), v(i), w(i) );
  end
%
%  Quadratic data.
%
  n = 8;
  u(1) = 25.0;
  u(2) = 16.0;
  u(3) = 9.0;
  u(4) = 4.0;
  u(5) = 1.0;
  u(6) = 0.0;
  u(7) = 1.0;
  u(8) = 4.0;

  v = daub14_transform ( n, u );

  w = daub14_transform_inverse ( n, v );

  fprintf ( 1, '\n' );
  fprintf ( 1, '   i      U(i)        D14(U)(i)  D14inv(D14(U))(i)\n' );
  fprintf ( 1, '\n' );
  for i = 1 : n
    fprintf ( 1, '  %2d  %10.4f  %10.4f  %10.4f\n', i, u(i), v(i), w(i) );
  end

  return
end


function [Y] = Rotation_ForceMoment_FromTireFixed(X,A_K_F,A_Rfl_F,A_Rfr_F,A_down_up)
%% Input parameters:
% A_K_F     [---]   Rotation Matrix from RearAxle Fixed Coordinate System to VehicleFixed Coordinate System
% A_Rfl_F   [---]   Rotation Matrix from RearAxle Fixed Coordinate System to WheelFixed Coordinate System FrontLeft
% A_Rfr_F   [---]   Rotation Matrix from RearAxle Fixed Coordinate System to WheelFixed Coordinate System FrontRight
% A_down_up [...]   Orientation Change Matrix between z-up Orientation and z-down Orientation
% X         [---]   Original Input Vectors [x1 x2 x3 x4] [3x4]
% Output parameters:
% Y         [---]   Transformed Output Vectors [y1 y2 y3 y4] [3x4]

%% Output of transformed vectors
% Quarter Vehicle Model along Vehicle Fixed z-Axis: Rotation from TireFixed Coordinate System into VehicleFixed Coordinate System
% Y=[(A_K_F*transpose(A_Rfl_F)*A_down_up*X(:,1)) (A_K_F*transpose(A_Rfr_F)*A_down_up*X(:,2)) (A_K_F*A_down_up*X(:,3)) (A_K_F*A_down_up*X(:,4))];

% Quarter Vehicle Model along Rear Axle Fixed z-Axis: Rotation from TireFixed Coordinate System into RearAxleFixed Coordinate System
Y=[(transpose(A_Rfl_F)*A_down_up*X(:,1)) (transpose(A_Rfr_F)*A_down_up*X(:,2)) (A_down_up*X(:,3)) (A_down_up*X(:,4))];

% Quarter Vehicle Model according MATLAB-Documentation
% Y=[(transpose(A_Rfl_F)*A_down_up*X(:,1)) (transpose(A_Rfr_F)*A_down_up*X(:,2)) (A_down_up*X(:,3)) (A_down_up*X(:,4))];

end


function hermite_cubic_test12 ( )

%*****************************************************************************80
%
%% HERMITE_CUBIC_TEST12 tests HERMITE_CUBIC_LAGRANGE_INTEGRAL.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2011
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'HERMITE_CUBIC_TEST12:\n' );
  fprintf ( 1, '  HERMITE_CUBIC_LAGRANGE_INTEGRAL returns the integrals\n' );
  fprintf ( 1, '  of the four Lagrange basis functions associated \n' );
  fprintf ( 1, '  with F1, D1, F2 and D2 such that\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  P(X) = F1 * LF1(X) + D1 * LD1(X)\n' );
  fprintf ( 1, '       + F2 * LF2(X) + D2 * LD2(X).\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The Lagrange basis function integrals:\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '        X1          X2          LF1         LD1         LF2         LD2\n' );
  fprintf ( 1, '\n' );

  x2 = 1.0;
  for x1 = -6 : +2
    q = hermite_cubic_lagrange_integral ( x1, x2 );
    fprintf ( 1, '  %10.4f  %10.4f  %10.4f  %10.4f  %10.4f  %10.4f\n', ...
      x1, x2, q(1:4) )
  end

  return
end


function adjacency_structure = gr_adjacency_structure ( node_num, ...
  node_coordinates, edge_num, edge_nodes )

%*****************************************************************************80
%
%% GR_ADJACENCY_STRUCTURE returns the adjacency structure of a graph.
%
%  Discussion:
%
%    Since we are using MATLAB, we return an actual structure.
%    That is, a cell array.  Entry {I} of the cell array is a
%    vector containing all the nodes which share an edge with
%    node I.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 January 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NODE_NUM, the number of nodes.
%
%    Input, real NODE_COORDINATES(2,NODE_NUM), the coordinates of the nodes.
%
%    Input, integer EDGE_NUM, the number of edges.
%
%    Input, integer EDGE_NODES(2,EDGE_NUM), the indices of the two nodes
%    that form each edge.
%
%    Output, cell array ADJACENCY_STRUCTURE{NODE_NUM}, the
%    adjacency structure.
%
  adjacency_list = gr_adjacency_list ( node_num, ...
    node_coordinates, edge_num, edge_nodes );

  adjacency_pointer = gr_adjacency_pointer ( node_num, ...
    node_coordinates, edge_num, edge_nodes );

  adjacency_structure = cell ( node_num );

  for i = 1 : node_num
    i1 = adjacency_pointer(i);
    i2 = adjacency_pointer(i+1) - 1;
    adjacency_structure{i} = adjacency_list(i1:i2);
  end

  return
end


function Y = Rotation_Velocity_ToTireFixed(X,A_K_F,A_Rfl_F,A_Rfr_F,A_down_up)
%% Input parameters:
% A_K_F [---]    Rotation Matrix from RearAxle Fixed Coordinate System to VehicleFixed Coordinate System
% X     [---]    Original Input Vectors [x1 x2 x3 x4] [3x4]
% Output parameters:
% Y     [---]    Transformed Output Vectors [y1 y2 y3 y4] [3x4]

%% Output of transformed vectors
% Quarter Vehicle Model along Vehicle Fixed z-Axis: Rotation from VehicleFixed Coordinate System into TireFixed Coordinate System
% Y=[(A_down_up*A_Rfl_F*transpose(A_K_F)*X(:,1)) (A_down_up*A_Rfr_F*transpose(A_K_F)*X(:,2)) (A_down_up*transpose(A_K_F)*X(:,3)) (A_down_up*transpose(A_K_F)*X(:,4))];

% Quarter Vehicle Model along Rear Axle Fixed z-Axis: Rotation from RearAxleFixed Coordinate System into TireFixed Coordinate System
Y=[(A_down_up*A_Rfl_F*X(:,1)) (A_down_up*A_Rfr_F*X(:,2)) (A_down_up*X(:,3)) (A_down_up*X(:,4))];

% Quarter Vehicle Model according MATLAB-Documentation
% Y=[(A_down_up*A_Rfl_F*X(:,1)) (A_down_up*A_Rfr_F*X(:,2)) (A_down_up*X(:,3)) (A_down_up*X(:,4))];

end


function perm=field2d(Nx,Ny,k_avg,V_dp,clx,cly)
% All credits go to http://www.mysimlabs.com/surface_generation.html
% modified by Ali A. Eftekhari
% See the license file
% Note: a very very simple approach! Use it with utmost care :-)
Lx=1.0; % domain length
x=linspace(-Lx/2.0,Lx/2.0,Nx);
y=linspace(-Lx/2.0,Lx/2.0,Ny);
[X,Y]=ndgrid(x,y);
s=-log(1-V_dp); % standard deviation
mu=log(k_avg)-s*s/2.0; % mean for a log-random field
Z=s*randn(Nx,Ny); % normal distribution
F = exp(-(X.^2/(clx*clx/2.0)+Y.^2/(cly*cly/2.0)));
% Gaussian filter
f =2.0/sqrt(pi)*Lx/sqrt(Nx*Ny)/sqrt(clx)/sqrt(cly).*ifft2(fft2(Z).*fft2(F)); % another filter
perm=exp(mu+real(f)); % perm field


function [ftH2O] = Pa2ftH2O(Pa)
% Convert pressure from pascals to feet of 4-degree-C water.
% Chad Greene 2012
ftH2O = Pa*0.000334553;


function [inH2O] = Pa2inH2O(Pa)
% Convert pressure from pascals to inches of 4-degree-C water.
% Chad Greene 2012
inH2O = Pa*0.00401463;


% This is material illustrating the methods from the book
% Financial Modelling  - Theory, Implementation and Practice with Matlab
% source
% Wiley Finance Series
% ISBN 978-0-470-74489-5
%
% Date: 02.05.2012
%
% Authors:  Joerg Kienitz
%           Daniel Wetterau
%
% Please send comments, suggestions, bugs, code etc. to
% kienitzwetterau_FinModelling@gmx.de
%
% (C) Joerg Kienitz, Daniel Wetterau
%
% Since this piece of code is distributed via the mathworks file-exchange
% it is covered by the BSD license
%
% This code is being provided solely for information and general
% illustrative purposes. The authors will not be responsible for the
% consequences of reliance upon using the code or for numbers produced
% from using the code.

% Calibrating kl, ku, mu and nu for a sabr model for Kienitz extrapolation
% the prices are calculated from known sabr parameters

f = 0.03; t =1;                         % forward time
a = 0.25; b = 0.5; r = -.5; n = 0.2;    % sabr parameters

k = 0.0001:0.0001:1;                         % strike range

call = sprice(a, b, r, n, f, k, t,1);   % call prices (standard sabr)
put = sprice(a, b, r, n, f, k, t,0);    % put prices (standard sabr)
put(1) = 0;
call(1) = f;                            % assures forward is matched

Nparam = 4;                             % number of calibrated params

% objective function
of = @(x) of_sabr(a, b, r, n, f, k, t, x(3), x(4), x(1), x(2), call, put);

x0 = [.25*f; 25.5*f; 1.8; 2.4];                   % starting values                  %

A = zeros(Nparam,Nparam); bc= zeros(Nparam,1);
Aeq = A; beq = bc;
lb = [.25*f; f; 1; 3];                        % lower bound
ub = [f; 30*f; 1.5; 5];                     % upper bound
y = fmincon(of,x0,A,bc,Aeq,beq,lb,ub);  % optimization

% verification of results
xval = 0:0.001:.25;                     % x-values
[cl, bl, al, cu, bu, au] = psabr_param_3(a, b, r, n, f, t,y(3),y(4),y(1),y(2));
yval = psabr_5(a, b, r, n, f, xval, t, y(1), y(2), ...
           y(3), cl, bl,al, y(4), cu,bu,au);% y-values calculated

plot(xval,yval);                        % plot the results
Factor = 1000000;                       % used for plotting

yval_call = sprice_5(a, b, r, n, f, xval, t, y(1), y(2), ...
    y(3), cl, bl,al, y(4), cu,bu,au, 1);    % SABR Call prices
figure; hold on;
    plot(xval,Factor*yval_call,'r');
    plot(k,Factor*call,'g');
hold off;

yval_put = sprice_5(a, b, r, n, f, xval, t, y(1), y(2), ...
    y(3), cl, bl,al, y(4), cu,bu,au, 0);    % SABR Put prices
figure; hold on;
    plot(xval,Factor*yval_put,'r');
    plot(k,Factor*put,'g');
hold off;

fval = sprice_5(a, b, r, n, f, 0, t, y(1), y(2), ...
    y(3), cl, bl,al, y(4), cu,bu,au, 1);    % calculate forward value
y                                       % calibrated values
f - fval                                % display difference to forward


% test for patchwise denoising
%
%   Copyright (c) 2007 Gabriel Peyre

path(path,'toolbox/');

%% load the image
rep = 'images/';    % directory where you can find the image
name = 'barb';
n = 80;
M = load_image([rep name]);
M = rescale( crop(M,n) );

sigma = 0.03; % variance of additional noise
if sigma>0
    % avoid saturation
    M = clamp( rescale(M,sigma,1-sigma) + sigma * randn(size(M)) );
end

%% options of NL means
options.k = 3;          % half size for the windows
options.T = 0.03;       % width of the gaussian, relative to max(M(:))  (=1 here)
options.max_dist = 15;  % search width, the smaller the faster the algorithm will be
options.ndims = 30;     % number of dimension used for distance computation (PCA dim.reduc. to speed up)
options.do_patchwise = 1;

%% do denoising
tic;
[M1,Wx,Wy] = perform_nl_means(M, options);
toc;

%% display results
ax = [];
clf;
ax(1) = subplot(2,2,1);
imagesc(M); axis image; axis off;
title('Original image');
ax(2) = subplot(2,2,2);
imagesc(clamp(M1)); axis image; axis off;
title('Denoised');
ax(3) = subplot(2,2,3);
imagesc(rescale(M-M1));
title('Removed noise');
axis image; axis off;
if size(M,3)>1
    colormap gray(256);
end
linkaxes(ax,'xy');


function [] = doc_examples
% File:      doc_examples.m
% Author:    Ioannis Filippidis, jfilippidis@gmail.com
% Date:      2012.06.14 - 2012.06.16
% Language:  MATLAB R2012a
% Purpose:   documentation examples of fig2u3d
% Copyright: Ioannis Filippidis, 2012-

%fname = two_variable_func;
%fname = three_variable_level_set;
%fname = molecule_structure;
%fname = robotic_manipulator;
fname = fluid_flow;
fig2u3d(gca, fname, '-pdf')

function [fname] = two_variable_func
%f(x,y) surface, gradient, contours
ax = newax;
hold(ax, 'on')

dom = 10 *[-1, 1, -1, 1];
res = [30, 40];

q = domain2vec(dom, res);
x = q(1, :);
y = q(2, :);
f = sin(sqrt(0.5 .*x.^2 +y.^2) ) +2;
v = bsxfun(@times, cos(sqrt(0.5 .*x.^2 +y.^2) ) .*(0.5 .*x.^2 +y.^2).^(-0.5), [0.5 .*x; y] );

vsurf(ax, q, f, res)
vcontour(ax, q, f, res)
quivermd(ax, q, v)
axis tight
view(ax, 3)

fname = 'two_var_func';

function [fname] = three_variable_level_set
% Elliptic supercylide level set surface, gradient and 2D field section
example_supercyclide
fname = 'example_supercyclide';

function [fname] = molecule_structure
draw_crystal_lattice
fname = 'diamond';

function [fname] = robotic_manipulator
% requires the Robotics Toolbox by Peter I. Corke
mdl_puma560
p560.plot(zeros(1,6) )
fname = 'puma560';

function [fname] = fluid_flow
Wind
fname = 'wind';


% corrcoef_cell() - compute pairwise correlations using arrays and
%                   cell array inputs.
%
% Usage:
%    >> c = corrcoef_cell( data );
%    >> c = corrcoef_cell( data );
%
% Inputs:
%   data       - [cell array] data consisting of PAIRED arrays to be compared.
%                The last dimension of embeded data arrays is used to compute
%                correlation (see examples).
% Outputs:
%   c   - Correlation values. Same size as data without the last dimension.
%
% Note: the main advantage over the corrcoef Matlab function is the
%       capacity to compute millions of pairwise correlations per second.
%
% Example:
%   a = { rand(1,10) rand(1,10) };
%   c1 = corrcoef_cell(a);
%   c2 = corrcoef(a{1}, a{2});
%   % in this case, c1 is equal to c2(2)
%
%   a = { rand(200,300,100) rand(200,300,100) };
%   c = corrcoef_cell(a);
%   % the call above would require 200 x 300 calls to the corrcoef function
%   % and be about 1000 times slower
%
% Author: Arnaud Delorme, SCCN/INC/UCSD, La Jolla, 2010

% Copyright (C) Arnaud Delorme
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 2 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program; if not, write to the Free Software
% Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

function c = corrcoef_cell(a,b);

if nargin < 1
    help corrcoef_cell;
    return;
end;

if nargin < 2
    b = a{2};
    a = a{1};
end;

nd = myndims(a);
if nd == 1
    aa = a-mean(a);
    bb = b-mean(b);
    cv  = aa'*bb/(10-1);
    cva = aa'*aa/(10-1);
    cvb = bb'*bb/(10-1);

    c = cv/sqrt(cva*cvb);
elseif nd == 2
    aa = a-repmat(mean(a,2),[1 size(a,2)]);
    bb = b-repmat(mean(b,2),[1 size(a,2)]);
    cv  = sum(aa.*bb,2);
    cva = sum(aa.*aa,2);
    cvb = sum(bb.*bb,2);

    c = cv./sqrt(cva.*cvb);
elseif nd == 3
    aa = a-repmat(mean(a,3),[1 1 size(a,3)]);
    bb = b-repmat(mean(b,3),[1 1 size(a,3)]);
    cv  = sum(aa.*bb,3);
    cva = sum(aa.*aa,3);
    cvb = sum(bb.*bb,3);

    c = cv./sqrt(cva.*cvb);
elseif nd == 4
    aa = a-repmat(mean(a,4),[1 1 1 size(a,4)]);
    bb = b-repmat(mean(b,4),[1 1 1 size(a,4)]);
    cv  = sum(aa.*bb,4);
    cva = sum(aa.*aa,4);
    cvb = sum(bb.*bb,4);

    c = cv./sqrt(cva.*cvb);
end;

function val = myndims(a)
    if ndims(a) > 2
        val = ndims(a);
    else
        if size(a,1) == 1,
            val = 2;
        elseif size(a,2) == 1,
            val = 1;
        else
            val = 2;
        end;
    end;


function net = rbfsetfw(net, scale)
%RBFSETFW Set basis function widths of RBF.
%
%	Description
%	NET = RBFSETFW(NET, SCALE) sets the widths of the basis functions of
%	the RBF network NET. If Gaussian basis functions are used, then the
%	variances are set to the largest squared distance between centres if
%	SCALE is non-positive and SCALE times the mean distance of each
%	centre to its nearest neighbour if SCALE is positive.  Non-Gaussian
%	basis functions do not have a width.
%
%	See also
%	RBFTRAIN, RBFSETBF, GMMEM
%

%	Copyright (c) Ian T Nabney (1996-2001)

% Set the variances to be the largest squared distance between centres
if strcmp(net.actfn, 'gaussian')
   cdist = dist2(net.c, net.c);
   if scale > 0.0
      % Set variance of basis to be scale times average
      % distance to nearest neighbour
      cdist = cdist + realmax*eye(net.nhidden);
      widths = scale*mean(min(cdist));
   else
      widths = max(max(cdist));
   end
   net.wi = widths * ones(size(net.wi));
end


function [ output_args ] = rcnn_assign_soft_cluster_score(expidx)

p = rcnn_exp_params(expidx);

fn = [p.expDir '/' p.shortName '/imdb/cache/clusters.mat'];
load(fn);

joint_no = 13;

vecs_j = vecs{joint_no};
vecs_j = vecs_j(:, 1:end-1);

C = clusters{joint_no};
k = size(C, 1);
num_all = size(vecs_j, 1);

%[idx, C] = spectralclustering(vecs_j, k);
%load('~/spectral_clustering', 'C', 'idx');

idx = zeros(num_all, 1);
for j = 1:num_all
    vec = vecs_j(j, :);
    diff = bsxfun(@minus, C, vec);
    [~, min_dist] = min(sum(diff.^2, 2));
    idx(j) = min_dist;
end

%assign soft scores
D = pdist(C);
min_dist = min(D);
sigma = min_dist/1.5;

figure(1);
hold on;

is_3d = size(vecs_j, 2) > 2;

% do PCA to display stuff
if is_3d
    [coeff,score,latent,tsquared,explained,mu] = pca(vecs_j);
    vecs_j = score;
end

for i = 1:k
    iset = (idx == i);
    if is_3d
        scatter3(vecs_j(iset, 1), vecs_j(iset, 2), vecs_j(iset, 3), 5, 'filled');
    else
        scatter(vecs_j(iset, 1), vecs_j(iset, 2), 5, 'filled');
    end
end

for i = 1:num_all
    idx = unidrnd(num_all);
    vec = vecs_j(idx, :);
    dists = pdist2(C,vec);
    scores = exp(-dists.^2/(sigma^2));
    scores = scores/sum(scores);
    if is_3d
        scatter3(vec(1), vec(2), vec(3), 20, [0 0 0], 'filled');
    else
        scatter(vec(1), vec(2), 20, [0 0 0], 'filled');
    end
    find(scores' > 0.1)
    pause;
end

end


%This m file is for locating a good enough "M". For a good value of "M",
%"PG" will be high. But it also adds computational load as we increase "M".
%Run this for different frames of "x" and see for yourself. Define the
%starting values of frames by defining a value to "b" at line 25.

%Here,  array of "k"=Reflection C-oefficients
%       array of "a"=LPCs
%       array of "R"=autocorrelation co-efficients
%every notation is according to page 112 [chapter4] of Speech Coding
%Algorithms by W. C. Chu.

%the indexes in this code is "+1" from the indexes in the book in some
%cases because there is no index "0" in matlab

clear all
% close all
clc

for M=3:100,

% INITIALIZATION:
inpfilenm = 's1ofwb.wav';
[x, fs]=wavread(inpfilenm);%"t_16k_2s.wav" is the file I used. Change according
                      %to your case.
% length(x)
b=3841;        %index no. of starting data point of current frame
fsize = 30e-3;    %frame size
frame_length = round(fs .* fsize);  %=number of data points in each framesize
                                    %of "x"
N= frame_length - 1;        %N+1 = frame length = number of data points in
                            %each frame
sk=0;       %initializing summartion term "sk"
a=[zeros(M+1);zeros(M+1)]; %defining a matrix of zeros for "a" for init.

%FRAME SEGMENTATION:
    y1=x(b:b+N);    %"b+N" denotes the end point of current frame.
                    %"y" denotes an array of the data points of the current
                    %frame
    y = filter([1 -.9378], 1, y1);  %pre-emphasis filtering

%MAIN BODY OF THIS PROGRAM STARTS FROM HERE>>>>>>>>>>>>>>
z=xcorr(y);

%finding array of R[l]
R=z( ( (length(z)+1) ./2 ) : length(z)); %R=array of "R[l]", where l=0,1,2,
                                         %...(b+N)-1
                                         %R(1)=R[lag=0], R(2)=R[lag=1],
                                         %R(3)=R[lag=2]... etc

%GETTING OTHER PARAMETERS OF PREDICTOR OF ORDER "0":
s=1;        %s=step no.
J(1)=R(1);          %J=array of "Jl", where l=0,1,2...(b+N)-1
                    %J(1)=J0, J(2)=J1, J(3)=J2 etc

%GETTING OTHER PARAMETERS OF PREDICTOR OF ORDER "(s-1)":
for s=2:M+1,
    sk=0;               %clearing "sk" for each iteration
    for i=2:(s-1),
        sk=sk + a(i,(s-1)).*R(s-i+1);
    end                 %now we know value of "sk", the summation term
                        %of formula of calculating "k(l)"
    k(s)=(R(s) + sk)./J(s-1);
    J(s)=J(s-1).*(1-(k(s)).^2);

    a(s,s)= -k(s);
    a(1,s)=1;
    for i=2:(s-1),
        a(i,s)=a(i,(s-1)) - k(s).*a((s-i+1),(s-1));
    end
end

a_final=a((1:s),s)';
est_y = filter([0 -a_final(2:end)],1,y);    % = s^(n) with a cap on page 92 of the book
e = y - est_y;      %supposed to be a white noise

pg(M) = 10.*log10( (sum(y.^2)) ./ (sum(e.^2)) );       %prediction gain
end
figure;
subplot(2,1,1), plot(x); title(['original speech file, ',inpfilenm]);
subplot(2,1,2),plot(pg);
title('Prediction Gain (PG) vs Prediction Order (M) for frame starting at data point "b"');
xlabel('M');
ylabel('PG');

disp('This m file is simply "func_lev_durb.m" with a little modification. If you run this for many frames, you will see that PG is good enough for M = a value around "10" for most of the frames');


function [Hx,Hy,Ez,time] = Maxwell2D(Hx, Hy, Ez, FinalTime)

% function [Hx,Hy,Ez] = Maxwell2D(Hx, Hy, Ez, FinalTime)
% Purpose :Integrate TM-mode Maxwell's until FinalTime starting with initial conditions Hx,Hy,Ez

Globals2D;
time = 0;

% Runge-Kutta residual storage
resHx = zeros(Np,K); resHy = zeros(Np,K); resEz = zeros(Np,K);

% compute time step size
rLGL = JacobiGQ(0,0,N); rmin = abs(rLGL(1)-rLGL(2));
dtscale = dtscale2D; dt = min(dtscale)*rmin*2/3

% outer time step loop
while (time<FinalTime)

  if(time+dt>FinalTime), dt = FinalTime-time; end

   for INTRK = 1:5
      % compute right hand side of TM-mode Maxwell's equations
      [rhsHx, rhsHy, rhsEz] = MaxwellRHS2D(Hx,Hy,Ez);

      % initiate and increment Runge-Kutta residuals
      resHx = rk4a(INTRK)*resHx + dt*rhsHx;
      resHy = rk4a(INTRK)*resHy + dt*rhsHy;
      resEz = rk4a(INTRK)*resEz + dt*rhsEz;

      % update fields
      Hx = Hx+rk4b(INTRK)*resHx; Hy = Hy+rk4b(INTRK)*resHy; Ez = Ez+rk4b(INTRK)*resEz;
   end;
   % Increment time
   time = time+dt;
end
return


% Computes a rotation of the PC factors consistent with a recursive assumption with the policy instrument
%
% Syntax:
%
% Fr = facrot(F,Ffast,Fslow)
%
% where:      F:      Unrestricted PC estimates (from all the dataset)
%             Ffast:  Factors assumed to be fast moving (e.g. policy instrument)
%             Fslow:  Proxy of the slow moving factors
%
%
% Bernanke, Boivin and Eliasz (2002)
% 12/17/02

function Fr = favar_facrot(F,Ffast,Fslow)

k1=size(Ffast,2);

b=bear.favar_olssvd(F,[ones(size(Ffast,1),1) Ffast Fslow]);
Fr = F - Ffast*b(2:k1+1,:);


msckf5_inf = load('msckf_500_1000_min5_maxInf.mat');
msckf10_20 = load('msckf_500_1000_min10_max20.mat');
msckf10_50 = load('msckf_500_1000_min10_max50.mat');
msckf20_100 = load('msckf_500_1000_min20_max100.mat');
swf10 = load('swf_500_1000_10_dataset3.mat');
swf50 = load('swf_500_1000_50_dataset3.mat');
swf100 = load('swf_500_1000_100_dataset3.mat');
imu = load('imu_500_1000.mat');

%% Calculate Average RMSE (Root-Mean-Squared Error)

% IMU Only RMSE
imu_trans = sqrt(sum(imu.msckf_trans_err.^2, 1)/3);
imu_rot = sqrt(sum(imu.msckf_rot_err.^2, 1)/3);

% MSCKF RMSE
msckf5_inf_trans = sqrt(sum(msckf5_inf.msckf_trans_err.^2, 1)/3);
msckf10_20_trans = sqrt(sum(msckf10_20.msckf_trans_err.^2, 1)/3);
msckf10_50_trans = sqrt(sum(msckf10_50.msckf_trans_err.^2, 1)/3);
msckf20_100_trans = sqrt(sum(msckf20_100.msckf_trans_err.^2, 1)/3);
disp('===============')
msckf5_inf_rot = sqrt(sum(msckf5_inf.msckf_rot_err.^2, 1)/3);
msckf10_20_rot = sqrt(sum(msckf10_20.msckf_rot_err.^2, 1)/3);
msckf10_50_rot = sqrt(sum(msckf10_50.msckf_rot_err.^2, 1)/3);
msckf20_100_rot = sqrt(sum(msckf20_100.msckf_rot_err.^2, 1)/3);

% SWF RMSE
swf10_trans = sqrt(sum(swf10.swf_trans_err.^2, 1)/3);
swf50_trans = sqrt(sum(swf50.swf_trans_err.^2, 1)/3);
swf100_trans = sqrt(sum(swf100.swf_trans_err.^2, 1)/3);

swf10_rot = sqrt(sum(swf10.swf_rot_err.^2, 1)/3);
swf50_rot = sqrt(sum(swf50.swf_rot_err.^2, 1)/3);
swf100_rot = sqrt(sum(swf100.swf_rot_err.^2, 1)/3);


function [inc]=look2inc(la,height,lat)
%LOOK2INC look angle to incidence angle
%
%    [INCIDENCE]=LOOK2INC(LOOK_ANGLE,HEIGHT,LATITUDE)
%
%    LOOK_ANGLE = Look angle (radians) (can be vector or matrix)
%    HEIGHT     = Height of satellite (m)
%    LATITUDE   = mean latitude of ground (degrees)
%
%     Copyright (C) 2015  Bekaert David - University of Leeds
%     Email: eedpsb@leeds.ac.uk or davidbekaert.com
%     With permission from Andy Hooper
%
%     This program is free software; you can redistribute it and/or modify
%     it under the terms of the GNU General Public License as published by
%     the Free Software Foundation; either version 2 of the License, or
%     (at your option) any later version.
%
%     This program is distributed in the hope that it will be useful,
%     but WITHOUT ANY WARRANTY; without even the implied warranty of
%     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%     GNU General Public License for more details.
%
%     You should have received a copy of the GNU General Public License along
%     with this program; if not, write to the Free Software Foundation, Inc.,
%     51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
%
% by Andrew Hooper, 2010

if nargin<3
   lat=40
end

lat1=lat*pi/180;

WGS84_A=6378137.0; % semimajor axis wgs84
WGS84_B=6356752.314; % semiminor axis wgs84
Re=WGS84_A*WGS84_B./sqrt(WGS84_A^2*sin(lat1).^2+WGS84_B^2*cos(lat1).^2);

a=Re+height; % Earth centre to satellite
inc=la;

[R] = fminsearch(@(p) (a^2+p^2-2*a*p*cos(mean(la(:)))-Re^2)^2,[600])
for i=1:length(la(:))
    R = fminsearch(@(p) (a^2+p^2-2*a*p*cos(la(i))-Re^2)^2,R);
    inc(i)=pi-acos((Re^2+R^2-a^2)/2/Re/R);
end