function SCKS = spm_SCK(SCKS)
% FORMAT SCKS = spm_SCK(SCKS)
%__________________________________________________________________________
% Square-root Cubature Kalman Filters [2] & Square-root Rauch-Tang-Striebel
% Smoother (SCKF-SCKS [1]).
%==========================================================================
% This function performs joint estimation of the states, input and parameters
% of a model that is described as a stochastic continuous-discrete
% state-space in terms of nonlinear blind deconvolution. The state equations
% must have the form of ordinary differential equations, where the
% discretization is performed through local-linearization scheme [3].
% Additionally, the parameter noise covariance is estimated online via
% stochastic Robbins-Monro approximation method [4], and the measurement noise
% covariance is estimated using a combined variational Bayesian (VB)
% approach with a nonlinear filter/smoother [5].
%__________________________________________________________________________
%
% SCKS.M  - model structure (based on DEM [6] in SPM8 toolbox)
% SCKS.Y  - response variable, output or data
%__________________________________________________________________________
%
% generative model:
%--------------------------------------------------------------------------
%   M(1).f  = dx/dt = f(x,v,P)    {inline function, string or m-file}
%   M(1).g  = y(t)  = g(x,v,P)    {inline function, string or m-file}
%
%   M(1).xP = state error covariance matrix
%   M(1).uP = input error variance
%   M(1).wP = parameter error covariance matrix
%
%   M(1).pE = prior expectation of p model-parameters
%   M(1).pC = prior covariances of p model-parameters
%   M(1).ip = parameter indices
%   M(1).cb = constrain on parameters [lower, upper];
%
%   M(1).Q  = precision components on observation noise
%   M(1).V  = fixed precision (input noise)
%   M(1).W  = precision on state noise (approximated by annealing)
%
%   M(i).m  = number of inputs v(i + 1);
%   M(1).n  = number of states x(i);
%   M(1).l  = number of output v(i);
%
%   M(1).Qf      = form of measurement noise cov estimate:
%                  'auto'[default],'min','mean'
%   M(1).E.nN    = number of SCKF-SCKS algorithm iterations
%   M(1).E.Itol  = tolerance value for SCKF-SCKS convergence
%   M(1).E.nD    = number of integration step between observations
%   M(1).VB.N    = number of VB algorithm iterations
%   M(1).VB.Itol = tolerance value for VB convergence
%   M(1).VB.l    = VB scaling factor;
%
%   conditional moments of model-states - q(u)
%--------------------------------------------------------------------------
%   qU.x  = Conditional expectation of hidden states (backward estimate)
%   qU.v  = Conditional expectation of input (backward estimate)
%   qU.z  = Conditional prediction error
%   qU.S  = Conditional covariance: cov(x) (states - backward estimate)
%   qU.C  = Conditional covariance: cov(u) (input - backward estimate)
%
% conditional moments of model-parameters - q(p)
%--------------------------------------------------------------------------
%   qP.P    = Conditional expectation
%   qP.C    = Conditional covariance
%
%      F    = negative log-likelihood
%__________________________________________________________________________
% Copyright (c) Brno University of Technology (2010)...
% Martin Havlicek 05-12-2010
%
% References:
% [1] Havlicek M et al (2011)
% [2] Arasaratnam, I., Haykin, S. (2009) Cubature Kalman Filters. IEEE
%     Transactions on Automatic Control 54, 1254-1269.
% [3] Jimenez, J.C. (2002) A simple algebraic expression to evaluate the
%     local linearization schemes for stochastic differential equations*
%     1. Applied Mathematics Letters 15, 775-780.
% [4] Van der Merwe, R., 2004. Sigma-point Kalman filters for probabilistic
%     inference in dynamic state-space models. Ph.D.thesis, Oregon Graduate
%     Institute of Science and Technology.
% [5] Sarkka, S., Hartikainen, J. (2011?) Extension of VB-AKF to Estimation
%     of Full Covariance and Non-Linear Systems. In Press.
% [6] Friston, K.J., et al. (2008) DEM: a variational treatment of dynamic
%     systems. Neuroimage 41, 849-885.
%__________________________________________________________________________
% Copyright (C) - Martin Havlicek

% Martin Havlicek
% $Id: spm_SCK.m 4628 2012-01-27 20:51:41Z karl $
% check model specification
%--------------------------------------------------------------------------
M  = SCKS.M;
M  = spm_DEM_M_set(M);

% get integration step dt:
%--------------------------------------------------------------------------
nD = M(1).E.nD;
dt = 1/nD;

% INITIALISATION:
% =========================================================================

% interpolate observation according to integration step
%--------------------------------------------------------------------------
y     = SCKS.Y;            % observations
if size(y,1) > size(y,2)   % check the dimensions
    y = y';
end

% interpolate if dt < 1:
%--------------------------------------------------------------------------
y    = spm_interp(y',1/dt)';
if size(y,1) > size(y,2)   % check dimensions again
    y = y';
end
T    = size(y,2);          % number of time points

% initial condition:
%--------------------------------------------------------------------------
x     = M(1).x;            % states
u     = M(2).v;            % input
pE    = spm_vec(M(1).pE);  % all model parameter
ip    = M(1).ip;           % parameter indices to be estimated
theta = pE(ip);            % selected parameters

try cb  = M(1).cb;               catch, cb = []; end; % parameter constraints
try tE  = spm_vec(SCKS.pP.P{1}); catch, tE = []; end; % true parameters for display (if available)

% covariances (square-roots)
%--------------------------------------------------------------------------
sR      = cell(1,T);
[sR{:}] = deal(sparse(real(chol(inv(M(1).V)))*dt));   % observation noise variance
sQ      = sparse(real(chol(inv(M(1).W)))*dt);         % hidden state noise variance
if ~isempty(M(2).v)
    sV  = sparse(real(chol(inv(M(2).V)))*dt);         % input noise variance
else
    sV  = [];
end

% process error covariances (square-roots)
%--------------------------------------------------------------------------
Sx = sparse(real(chol(M(1).xP))*dt);
if ~isempty(M(2).v)
    Su = sparse(real(chol(M(1).uP))*dt);
else
    Su = [];
end
if ~isempty(ip)
    Sw = sparse(real(chol(M(1).wP(ip,ip)))*dt);
    sW = sparse(real(chol(M(1).pC(ip,ip)))*dt);  % parameter noise variance
    dv = diag(sW);
else
    Sw = [];
    sW = [];
end

% number of states, inputs and parameters:
%--------------------------------------------------------------------------
nx      = size(Sx,1);         % number of states
nu      = size(sV,1);         % number of states
nw      = size(sW,1);         % number of parameters
no      = size(sR{1},1);      % number of observations
noises  = nx + nu + nw + no;  % number of noise components

% concatenate state vector and square-root error covariance:
%--------------------------------------------------------------------------
xc      = [x(:); u(:); theta(:)];
xx      = zeros(nx+nu+nw,T);
xx(:,1) = xc;
Sc      = cell(1,T);
[Sc{:}] = deal(sparse(nx+nu+nw,nx+nu+nw));
Sc{1}   = blkdiag(Sx,Su,Sw);

% get vector indices for components of concatenated state vector
%--------------------------------------------------------------------------
xmask   = [ones(1,nx),ones(1,nu)*2,ones(1,nw)*3,ones(1,no)*4];
xind    = find(xmask==1);
uind    = find(xmask==2);
wind    = find(xmask==3);
clear xmask;

% setting for VB: observation noise estimation:
%--------------------------------------------------------------------------
if ~isempty(M(1).Q)
    try, iter0  = M(1).VB.N;   catch, iter0 = 3;          end
    try, lambda = M(1).VB.l;   catch, lambda = 1-exp(-2); end
    NU      = 6;
    V       = diag(repmat(1e-4,1,no));
    [sR{:}] = deal(sqrt(1./(NU-no-1).*V));
    B       = sqrt(lambda)*eye(no);
    k       = size(sR{1},1);
    iter    = iter0;
    MSE0    = zeros(no,1);
    RR0     = zeros(no,T);
    VBrun   = [];
else
    iter0   = 1;
    iter    = iter0;
    RR      = [];
    VBrun   = [];
end

% Pre-calculate cubature points:
%--------------------------------------------------------------------------
n          = nx + nu + nw + noises;       % total state vector dimension
nPts       = 2*n;                         % number of cubature points
CubPtArray = sqrt(n)*[eye(n) -eye(n)];    % cubature points array

% augment paramter matrix by number of cubature points:
%--------------------------------------------------------------------------
pE   = pE(:,ones(1,nPts));

% prepare matrix template for integration by Local linearization scheme:
%--------------------------------------------------------------------------
EXPm = repmat({[ones(nx),2*ones(nx,1);zeros(1,nx+1)]},1,nPts);
EXPm = sparse(blkdiag(EXPm{:}));
xt   = repmat([zeros(1,nx) 1],1,nPts)';

% =========================================================================
% Iteration scheme:
% =========================================================================
% get maximum number of iterations and tolerance:
%--------------------------------------------------------------------------
try, ItolVB = M(1).VB.Itol;  catch,  ItolVB = 1e-4;      end
try, Itol   = M(1).E.Itol;   catch,  Itol   = 1e-3;      end
try, RUN    = M(1).E.nN;     catch,  RUN    = 32;        end
try, ap     = M(1).E.RM;     catch,  ap     = [1e3 1e6]; end   % Robins-Monro approximation parameters

MLdiff0  = 1e-1;
mloglik0 = 0;
ML       = [];
VBrun    = RUN;
t0       = tic;
% =========================================================================
% Iteration loop (until convergence)
% =========================================================================

for run = 1:RUN
    t1  = tic;
    mloglik = -log(2.*pi).*(T/dt);
    % =====================================================================
    %   Forward pass:
    % =====================================================================
     for t = 2:T

        sQ = diag(diag((1/sqrt(0.9995)-1)*Sc{t-1}(xind,xind)));
        Sa = blkdiag(Sc{t-1},sQ,sV,sW,sR{t-1});
        xa = [xc;zeros(noises,1)];
        Xi =  xa(:,ones(1,nPts)) + Sa*CubPtArray;

        %------------------------------------------------------------------
        % PREDICTION STEP:
        %------------------------------------------------------------------
        xPred(uind,:) = Xi(uind,:) + Xi(uind+nx+nu+nw,:);  % add input noise
        xPred(wind,:) = Xi(wind,:) + Xi(wind+nx+nu+nw,:);  % add parameter noise

        % parameter constraint:
        %------------------------------------------------------------------
        if ~isempty(cb) && ~isempty(ip)
            xPred(wind,:) = min(cb(:,2*ones(1,nPts)),xPred(wind,:)); % upper constrain
            xPred(wind,:) = max(cb(:,1*ones(1,nPts)),xPred(wind,:)); % lower constrain
        end
        pE(ip,:) = xPred(wind,:);

        % propagation of cubature points through nonlinear function:
        %------------------------------------------------------------------
        f             = M(1).f(Xi(xind,:),xPred(uind,:),pE);

        % integration by local-linearization scheme:
        %------------------------------------------------------------------
        dfdx          = spm_diff_all(M(1).f,Xi(xind,:),xPred(uind,:),pE,1);
        dx            = expmall(dfdx,f,dt,EXPm)*xt;
        xPred(xind,:) = Xi(xind,:) + reshape(dx(~xt),nx,nPts) + Xi(xind+nx+nu+nw,:);

        % mean prediction:
        %------------------------------------------------------------------
        x1      = sum(xPred,2)/nPts;
        X       = (xPred-x1(:,ones(1,nPts)))/sqrt(nPts);
        [foo,S] = qr([X]',0);
        S       = S';

        % in the case of VB observation noise estimation:
        %------------------------------------------------------------------
        if ~isempty(M(1).Q) && iter~=1
            NU   = lambda.*(NU-k-1)+k+1;
            V    = B*V*B';
            NU   = NU + 1;
            V0   = V;
        end
        x10    = x1;
        S0     = S;
        yPred0 = M(1).g(xPred(xind,:),xPred(uind,:),pE);

        %------------------------------------------------------------------
        % UPDATE STEP:
        %------------------------------------------------------------------

        % VB estimation of sR (iteratively)
        %------------------------------------------------------------------
        for it = 1:iter

           % VB part - update of square-root measurement noise cov
           %---------------------------------------------------------------
            if ~isempty(M(1).Q) && iter ~= 1
                sR{t} = real(chol(1/(NU - no - 1)*V));
            end

            % propagate cubature points through observation function:
            %--------------------------------------------------------------
            yPred    = yPred0 + sR{t}*CubPtArray(end-no+1:end,:);
            y1       = sum(yPred,2)/nPts;

            Y        = (yPred-y1(:,ones(1,nPts)))/sqrt(nPts);
            [foo,Sy] = qr([Y]',0);
            Sy       = Sy';

            Pxy      = X*Y';                % cross covariance
            K        = (Pxy/Sy')/Sy;        % Kalman gain
            resid    =  y(:,t) - y1;        % innovations

            % state (input,parameter) estimates:
            %--------------------------------------------------------------
            xc = x10 + K*(resid);

            % check parameter constraints:
            %--------------------------------------------------------------
            if ~isempty(cb) && ~isempty(ip)
                xc(wind) = min(cb(:,2),xc(wind)); % upper constrain
                xc(wind) = max(cb(:,1),xc(wind)); % lower constrain
            end
            % estimate of process error covarinace:
            %--------------------------------------------------------------
            [foo,S] = qr([(X - K*Y)]',0);
            S       = S';

            % VB part
            %--------------------------------------------------------------
            if ~isempty(M(1).Q) && iter~=1
                Sa     = blkdiag(S,sQ,sV,sW,sR{t});
                Xi     = repmat([xc;zeros(noises,1)],1,nPts) + Sa*CubPtArray;
                yPreds = M(1).g(Xi(xind,:),Xi(uind,:),pE);  % no additive noise!
                D      = repmat(y(:,t),1,nPts)-yPreds;
                D      = D*D'/nPts;
                V      = V0 + D;
            end

        end
        Sc{t}   = S;
        xx(:,t) = xc;

        % Maximum log-Likelihood
        %------------------------------------------------------------------
         mloglik = mloglik - log(det(Sy*Sy')) - resid'/(Sy*Sy')*resid;

        % Robins-Monro stochastic approximation for of parameters noise cov
        %------------------------------------------------------------------
         if ~isempty(ip)
             subKG = K(wind,:);
             dv    = sqrt((1-1/ap(1))*(dv.^2) + 1/ap(1)*diag(subKG*(subKG*resid*resid')'));
             sW    = diag(dv);
             ap(1) = min(ap(1)+1,ap(2));
         end
         if ~isempty(M(1).Q) && iter~=1
             RR(:,t) = diag(sR{t});
         end
    end
    xxf = xx;
    Sf  = Sc;
    %----------------------------------------------------------------------
    % END of forward pass
    % ---------------------------------------------------------------------

    % =====================================================================
    %   Backward pass:
    % =====================================================================
    for t = T-1:-1:1
        % VB part:
        %------------------------------------------------------------------
        if ~isempty(M(1).Q) && iter~=1
            NU    = lambda.*(NU-k-1)+k+1;
            V     = B*V*B';
            NU    = NU + 1;
            V0    = V;
            sR{t} = real(chol(1/(NU-no-1)*V));
        end

        % Square-root Cubature Rauch-Tung-Striebel smoother
        %------------------------------------------------------------------

        % evaluate cubature points:
        %------------------------------------------------------------------
        Sa = blkdiag(Sc{t},sQ,sV,sW,sR{t});
        xa = [xx(:,t);zeros(noises,1)];
        Xi = xa(:,ones(1,nPts)) + Sa*CubPtArray;

        xPred(uind,:) = Xi(uind,:) + Xi(uind+nx+nu+nw,:);
        xPred(wind,:) = Xi(wind,:) + Xi(wind+nx+nu+nw,:);

        % check parameter constraints:
        %------------------------------------------------------------------
        if ~isempty(cb) && ~isempty(ip)
            xPred(wind,:) = min(cb(:,2*ones(1,nPts)),xPred(wind,:)); % upper constrain
            xPred(wind,:) = max(cb(:,1*ones(1,nPts)),xPred(wind,:)); % lower constrain
        end

        pE(ip,:)      = xPred(wind,:);
        % propagate cubature points through nonlinear function:
        %------------------------------------------------------------------
        f             = M(1).f(Xi(xind,:),xPred(uind,:),pE);
        dfdx          = spm_diff_all(M(1).f,Xi(xind,:),xPred(uind,:),pE,1);
        dx            = expmall(dfdx,f,dt,EXPm)*xt;
        xPred(xind,:) = Xi(xind,:) + reshape(dx(~xt),nx,nPts) + Xi(xind+nx+nu+nw,:) ;

        x1       = sum(xPred,2)/nPts;
        X        = (xPred-x1(:,ones(1,nPts)))/sqrt(nPts) + eps;
        x01      = xx(:,t);
        X01      = (Xi([xind,uind,wind],:) - repmat(x01,1,nPts))/sqrt(nPts);
        [foo,S]  = qr([X]',0);
        S        = S';

        Pxy      = X01*X';      % cross covariance
        K        = (Pxy/S')/S;  % Kalman gain

        % smoothed estimate of the states (input, parameters)
        % and process error covariance:
        %------------------------------------------------------------------
        xx(:,t)  = xx(:,t) + K*(xx(:,t+1) - x1);
        [foo,S]  = qr([X01 - K*X, K*Sc{t+1}]',0);
        S        = S';
        Sc{t}    = S;

        % check parameter constraint:
        %------------------------------------------------------------------
        if ~isempty(cb) && ~isempty(ip)
            xx(wind,t) = min(cb(:,2),xx(wind,t)); % upper constrain
            xx(wind,t) = max(cb(:,1),xx(wind,t));
        end

        % VB part (smoothing):
        %------------------------------------------------------------------
        if ~isempty(M(1).Q) && iter~=1
            Sa     = blkdiag(S,sQ,sV,sW,sR{t});
            Xi     = repmat([xx(:,t);zeros(noises,1)],1,nPts) + Sa*CubPtArray;
            yPreds = M(1).g(Xi(xind,:),Xi(uind,:),pE);  % no additive noise!
            D      = repmat(y(:,t),1,nPts)-yPreds;
            D      = D*D'/nPts;
            V      = V0 + D;
        end

        if ~isempty(M(1).Q) && iter~=1
            RR(:,t) = diag(sR{t});
        end

    end
    xxb = xx;
    Sb  = Sc;
    %----------------------------------------------------------------------
    % END of backward pass
    %----------------------------------------------------------------------

    str{1} = sprintf('SCKS: %i (1:%i)',run,iter);

    % iteration condition for measurement noise estimate:
    % iterate until stabilization of sR estimate
    %----------------------------------------------------------------------
    if ~isempty(M(1).Q) && iter0 ~= 1

        MSE     = mean((RR -(RR0)).^2,2);
        RR0     = RR;
        MSEdiff = abs(MSE - MSE0);
        MSE0    = MSE;

        if MSEdiff < ItolVB  % (till it gets stable)
            switch(lower(M(1).Qf))
                case('all')
                    % take all
                case('mean')
                    sR      = cell(1,T);
                    [sR{:}] = deal(diag(mean(RR,2)));
                case('min')
                    RRs     = sort(RR,2,'descend');
                    sR      = cell(1,T);
                    [sR{:}] = deal(diag(mean(RRs(:,round(T*0.90):end),2)));
                case('auto')
                    dlim    = min(RR,[],2);
                    ulim    = max(RR,[],2);
                    if all(ulim./dlim<4)
                        % take all
                    else
                        RRs     = sort(RR,2,'descend');
                        sR      = cell(1,T);
                        [sR{:}] = deal(diag(mean(RRs(:,round(T*0.90):end),2)));
                    end
            end
            iter0    = 1;
            iter     = iter0;
            mloglik0 = 0;
            VBrun    = run;
        end
    end

    % log-likelihood difference:
    %----------------------------------------------------------------------
    MLdiff(run) = abs(mloglik-mloglik0);
    ML(run)     = mloglik;
    if mloglik > 0
        mloglik     = mloglik - 5000;
        MLdiff(run) = abs(mloglik-mloglik0);
        ML(run)     = mloglik;
    end

    timed  = toc(t1);
    str{2} = sprintf('F:%.4e',ML(end));
    str{3} = sprintf('dF:%.4e',MLdiff(end));
    str{4} = sprintf('(%.2e sec)',timed);
    fprintf('%-16s%-16s%-16s%-16s\n',str{:})

    % plot estimates:
    %----------------------------------------------------------------------
    doplotting(M,xxf,xxb,Sf,Sb,ML,T,wind,ip,run,RR,VBrun);

    % stopping condition:
    %----------------------------------------------------------------------
    if RUN > 1 && (~isempty(ip) || ~isempty(M(1).Q))
        if run == 2
            MLdiff0 = MLdiff(run);
        elseif run > 2
            if MLdiff0 < MLdiff(run),
                MLdiff0 = MLdiff(run);
            end
        end
        if (((MLdiff(run)/MLdiff0)<Itol || run==RUN) || (isempty(ip)&&MLdiff(run)<Itol)) && iter0 == 1,

            yy       = M(1).g(xx(xind,:),xx(uind,:),pE(:,1));
            res      = y - yy;

            try SCKS = rmfield(SCKS,'qU'); end
            try SCKS = rmfield(SCKS,'qP'); end
            try SCKS = rmfield(SCKS,'qH'); end

            % save results in structure:
            %--------------------------------------------------------------
            SCKS.qU.v{1} = yy(:,1:nD:end);
            SCKS.qU.x{1} = xxb(xind,1:nD:end);
            SCKS.qU.v{2} = xxb(uind,1:nD:end);
            SCKS.qU.z{1} = res(:,1:nD:end);
            if ~isempty(ip)
                qP           = SCKS.M(1).pE;
                qP(ip)       = mean(xx(wind,:),2);
                SCKS.qP.P{1} = qP;
                SCKS.qP.P{2} = [];
            end
            SCKS.F       = ML;

            for i = 1:nD:T
                j = 1 + (i - 1)/nD;
                SCKS.qU.S{j} = Sb{i}(xind,xind);
                SCKS.qU.C{j} = Sb{i}(uind,uind);
                if ~isempty(ip)
                    SCKS.qP.p{j} = xxb(wind,:);
                    SCKS.qP.c{j} = Sb{i}(wind,wind);
                    SCKS.qP.ip   = ip;
                end
            end
            return
        end

        mloglik0 = mloglik;
        xc       = [xx([xind,uind],1); mean(xx(wind,:),2)];
        xx(:,1)  = xc;

    else

        pE(ip,1) = mean(xx(wind,:),2);
        yy       = M(1).g(xx(xind,:),xx(uind,:),pE(:,1));
        res      = y - yy;

        try SCKS = rmfield(SCKS,'qU'); end
        try SCKS = rmfield(SCKS,'qP'); end
        try SCKS = rmfield(SCKS,'qH'); end

        % save results in structure:
        %------------------------------------------------------------------
        SCKS.qU.v{1} = yy(:,1:nD:end);
        SCKS.qU.x{1} = xxb(xind,1:nD:end);
        SCKS.qU.v{2} = xxb(uind,1:nD:end);
        SCKS.qU.z{1} = res(:,1:nD:end);
        if ~isempty(ip)
            qP           = SCKS.M(1).pE;
            qP(ip)       = mean(xx(wind,:),2);
            SCKS.qP.P{1} = qP;
            SCKS.qP.P{2} = [];
        end
        SCKS.F       = ML;

        for i = 1:nD:T
            j = 1 + (i - 1)/nD;
            SCKS.qU.S{j} = Sb{i}(xind,xind);
            SCKS.qU.C{j} = Sb{i}(uind,uind);
            if ~isempty(ip)
                SCKS.qP.p{j} = xxb(wind,:);
                SCKS.qP.c{j} = Sb{i}(wind,wind);
                SCKS.qP.ip   = ip;
            end
        end
        return
    end
end

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

%--------------------------------------------------------------------------
% Plot estimates at each iteration:
%--------------------------------------------------------------------------
function doplotting(M,xxf,xxb,Sf,Sb,ML,T,wind,ip,run,RR,VBrun)

% Initialize display:
%--------------------------------------------------------------------------
spm_figure('GetWin','SCKF-SCKS estimates');
set(gcf,'Renderer','painter'); clf

% Hidden states
%--------------------------------------------------------------------------
for p=1:2
    subplot(3,3,[1:3]+3*(p-1)),
    hax = gca;
    si    = spm_invNcdf(1 - 0.05);
    s     = [];
    if p == 1,
        xxfig = xxf;
        Sfig  = Sf;
        tit   = 'SCKF - forward pass';
    else
        xxfig = xxb;
        Sfig  = Sb;
        tit   = 'SCKS - backward pass';
    end
    for i = 1:T
        s = [s abs(diag(Sfig{i}))];
    end

    % conditional covariances
    %----------------------------------------------------------------------
    j           = [1:size(xxfig(:,:),1)];
    ss          = si*s(j,:);
    [ill indss] = sort(full(mean(ss,2)),'descend');

    pf = plot(1:T,xxfig,'linewidth',1.5);
    set(hax,'xlim',[1,T],'nextplot','add')
    box(hax,'on')
    for ic = 1:size(xxfig,1)
        col0 = get(pf(indss(ic)),'color');
        col  = (ones(1,3)-col0)*0.65 + col0;
        fill([(1:T) fliplr(1:T)],[(xxfig(indss(ic),:) + ss(indss(ic),:)) fliplr((xxfig(indss(ic),:) - ss(indss(ic),:)))],...
            'r',...
            'FaceColor',col,...
            'EdgeColor',col);
        hold on;
        COL{ic} = col0;
    end
    for ic = 1:size(xxfig,1)
        plot(xxfig(indss(ic),:),'color',COL{ic},'linewidth',0.75);
    end
    title(tit);
    grid(hax,'on');
    axis(hax,'tight');
end

% Parameter estimates
%--------------------------------------------------------------------------
subplot(3,3,7)
h = plot([1:length(ML)],ML);
if ~isempty(M(1).Q)
    AYlim = get(gca,'Ylim');
    if VBrun>=run
        bkg = ones(1,run)*max(AYlim);
    else
        bkg = [ones(1,VBrun)*max(AYlim),ones(1,abs(VBrun-run))*min(AYlim)];
    end
    a = area(bkg,min(AYlim));
    axis([1 run+1 AYlim(1) AYlim(2)]); hold on;
    set(a(1),'FaceColor',ones(1,3)*0.6,'EdgeColor',ones(1,3)*0.6)
    h = plot([1:length(ML)],ML);
    axis([1 run+1 AYlim(1) AYlim(2)]); hold on;
    if VBrun>=run
        text(run/2+1,mean(AYlim),'VB-SCKS','HorizontalAlignment','center','VerticalAlignment','top');
    else
        text(VBrun/2+1,mean(AYlim),'VB-SCKS','HorizontalAlignment','center','VerticalAlignment','top');
        text(VBrun+(abs(VBrun-run)/2)+1,mean(AYlim),'SCKS','HorizontalAlignment','center','VerticalAlignment','top');
    end
    set(gca,'Layer','top')
    hold off
end
set(h,'color','k','Marker','o','Markersize',4,'MarkerFaceColor','k','linewidth',1);
title('Log-Likelihood');

if ~isempty(ip)
    subplot(3,3,8)
    b1 = bar(ip,mean(xxb(wind,:),2)','FaceColor','k');
    set(b1,'BarWidth',0.5); hold on;
    title('Parameters');
end

% Standard deviation of residuals
%--------------------------------------------------------------------------
if ~isempty(M(1).Q)
    subplot(3,3,9)
    if VBrun >= run
        plot(RR'); hold on;
    else
        plot(RR'); hold on;
        switch(lower(M(1).Qf))
            case('all')
                sR  = RR';
            case('mean')
                sR  = repmat(mean(RR,2),1,T)';
            case('min')
                RRs = sort(RR,2,'descend');
                sR  = repmat(mean(RRs(:,round(T*0.90):end),2),1,T)';
            case('auto')
                dlim    = min(RR,[],2);
                ulim    = max(RR,[],2);
                if all(ulim./dlim<4)
                    sR  = RR';
                else
                    RRs  = sort(RR,2,'descend');
                    sR   = repmat(mean(RRs(:,round(T*0.90):end),2),1,T)';
                end
        end
        plot([1:T],sR,'r','linewidth',2);
    end
    axis(gca,'tight');
    title('Standard deviation of residuals');
    hold off;
end
drawnow


function results = vl_test_kdtree(varargin)
% VL_TEST_KDTREE
vl_test_init ;

function s = setup()
randn('state',0) ;
s.X = single(randn(10, 1000)) ;
s.Q = single(randn(10, 10)) ;

function test_nearest(s)
for tmethod = {'median', 'mean'}
  for type = {@single, @double}
    conv = type{1} ;
    tmethod = char(tmethod) ;

    X = conv(s.X) ;
    Q = conv(s.Q) ;
    tree = vl_kdtreebuild(X,'ThresholdMethod', tmethod) ;
    [nn, d2] = vl_kdtreequery(tree, X,Q) ;

    D2 = vl_alldist2(X, Q, 'l2') ;
    [d2_, nn_] = min(D2) ;

    vl_assert_equal(...
      nn,uint32(nn_),...
      'incorrect nns: type=%s th. method=%s', func2str(conv), tmethod) ;
    vl_assert_almost_equal(...
      d2,d2_,...
      'incorrect distances: type=%s th. method=%s', func2str(conv), tmethod) ;
  end
end

function test_nearests(s)
numNeighbors = 7 ;
tree = vl_kdtreebuild(s.X) ;
[nn, d2] = vl_kdtreequery(tree, s.X, s.Q, ...
                          'numNeighbors', numNeighbors) ;

D2 = vl_alldist2(s.X, s.Q, 'l2') ;
[d2_, nn_] = sort(D2) ;
d2_ = d2_(1:numNeighbors, :) ;
nn_ = nn_(1:numNeighbors, :) ;

vl_assert_equal(nn,uint32(nn_)) ;
vl_assert_almost_equal(d2,d2_) ;

function test_ann(s)
vl_twister('state', 1) ;
numNeighbors = 7 ;
maxComparisons = numNeighbors * 50 ;
tree = vl_kdtreebuild(s.X) ;
[nn, d2] = vl_kdtreequery(tree, s.X, s.Q, ...
                          'numNeighbors', numNeighbors, ...
                          'maxComparisons', maxComparisons) ;

D2 = vl_alldist2(s.X, s.Q, 'l2') ;
[d2_, nn_] = sort(D2) ;
d2_ = d2_(1:numNeighbors, :) ;
nn_ = nn_(1:numNeighbors, :) ;

for i=1:size(s.Q,2)
  overlap = numel(intersect(nn(:,i), nn_(:,i))) / ...
            numel(union(nn(:,i), nn_(:,i))) ;
  assert(overlap > 0.6, 'ANN did not return enough correct nearest neighbors') ;
end

function test_ann_forest(s)
vl_twister('state', 1) ;
numNeighbors = 7 ;
maxComparisons = numNeighbors * 25 ;
numTrees = 5 ;
tree = vl_kdtreebuild(s.X, 'numTrees', 5) ;
[nn, d2] = vl_kdtreequery(tree, s.X, s.Q, ...
                          'numNeighbors', numNeighbors, ...
                          'maxComparisons', maxComparisons) ;

D2 = vl_alldist2(s.X, s.Q, 'l2') ;
[d2_, nn_] = sort(D2) ;
d2_ = d2_(1:numNeighbors, :) ;
nn_ = nn_(1:numNeighbors, :) ;

for i=1:size(s.Q,2)
  overlap = numel(intersect(nn(:,i), nn_(:,i))) / ...
            numel(union(nn(:,i), nn_(:,i))) ;
  assert(overlap > 0.6, 'ANN did not return enough correct nearest neighbors') ;
end


function test_suite = test_survival_coxph

%   Run specific demo and save values for comparison.
%
%   See also
%     TEST_ALL, DEMO_SURVIVAL_COXPH

% Copyright (c) 2011-2012 Ville Tolvanen

initTestSuite;

function testDemo
% Set random number stream so that failing isn't because randomness. Run
% demo & save test values.
prevstream=setrandstream(0);

disp('Running: demo_survival_coxph')
demo_survival_coxph;
path = which('test_survival_coxph.m');
path = strrep(path,'test_survival_coxph.m', 'testValues');
if ~(exist(path, 'dir') == 7)
    mkdir(path)
end
path = strcat(path, '/testSurvival_coxph');
save(path, 'Ef1', 'Ef2', 'Varf1', 'Varf2');

% Set back initial random stream
setrandstream(prevstream);
drawnow;clear;close all

% Compare test values to real values.

function testPredictionsCoxph
values.real = load('realValuesSurvival_coxph', 'Ef1', 'Varf1', 'Ef2', 'Varf2');
values.test = load(strrep(which('test_survival_coxph.m'), 'test_survival_coxph.m', 'testValues/testSurvival_coxph'), 'Ef1', 'Varf1', 'Ef2', 'Varf2');
assertElementsAlmostEqual(values.real.Ef1, values.test.Ef1, 'absolute', 0.10);
assertElementsAlmostEqual(values.real.Ef2, values.test.Ef2, 'absolute', 0.10);
assertElementsAlmostEqual(values.real.Varf1, values.test.Varf1, 'absolute', 0.10);
assertElementsAlmostEqual(values.real.Varf2, values.test.Varf2, 'absolute', 0.10);


%% Demo: Threshold-Free Cluster Enhancement (TFCE) on surface dataset
%
% The data used here is available from http://cosmomvpa.org/datadb.zip
%
% This example uses the following dataset:
% + 'digit'
%    A participant made finger pressed with the index and middle finger of
%    the right hand during 4 runs in an fMRI study. Each run was divided in
%    4 blocks with presses of each finger and analyzed with the GLM,
%    resulting in 2*4*4=32 t-values
%
% This example illustrates the use of Threshold-Free Cluster Enhancement
% with a permutation test to correct for multiple comparisons.
%
% TFCE reference: Stephen M. Smith, Thomas E. Nichols, Threshold-free
% cluster enhancement: Addressing problems of smoothing, threshold
% dependence and localisation in cluster inference, NeuroImage, Volume 44,
% Issue 1, 1 January 2009, Pages 83-98.
%
% #   For CoSMoMVPA's copyright information and license terms,   #
% #   see the COPYING file distributed with CoSMoMVPA.           #

%% Check externals
cosmo_check_external({'surfing','afni'});

%% Set data paths
% The function cosmo_config() returns a struct containing paths to tutorial
% data. (Alternatively the paths can be set manually without using
% cosmo_config.)
config=cosmo_config();

digit_study_path=fullfile(config.tutorial_data_path,'digit');
readme_fn=fullfile(digit_study_path,'README');
cosmo_type(readme_fn);

output_path=config.output_data_path;

% resolution parameter for input surfaces
% 64 is for high-quality results; use 16 for fast execution
ld=16;

% reset citation list
cosmo_check_external('-tic');

% load single surface
intermediate_fn=fullfile(digit_study_path,...
                            sprintf('ico%d_mh.intermediate_al.asc', ld));
[vertices,faces]=surfing_read(intermediate_fn);

%% Load functional data
data_path=digit_study_path;
data_fn=fullfile(data_path,'glm_T_stats_perblock+orig');

targets=repmat(1:2,1,16)';    % class labels:  1 2 1 2 1 2 1 2 1 2 ... 1 2
chunks=floor(((1:32)-1)/4)+1; % half-run:      1 1 1 1 2 2 2 2 3 3 ... 8 8

vol_ds = cosmo_fmri_dataset(data_fn,'targets',targets,'chunks',chunks);

%% Map univariate response data to surface

% this measure averages the data near each node to get a surface dataset
radius=0;
surf_band_range=[-2 2]; % get voxel data within 2mm from surface
surf_def={vertices,faces,[-2 2]};
nbrhood=cosmo_surficial_neighborhood(vol_ds,surf_def,'radius',radius);

measure=@(x,opt) cosmo_structjoin('samples',mean(x.samples,2),'sa',x.sa);

surf_ds=cosmo_searchlight(vol_ds,nbrhood,measure);

fprintf('Univariate surface data:\n');
cosmo_disp(surf_ds);

%% Average data in each chunk
% for this example only consider the samples in the first condition
% (targets==1), and average the samples in each chunk
%
% for group analysis: set chunks to (1:nsubj)', assuming each sample is
% data from a single participant
surf_ds=cosmo_slice(surf_ds,surf_ds.sa.targets==1);
surf_ds=cosmo_average_samples(surf_ds);

fn_surf_ds=fullfile(output_path, 'digit_target1.niml.dset');

% save to disc
cosmo_map2surface(surf_ds, fn_surf_ds);
fprintf('Input data saved to %s\n', fn_surf_ds);

%% Run Threshold-Free Cluster Enhancement (TFCE)

% All data is prepared; surf_ds has 8 samples and 5124 nodes. We want to
% see if there are clusters that show a significant difference from zero in
% their response. Thus, .sa.targets is set to all ones (the same
% condition), whereas .sa.chunks is set to (1:8)', indicating that all
% samples are assumed to be independent.
%
% (While this is a within-subject analysis, exactly the same logic can be
% applied to a group-level analysis)

% define neighborhood for each feature
% (cosmo_cluster_neighborhood can be used also for meeg or volumetric
% fmri datasets)
cluster_nbrhood=cosmo_cluster_neighborhood(surf_ds,...
                                        'vertices',vertices,'faces',faces);

fprintf('Cluster neighborhood:\n');
cosmo_disp(cluster_nbrhood);

opt=struct();

% number of null iterations. for publication-quality, use >=1000;
% 10000 is even better
opt.niter=250;

% in this case we run a one-sample test against a mean of 0, and it is
% necessary to specify the mean under the null hypothesis
% (when testing classification accuracies, h0_mean should be set to chance
% level, assuming a balanced crossvalidation scheme was used)
opt.h0_mean=0;

% this example uses the data itself (with resampling) to obtain cluster
% statistcs under the null hypothesis. This is (in this case) somewhat
% conservative due to how the resampling is performed.
% Alternatively, and for better estimates (at the cost of computational
% cost), one can generate a set of (say, 50) datasets using permuted data
% e.g. using cosmo_randomize_targets), put them in a cell and provide
% them as the null argument.
opt.null=[];

fprintf('Running multiple-comparison correction with these options:\n');
cosmo_disp(opt);

% Run TFCE-based cluster correction for multiple comparisons.
% The output has z-scores for each node indicating the probablity to find
% the same, or higher, TFCE value under the null hypothesis
tfce_ds=cosmo_montecarlo_cluster_stat(surf_ds,cluster_nbrhood,opt);

%% Show results

fprintf('TFCE z-score dataset\n');
cosmo_disp(tfce_ds);

nfeatures=size(tfce_ds.samples,2);
percentiles=(1:nfeatures)/nfeatures*100;
plot(percentiles,sort(tfce_ds.samples))
title('sorted TFCE z-scores');
xlabel('feature percentile');
ylabel('z-score');

nvertices=size(vertices,1);
disp_opt=struct();
disp_opt.DataRange=[-2 2];

DispIVSurf(vertices,faces,1:nvertices,tfce_ds.samples',0,disp_opt);

% store results
fn_tfce_ds=fullfile(output_path, 'digit_target1_tfce.niml.dset');
cosmo_map2surface(tfce_ds, fn_tfce_ds);

surf_fn=fullfile(output_path, 'digit_intermediate.asc');
surfing_write(surf_fn,vertices,faces);

% show citation information
cosmo_check_external('-cite');


function cond = conex4_condition ( )

%*****************************************************************************80
%
%% CONEX4_CONDITION returns the L1 condition of the CONEX4 matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 January 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real COND, the L1 condition number.
%
  a_norm = 33.0;
  b_norm = 136.0;
  cond = a_norm * b_norm;

  return
end


classdef CEC2017_F8 < PROBLEM
% <single> <real> <constrained>
% CEC'2017 constrained optimization benchmark problem

%------------------------------- Reference --------------------------------
% G. Wu, R. Mallipeddi, and P. N. Suganthan, Problem definitions and
% evaluation criteria for the CEC 2017 competition on constrained real-
% parameter optimization, National University of Defense Technology, China,
% 2016.
%------------------------------- 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".
%--------------------------------------------------------------------------

    properties
        O;  % Optimal decision vector
    end
    methods
        %% Default settings of the problem
        function Setting(obj)
            CallStack = dbstack('-completenames');
            load(fullfile(fileparts(CallStack(1).file),'CEC2017.mat'),'Data');
            obj.O = Data{8};
            obj.M = 1;
            if isempty(obj.D); obj.D = 10; end
            obj.D = min(obj.D,length(obj.O));
            obj.lower    = zeros(1,obj.D) - 100;
            obj.upper    = zeros(1,obj.D) + 100;
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,PopDec)
            Z = PopDec - repmat(obj.O(1:size(PopDec,2)),size(PopDec,1),1);
            PopObj = max(Z,[],2);
        end
        %% Calculate constraint violations
        function PopCon = CalCon(obj,PopDec)
            Z = PopDec - repmat(obj.O(1:size(PopDec,2)),size(PopDec,1),1);
            Y = Z(:,1:2:end);
            W = Z(:,2:2:end);
            PopCon(:,1) = abs(sum(cumsum(Y,2).^2,2)) - 1e-4;
            PopCon(:,2) = abs(sum(cumsum(W,2).^2,2)) - 1e-4;
        end
    end
end


function order = r8poly_order ( na, a )

%*****************************************************************************80
%
%% R8POLY_ORDER returns the order of a polynomial in power sum form.
%
%  Discussion:
%
%    The order of a constant polynomial is 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 September 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NA, the number of coefficients.
%
%    Input, real A(1:NA), the coefficients of the polynomials.
%
%    Output, integer ORDER, the order of the polynomial.
%
  order = na;

  while ( 1 < order )

    if ( a(order) ~= 0.0 )
      return
    end

    order = order - 1;

  end

  return
end


function [x_tplus1,z_tplus1,pseudo_data] = DIT_oneIter(y,z_t,x_t,width,height,denoiser,M_func,Mt_func)
% function x_hat = DIT_oneIter(y,z_t,x_t,width,height,denoiser,M_func,Mt_func)
% This function computes one iteration of the D-IT algorithm
% Input:
%       y       : the measurements
%       z_t     : current residual
%       x_t     : current signal estimate
%       iters   : the number of iterations
%       width   : width of the sampled signal
%       height  : height of the sampeled signal. height=1 for 1D signals
%       denoiser: string that determines which denosier to use. e.g.
%       denoiser='BM3D'
%       M_func  : function handle that projects onto M. Or a matrix M.
%       Mt_func : function handle that projects onto M'. Or no entry
%Output:
%       x_tplus1 : next signal estimate.
%       z_tplus1 : next residual

if nargin==8%function
    M=@(x) M_func(x);
    Mt=@(z) Mt_func(z);
else%Matrix
    M=@(x)M_func*x;
    Mt=@(z)M_func'*z;
end
denoi=@(noisy,sigma_hat) denoise(noisy,sigma_hat,width,height,denoiser);

n=width*height;
m=length(y);

pseudo_data=Mt(z_t)+x_t;
sigma_hat=sqrt(1/m*sum(abs(z_t).^2));
x_tplus1=denoi(pseudo_data,sigma_hat);
z_tplus1=y-M(x_tplus1);
end


function matlab_mistake_test01 ( )

%*****************************************************************************80
%
%% MATLAB_MISTAKE_TEST01 puts numbers in an array, except they don't get there!
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 January 2014
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'MATLAB_MISTAKE_TEST01:\n' );
  fprintf ( 1, '  Set X(I) = 1 / ( I - 5 ) for I = 1 to 10\n' );
  fprintf ( 1, '  but set X(5) to zero.\n' );

  x = zeros ( 10, 1 );

  for i = 1 : 10
    if ( x(i) ~= 5 )
      x(i) == 1 / ( i - 5 );
    else
      x(i) = 0;
    end
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Now let''s print out X!\n' );
  fprintf ( 1, '\n' );

  for i = 1 : 10
    fprintf ( 1, '  X(%d) = %d\n', i, x(i) );
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, '  That was NOT what we expected!\n' );

  return
end


function blas1_c_test09 ( )

%*****************************************************************************80
%
%% TEST09 tests CSCAL.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 April 2006
%
%  Author:
%
%    John Burkardt
%
  n = 6;

  for j = 1 : n
    x(j) = 10 * j + j * i;
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST09\n' );
  fprintf ( 1, '  CSCAL multiplies a complex scalar times a vector.\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  X = \n' );
  fprintf ( 1, '\n' );
  for j = 1 : n
    fprintf ( 1, '  %6d  %10f  %10f\n', j, real ( x(j) ), imag ( x(j) ) );
  end

  da = 5.0 + 0.0 * i;
  x = cscal ( n, da, x, 1 );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  CSCAL ( N, ( %f  %f), X, 1 )\n', real ( da ), imag ( da ) );
  fprintf ( 1, '\n' );
  for j = 1 : n
    fprintf ( 1, '  %6d  %10f  %10f\n', j, real ( x(j) ), imag ( x(j) ) );
  end

  for j = 1 : n
    x(j) = 10 * j + j * i;
  end

  da = -2.0 + 1.0 * i;
  x = cscal ( 3, da, x, 2 );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  CSCAL ( 3, ( %f  %f), X, 2 )\n', real ( da ), imag ( da ) );
  fprintf ( 1, '\n' );
  for j = 1 : n
    fprintf ( 1, '  %6d  %10f  %10f\n', j, real ( x(j) ), imag ( x(j) ) );
  end

  return
end


function blurTSeriesPlot(blurFcn)
% Blurs the tseries data in the current graph window
%
%   blurTSeriesPlot(blurFcn)
%
% The blurring function, by default, sums to one to preserve the mean. It
% is [0.2500    0.5000    0.2500].  For more blurring, repeat this call.
%
% The plot is created either from plotting mean tSeries, or multiple
% tSeries.  This routine also runs on Single Cycle Plots.
%
% Re-written so that it might be extended in the future.  We should
% probably add identifiers to the userData structure in the plot windows.
% (BW)
%
% djh, 3/2001
global GRAPHWIN
if (isempty(GRAPHWIN) || GRAPHWIN==0), myErrorDlg('blurPlot: no graph to blur');
else  set(0,'CurrentFigure',GRAPHWIN);
end
if notDefined('blurFcn'), blurFcn = [1 2 1]; blurFcn = blurFcn/sum(blurFcn); end
axisHandles = get(gcf,'Children');
for h = 1:length(axisHandles)
    subplot(axisHandles(h));
    data = get(gca,'userData');

    if isfield(data,'tSeries')
        typeOfPlot = 'tseries';
        data.time = data.frameNumbers;
        data.tSeries = conv2(data.tSeries,blurFcn,'same');
    elseif isfield(data,'x') && isfield(data,'y')
        typeOfPlot = 'singlecycle';
        data.time = data.x;
        data.tSeries = conv2(data.y,blurFcn,'same');
    else
        myErrorDlg('blurPlot: cannot identify plot type.  Edit blurTseriesPlot')
    end

    % get plot properties
    xLim = get(gca,'xLim');    yLim = get(gca,'yLim');
    xTick = get(gca,'xTick');  yTick = get(gca,'yTick');
    xGrid = get(gca,'xGrid');  yGrid = get(gca,'yGrid');
    fontName = get(get(gca,'xlabel'),'fontName');
    fontSize = get(get(gca,'xLabel'),'fontSize');
    xLabel   = get(get(gca,'xLabel'),'String');
    yLabel   = get(get(gca,'yLabel'),'String');
    titleStr = get(get(gca,'title'),'String');

    color = get(get(gca,'Children'),'Color');
    if iscell(color), color = color{1}; end
    lineWidth = get(get(gca,'Children'),'lineWidth');
    if iscell(lineWidth), lineWidth = lineWidth{1}; end

    switch lower(typeOfPlot)
        case 'tseries'
            p = plot(data.time,data.tSeries);
        case 'singlecycle'
            p = errorbar(data.time,data.tSeries,data.e);
        otherwise
    end

    % Replot the data
    set(p,'lineWidth',lineWidth);
    set(p,'Color',color);

    % reset properties
    set(gca,'xLim',xLim);   set(gca,'yLim',yLim);
    set(gca,'xTick',xTick); set(gca,'yTick',yTick);
    set(gca,'xGrid',xGrid); set(gca,'yGrid',yGrid);
    xlabel(xLabel,'fontName',fontName,'fontSize',fontSize);
    ylabel(yLabel,'fontName',fontName,'fontSize',fontSize);
    title(titleStr,'fontName',fontName,'fontSize',fontSize);
    set(gca,'UserData',data);

end
return


function [marginals, marginalsT] = project_joint_onto_marginals(joint, hnodes, ns)

ss = length(ns);
jointT = dpot(hnodes, ns(hnodes), joint);
marginalsT = cell(1, ss);
marginals = cell(1,ss);
for i=hnodes(:)'
  marginalsT{i} = marginalize_pot(jointT, i);
  m = pot_to_marginal(marginalsT{i});
  marginals{i} = m.T(:);
end


% dp_test_yzz
% test distance_power bug (?) for yzz

if ~isvar('R2')
	ig = image_geom('nx', 22, 'ny', 20, 'nz', 8, 'dx', 2, 'dz', 1);
	ej = ig.unitv;
	l2b = 0;
	offsets = ''; % default is x,y,z
	offsets = '3d:26'; % full 3d neighborhood
	offsets = [ig.nx+1]; % test a diagonal
	arg = {ig.mask, 'edge_type', 'tight', 'beta', 2^l2b, ...
		'type_denom', 'matlab', ...
		'offsets', offsets, 'distance_power'};
%	Rgen = @(dp) Robject(arg{:}, dp);
	Rgen = @(dp) Reg1(arg{:}, dp);
	R0 = Rgen(0);
	R1 = Rgen(1);
	R2 = Rgen(2);
end

if 1
	pr R0.penal(R0, ej(ig.mask))
	pr R1.penal(R1, ej(ig.mask))
	pr R2.penal(R2, ej(ig.mask))
end

if 0
	w0 = reshape(R0.wt, [ig.dim length(R0.offsets)]);
	w1 = reshape(R1.wt, [ig.dim length(R0.offsets)]);
	w2 = reshape(R2.wt, [ig.dim length(R0.offsets)]);
	im(w2), cbar
return
end

%tmp = R0.C * ej(ig.mask);
%return

psf0 = ig.embed(R0.C' * (R0.C * ej(ig.mask)));
psf1 = ig.embed(R1.C' * (R1.C * ej(ig.mask)));
psf2 = ig.embed(R2.C' * (R2.C * ej(ig.mask)));
minmax(psf0)
minmax(psf1)
minmax(psf2)

try0 = ig.embed(R0.cgrad(R0, 1e-2 * ej(R0.mask(:))) / 1e-2);
try1 = ig.embed(R1.cgrad(R1, 1e-2 * ej(R1.mask(:))) / 1e-2);
try2 = ig.embed(R2.cgrad(R2, 1e-2 * ej(R2.mask(:))) / 1e-2);

im pl 2 3
im(1, psf0)
im(2, psf1)
im(3, psf2)
im(4, try0)
im(5, try1)
im(6, try2)


function c = comp_fwt(f,h,a,J,ext)
%COMP_FWT Compute DWT using FWT
%   Usage:  c=comp_fwt(f,h,J,a,Lc,ext);
%
%   Input parameters:
%         f     : Input data - L*W array.
%         h     : Analysis Wavelet filters - cell-array of length *filtNo*.
%         J     : Number of filterbank iterations.
%         a     : Subsampling factors - array of length *filtNo*.
%         ext   : 'per','zero','even','odd' Type of the forward transform boundary handling.
%
%   Output parameters:
%         c     : Cell array of length M. Each element is Lc(m)*W array.
%

% This could be removed with some effort. The question is, are there such
% wavelet filters? If your filterbank has different subsampling factors following the first two filters, please send a feature request.
assert(a(1)==a(2),'First two elements of *a* are not equal. Such wavelet filterbank is not suported.');

% Time-reversed, complex conjugate impulse responses.
filtNo = length(h);
%hCell = cellfun(@(hEl) conj(flipud(hEl.h(:))),h,'UniformOutput',0);
hCell = cellfun(@(hEl) hEl.h(:),h,'UniformOutput',0);

if(strcmp(ext,'per'))
   % Initial shift of the filter to compensate for it's delay.
   % "Zero" delay transform is produced
   % offset = cellfun(@(hEl) 1-numel(hEl.h)-hEl.offset,h);
   offset = cellfun(@(hEl) hEl.offset,h);
elseif strcmp(ext,'valid')
   offset = -cellfun(@(hEl) numel(hEl.h)-1,h);
else
   % No compensation for the filter delay (filters are made causal with respect to the output sequences).
   % This creates relative shift between levels of coefficients.
   % Initial shift determines type of subsampling.
   % This is even subsampling. e.g. subs. [1,2,3,4,5,6] by a factor 3 becomes [3,6]
   % The number of output coefficients depends on it.
   offset = -(a-1);
   % For odd subsampling skip = 0; but it requires slight touches
   % elsewhere.
end

M = (filtNo-1)*J+1;
c = cell(M,1);
runPtr = M-filtNo+2;
ctmp = f;
for jj=1:J
    % Run filterbank
    ctmp = comp_filterbank_td(ctmp,hCell,a,offset,ext);
    % Bookkeeping
    c(runPtr:runPtr+filtNo-2) = ctmp(2:end);
    ctmp = ctmp{1};
    runPtr = runPtr - (filtNo - 1);
end
% Save final approximation coefficients
c{1} = ctmp;


function a = erfc(a)
%ERFC         Gradient complementary error function erfc(a)
%

% written  05/31/13     S.M. Rump
%

  e = 1e-30;
  if 1+e==1-e                           % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end

  N = getappdata(0,'INTLAB_GRADIENT_NUMVAR');
  % factorLB <= 2/sqrt(pi) <= factorUB
  INTLAB_STDFCTS_ERF = getappdata(0,'INTLAB_STDFCTS_ERF');
  factorLB = INTLAB_STDFCTS_ERF.TWO_SQRTPIINF;  % round to nearest
  factorUB = INTLAB_STDFCTS_ERF.TWO_SQRTPISUP;  % ~ 1.12

  % use full(a.x(:)): cures Matlab V6.0 bug
  % a=7; i=[1 1]; x=a(i), b=sparse(a); y=b(i)  yields row vector x but column vector y
  % ax is full anyway
  a.x = full(a.x);
  ax = exp(-a.x(:).^2);
  a.x = erfc(a.x);
  if issparse(a.dx)
    sizeax = size(a.dx,1);
    [ia,ja,sa] = find(a.dx);
    if isa(a.x,'intval')
      adx = times(ax(ia),sa,0);
      a.dx = intval(-factorUB,-factorLB,'infsup') * ...
          intval( sparse(ia,ja,adx.inf,sizeax,N) , sparse(ia,ja,adx.sup,sizeax,N) , 'infsup' );
    else
      a.dx = (-factorLB) * sparse(ia,ja,ax(ia).*sa,sizeax,N);
    end
  else
    if isa(a.x,'intval')
      ax = intval(-factorUB,-factorLB,'infsup') * ax;
      a.dx = a.dx .* ax(:,ones(1,N));
    else
      ax = (-factorLB) * ax;
      a.dx = a.dx .* ax(:,ones(1,N));
    end
  end

  if rndold
    setround(rndold)
  end


function [predict_label,accuracy,prob_estimates] = jdsvm_w(x_train,y_train,x_test,y_test,w,c,g,kernel_type)
%%jdsvm:packaged libsvm,just for easier use.
%%==============================================================================
%%input:
%%------x_train,...,y_test  :   training and testing sets.                               [required]
%%------c                   :   punishment factor(the more,the heavier),default is 1.    [not required]
%%------g                   :   gamma function,default is 1/(#features).                 [not required]
%%------kernel_type         :   kernel type,value is int,the choices are:                [not required]
%%%%----------------0       :   linear
%%%%----------------1       :   polynomial
%%%%----------------2       :   rbf(default)
%%%%----------------3       :   sigmoid
%%------w                   :   weight vector

%%output:
%%------predict_label       :   predicted label vector for test case.
%%------accuracy            :   accuracy
%%------prob_estimates      :   prob estimates
%%==============================================================================
    predict_label = [];
    accuracy = 0;
    prob_estimates = [];
    c_default = 1;
	g_default = 1 / size(x_train,2);
	kernel_type_default = 2;
    weight_default = '';

	c_para = c_default;
	g_para = g_default;
	kernel_para = kernel_type_default;
    weight_para = weight_default;

	switch nargin
		case 4
			c_para = c_default;
			g_para = g_default;
			kernel_para = kernel_type_default;
            weight_para = weight_default;
		case 5
			c_para = c_default;
			g_para = g_default;
			kernel_para = kernel_type_default;
            weight_para = w;
		case 6
			c_para = c;
			g_para = g_default;
			kernel_para = kernel_type_default;
            weight_para = w;
		case 7
			c_para = c;
			g_para = g;
			kernel_para = kernel_type;
            weight_para = w;
        case 8
			c_para = c;
			g_para = g;
			kernel_para = kernel_type;
            weight_para = w;
        otherwise
            fprintf('++++++Fatal error!Please check the input!');
            return
	end

	options = ['-c ',num2str(c_para),' -g ',num2str(g_para),' -t ',num2str(kernel_para),weight_para];
	model = libsvmtrain(y_train,x_train,options);
	[predict_label,accuracy,prob_estimates] = libsvmpredict(y_test,x_test,model);
	accuracy = accuracy(1) / 100;

end


function gf = gradddivHexp(Hkt_r, varargin)
% function gf = gradddivH(Hkt_r, varargin)
% gf is 1 x k*t row vector
% Hkt_r = reshape(Hkt,1,k*t) -> row vector
% Vxt = varargin{1};  %data
% Wxk = varargin{2};  %W matrix
% Wxk_fix = varargin{3}; %fixed part of the Wxk matrix (e.g. background) ->rows
% Hkt_fix = varargin{4}; %fixed part (lines) of the H matrix (e.g. background)

alphaH=1; %for now....
Vxt = varargin{1};  %data
Wxk_tmp = varargin{2};  %W matrix
Wxk_fix = varargin{3}; %fixed part of the Wxk matrix (e.g. background) ->rows
Hkt_fix = varargin{4}; %fixed part (lines) of the H matrix (e.g. background)
peval = varargin{5}; %parameters

if ~isfield(peval, 'w_lambda') peval.w_lambda=0; end

t=size(Vxt,2);
k=length(peval.h_dovec);

Hkt_tmp = exp(reshape(Hkt_r,k,t));

Wxk = zeros(peval.numpix, peval.ncomp+1);
Hkt = zeros(peval.ncomp+1, peval.nt);

Wxk(:,peval.w_dovec)=Wxk_tmp;
Hkt(peval.h_dovec,:)=Hkt_tmp;

Wxk(:,peval.w_fixvec)=Wxk_fix;
Hkt(peval.h_fixvec,:)=Hkt_fix;

deltasum=sum(sum(Wxk_tmp,1))-k;
if and(~isempty(Wxk_tmp), abs(deltasum)>10^-6)
    error('Wxk is not correctly normalized! (sum(Wxk_tmp,1)<>1)\n sum(Wxk_tmp,1)=%f',deltasum)
end

% gfkt = (1-Wxk'*(Vxt./(Wxk*Hkt)))*alphaH.*Hkt; %d/dh(d-divergence)
% fprintf('!!!test in gradddivHexp!!!')
denomin=(sum((sum(Hkt,2)).^2))^2;
sumt_h = repmat(sum(Hkt,2),1, peval.nt);
gfkt = (1-Wxk'*(Vxt./(Wxk*Hkt)))*alphaH.*Hkt - 2*sumt_h/denomin; %d/dh(d-divergence)
% one is tehre because Wxt is normalized: sum(Wxt,1)=1
gf=reshape(gfkt(1:k,:),1,k*t); %making row vector


function f = fix(f)
%FIX   Round a CHEBFUN pointwise toward zero.
%   G = FIX(F) returns the CHEBFUN G such that G(x) = FIX(F(x)) for each x in
%   F.domain.
%
%   If F is complex, then the G = FIX(REAL(F)) + 1i*FIX(IMAG(F)).
%
% See also ROUND, CEIL, FLOOR.

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

% Deal with the trivial empty case:
if ( isempty(f) )
    return
end

% Deal with unbounded functions:
if ( ~isfinite(f) )
    error('CHEBFUN:CHEBFUN:fix:inf', ...
        'fix() is not defined for functions which diverge to infinity.');
end

% Deal with complex-valued functions:
if ( ~isreal(f) )
    if ( isreal(1i*f) )
        f = 1i*fix(imag(f));
    else
        f = fix(real(f)) + 1i*fix(imag(f));
    end
    return
end

% Find all the integer crossings for f:
mmvals = minandmax(f); % [TODO]: Only need a good bound?
minf = min(mmvals(1, :));
maxf = max(mmvals(2, :));
range = floor([minf, maxf]);
for k = (range(1)+1):range(2)
    f = addBreaksAtRoots(f - k) + k;
end

% Loop over the FUNs:
for k = 1:numel(f.funs)
    f.funs{k} = fix(f.funs{k});
end

% Fix the pointValues:
f.pointValues = fix(f.pointValues);

% Simplify the result:
f = merge(f);

end


%BRIGHTEDGES  Bright edges detector
%
%     edge = cv.BrightEdges(img)
%     edge = cv.BrightEdges(img, 'OptionName',optionValue, ...)
%
% ## Input
% * __img__ input color image.
%
% ## Output
% * __edge__ output edge image.
%
% ## Options
% * __Contrast__ default 1
% * __ShortRange__ default 3
% * __LongRange__ default 9
%
% The function implements a new way of detecting edges used is low resolution
% image object recognition in real projects (e.g. 50cm per pixel). It corrects
% surfaces for objects partially under a lighting shadow, and reveal low
% visibility edges. The result is used to feed further object detection
% processes particularly in context where small edges or details are important
% for object discrimination.
%
% The function provides an implementation of an equalized absolute difference
% of blurs, and an optional further treatment to contrast edges based on
% finding local minimum along at least two directions. The local minimum
% detection contrast is a parameter, using 10 as default (10 on 255 maximum).
% The kernel size for the gaussian and the average blur can be passed as
% parameters too.
%
% See also: cv.Canny, cv.blur, cv.GaussianBlur, cv.equalizeHist
%


function [blocks,idx] = my_im2col(I,blkSize,slidingDis);
if (slidingDis==1)
    blocks = im2col(I,blkSize,'sliding');
    idx = [1:size(blocks,2)];
    return
end

idxMat = zeros(size(I)-blkSize+1);
idxMat([[1:slidingDis:end-1],end],[[1:slidingDis:end-1],end]) = 1; % take blocks in distances of 'slidingDix', but always take the first and last one (in each row and column).
idx = find(idxMat);
[rows,cols] = ind2sub(size(idxMat),idx);
blocks = zeros(prod(blkSize),length(idx));
for i = 1:length(idx)
    currBlock = I(rows(i):rows(i)+blkSize(1)-1,cols(i):cols(i)+blkSize(2)-1);
    blocks(:,i) = currBlock(:);
end


function scores= crossValidate(data,labels,obj,binVals,bins,params)

k = params.CVfolds;

boostIterations = 100;
% Learn classifier with all the data.

numEx = size(data,1);
posEx = labels == 1;
posCum = cumsum(posEx)/nnz(posEx);
negEx = ~posEx;
negCum = cumsum(negEx)/nnz(negEx);
numPos = sum(posEx);
numNeg = sum(negEx);

scores = zeros(1,numEx);
if numPos<1 || numNeg<1,
  obj.setStatus('Training set has only one kind of labels');
  return;
end

modLabels = sign( (labels==1)-0.5);

% rr = 1:numEx;
% splitPt = randi(numEx);
% rr = [rr(splitPt:end) rr(1:splitPt-1)];
% bStarts = round(linspace(1,numEx+1,k+1));

for bno = 1:k
%   curBlock = rr(bStarts(bno):bStarts(bno+1)-1);
%   curTest = ismember(1:numEx,curBlock);
%   curTrain = ~curTest;

  curPos = (posCum <= (bno/k)) & (posCum > ( (bno-1)/k)) & posEx;
  curNeg = (negCum <= (bno/k)) & (negCum > ( (bno-1)/k)) & negEx;

  curTest = curPos | curNeg ;
  curTrain = ~curTest;

  curTrainLabels = modLabels(curTrain);

  wt = getWeights(curTrainLabels);
  tt = tic;
  [~,curModel] = loglossboostLearnMod(data(curTrain,:),curTrainLabels,...
    boostIterations,wt,binVals,bins(:,curTrain),params);
  tScores = myBoostClassify(data(curTest,:),curModel);
  scores(curTest) = tScores;

  etime = toc(tt);
  obj.SetStatus('%d%% cross validation done. Time Remaining:%ds ',...
    round( bno/k*100), round((k-bno)*etime));
  drawnow();
end


% absolute azimuthal angle to from fly to closest fly according to type
function [data,units] = compute_absanglefrom1to2(trx,n,type)

flies = trx.exp2flies{n};
nflies = numel(flies);
data = cell(1,nflies);

for i1 = 1:nflies,
  fly1 = flies(i1);
  data{i1} = abs(trx(fly1).(['anglefrom1to2_',type]));
end

units = parseunits('rad');


function [xEst,PEst,logLikes]=singleScanUpdate(xHyp,PHyp,A,algSel1,algSel2,param3)
%%SINGLESCANUPDATE Perform the measurement update step in a single-scan
%                  tracking algorithm that uses Gaussian approximations
%                  to represent the target state before and after the
%                  measurement update given a matrix of likelihoods and a
%                  set of track update hypotheses. The algorithm can be a
%                  global nearest neighbor (GNN) update, a joint
%                  probabilistic data association (JPDA) update, a JPDA*
%                  update, a GNN-JPDA update, or approximate JPDA and
%                  GNN-JPDA updates as well as a naive nearest neighbor
%                  algorithm. In all instances except for the naive nearest
%                  neighbor algorithm, it is assumed that each measurement
%                  can be assigned to at most one target, and each target
%                  can be assigned to at most one measurement.
%
%INPUTS: xHyp An xDimXnumTarXnumHyp set of track states updated with each
%             of the (numHyp-1) measurements for each of the numTar
%             targets, with the last hypothesis being the update for the
%             missed detection hypothesis. The order of the measurement
%             hypotheses is the same as the ordering of the likelihoods in
%             the columns of the A matrix.
%        PHyp An xDimXxDimXnumTarXnumHyp set of covariance matrices for
%             each of the track states for each of the targets updated
%             conditioned on each of the numHyp measurements with the last
%             one being for the missed detection hypothesis.
%           A A matrix of positive likelihoods or likelihood ratios (NOT
%             log-likelihood ratios). A is a numTar X (numMeas+numTar)
%             matrix of all-positive likelihoods or likelihood ratios for
%             assigning the target specified by the row to the measurement
%             specified by the column. Columns > numMeas hold
%             missed-detection likelihoods. Thus, off-diagonal terms for
%             columns > numMeas should be set to 0 and the diagonal terms
%             set to the costs of a missed detection for each given
%             target.
%     algSel1 An optional parameter that selects the assignment algorithm
%             to use. Approximate variants of the algorithms also use
%             algSel2. If this parameter and the next parameter are both
%             omitted, then the algorithm is chosen as described below.
%             Possible values are
%             0) GNN-JPDA
%             1) JPDA
%             2) GNN
%             3) Parallel single-target PDAs
%             4) Naive nearest neighbor
%             5) JPDA*
%             6) Approximate GNN-JPDA
%             7) Approximate JPDA
%             8) Set JPDA
%             9) Naive nearest neighbor JPDA
%             10) Approximate naive nearest neighbor JPDA
%     algSel2 An optional parameter that further specifies the algorithm
%             used when algSel1=6-7. If omitted but algSel1 is specified, a
%             default value of 0 is used. If both algSel1 and algSel2
%             are omitted, the algorithm is chosen as described below.
%             algSel2 chooses the approximation for the beta terms in the
%             function calc2DAssignmentProbsApprox (the approxType input)
%             when using algSel1=6,7; the comments to that function
%             describe the values it can take. If algSel1=8 (Set JPDAF),
%             this is the number of position components in the state (the
%             posDim input in the calcSetJPDAUpdate function).
%      param3 For the case where algSel1=6,7 and algSel2=0, this is the
%             optional delta input to the function
%             calc2DAssignmentProbsApprox. If omitted or an empty matrix
%             is passed, the default value for that function is used.
%
%OUTPUTS: xEst An xDimXnumTar matrix of updated target states.
%         PEst An xDimXxDimXnumtar matrix of updated covariance matrices
%              for the targets.
%      logLike The numTarX1 set of logarithms of the likelihood (ratio)
%              function for the update for each target. If A contains
%              likelihoods, then this is the logarithm of the likelihood;
%              if A constains likelihood ratios, then this is a logarithm
%              of a likelihood ratio, For hard assignments, this is clearly
%              defined (it is just the entry in A assigned to that target).
%              For soft assignments in the JPDA, JPDA*, PDA, and
%              approximate JPDA this is the expected value of the
%              log-likelihood computed as using the beta terms. If the Set
%              JPDAF is chosen, an empty matrix is returned for logLike.
%
%If both algSel1 and algSel2 are omitted, then the algorithm chosen depends
%on the size of A. If A has only one row (one target), then the JPDA is
%used. Otherwise, the approximate GNN-JPDA with algSel2=4 is used (which is
%actually exact if the number of targets is two).
%
%The JPDA is discussed in Chapter 6.2 of [1], and the PDA is discussed in
%various other sections as the JPDA is a generalization of the PDA. The
%JPDA* is discussed in [2]. The concept of the GNN-JPDA is described (but
%not named) in [3]. The GNN-JPDA consists of using the global nearest
%neighbor estimate with a covariance matrix computed as in the JPDA. Thus
%the same approximations that can be used in the JPDA can be used in the
%GNN-JPDA. The Set JPDA is described in [4].
%
%The approximate routines simply change how the target-measurement
%association probabilities are computed. They are described in the comments
%to the function calc2DAssignmentProbsApprox.
%
%Naive nearest neighbor association consists of just choosing the most
%likely assignment of a target to a measurement or missed detection without
%regard to whether one measurmeent is assigned to multiple targets.
%
%One possible way to form the A matrix is to use likelihood ratios going
%into the dimensionless score function from [5]. This is implemented for a
%standard Cartesian/Gaussian model using makeStandardCartOnlyLRMatHyps.
%
%REFERENCES:
%[1] Y. Bar-Shalom, P. K. Willett, and X. Tian, Tracking and Data Fusion.
%    Storrs, CT: YBS Publishing, 2011.
%[2] H. A. Blom and E. A. Bloem, "Probabilistic data association avoiding
%    track coalescence," IEEE Transactions on Automatic Control, vol. 45,
%    no. 2, pp. 247-259, Feb. 2000.
%[3] O. E. Drummond, "Best hypothesis target tracking and sensor fusion,"
%    in Proceedings of SPIE: Signal and Data Processing of Small Targets
%    Conference, vol. 3809, Denver, CO, Oct. 1999, pp. 586-600.
%[4] L. Svensson, D. Svensson, M. Guerriero, and P. Willett, "Set JPDA
%    filter for multitarget tracking," IEEE Transactions on Signal
%    Processing, vol. 59, no. 10, pp. 4677-4691, Oct. 2011.
%[5] Y. Bar-Shalom, S. S. Blackman, and R. J. Fitzgerald, "Dimensionless
%    score function for multiple hypothesis tracking," IEEE Transactions on
%    Aerospace and Electronic Systems, vol. 43, no. 1, pp. 392-400, Jan.
%    2007.
%
%March 2015 David F. Crouse, generalizing the basic JPDAF code of David
%Karnick to many more algorithms, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

    numTar=size(A,1);
    numMeas=size(A,2)-numTar;
    numHyp=numMeas+1;

    if(nargin<6)
        param3=[];
    end

    if(nargin<4||isempty(algSel1))
        if(numTar==1)
            algSel1=1;
        else
            algSel1=6;
            algSel2=4;
        end
    end

    if(nargin==4||isempty(algSel2))%If only algSel2 was omitted.
        algSel2=0;
    end
    xDim=size(xHyp,1);

    xEst=zeros(xDim,numTar);
    PEst=zeros(xDim,xDim,numTar);
    switch(algSel1)
        case 0%GNN-JPDA
            %Perform 2D assignment
            ALog=log(A);
            tar2Meas=assign2D(ALog,true);
            logLikes=zeros(numTar,1);
            for curTar=1:numTar
                logLikes(curTar)=ALog(curTar,tar2Meas(curTar));
            end

            %Adjust for the index of the missed detection hypothesis.
            sel=tar2Meas>numMeas;
            tar2Meas(sel)=numMeas+1;

            %Set the target estimate to the ML estimate.
            for curTar=1:numTar
                maxIdx=tar2Meas(curTar);
                xEst(:,curTar)=xHyp(:,curTar,maxIdx);
            end

            %Compute the JPDAF probabilities.
            beta=calc2DAssignmentProbs(A,true);

            %Compute the covariance matrix in the manner of the JPDAF, but
            %about the ML estimate, so the result is a MSE matrix estimate.
            for curTar=1:numTar
                x=reshape(xHyp(:,curTar,:),[xDim,numHyp,1]);
                P=reshape(PHyp(:,:,curTar,:),[xDim,xDim,numHyp]);
                [~,PEst(:,:,curTar)]=calcMixtureMoments(x,beta(curTar,:),P,xEst(:,curTar));
            end
        case 1%JPDA
            %Compute the JPDAF probabilities.
            beta=calc2DAssignmentProbs(A,true);

            %The JPDAF is just the mixture mean and covariance matrix.
            for curTar=1:numTar
                x=reshape(xHyp(:,curTar,:),[xDim,numHyp,1]);
                P=reshape(PHyp(:,:,curTar,:),[xDim,xDim,numHyp]);
                [xEst(:,curTar),PEst(:,:,curTar)]=calcMixtureMoments(x,beta(curTar,:),P);
            end

            %If a likelihood is desired, then find the weighted likelihood.
            if(nargout>2)
                logLikes=logLikefromABeta(A,beta);
            end
        case 2%GNN
            %Perform 2D assignment
            ALog=log(A);
            tar2Meas=assign2D(ALog,true);
            logLikes=zeros(numTar,1);
            for curTar=1:numTar
                logLikes(curTar)=ALog(curTar,tar2Meas(curTar));
            end
            %Adjust for the index of the missed detection hypothesis.
            sel=tar2Meas>numMeas;
            tar2Meas(sel)=numMeas+1;

            for curTar=1:numTar
                maxIdx=tar2Meas(curTar);
                xEst(:,curTar)=xHyp(:,curTar,maxIdx);
                PEst(:,:,curTar)=PHyp(:,:,curTar,maxIdx);
            end
        case 3%Paralle single-target PDAs
            %This is just a bunch of independent PDAFs for each target.
            beta=zeros(numTar,numHyp);
            for curTar=1:numTar
                hypIdx=[1:numMeas,numMeas+curTar];
                beta(curTar,:)=calc2DAssignmentProbs(A(curTar,hypIdx),true);
                x=reshape(xHyp(:,curTar,:),[xDim,numHyp,1]);
                P=reshape(PHyp(:,:,curTar,:),[xDim,xDim,numHyp]);
                [xEst(:,curTar),PEst(:,:,curTar)]=calcMixtureMoments(x,beta(curTar,:),P);
            end

            %If a likelihood is desired, then find the weighted likelihood.
            if(nargout>2)
                logLikes=logLikefromABeta(A,beta);
            end
        case 4%Na�ve nearest neighbor
            %The assignment for each target
            logLikes=zeros(numTar,1);
            for curTar=1:numTar
                [maxVal,maxIdx]=max(A(curTar,:));
                %If the missed detection hypothesis is the most likely.
                if(maxIdx>numMeas)
                    maxIdx=numMeas+1;
                end

                xEst(:,curTar)=xHyp(:,curTar,maxIdx);
                PEst(:,:,curTar)=PHyp(:,:,curTar,maxIdx);

                logLikes(curTar)=log(maxVal);
            end
        case 5%JPDA*
            %This is the same as the JPDA, except the method for computing
            %the betas is different.
            %Compute the JPDAF* probabilities.
            beta=calcStarBetasBF(A);

            %The JPDAF* is just the mixture mean and covariance matrix.
            for curTar=1:numTar
                x=reshape(xHyp(:,curTar,:),[xDim,numHyp,1]);
                P=reshape(PHyp(:,:,curTar,:),[xDim,xDim,numHyp]);
                [xEst(:,curTar),PEst(:,:,curTar)]=calcMixtureMoments(x,beta(curTar,:),P);
            end

            %If a likelihood is desired, then find the weighted likelihood.
            if(nargout>2)
                logLikes=logLikefromABeta(A,beta);
            end
        case 6%Approximate GNN-JPDA
            %This is the same as the GNN-JPDA, except the method for
            %computing the betas is different.

            %Perform 2D assignment
            ALog=log(A);
            tar2Meas=assign2D(ALog,true);
            logLikes=zeros(numTar,1);
            for curTar=1:numTar
                logLikes(curTar)=ALog(curTar,tar2Meas(curTar));
            end

            %Adjust for the index of the missed detection hypothesis.
            sel=tar2Meas>numMeas;
            tar2Meas(sel)=numMeas+1;

            %Set the target estimate to the ML estimate
            for curTar=1:numTar
                maxIdx=tar2Meas(curTar);
                xEst(:,curTar)=xHyp(:,curTar,maxIdx);
            end

            %Compute the approximate JPDAF probabilities.
            beta=calc2DAssignmentProbsApprox(A,algSel2,true,param3);

            %Compute the covariance matrix in the manner of the JPDAF, but
            %about the ML estimate, so the result is a MSE matrix estimate.
            for curTar=1:numTar
                x=reshape(xHyp(:,curTar,:),[xDim,numHyp,1]);
                P=reshape(PHyp(:,:,curTar,:),[xDim,xDim,numHyp]);
                [~,PEst(:,:,curTar)]=calcMixtureMoments(x,beta(curTar,:),P,xEst(:,curTar));
            end
        case 7%Approximate JPDA
            %This is the same as the JPDA, except the method for computing
            %the betas is different.

            %Compute the approximate JPDAF probabilities.
            beta=calc2DAssignmentProbsApprox(A,algSel2,true,param3);

            %The JPDAF* is just the mixture mean and covariance matrix.
            for curTar=1:numTar
                x=reshape(xHyp(:,curTar,:),[xDim,numHyp,1]);
                P=reshape(PHyp(:,:,curTar,:),[xDim,xDim,numHyp]);
                [xEst(:,curTar),PEst(:,:,curTar)]=calcMixtureMoments(x,beta(curTar,:),P);
            end

            %If a likelihood is desired, then find the weighted likelihood.
            if(nargout>2)
                logLikes=logLikefromABeta(A,beta);
            end
        case 8%Set JPDAF
            %This is the same as the JPDA, except the method for computing
            %the betas is different.
            [xEst,PEst]=calcSetJPDAUpdate(xHyp,PHyp,A,AlgSel2);

            if(nargout>2)
                logLikes=[];
            end
        case 9%Na�ve nearest neighbor-JPDA
            %The assignment for each target
            logLikes=zeros(numTar,1);
            for curTar=1:numTar
                [maxVal,maxIdx]=max(A(curTar,:));

                %If the missed detection hypothesis is the most likely.
                if(maxIdx>numMeas)
                    maxIdx=numMeas+1;
                end

                xEst(:,curTar)=xHyp(:,curTar,maxIdx);
                logLikes(curTar)=log(maxVal);
            end

            %Compute the JPDAF probabilities.
            beta=calc2DAssignmentProbs(A,true);

            %Compute the covariance matrix in the manner of the JPDAF, but
            %about the ML estimate, so the result is a MSE matrix estimate.
            for curTar=1:numTar
                x=reshape(xHyp(:,curTar,:),[xDim,numHyp,1]);
                P=reshape(PHyp(:,:,curTar,:),[xDim,xDim,numHyp]);
                [~,PEst(:,:,curTar)]=calcMixtureMoments(x,beta(curTar,:),P,xEst(:,curTar));
            end
        case 10%Approxmate na�ve nearest neighbor-JPDA
            %This is the same as the na�ve nearest neighbor-JPDA, except
            %the method for computing the betas is different.

            %The assignment for each target
            logLikes=zeros(numTar,1);
            for curTar=1:numTar
                [maxVal,maxIdx]=max(A(curTar,:));

                %If the missed detection hypothesis is the most likely.
                if(maxIdx>numMeas)
                    maxIdx=numMeas+1;
                end

                xEst(:,curTar)=xHyp(:,curTar,maxIdx);
                logLikes(curTar)=log(maxVal);
            end

            %Compute the approximate JPDAF probabilities.
            beta=calc2DAssignmentProbsApprox(A,algSel2,true,param3);

            %Compute the covariance matrix in the manner of the JPDAF, but
            %about the ML estimate, so the result is a MSE matrix estimate.
            for curTar=1:numTar
                x=reshape(xHyp(:,curTar,:),[xDim,numHyp,1]);
                P=reshape(PHyp(:,:,curTar,:),[xDim,xDim,numHyp]);
                [~,PEst(:,:,curTar)]=calcMixtureMoments(x,beta(curTar,:),P,xEst(:,curTar));
            end
        otherwise
            error('Unknown algorithm selected.')
    end
end

function logLike=logLikefromABeta(A,beta)
    numTar=size(A,1);
    numHyp=size(beta,2);

    %Create a copy of A with the missed detection likelihoods
    %in the last column and  take the logarithm of it.
    ANew=zeros(numTar,numHyp);
    ANew(:,1:(numHyp-1))=A(:,1:(numHyp-1));
    ANew(:,numHyp)=diag(A(:,numHyp:(numHyp+numTar-1)));
    ANew=log(ANew);

    %Multiply each log-likelihood by the probability of it
    %being correct.
    ANew=ANew.*beta;
    %Get rid of non-finite terms, which should generally only
    %arise in things with zero likelihood.
    sel=~isfinite(ANew);
    ANew(sel)=0;
    %The likelihood is the sum of all of the weighted
    %log-likelihoods.
    logLike=sum(ANew,2);
end

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


function [im] = blend(im1, im2, c1, c2)

[height, width, ~] = size(im1);

alpha1 = zeros(size(im1));
alpha2 = zeros(size(im2));

im1 = im2double(im1);
im2 = im2double(im2);

im = (alpha2 .* im2 + alpha1 .* im1) ./ (alpah1 + alpha2);


function minus_log_dnsty = blp_opt_hyperpara(hyperpara,hh,prior,olsreg_,F,G,Fo,positions_nylags,position_constant)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 'blp_opt_hyperpara' computes the marginal likelihood over the
% hyperparameters of for the LP

% Inputs:
% - hyperpara, prior shrinkage
% likelihood

% Output: marginal data density

% Filippo Ferroni, 3/21/2020
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

hyperpara       = exp(hyperpara);
log_dnsty       = blp_ml(hyperpara,hh,prior,olsreg_,F,G,Fo,positions_nylags,position_constant);
minus_log_dnsty = -log_dnsty;


function [nota1, nota2]=check_my_kinematics(robot)

   %q=zeros(1, robot.DOF) + 0.05*rand(1, robot.DOF);
   q=[0.1 0.1 0.1 0.1 0.1 0.1] + 0.05*rand(1, robot.DOF);

   %Now compute direct kinematics for this position q
   T = directkinematic(robot, q);

   %Set to zero if you want to see the robot transparent
   robot.graphical.draw_transparent=1;
   robot.graphical.draw_axes=1;

   qinv = inversekinematic(robot, T);

resultado_q = [];
resultado_T = [];

for i=1:size(qinv,2),
    Ti = directkinematic(robot, qinv(:,i)); %Ti is constant for the different solutions

    resultado_q= [resultado_q sum(qinv(:,i)-q')];
    resultado_T= [resultado_T sum(sum(T-Ti))];

    %now draw the robot to see the solution
    drawrobot3d(robot, qinv(:,i))

    pause(1)
end

indice1 = find(abs(resultado_q) < 0.05);
indice2 = sum(abs(resultado_T));

if size(indice1,2)==0
   nota1=0;
else
   nota1=10;
end

if indice2>0.01
    nota2=0;
else
    nota2=10;
end


% plotfresp() - Plot a filter's impulse, step, magnitude, and phase response
%
% Usage:
%   >> plotfresp(b, a, nfft, fs, dir);
%
% Inputs:
%   b     - vector numerator coefficients
%
% Optional inputs:
%   a     - scalar or vector denominator coefficients (IIR support is
%           experimental!) {default 1}
%   nfft  - scalar number of points {default 512}
%   fs    - scalar sampling frequency {default 1}
%   dir   - string filter direction {default 'onepass'}
%
% Author: Andreas Widmann, University of Leipzig, 2005
%
% See also:
%   firws

%123456789012345678901234567890123456789012345678901234567890123456789012

% Copyright (C) 2005-2014 Andreas Widmann, University of Leipzig, widmann@uni-leipzig.de
%
% 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
%
% $Id$

function plotfresp(b, a, nfft, fs, dir)

if nargin < 5 || isempty(dir)
    dir = 'onepass';
end
if nargin < 4  || isempty(fs)
    fs = 1;
end
if nargin < 3 || isempty(nfft)
    nfft = 512;
end
if nargin < 2 || isempty(a)
    a = 1;
end
if nargin < 1
    ft_error('Not enough input arguments.');
end

% FIR?
if isscalar(a) && a == 1
    isFIR = true;
else
    isFIR = false;
end

% Linear phase FIR
if isFIR && all(b(:)' == fliplr(b(:)')) % TODO: antisymmetric
    isLinPhaseFir = true;
else
    isLinPhaseFir = false;
end

% Twopass/zerophase?
if strncmp('twopass', dir, 7)
    isTwopass = true;
    isZerophase = true;
elseif strcmp('onepass-zerophase', dir) || strcmp('onepass-reverse-zerophase', dir)
    if ~isLinPhaseFir
        ft_error('Onepass-zerophase filtering is only allowed for linear-phase FIR filters.')
    end
    isTwopass = false;
    isZerophase = true;
else
    isTwopass = false;
    isZerophase = false;
end

% Impulse response
if isFIR
    impresp = b(:)';
else
    if ~exist('impz', 'file')
        ft_warning('Plotting IIR filter responses requires signal processing toolbox.')
        return
    end
    impresp = impz(b, a)';
end

% Twopass
if isTwopass
    impresp = conv(impresp, fliplr(impresp));
end
n = length(impresp);

% Zerophase
if isZerophase
    groupdelay = (n - 1) / 2;
    x = -groupdelay:groupdelay;
else
    x = 0:n - 1;
end

nfft = max([2^ceil(log2(n)) nfft]); % Do not truncate impulse response
f = linspace(0, fs / 2, nfft / 2 + 1);
z = fft(impresp, nfft);
z = z(1:nfft / 2 + 1);

% Find open figure window
H = findobj('Tag', 'plotfiltresp', 'type', 'figure');
if ~isempty(H)
    figure(H);
else
    H = figure;
    set(H, 'Tag', 'plotfiltresp');
    posArray = get(H, 'Position');
    posArray(3) = posArray(4) * 1.6;
    set(H, 'Position', posArray);
end

% Formatting
titlePropArray = {'Fontweight', 'bold'};
axisPropArray = {'NextPlot', 'add', 'XGrid', 'on', 'YGrid', 'on', 'Box', 'on'};

% Impulse resonse
ax(1) = subplot(2, 3, 1, axisPropArray{:});
stem(x, impresp, 'fill')
title('Impulse response', titlePropArray{:});
ylabel('Amplitude');

% Step response
ax(4) = subplot(2, 3, 4, axisPropArray{:});
stem(x, cumsum(impresp), 'fill');
title('Step response', titlePropArray{:});
ylimArray = ylim;
if ylimArray(2) < -ylimArray(1) + 1;
    ylimArray(2) = -ylimArray(1) + 1;
    ylim(ylimArray);
end
xMin = []; xMax = [];
childrenArray = get(ax(4), 'Children');
for iChild =1:length(childrenArray)
    xData = get(childrenArray(iChild), 'XData');
    xMin = min([xMin min(xData)]);
    xMax = max([xMax max(xData)]);
end
set(ax([1 4]), 'XLim', [xMin xMax]);
ylabel('Amplitude');

% Magnitude response
ax(2) = subplot(2, 3, 2, axisPropArray{:});
plot(f, abs(z));
title('Magnitude response', titlePropArray{:});
ylabel('Magnitude (linear)');

ax(5) = subplot(2, 3, 5, axisPropArray{:});
plot(f, 20 * log10(abs(z)));
title('Magnitude response', titlePropArray{:});
ylimArray = ylim;
if ylimArray(1) < -200
    ylimArray(1) = -200;
    ylim(ylimArray);
end
ylabel('Magnitude (dB)');

% Phase response
ax(3) = subplot(2, 3, 3, axisPropArray{:});
phaseresp = unwrap(angle(z));
if isZerophase % Correct delay for zero-phase FIR filter?
    delay = -f / fs * groupdelay * 2 * pi;
    phaseresp = phaseresp - delay;
    phaseresp = mod(round(phaseresp / pi), 2) * pi; % Avoid rounding errors; linear-phase FIR only!
end
plot(f, phaseresp);
title('Phase response', titlePropArray{:});
ylabel('Phase (rad)');

% Formatting
xlabelArray = get(ax(1:5), 'XLabel');
if fs == 1
    set([xlabelArray{[2 3 5]}], 'String', 'Normalized frequency (2 \pi rad / sample)');
else
    set([xlabelArray{[2 3 5]}], 'String', 'Frequency (Hz)');
end
set([xlabelArray{[1 4]}], 'String', 'n (samples)');
set(ax([2 3 5]), 'XLim', [0 fs / 2]);
set(ax(1:5), 'ColorOrder', circshift(get(ax(1), 'ColorOrder'), -1));

end


report_this_filefun(mfilename('fullpath'));

b = a;

% Hier vielleicht dx, dy aendern ...
ve = [];dx = 0.1; dy = 0.1;

x0 = min(a.Longitude);
x1 = max(a.Longitude);
y0 = min(a.Latitude);
y1 = max(a.Latitude);

for x = x0:dx:x1
    for y = y0:dy:y1
        ve =    [ ve ; x y ];
    end
end

le = length(ve);
Y0 = zeros(le,1);

% Hier muss die richtige abminderung rein ...
for i = 1:length(b)
    di2 = deg2km((distance(ve(:,2),ve(:,1),repmat(b(i,2),le,1),repmat(b(i,1),le,1))));
    R = di2;
    r = sqrt(R.^2 + 5.57^2);
    M = b(i,6); % wenn mit felhler: + dM  (random mit standart)
    Y = -0.136 + 0.229*(M-6) - 0.778 * log10(r) ;
    Y = 10.^Y;
    c = [Y , Y0];
    mapga = max(c');
    Y0 = mapga';
end

mapga = mapga';

l1 = length(x0:dx:x1);
l2 = length(y0:dy:y1);

re = reshape(mapga,l2,l1);
rey = reshape(ve(:,2),l2,l1);
rex = reshape(ve(:,1),l2,l1);

figure
pcolor(rex,rey,re)
hold on
shading interp
overlay
colorbar


% DEFINE RAY
function [rayObj] = ray(origin,direction,magnitude)
% Define a ray with a given a origin and direction.
assert(numel(origin) == 3 && numel(direction) == 3,'Parameters must be 3D');
if nargin < 3
    magnitude = 1;
end
% NORMALISE THE DIRECTION VECTOR
direction = direction/norm(direction);
% DEFINE THE RAY STRUCTURE
rayObj = struct('origin',origin,...
                'direction',direction,...
                'magnitude',magnitude);
end


function G = getGroupSuperColor27(I, param)

    N = size(I,1) * size(I, 2);

    g = sparse(N, N);

    imlab = vl_xyz2lab(vl_rgb2xyz(I)) ;
    imlab = single(imlab);
    slicParam = param.superpixel.slicParam;

    groupCount = 0;
    for i=1:size(slicParam,1)

        segments = vl_slic(imlab, slicParam(i,1),  slicParam(i,2)) ;
        segments = segments(:);

        maxLabel = max(segments);
        for j = 0:maxLabel
            g(:, groupCount+j+1) = segments==j;
        end
        groupCount  = groupCount+ maxLabel+1;

    end

    % croping G
    g =g(:, 1:groupCount);

    G = [g;g;g];

end