function varargout = log(varargin)

switch class(varargin{1})

    case 'sdpvar'
         % Try to detect logsumexp construction etc
        varargout{1} = check_for_special_cases(varargin{:});
        % Nope, then just define this logarithm
        if isempty(varargout{1})
            varargout{1} = InstantiateElementWise(mfilename,varargin{:});
        end

    case 'char'

        operator = CreateBasicOperator('concave','increasing','callback');
        operator.derivative = @(x)(1./(abs(x)+eps));
        operator.inverse = @(x)(exp(x));
        operator.domain = [0 inf];
        operator.singularity = [0 -inf -inf] ;

        varargout{1} = [];
        varargout{2} = operator;
        varargout{3} = varargin{3};

    otherwise
        error(['SDPVAR/' upper(mfilename) ' called with weird argument']);
end

function f = check_for_special_cases(x)
f = [];
% Check for log(1+x)
base = getbase(x);
if all(base(:,1)==1)
    f = slog(x-1);
    return;
end
% Check if user is constructing log(sum(exp(x)))
if base(1)~=0
    return
end
if ~all(base(2:end)==1)
    return
end
modelst = yalmip('extstruct',getvariables(x));
if isempty(modelst)
    return;
end
if length(modelst)==1
    models{1} = modelst;
else
    models = modelst;
end
% LOG(DET(X))
if length(models)==1
    if strcmp(models{1}.fcn,'det_internal')
        n = length(models{1}.arg{1});
        try
            f = logdet(reshape(models{1}.arg{1},sqrt(n),sqrt(n)));
        catch
        end
        return
    end
end
% LOG(EXP(x1)+...+EXP(xn))
for i = 1:length(models)
    if ~strcmp(models{i}.fcn,'exp')
        return
    end
end
p = [];
for i = 1:length(models)
    p = [p;models{i}.arg{1}];
end
f = logsumexp(p);


% LINESEG - Form straight line segements from an edge list.
%
% Usage: [seglist, nedgelist] = lineseg(edgelist, tol, angtol, linkrad)
%
% Arguments:  edgelist - Cell array of edgelists (row col) coords.
%             tol      - Maximum deviation from straight line before a
%                        segment is broken in two (measured in pixels).
%             angtol   - Angle tolerance used when attempting to merge line
%                        segements (radians).
%             linkrad  - Maximum distance between end points of line
%                        segments for segments to be elegible for
%                        linking (pixels).
%  angtol and linkrad are optional.  If these parameters are omitted the
%  merging phase is omitted.
%
% Returns:  seglist - an Nx4 array storing line segments in the form
%                      [x1 y1 x2 y2
%                       x1 y1 x2 y2
%                          . . .    ] etc
%
%         nedgelist - a new cell array of edge lists where each edge list
%                     corresponds to each segment in seglist above.  The
%                     edgelist is in row,column coords in the form
%                     { [r1 c1   [r1 c1   etc }
%                        r2 c2    ...
%                        ...
%                        rN cN]   ....]
%
% * Note that a non-empty nedgelist is only returned if there is no segment
%   merging.
%
% This function takes each array of edgepoints in edgelist, finds the
% size and position of the maximum deviation from the line that joins the
% endpoints, if the maximum deviation exceeds the allowable tolerance the
% edge is shortened to the point of maximum deviation and the test is
% repeated.  In this manner each edge is broken down to line segments,
% each of which adhere to the original data with the specified tolerance.
%
% The optional final edge merging phase is provided because the initial
% edge linking phase may have separated edges at `Y' junctions in the
% image.  The merging phase can reconnect broken branches.
% Note however that the merging process can be slow.
%
% See also:  EDGELINK, MAXLINEDEV, MERGESEG, DRAWSEG
%

% Copyright (c) 2000-2005 Peter Kovesi
% School of Computer Science & Software Engineering
% The University of Western Australia
% http://www.csse.uwa.edu.au/
%
% Permission is hereby granted, free of charge, to any person obtaining a copy
% of this software and associated documentation files (the "Software"), to deal
% in the Software without restriction, subject to the following conditions:
%
% The above copyright notice and this permission notice shall be included in
% all copies or substantial portions of the Software.
%
% The Software is provided "as is", without warranty of any kind.

% December 2000 - Original version
% February 2003 - Added the returning of nedgelist data.

% ** need to preallocate memory to improve speed ***

function [linelist, nedgelist] = lineseg(edgelist, tol, angtol, linkrad)

    linelist = [];
    Nline = 0;
    Nedge = length(edgelist);

    if nargin == 2
	merge = 0;
    else
	merge = 1;
    end

    for e = 1:Nedge
        y = edgelist{e}(:,1);
	x = edgelist{e}(:,2);

	fst = 1;                % Indecies of first and last points in edge
	lst = length(x);        % segment being considered.

	while  fst<lst
	    [m,i,d] = maxlinedev(x(fst:lst),y(fst:lst));  % Find size & posn of
                                                          % maximum deviation.

	    while m > tol       % While deviation is > tol  (m/d) ?
		lst = i+fst-1;  % Shorten line to point of max deviation by adjusting lst
		[m,i,d] = maxlinedev(x(fst:lst),y(fst:lst));
	    end

	    Nline = Nline+1;
            % Record line segment. Note that (c,r) corresponds to (x,y)
	    linelist(Nline,:) = [x(fst) y(fst) x(lst) y(lst)];
	    % Record edgelist that corresponds to the segment.
	    nedgelist{Nline} = [y(fst:lst) x(fst:lst)];

%	    fst = lst+1;        % Reset fst and lst.
	    fst = lst;        % make new fst match last lst so that segments
                              % share endpoints.
	    lst = length(x);
	end

    end
    % fprintf('No of segments = %d\n',length(linelist));
    % drawseg(linelist,1), title('Raw segments');

    if merge
%	fprintf('\nMerging Segments\n');
	linelist = mergeseg(linelist, angtol, linkrad, tol);
	nedgelist = {};
%	fprintf('No of merged segments = %d\n',length(linelist));
        % drawseg(linelist,2), title('Merged segments');
    end


function varargout = tanh(varargin)
%TANH   Hyperbolic tangent of a CHEBFUN2.
%
%   TANH(F) returns the hyperbolic tangent of a CHEBFUN2.

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

[varargout{1:nargout}] = tanh@separableApprox(varargin{:});

end


function [u,p] = StokesLSCDGS(u,p,f,g,A,B,auxMat,elem,smootherOpt,Ai,Si,SSi,Res,Pro)
%% STOKESLSCDGSTRI Matrix-vecotr form DGS relaxation for Stokes eqns
%
%************************** Algorithm Description *************************
%
%  In matrix form, we need to solve the equations
%
%         Lx =  |A B' | |u| = |f|
%               |B -D | |p| = |0|
%  Distributive matrix will be given by
%
%         M = |I            B'|
%             |0 -inv(BB')BAB'|,
%
%  Therefore, the transformed matrix T will be
%
%         T = L*M = |A  PAB'               |    with P = I-B'inv(BB')B
%                   |B  BB'+D*inv(BB')BAB' |
%
%         tildeT = |Su   0|    with Su and Sp smoother for A and BB'.
%                  |B   Sp|
%
%  The matrix form DGS update can be written as
%
%    |uk+1|   |uk|                 |ru|     with ru = f-Auk -B'pk,
%    |    | = |  | + M*inv(tildeT)*|  |
%    |pk+1|   |pk|                 |rp|     and  rp = 0-Buk
%
%********************** End-of- Algorithm Description *********************
%
%  Created by Ming Wang (with discussion with Long Chen) at Jan, 2012.

%% Set up of smoothers
smoothingstep = smootherOpt.smoothingstep;
smootherSp = upper(smootherOpt.smootherSp);
smootherbarSp = upper(smootherOpt.smootherbarSp);
if isfield(smootherOpt,'smootherbarSpPara')
    smootherbarSpPara = smootherOpt.smootherbarSpPara;
else
    smootherbarSpPara = 1;
end
if strcmp(smootherbarSp,'VCYCLE') || strcmp(smootherSp,'VCYCLE')
    % mg Parameters for barSp
    optionmg.solvermaxit = 1;
%     optionmg.tol = 0.1;
    optionmg.solver = 'VCYCLE';
    optionmg.smoothingstep = 2;
    optionmg.printlevel=0;
    optionmg.setupflag = 0;
end

%% initialize smoother
Bt = auxMat.Bt;
BBt = auxMat.BBt;
BABt = auxMat.BABt;
Su = auxMat.Su;
Sp = auxMat.Sp;
Spt = auxMat.Spt;
DSp = auxMat.DSp;

%% DGS relaxation step
for k = 1: smoothingstep
    % Step 1: relax Momentum eqns
    u = u + Su\(f-Bt*p-A*u);
    % Step 2: relax transformed Continuity eqns
    rp = g - B*u;
    switch(smootherSp)
        case 'SGS'
            dq = Spt\(DSp.*(Sp\rp)); % symmetric Gauss-Seidel iteration
        case 'GS'
            dq = Sp\rp; % Gauss-Seidel iteration
        case 'VCYCLE'
            if exist('elem','var')
                dq = mg(BBt,rp,elem,optionmg,Ai,Si,SSi,Res,Pro); % mg vcycle
            else
                dq = amg(BBt,rp,optionmg); % mg vcycle
            end
    end
    % Step 3: transform the correction back to the original variables
    %   Step 3.1: velocity
    u = u + Bt*dq;
    %   Step 3.2: pressure
    dq = BABt*dq;
%     dq = dq - mean(dq);
    switch(smootherbarSp)
        case 'GS'
            dp = Spt\dq;
        case 'SGS'
            dp = Sp\(DSp.*(Spt\dq));
%             dp = dp + Sp\(DSp.*(Spt\(dq - BBt*dp)));
        case 'VCYCLE'
%            dp = BBt\dq;
            if exist('elem','var')
%                 dq =dq-mean(dq);
                dp = mg(BBt,dq,elem,optionmg,Ai,Si,SSi,Res,Pro); % mg vcycle;
            else
                dp = amg(BBt,dq,optionmg);
            end
    end
    p = p - smootherbarSpPara*dp;

end


% Function to compute the gradient of the given optical flow
%
%    Copyright (C) 2013  Anestis Papazoglou
%
%    You can redistribute and/or modify this software for non-commercial use
%    under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.
%
%    This program is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with this program.  If not, see <http://www.gnu.org/licenses/>.
%
%    For commercial use, contact the author for licensing options.
%
%    Contact: a.papazoglou@sms.ed.ac.uk

function result = getFlowGradient( flow )

    if( iscell( flow ) )
        framesNumber = length( flow );

        gradients = cell( 1, framesNumber );
        for( i = 1: framesNumber )
            grad( :, :, 1 ) = gradient( single( flow{ i }( :, :, 1 ) ) );
            [ ~, grad( :, :, 2 ) ] = ...
                gradient( single( flow{ i }( :, :, 2 ) ) );

            gradients{ i } = grad;
        end
        result = gradients;
    else
        [ height, width, ~ ] = size( flow );
        grad = zeros( height, width, 2, 'single' );
        if( ~isfloat( flow ) )
            flow = single( flow );
        end

        grad( :, :, 1 ) = gradient( flow( :, :, 1 ) );
        [ ~, grad( :, :, 2 ) ] = gradient( flow( :, :, 2 ) );

        result = grad;
    end
end


function view = plotResidualError(view,scan,baseFrame)
%
% view = plotResidualError(view,scan,[baseFrame])
%
% Plots the rmse between each frame and a baseFrame
%
% If you change this function make parallel changes in:
%   plotMaxTSErr
%
% djh, 2/2001. rmse instead of var, conver to 3.0

if ~exist('scan','var')
   scan = getCurScan(view);
end
if ~exist('baseFrame','var')
   baseFrame=1;
   %baseFrame=nFrames;
end

slices = sliceList(view,scan);
nSlices = length(slices);
nFrames = numFrames(view,scan);

% Load tSerises and compute RMSE slice by slice
waitHandle = mrvWaitbar(0,'Loading tSeries and computing RMSE.  Please wait...');
vres = zeros(nSlices,nFrames);
for slice=slices
   mrvWaitbar(slice/nSlices);
   tSeries = loadtSeries(view,scan,slice);
   for frame=1:nFrames
      vres(slice,frame) = sqrt(mse(tSeries(frame,:),tSeries(baseFrame,:)));
   end
end
close(waitHandle)

% mean across slices
vres = mean(vres);

% dont plot anything for the base frame (otherwise it would be 0)
vres(baseFrame)=NaN;

% plot it
selectGraphWin;
fontSize = 14;
set(gcf,'Name',['RMSE, scan: ',num2str(scan),', baseframe:',int2str(baseFrame)]);
x = 1:nFrames;
plot(x,vres,'-b','LineWidth',2)
set(gca,'FontSize',fontSize)
set(gca,'XLim',[0,nFrames]);
xlabel('Frame number','FontSize',fontSize)
ylabel('RMSE (raw intensity units)','FontSize',fontSize)

% Save the data in gca('UserData')
data.frameNumbers = x;
data.tSeries  =  vres;
set(gca,'UserData',data);

return


%implementation of the TVE SLM model by Martha Banbura and Andries van Vlodrop (2017)
%by Ben Schumann (2020)

function [Ys, Yt, Y, data_endo, const, priorValues, dataValues, sizetraining]=...
    TVESLM_prior(data_endo, data_exo, names, endo, lags, lambda1, lambda2, lambda3, lambda5, ar, bex, dataSLM, namesSLM, datesSLM, const, priorexo, gamma)
%input n, data_endo, data_exo lags, q, lambda1, lambda2, lambda3, lambda5,ar bex,dataSLM, namesSLM, datesSLM, endo, names,n
%output ys, data_endo, Yt, const, priorValues,dataValues. data.values

%% Preliminaries
p=lags;
T = size(data_endo,1);
n = size(data_endo,2);
% compute m, the number of exogenous variables in the model
% if data_exo is empty, set m=0
if isempty(data_exo)==1
m=0;
% if data_exo is not empty, count the number of exogenous variables that will be included in the model
else
m=size(data_exo,2);
% Also, trim a number initial rows equal to the number of lags, as they will be suppressed from the endogenous as well to create initial conditions
data_exo=data_exo(p+1:end,:);
end

if const ==1
    m=m-1;
    const=0; %no constant as the VAR is estimated on the local mean adjusted variables
end

% determine k, the number of parameters to estimate in each equation; it is equal to np+m
k=n*p+m;
% determine q, the total number of VAR parameters to estimate; it is equal to n*k
q=n*k;
%Divide the sample into a training sample and an estimation sample
sizetraining = floor(size(data_endo,1)/5) + lags;
Yt = data_endo(1:sizetraining,:);           %training sample
dataValues.Yt = Yt;                         %training sample

Y = data_endo((sizetraining-lags)+1:end,:); %estimation sample
dataValues.Y = Y;                           %estimation sample

dataValues.data_endo_full = data_endo;
data_endo = data_endo((sizetraining-lags)+1:end,:);

%Determine for which variables there is a survey local mean and build selection matrix

HaveSLM = ismember(endo,namesSLM)'; %first establish for which variables we have survey local mean data
ns = sum(HaveSLM);                  %check how many endogenous variables have a survey local mean
Ys = nan(size(Y,1),ns);
ni = 0;
Ppsi = zeros(ns,n);                 %create selection matrix for measurement equation
% for kk = 1:n
%     if HaveSLM(1,kk) ==1
%         ni = ni+1;
%         Ys(:,kk) = dataSLM(sizetraining-lags+1:end,kk);
%         Ppsi(ni,kk) = 1;            %selection matrix for local mean measurement equation
%     end
% end
for kk = 1:n
    if ismember(endo{kk,1},namesSLM)
        %find entry in ismember
        IndexC= find(strcmp(namesSLM, endo{kk,1}));
        ni = ni+1;
        Ys(:,IndexC) = dataSLM(sizetraining-lags+1:end,IndexC);
        Ppsi(IndexC,kk) = 1;            %selection matrix for local mean measurement equation
    end
end

dataValues.Ppsi = Ppsi;

dataValues.Ys = Ys;                 %Survey local mean after training sample

%%%%%%%%%%%%%%%%%% set the prior values%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% prior for stochastic volatility in VAR residuals priors (H = A^-1'*Lambda_t*A^-1')
%estimate an ar(p) process for setting the prior variances
[arvar]=bear.arloop(Yt,1,lags,n);
arvar = arvar*((size(Yt,1)-lags-lags-1)/(size(Yt,1)-lags-1)); %translate bear into Banbura et al (2017) estimate of ar(4) VCV

priorValues.priorVarAscaling_H =10;  %prior variance for the below diagonal elements of A (constant part of VCV
priorValues.priorMeanAscaling_H=0;   %prior mean for the below diagonal elements of A

priorValues.mean_ln_h0=log(arvar);    %prior mean of the initial condition for stochastic volatilty VAR residuals
priorValues.var_ln_h0 = 10*ones(n,1); %prior variance for the inital condition  for stochastic volatilty VAR residuals

priorValues.phi_h=0.01;               %centering parameter for the variance of the stochastic volatility process in the VAR residuals
priorValues.d_h=10;                   %scaling parameter for the variance of the stochastic volatility process in the VAR residuals

priorValues.offset_c = 0.001;         %constant for log transformation in order to avoid numerical problems of log close to 0

%% Priors for Reduced form VAR coefficients (independent normal wishart)
priorValues.vars  =arvar;
%for bear sampling
priorValues.lambda1=lambda1;        % overall tightness
priorValues.lambda2=lambda2;        % cross variable shrinkage
priorValues.lambda3=lambda3;        % lag shrinkage
priorValues.lambda5=lambda5;        % block exogeneity shrinkage
priorValues.ar = ar;                % prior values for ar(1) coefficients of the variable

%for banbura et al sampling
priorValues.lambda=lambda1^2;       % only used for banbura large model sampling
priorValues.theta=lambda3;          % only used for banbura large model sampling

if size(ar,1) < n              % if no variable specific priors were given
    artemp=zeros(n,1);
    for kk=1:n
        artemp(kk,1)=ar;
    end
    ar = artemp;
end

% start with beta0, defined in (1.3.4)
% it is a q*1 vector of zeros, save for the n coefficients of each variable on their own first lag
beta0=zeros(q,1);

for ii=1:n
beta0((ii-1)*k+ii,1)=ar(ii,1);
end

% if a prior for the exogenous variables is selected put it in here:
for ii=1:n
    beta0(k*ii)=priorexo(ii,1);
end

% next compute omega0, the variance-covariance matrix of beta, defined in (1.3.8)
% set it first as a q*q matrix of zeros
omega0=zeros(q,q);

% set the variance on coefficients related to own lags, using (1.3.5)
for ii=1:n
   for jj=1:p
   omega0((ii-1)*k+(jj-1)*n+ii,(ii-1)*k+(jj-1)*n+ii)=(lambda1/jj^lambda3)^2;
   end
end

%  set variance for coefficients on cross lags, using (1.3.6)
for ii=1:n
   for jj=1:p
      for kk=1:n
      if kk==ii
      else
      omega0((ii-1)*k+(jj-1)*n+kk,(ii-1)*k+(jj-1)*n+kk)=(arvar(ii,1)/arvar(kk,1))*(((lambda1*lambda2)/(jj^lambda3))^2);
      end
      end
   end
end

% finally set the variance for exogenous variables, using (1.3.7)
for ii=1:n
   for jj=1:m
   omega0(ii*k-m+jj,ii*k-m+jj)=arvar(ii,1)*((lambda1*lambda4)^2);
   end
end

% if block exogeneity has been selected, implement it, according to (1.7.4)
if bex==1
   for ii=1:n
      for jj=1:n
         if blockexo(ii,jj)==1
            for kk=1:p
            omega0((jj-1)*k+(kk-1)*n+ii,(jj-1)*k+(kk-1)*n+ii)=omega0((jj-1)*k+(kk-1)*n+ii,(jj-1)*k+(kk-1)*n+ii)*lambda5^2;
            end
         else
         end
      end
   end
% if block exogeneity has not been selected, don't do anything
else
end

priorValues.beta0=beta0;        %prior for the reduced form VAR coefficients in vectorized form
priorValues.omega0=omega0;       %prior variance for reduced form VAR coefficients

%% Priors for the measurement equation (G)

priorValues.mean_ln_g0=zeros(ns,1); %mean of the initial condition in the variance of measurement equation residuals
priorValues.var_ln_g0 = 10*ones(ns,1); %variance of the initial condition in the variance of measurement equation residuals

priorValues.phi_g=0.01;    %centering parameter for the variance of measurement equation residuals
priorValues.d_g=10;        %scaling parameter for the variance of the stochastic volatility process in the VAR residuals

%% Priors for the local mean equation
priorValues.meanTS=mean(Yt)';          %start values for mean corrected Kalman smoother simulation in order to draw from the local mean process
priorValues.kappa = 1000;              %variance of the initial state for the local mean

priorValues.gamma = gamma;             %value for ar coefficient in

priorValues.mean_ln_v0=log(arvar);     %prior mean of the initial condition for stochastic volatilty in the state transition equation
priorValues.var_ln_v0 = 10*ones(n,1);  %prior variance of the initial condition for stochastic volatilty in the state transition equation

priorValues.d_v=10;                    %scaling parameter for the variance of the stochastic volatility process in the VAR residuals
priorValues.phi_v=0.01;                %centering parameter for the variance of transition equation residuals


function [Y X Z n m p T k1 k3 q1 q2 q3]=maprelim(data_endo,data_exo,const,lags,regimeperiods,names)

% function [Y X Z n m p T k1 k3 q1 q2 q3]=maprelim(data_endo,data_exo,const,lags)
% creates the preliminary values necessary for all the subsequent computations
% inputs:  - matrix 'data_endo': the matrix storing the endogenous time series data used to estimate the model
%          - matrix 'data_exo': the matrix storing the exogenous time series data used to estimate the model
%          - integer 'const': 0-1 value determining whether a constant term should be included in the model
%          - integer 'lags': the number of lags to include in the model
% outputs: - matrix 'Y': the matrix of endogenous variables, defined in (3.5.10)
%          - matrix 'X': the matrix of endogenous regressors, defined in (3.5.10)
%          - matrix 'Z': the matrix of exogenous regressors, defined in (3.5.10)
%          - integer 'n': the number of endogenous variables in the model
%          - integer 'm': the number of exogenous variables in the model
%          - integer 'p': the number of lags in the model
%          - integer 'T': the sample size, i.e. the number of time periods used to estimate the model
%          - integer 'k1': the number of coefficients related to the endogenous variables for each equation in the model
%          - integer 'k3': the number of coefficients related to the exogenous variables for each equation, in the reformulated model (3.5.5)
%          - integer 'q1': the total number of VAR coefficients related to the endogenous variables
%          - integer 'q2': the total number of VAR coefficients related to the exogenous variables
%          - integer 'q3': the total number of VAR coefficients related to the exogenous variables, in the reformulated model (3.5.5)

% first compute p, the number of lags in the model, defined p77
p=lags;

% then compute n, the number of endogenous variables in the model; it is simply the number of columns in the matrix 'data_endo'
n=size(data_endo,2);

% augment the matrix of exogenous with a column of ones to account for the constant
%data_exo=[ones(size(data_endo,1),1) data_exo];
%alternative data exo

if isempty(regimeperiods);
    data_exo=[ones(size(data_endo,1),1)];
else
data_exo=[ones(size(data_endo,1),1) zeros(size(data_endo,1),1)];
data_exo(find(strcmp(names(2:end,1),regimeperiods(1))):find(strcmp(names(2:end,1),regimeperiods(2))),2)=1;
data_exo(find(strcmp(names(2:end,1),regimeperiods(1))):find(strcmp(names(2:end,1),regimeperiods(2))),1)=0;
end

if isempty(regimeperiods);
    data_exo=[ones(size(data_endo,1),1)];
else
data_exo=[ones(size(data_endo,1),1) zeros(size(data_endo,1),1)];
data_exo(find(strcmp(names(2:end,1),regimeperiods(1))):find(strcmp(names(2:end,1),regimeperiods(2))),2)=1;
end
% then compute m, the number of exogenous variables in the model, defined p77
% if data_exo is empty, set m=0
m=size(data_exo,2);

% estimate k1, the number of parameters related to endogenous variables in each equation, defined p77
k1=n*p;

% estimate q1, the total number of parameters related to endogenous variables, defined p77
q1=n*k1;

% estimate q2, the total number of parameters related to exogenous variables, defined p77
q2=n*m;

% estimate k3, the number of parameters related to exogenous variables in each equation of the modified system, defined p78
k3=m*(p+1);

% estimate q3, the total number of parameters related to endogenous variables in the modified system, defined p78
q3=n*k3;

% obtain the matrices Y and X, defined in (3.6.10)
% to do so, use the lagx function on the data matrix
temp=bear.lagx(data_endo,lags);

% to build X, take off the n initial columns of current data
X=temp(:,n+1:end);

% save the n first columns of temp as Y
Y=temp(:,1:n);

% obtain the matrix Z, defined in (3.6.10)
temp=bear.lagx(data_exo,lags);
temp(:,m+1:end)=-temp(:,m+1:end);
Z=temp;

% Define T, the number of periods of the model, as the number of rows of Y
T=size(Y,1);

% obtain eventually y, Xbar and Zbar, as defined in (XXX)
y=Y(:);
Xbar=kron(eye(n),X);
Zbar=kron(eye(n),Z);


function S = HALMSinit(w0,mu,fb,ff,delay)

% HALMSinit     Initialize Parameter Structure of the LMS Algorithm
%               for Hearing Aids. Refer to Fig. 1.9
%
% Arguments:
% w0            Coefficients of FIR filter at start (@n=1)
% mu            Step size for LMS algorithm
% ff            Feedforward path
% fb            Feedback path
% delay         Delay to adaptive filter
%
% by Lee, Gan, and Kuo, 2008
% Subband Adaptive Filtering: Theory and Implementation
% Publisher: John Wiley and Sons, Ltd

% Assign structure fields

S.coeffs     = w0(:);       % Weight (column) vector of FIR filter
S.step       = mu;          % Step size of the LMS algorithm
S.iter       = 0;           % Iteration count
S.ff         = ff;          % Feedforward path
S.fb         = fb;          % Feedback path
S.delay      = delay;       % Delay
S.AdaptStart = length(w0);  % Running effect of adaptive filter


function gX = mlpKernGradX(kern, X, X2)

% MLPKERNGRADX Gradient of MLP kernel with respect to input locations.
% FORMAT
% DESC computes the gradident of the multi-layer perceptron
% kernel with respect to the input positions where both the row
% positions and column positions are provided separately.
% ARG kern : kernel structure for which gradients are being
% computed.
% ARG x1 : row locations against which gradients are being computed.
% ARG x2 : column locations against which gradients are being computed.
% RETURN g : the returned gradients. The gradients are returned in
% a matrix which is numData2 x numInputs x numData1. Where numData1 is
% the number of data points in X1, numData2 is the number of data
% points in X2 and numInputs is the number of input
% dimensions in X.
%
% SEEALSO mlpKernParamInit, kernGradX, mlpKernDiagGradX
%
% COPYRIGHT : Neil D. Lawrence, 2004, 2005, 2006, 2009

% KERN

gX = zeros(size(X2, 1), size(X2, 2), size(X, 1));
for i = 1:size(X, 1);
  gX(:, :, i) = mlpKernGradXpoint(kern, X(i, :), X2);
end

function gX = mlpKernGradXpoint(kern, x, X2)

% MLPKERNGRADXPOINT Gradient with respect to one point of x.

innerProd = X2*x';
numer = innerProd*kern.weightVariance + kern.biasVariance;
vec1 = sum(x.*x, 2)*kern.weightVariance + kern.biasVariance + 1;
vec2 = sum(X2.*X2, 2)*kern.weightVariance + kern.biasVariance + 1;
denom = sqrt(vec2*vec1');
arg = numer./denom;
gX = zeros(size(X2));
twooverpi = 2/pi;
for j = 1:size(X2, 2)
  gX(:, j)=X2(:, j)./denom - vec2.*x(:, j).*numer./denom.^3;
  gX(:, j) = twooverpi*kern.weightVariance*kern.variance*gX(:, j)./sqrt(1-arg.*arg);
end


classdef VariationalRefinement < handle
    %VARIATIONALREFINEMENT  Variational optical flow refinement
    %
    % This class implements variational refinement of the input flow field,
    % i.e. it uses input flow to initialize the minimization of the following
    % functional:
    %
    %     E(U) = integral_{Omega}(delta * Psi(E_I) + gamma * Psi(E_G) + alpha * Psi(E_S))
    %
    % where `E_I`, `E_G`, `E_S` are color constancy, gradient constancy and
    % smoothness terms respectively. `Psi(s^2) = sqrt(s^2 + epsilon^2)` is a
    % robust penalizer to limit the influence of outliers. A complete
    % formulation and a description of the minimization procedure can be found
    % in [Brox2004].
    %
    % ## References
    % [Brox2004]:
    % > Thomas Brox, Andres Bruhn, Nils Papenberg, and Joachim Weickert.
    % > "High accuracy optical flow estimation based on a theory for warping".
    % > In Computer Vision-ECCV 2004, pages 25-36. Springer, 2004.
    %
    % See also: cv.VariationalRefinement.calc
    %

    properties (SetAccess = private)
        % Object ID
        id
    end

    properties (Dependent)
        % Number of outer (fixed-point) iterations in the minimization
        % procedure. default 5
        FixedPointIterations
        % Number of inner successive over-relaxation (SOR) iterations in the
        % minimization procedure to solve the respective linear system.
        % default 5
        SorIterations
        % Relaxation factor in SOR. default 1.6
        Omega
        % Weight of the smoothness term. default 20.0
        Alpha
        % Weight of the color constancy term. default 5.0
        Delta
        % Weight of the gradient constancy term. default 10.0
        Gamma
    end

    %% Constructor/destructor
    methods
        function this = VariationalRefinement()
            %VARIATIONALREFINEMENT  Creates an instance of VariationalRefinement
            %
            %     obj = cv.VariationalRefinement()
            %
            % See also: cv.VariationalRefinement.calc
            %
            this.id = VariationalRefinement_(0, 'new');
        end

        function delete(this)
            %DELETE  Destructor
            %
            %     obj.delete()
            %
            % See also: cv.VariationalRefinement
            %
            if isempty(this.id), return; end
            VariationalRefinement_(this.id, 'delete');
        end
    end

    %% DenseOpticalFlow
    methods
        function flow = calc(this, I0, I1, varargin)
            %CALC  Calculates an optical flow
            %
            %     flow = obj.calc(I0, I1)
            %     flow = obj.calc(I0, I1, 'OptionName',optionValue, ...)
            %
            % ## Input
            % * __I0__ first 8-bit single-channel input image.
            % * __I1__ second input image of the same size and the same type
            %   as `I0`.
            %
            % ## Output
            % * __flow__ computed flow image that has the same size as `I0`
            %   and type `single` (2-channels).
            %
            % ## Options
            % * __InitialFlow__ specify the initial flow. Not set by default.
            %
            % See also: cv.VariationalRefinement
            %
            flow = VariationalRefinement_(this.id, 'calc', I0, I1, varargin{:});
        end

        function collectGarbage(this)
            %COLLECTGARBAGE  Releases all inner buffers
            %
            %     obj.collectGarbage()
            %
            VariationalRefinement_(this.id, 'collectGarbage');
        end
    end

    %% VariationalRefinement
    methods
        function [flow_u, flow_v] = calcUV(this, I0, I1, varargin)
            %CALCUV  calc function overload to handle separate horizontal (u) and vertical (v) flow components (to avoid extra splits/merges)
            %
            %     [flow_u, flow_v] = obj.calcUV(I0, I1)
            %     [flow_u, flow_v] = obj.calcUV(I0, I1, 'OptionName',optionValue, ...)
            %
            % ## Input
            % * __I0__ first 8-bit single-channel input image.
            % * __I1__ second input image of the same size and the same type
            %   as `I0`.
            %
            % ## Output
            % * **flow_u** computed horizontal flow image that has the same
            %   size as `I0` and type `single` (1-channel1).
            % * **flow_v** computed vertical flow image that has the same size
            %   as `I0` and type `single` (1-channel1).
            %
            % ## Options
            % * __InitialFlowU__ specify initial U-flow. Not set by default.
            % * __InitialFlowV__ specify initial V-flow. Not set by default.
            %
            % Note that `flow(:,:,1)==flow_u`, and `flow(:,:,2)==flow_v`.
            %
            % See also: cv.VariationalRefinement.calc
            %
            [flow_u, flow_v] = VariationalRefinement_(this.id, 'calcUV', I0, I1, varargin{:});
        end
    end

    %% Algorithm
    methods (Hidden)
        function clear(this)
            %CLEAR  Clears the algorithm state
            %
            %     obj.clear()
            %
            % See also: cv.VariationalRefinement.empty
            %
            VariationalRefinement_(this.id, 'clear');
        end

        function b = empty(this)
            %EMPTY  Returns true if the algorithm is empty
            %
            %     b = obj.empty()
            %
            % ## Output
            % * __b__ Returns true if the algorithm is empty (e.g. in the very
            %   beginning or after unsuccessful read).
            %
            % See also: cv.VariationalRefinement.clear
            %
            b = VariationalRefinement_(this.id, 'empty');
        end

        function name = getDefaultName(this)
            %GETDEFAULTNAME  Returns the algorithm string identifier
            %
            %     name = obj.getDefaultName()
            %
            % ## Output
            % * __name__ This string is used as top level XML/YML node tag
            %   when the object is saved to a file or string.
            %
            % See also: cv.VariationalRefinement.save, cv.VariationalRefinement.load
            %
            name = VariationalRefinement_(this.id, 'getDefaultName');
        end

        function save(this, filename)
            %SAVE  Saves the algorithm to a file
            %
            %     obj.save(filename)
            %
            % ## Input
            % * __filename__ Name of the file to save to.
            %
            % This method stores the algorithm parameters in a file storage.
            %
            % See also: cv.VariationalRefinement.load
            %
            VariationalRefinement_(this.id, 'save', filename);
        end

        function load(this, fname_or_str, varargin)
            %LOAD  Loads algorithm from a file or a string
            %
            %     obj.load(fname)
            %     obj.load(str, 'FromString',true)
            %     obj.load(..., 'OptionName',optionValue, ...)
            %
            % ## Input
            % * __fname__ Name of the file to read.
            % * __str__ String containing the serialized model you want to
            %   load.
            %
            % ## Options
            % * __ObjName__ The optional name of the node to read (if empty,
            %   the first top-level node will be used). default empty
            % * __FromString__ Logical flag to indicate whether the input is a
            %   filename or a string containing the serialized model.
            %   default false
            %
            % This method reads algorithm parameters from a file storage.
            % The previous model state is discarded.
            %
            % See also: cv.VariationalRefinement.save
            %
            VariationalRefinement_(this.id, 'load', fname_or_str, varargin{:});
        end
    end

    %% Getters/Setters
    methods
        function value = get.FixedPointIterations(this)
            value = VariationalRefinement_(this.id, 'get', 'FixedPointIterations');
        end
        function set.FixedPointIterations(this, value)
            VariationalRefinement_(this.id, 'set', 'FixedPointIterations', value);
        end

        function value = get.SorIterations(this)
            value = VariationalRefinement_(this.id, 'get', 'SorIterations');
        end
        function set.SorIterations(this, value)
            VariationalRefinement_(this.id, 'set', 'SorIterations', value);
        end

        function value = get.Omega(this)
            value = VariationalRefinement_(this.id, 'get', 'Omega');
        end
        function set.Omega(this, value)
            VariationalRefinement_(this.id, 'set', 'Omega', value);
        end

        function value = get.Alpha(this)
            value = VariationalRefinement_(this.id, 'get', 'Alpha');
        end
        function set.Alpha(this, value)
            VariationalRefinement_(this.id, 'set', 'Alpha', value);
        end

        function value = get.Delta(this)
            value = VariationalRefinement_(this.id, 'get', 'Delta');
        end
        function set.Delta(this, value)
            VariationalRefinement_(this.id, 'set', 'Delta', value);
        end

        function value = get.Gamma(this)
            value = VariationalRefinement_(this.id, 'get', 'Gamma');
        end
        function set.Gamma(this, value)
            VariationalRefinement_(this.id, 'set', 'Gamma', value);
        end
    end

end


%% Parameters

F_update = [1 0 1 0; 0 1 0 1; 0 0 1 0; 0 0 0 1];

Npop_particles = 4000;

Xstd_rgb = 50;
Xstd_pos = 25;
Xstd_vec = 5;

Xrgb_trgt = [255; 0; 0];

%% Loading Movie

vr = VideoReader('Person.wmv');

Npix_resolution = [vr.Width vr.Height];
Nfrm_movie = floor(vr.Duration * vr.FrameRate);

%% Object Tracking by Particle Filter

X = create_particles(Npix_resolution, Npop_particles);

for k = 1:Nfrm_movie

    % Getting Image
    Y_k = read(vr, k);

    % Forecasting
    X = update_particles(F_update, Xstd_pos, Xstd_vec, X);

    % Calculating Log Likelihood
    L = calc_log_likelihood(Xstd_rgb, Xrgb_trgt, X(1:2, :), Y_k);

    % Resampling
    X = resample_particles(X, L);

    % Showing Image
    show_particles(X, Y_k);
%    show_state_estimated(X, Y_k);

end


%data comes from the EUROC MAV dataset V1_01_medium
clear;
close all;
addpath myToolbox
addpath filters
addpath data

tMin = 1081; % starting IMU time
tImagesMin = 109; % starting camera time

%% Initialization
freqIMU = 200; %Hz
freqCam = 20; %Hz

ParamGlobal.dirImage = 'V1_02_medium/mav0/cam0/data/';
dirImage = 'V1_02_medium/mav0/cam0/data/';
fileData = 'DATA_MEDIUM.mat';
fileImages = 'fileImages_MEDIUM.mat';
offset = 881;

% IMU and frame date
load(fileData);
t = tIMU;
t = t/10^9; % ns -> s
g = 9.81*[0;0;-1]; %gravity field
omega = omega(:,tMin:end);
acc = acc(:,tMin:end);
t = t(tMin:end);
tIMU = tIMU(tMin:end);
ParamGlobal.tIMU = tIMU;
NbSteps = length(tIMU);
NbStepsMax = 8000;

load(fileImages);
tImages = tImages(tImagesMin:end);
fileImages = fileImages(tImagesMin:end);
ParamGlobal.fileImages = fileImages;
%corect offset
tReal(1:offset) = [];
trajReal.x(:,1:offset) = [];
trajReal.v(:,1:offset) = [];
trajReal.quat(1:offset) = [];
trajReal.psi(1:offset) = [];
trajReal.theta(1:offset) = [];
trajReal.phi(1:offset) = [];
trajReal.omega_b(:,1:offset) = [];
trajReal.a_b(:,1:offset) = [];
obsTimes = zeros(length(t),1);
obsTimes(1:10:end) = 1; % IMU is 10 times faster than camera

%% Init Filter
P0amers = diag([1;1;1]*1.e-3); %initial landmark covariance
R = 1.0^2*eye(2); %measurement noise for one landmark
ParamFilter.NbAmers = 30; %nominal number of landmarks in the state
ParamFilter.NbAmersMin = ParamFilter.NbAmers;
ParamFilter.EcartPixelMax = 20;

% init covariance
p0Rot = (0.01*pi/180)^2;
p0v =  1.e-4;
p0x =  1.e-8;
p0omegab = 1.e-6;
p0ab = 1.e-6;
P0 = diag([p0Rot*ones(3,1);p0v*ones(3,1);p0x*ones(3,1);...
    p0omegab*ones(3,1);p0ab*ones(3,1)]);

% process noises
q_omega = (1.6968e-4)^2*200;
q_a = (2e-3)^2*200;
q_omegab = (1.9393e-5)^2*200;
q_ab = (3e-3)^2*200;
Q = diag([q_omega*ones(3,1);q_a*ones(3,1);q_omegab* ...
    ones(3,1);q_ab*ones(3,1)]);
Qc = chol(Q);
P0 = blkdiag(P0,kron(eye(ParamFilter.NbAmers),P0amers));

%depending on the chosen camera %cam0
ParamFilter.chiC = [0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975;
    0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768;
    -0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949;
    0.0, 0.0, 0.0, 1.0]; % camera pose
ParamFilter.Pi = [458.654 0 0; 0 457.296 0; 367.215, 248.375 1]';%camera calibration matrix
ParamFilter.cameraParams = cameraParameters('IntrinsicMatrix', ParamFilter.Pi',...
    'RadialDistortion',[-0.28340811, 0.07395907],...
    'TangentialDistortion',[0.00019359, 1.76187114e-05]);

% Initialisation of the state is obtained following [Mur-Artal,2017],
load('data/ORB_SLAM_init.mat');
Rot0 = eul2rotm([trajReal.psi(1),trajReal.theta(1),trajReal.phi(1)]);
x0 = trajReal.x(:,1);
v0 = trajReal.v(:,1);
omega_b0 = orb_slam.omega_b;
a_b0 = orb_slam.a_b;
PosAmers0 = orb_slam.PosAmers;
trackerMain = orb_slam.trackerMain;
trackerBis = orb_slam.trackerBis;
myTracks = orb_slam.myTracks;

IdxImage = 2; % image index

trajL = initTraj(NbSteps);
RotL = Rot0;
vL = v0;
xL = x0;
omega_bL = omega_b0;
a_bL = a_b0;
PosAmersL = PosAmers0;
P_L = P0;
S_L = chol(P_L);
chiL = state2chi(RotL,vL,xL,PosAmersL);

trajR = initTraj(NbSteps);
RotR = Rot0;
vR = v0;
xR = x0;
omega_bR = omega_b0;
a_bR = a_b0;
PosAmersR = PosAmers0;
P_R = P0;
S_R = chol(P_R);
chiR = state2chi(RotR,vR,xR,PosAmersR);

trajRef = initTraj(NbSteps);
RotRef = Rot0;
vRef = v0;
xRef = x0;
omega_bRef = omega_b0;
a_bRef = a_b0;
PosAmersRef = PosAmers0;
P_Ref = blkdiag(P0);
S_Ref = chol(P_Ref);
chiRef = [RotRef xRef;0 0 0 1];
xidotRef = zeros(6,1);

trajU = initTraj(NbSteps);
RotU = Rot0;
vU = v0;
xU = x0;
omega_bU = omega_b0;
a_bU = a_b0;
PosAmersU = PosAmers0;
P_U = blkdiag(P0);
S_U = chol(P_U);

trajI = initTraj(NbSteps);
RotI = Rot0;
vI = v0;
xI = x0;
omega_bI = omega_b0;
a_bI = a_b0;
PosAmersI = PosAmers0;
P_I = blkdiag(P0);

%% Filtering
for i = 2:NbStepsMax
    % propagation
    dt = t(i)-t(i-1);
    omega_i = omega(:,i);
    acc_i = acc(:,i);

    chiAntR = chiR;
    RotR = RotR*expSO3((omega_i-omega_bR)*dt);
    vR = vR+(RotR*(acc_i-a_bR)+g)*dt;
    xR = xR+vR*dt;
    chiR = state2chi(RotR,vR,xR,PosAmersR);
    S_R = rukfPropagation(dt,chiR,chiAntR,omega_bR,a_bR,S_R,omega_i,...
        acc_i,Qc,g);

    % propagation for others filters
    chiAntL = chiL;
    RotL = RotL*expSO3((omega_i-omega_bL)*dt);
    vL = vL+(RotL*(acc_i-a_bL)+g)*dt;
    xL = xL+vL*dt;
    chiL = state2chi(RotL,vL,xL,PosAmersL);
    S_L = lukfPropagation(dt,chiL,chiAntL,omega_bL,a_bL,S_L,omega_i,...
        acc_i,Qc,g);

    chiAntRef = [RotRef,xRef;zeros(1,3) 1];
    vAnt = vRef;
    RotRef = RotRef*expSO3((omega_i-omega_bRef)*dt);
    vRef = vRef+(RotRef*(acc_i-a_bRef)+g)*dt;
    xRef = xRef+vRef*dt;
    xidotRef = xidotRef + [omega_i-omega_bRef;vRef]*dt;
    chiRef = [RotRef,xRef;zeros(1,3) 1];
    S_Ref = ukfRefPropagation(dt,chiRef,chiAntRef,vAnt,omega_bRef,...
        a_bRef,S_Ref,omega_i,acc_i,Qc,g);

    RotAntU = RotU;
    RotU = RotU*expSO3((omega_i-omega_bU)*dt);
    vU = vU+(RotU*(acc_i-a_bU)+g)*dt;
    xU = xU+vU*dt;
    chiU = [RotU,xU;zeros(1,3) 1];
    S_U = ukfPropagation(dt,RotU,RotAntU,vAnt,omega_bU,a_bU,S_U,omega_i,...
        acc_i,Qc,g);

    [RotI,vI,xI,PosAmersI,P_I] = iekfPropagation(dt,RotI,vI,xI,...
        omega_bI,a_bI,PosAmersI,P_I,omega_i,acc_i,Q,g);
    chiI = state2chi(RotI,vI,xI,PosAmersI);

    % if measurement
    if obsTimes(i) == 1
        % track points in image
        [y,yAmers,trackerMain,trackerBis,pointsMain,validityMain,...
            myTracks,pointsBis] = ...
            ObserveLandmarks(trackerMain,trackerBis,dirImage,IdxImage,...
            fileImages,ParamFilter,RotR,xR,PosAmersR,i,S_R,myTracks);

        % update state
        param.yAmers = yAmers;
        [chiR,omega_bR,a_bR,S_R] = rukfUpdate(chiR,omega_bR,...
            a_bR,S_R,y,param,R,ParamFilter);
        [RotR,vR,xR,PosAmersR] = chi2state(chiR);

        % update other filters
        chiL = state2chi(RotL,vL,xL,PosAmersL);
        [chiL,omega_bL,a_bL,S_L] = lukfUpdate(chiL,omega_bL,...
            a_bL,S_L,y,param,R,ParamFilter);
        [RotL,vL,xL,PosAmersL] = chi2state(chiL);

        param.PosAmers = PosAmersRef;
        chiRef = state2chi(RotRef,vRef,xRef,PosAmersRef);
        [chiRef,vRef,PosAmersRef,omega_bRef,a_bRef,S_Ref,xidotRef] = ukfRefUpdate(chiRef,vRef,omega_bRef,...
            a_bRef,S_Ref,y,param,R,ParamFilter,PosAmersRef,xidotRef);
        RotRef = chiRef(1:3,1:3);
        xRef = chiRef(1:3,4);

        param.PosAmers = PosAmersU;
        chiU = state2chi(RotU,vU,xU,PosAmersU);
        [RotU,vU,xU,PosAmersU,omega_bU,a_bU,S_U] = ukfUpdate(RotU,vU,xU,omega_bU,...
            a_bU,S_U,y,param,R,ParamFilter,PosAmersU);

        [chiI,omega_bI,a_bI,P_I] = iekfUpdate(chiI,omega_bI,a_bI,...
            P_I,y,R,ParamFilter,yAmers);
        [RotI,vI,xI,PosAmersI] = chi2state(chiI);

        % save trajectory
        trajR = updateTraj(trajR,RotR,vR,xR,omega_bR,a_bR,i);
        trajL = updateTraj(trajL,RotL,vL,xL,omega_bL,a_bL,i);
        trajRef = updateTraj(trajRef,RotRef,vRef,xRef,omega_bRef,a_bRef,i);
        trajU = updateTraj(trajU,RotU,vU,xU,omega_bU,a_bU,i);
        trajI = updateTraj(trajI,RotI,vI,xI,omega_bI,a_bI,i);

        % remplace non visible landmarks
        [S_R,PosAmersR,ParamFilter,trackerBis,myTracks,PosAmersNew,...
            IdxAmersNew,trackCov,pointsMain,validityMain] = manageAmers(S_R,...
            PosAmersR,ParamFilter,ParamGlobal,trackerBis,...
            trajR,i,pointsMain,validityMain,IdxImage,myTracks,pointsBis);
        chiR = state2chi(RotR,vR,xR,PosAmersR);

        setPoints(trackerMain,pointsMain,validityMain);

        % remplace non visible landmarks for others filters with same
        % landmarks
        if isempty(IdxAmersNew) == 0
            P_L = S_L'*S_L;
            P_Ref = S_Ref'*S_Ref;
            P_U = S_U'*S_U;
            for jj = 1:length(IdxAmersNew)
                idx = IdxAmersNew(jj);
                idxP = 15+(3*idx-2:3*idx);
                P_L(:,idxP) = 0;
                P_L(idxP,:) = 0;
                P_L(idxP,idxP) = trackCov{jj};
                PosAmersL(:,idx) = PosAmersNew(jj,:)';
                P_Ref(:,idxP) = 0;
                P_Ref(idxP,:) = 0;
                P_Ref(idxP,idxP) = trackCov{jj};
                PosAmersRef(:,idx) = PosAmersNew(jj,:)';
                P_U(:,idxP) = 0;
                P_U(idxP,:) = 0;
                P_U(idxP,idxP) = trackCov{jj};
                PosAmersU(:,idx) = PosAmersNew(jj,:)';
                P_I(:,idxP) = 0;
                P_I(idxP,:) = 0;
                P_I(idxP,idxP) = trackCov{jj};
                PosAmersI(:,idx) = PosAmersNew(jj,:)';
            end
            S_L = chol(P_L);
            chiL = state2chi(RotL,vL,xL,PosAmersL);
            S_Ref = chol(P_Ref);
            chiRef = [RotRef,xRef;zeros(1,3) 1];
            S_U = chol(P_U);
            chiI = state2chi(RotI,vI,xI,PosAmersI);
        end
        disp(i/200);
        IdxImage = IdxImage+1;
    else
        trajR = updateTraj(trajR,RotR,vR,xR,omega_bR,a_bR,i);
        trajL = updateTraj(trajL,RotL,vL,xL,omega_bL,a_bL,i);
        trajRef = updateTraj(trajRef,RotRef,vRef,xRef,omega_bRef,a_bRef,i);
        trajU = updateTraj(trajU,RotU,vU,xU,omega_bU,a_bU,i);
        trajI = updateTraj(trajI,RotI,vI,xI,omega_bI,a_bI,i);
    end
end

%% Plots
errorR = computeError(trajR,trajReal,i);
errorL = computeError(trajL,trajReal,i);
errorU = computeError(trajU,trajReal,i);
errorRef = computeError(trajRef,trajReal,i);
errorI = computeError(trajI,trajReal,i);

figure;hold on;
plot(t(2:i-1)-t(2),errorR.errorR);
plot(t(2:i-1)-t(2),errorL.errorR);
plot(t(2:i-1)-t(2),errorRef.errorR);
plot(t(2:i-1)-t(2),errorU.errorR);
plot(t(2:i-1)-t(2),errorI.errorR);
disp(sqrt(mean([errorR.errorR errorL.errorR  errorRef.errorR errorU.errorR errorI.errorR].^2)));
legend('R-UKF-LG','L-UKF-LG','SE(3)-UKF','UKF','IEKF')
xlabel('t (s)')
ylabel('RMSE attitdude (°)')
title('RMSE on attitude as function of time')
figure
hold on;
plot(t(2:i-1)-t(2),errorR.errorX);
plot(t(2:i-1)-t(2),errorL.errorX);
plot(t(2:i-1)-t(2),errorRef.errorX);
plot(t(2:i-1)-t(2),errorU.errorX);
plot(t(2:i-1)-t(2),errorI.errorX);
disp(sqrt(mean([errorR.errorX  errorL.errorX errorRef.errorX errorU.errorX errorI.errorX].^2)));
legend('R-UKF-LG','L-UKF-LG','SE(3)-UKF','UKF','IEKF')
xlabel('t (s)')
ylabel('RMSE position (m)')
title('RMSE on position as function of time')


% Build and initialize the computational graph for extracting log Mel
% filterbank features
%
function [layer, para] = BuildFbankExtractionNet()
para.output = 'tmp';
para.IO.nStream = 1;
para.NET.sequential = 1;
para.cost_func.layer_idx = [];

para = ConfigBasicSTFT(para);
layer = genNetworkFbankExtraction(para.topology);     % generate the network graph

% generating the scaling factor for the input, as we will need to use a
% small constant in the logarithm. We need to make sure that the power of
% speech are larger than this constant most of the time.
scale = 1e4;        % we hard code the scale to be a constant so that all network will use the same number
scale = scale/2^16; % note that we are using int16 to store waveform samples, so need to scale down
layer = InitWavScaleLayer(layer, scale);

% set Mel filterbank linear transform
layer = InitMelLayer(layer, para);

para.out_layer_idx = length(layer);

para = ParseOptions2(para);

end


function [y,nz] = ompdenoise1(params,msgdelta)
%OMPDENOISE1 OMP denoising of 1-D signals.
%  OMPDENOISE1 denoises a 1-dimensional signal using OMP denoising. The
%  function syntax is identical to OMPDENOISE, but it runs significantly
%  faster on 1-D signals. OMPDENOISE1 requires somewhat more memory than
%  OMPDENOISE (approximately the size of the input signal), so if memory is
%  limited, OMPDENOISE can be used instead.
%
%  See also OMPDENOISE.

%  Ron Rubinstein
%  Computer Science Department
%  Technion, Haifa 32000 Israel
%  ronrubin@cs
%
%  August 2009

% parse input arguments %

x = params.x(:);
D = params.dict;
blocksize = params.blocksize;

% maxval %
if (isfield(params,'maxval'))
  maxval = params.maxval;
else
  maxval = 1;
end

% gain %
if (isfield(params,'gain'))
  gain = params.gain;
else
  gain = 1.15;
end

% maxatoms %
if (isfield(params,'maxatoms'))
  maxatoms = params.maxatoms;
else
  maxatoms = floor(prod(blocksize)/2);
end

% stepsize %
if (isfield(params,'stepsize'))
  stepsize = params.stepsize;
else
  stepsize = 1;
end
if (any(stepsize<1))
  error('Invalid step size.');
end

% noise mode %
if (isfield(params,'noisemode'))
  switch lower(params.noisemode)
    case 'psnr'
      sigma = maxval / 10^(params.psnr/20);
    case 'sigma'
      sigma = params.sigma;
    otherwise
      error('Invalid noise mode specified');
  end
elseif (isfield(params,'sigma'))
  sigma = params.sigma;
elseif (isfield(params,'psnr'))
  sigma = maxval / 10^(params.psnr/20);
else
  error('Noise strength not specified');
end

% lambda %
if (isfield(params,'lambda'))
  lambda = params.lambda;
else
  lambda = maxval/(10*sigma);
end

% msgdelta %
if (nargin <2)
  msgdelta = 5;
end
if (msgdelta<=0)
  msgdelta = -1;
end

epsilon = sqrt(prod(blocksize)) * sigma * gain;   % target error for omp

MEM_LOW = 1;
MEM_NORMAL = 2;
MEM_HIGH = 3;

if (isfield(params,'memusage'))
  switch lower(params.memusage)
    case 'low'
      memusage = MEM_LOW;
    case 'normal'
      memusage = MEM_NORMAL;
    case 'high'
      memusage = MEM_HIGH;
    otherwise
      error('Invalid memory usage mode');
  end
else
  memusage = MEM_NORMAL;
end

% denoise the signal %

if (memusage >= MEM_NORMAL)
  G = D'*D;
end

% process the signal in batches to conserve memory
% choose batchsize so im2col returns a matrix of approximately the same
% size as the signal
batchsize = ceil(length(x)*stepsize/blocksize + blocksize);

y = zeros(size(x));
ids = 1:min(batchsize,length(x));
nz = 0;

tid = timerinit('ompdenoise', length(x));
while (length(ids)>=blocksize)

  % extract the signal blocks
  blocks = im2col(x(ids),[blocksize 1],'sliding');
  blocks = blocks(:,1:stepsize:end);

  % remove DC
  [blocks, dc] = remove_dc(blocks,'columns');

  % denoise the blocks
  if (memusage == MEM_LOW)
    gamma = omp2(D,blocks,[],epsilon,'maxatoms',maxatoms);
  else
    gamma = omp2(D,blocks,G,epsilon,'maxatoms',maxatoms);
  end
  nz = nz + nnz(gamma);
  cleanblocks = add_dc(D*gamma, dc, 'columns');

  y(ids) = y(ids) + col2imsum(cleanblocks, [blocksize 1], [length(ids) 1], [stepsize 1]);
  ids = ids + floor((batchsize-blocksize)/stepsize)*stepsize + stepsize;
  if (ids(end)>length(x))
    ids = ids(ids<=length(x));
  end

  % display status
  if (msgdelta>0 && ~isempty(ids)>0)
    timereta(tid, ids(1), msgdelta);
  end

end

if (msgdelta>0)
  timereta(tid, length(x));
end

cnt = countcover(size(x),[blocksize 1],[stepsize 1]);
y = (y+lambda*x)./(cnt + lambda);
y = reshape(y,size(params.x));


function X = ttm(X,V,varargin)
%TTM Tensor times matrix for ktensor.
%
%   Y = TTM(X,A,N) computes the n-mode product of the ktensor X with a
%   matrix A; i.e., X x_N A.  The integer N specifies the dimension
%   (or mode) of X along which A should be multiplied.  If size(A) =
%   [J,I], then X must have size(X,N) = I.  The result will be a
%   ktensor of the same order and size as X except that size(Y,N) = J.
%
%   Y = TTM(X,{A,B,C,...}) computes the n-mode product of the ktensor
%   X with a sequence of matrices in the cell array.  The n-mode
%   products are computed sequentially along all dimensions (or modes)
%   of X. The cell array contains ndims(X) matrices.
%
%   Y = TTM(X,{A,B,C,...},DIMS) computes the sequence tensor-matrix
%   products along the dimensions specified by DIMS.
%
%   Y = TTM(...,'t') performs the same computations as above except
%   the matrices are transposed.
%
%   Examples
%   X = ktensor({rand(5,2),rand(3,2),rand(4,2),rand(2,2)});
%   A = rand(4,5); B = rand(4,3); C = rand(3,4); D = rand(3,2);
%   Y = ttm(X, A, 1)         %<-- computes X times A in mode-1
%   Y = ttm(X, {A,B,C,D}, 1) %<-- same as above
%   Y = ttm(X, A', 1, 't')   %<-- same as above
%   Y = ttm(X, {A,B,C,D}, [1 2 3 4]) %<-- 4-way multiply
%   Y = ttm(X, {D,C,B,A}, [4 3 2 1]) %<-- same as above
%   Y = ttm(X, {A,B,C,D})            %<-- same as above
%   Y = ttm(X, {A',B',C',D'}, 't')   %<-- same as above
%   Y = ttm(X, {C,D}, [3 4])     %<-- X times C in mode-3 & D in mode-4
%   Y = ttm(X, {A,B,C,D}, [3 4]) %<-- same as above
%   Y = ttm(X, {A,B,D}, [1 2 4])   %<-- 3-way multiply
%   Y = ttm(X, {A,B,C,D}, [1 2 4]) %<-- same as above
%   Y = ttm(X, {A,B,D}, -3)        %<-- same as above
%   Y = ttm(X, {A,B,C,D}, -3)      %<-- same as above
%
%   See also KTENSOR, KTENSOR/ARRANGE, TENSOR/TTM
%
%MATLAB Tensor Toolbox.
%Copyright 2012, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2012) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

%%%%%%%%%%%%%%%%%%%%%%
%%% ERROR CHECKING %%%
%%%%%%%%%%%%%%%%%%%%%%

% Check the number of arguments
if (nargin < 2)
    error('TTM requires at least two arguments.');
end

% Check for transpose option
isTranspose = false;
if numel(varargin) > 0
  if isnumeric(varargin{1});
    dims = varargin{1};
  end
  isTranspose =  (ischar(varargin{end}) && (varargin{end} == 't'));
end

% Check for dims argument
if ~exist('dims','var')
    dims = [];
end

% Check that 2nd argument is cell array. If not, recall with V as a
% cell array with one element.
if ~iscell(V)
    X = ttm(X,{V},dims,varargin{end});
    return;
end

% Get sorted dims and index for multiplicands
[dims,vidx] = tt_dimscheck(dims,ndims(X),numel(V));

% Determine correct size index
if isTranspose
  j = 1;
else
  j = 2;
end

% Check that each multiplicand is the right size.
for i = 1:numel(dims)
    if (ndims(V) ~= 2) || (size(V{vidx(i)},j) ~= size(X,dims(i)))
disp(size(V{vidx(i)}))
disp(size(X))

        error('Multiplicand is wrong size');
    end
end

% Do the multiplications in the specified modes.
for i = 1:numel(dims)
  if isTranspose
    X.u{dims(i)} = V{vidx(i)}'* X.u{dims(i)};
  else
    X.u{dims(i)} = V{vidx(i)} * X.u{dims(i)};
  end
end


function poly2 = clipConvexPolygon3dHP(poly, plane)
%CLIPCONVEXPOLYGON3DHP Clip a convex 3D polygon with Half-space
%
%   POLY2 = clipConvexPolygon3dHP(POLY, PLANE)
%   POLY is a N-by-3 array of points, and PLANE is given as:
%   [x0 y0 z0 dx1 dy1 dz1 dx2 dy2 dz2].
%   The result POLY2 is also an array of 3d points, sometimes smaller than
%   poly, and that can be 0-by-3 (empty polygon).
%
%   POLY2 = clipConvexPolygon3dHP(POLY, PT0, NORMAL)
%   uses plane with normal NORMAL and containing point PT0.
%
%
%   See also:
%   polygons3d, polyhedra
%
% ------
% Author: David Legland
% e-mail: david.legland@grignon.inra.fr
% Created: 2007-01-05
% Copyright 2007 INRA - BIA PV Nantes - MIAJ Jouy-en-Josas.

%   HISTORY
%   2007/14/09 fix postprocessing of last point

% ensure last point is the same as the first one
if sum(poly(end, :) == poly(1,:)) ~= 3
    poly = [poly; poly(1,:)];
end

% initialize empty polygon
poly2 = zeros(0, 2);

% compute visible points
below = isBelowPlane(poly, plane);

% case of empty polygon
if sum(below) == 0
    return;
end

% case of totally clipped polygon
if sum(below) == length(below)
    poly2 = poly;
    return;
end

% indices of edges intersecting the plane
ind = find(below ~= below([2:end 1]));

% compute intersection points: they are 2 for a convex polygon
lines = createLine3d(poly(ind, :), poly(ind+1, :));
pInt = intersectLinePlane(lines, plane);

% insert intersection points and remove invisible points
if below(1)
    poly2 = [poly(1:ind(1), :); pInt; poly(ind(2)+1:end, :)];
else
    poly2 = [pInt(1, :); poly(ind(1)+1:ind(2), :); pInt(2, :)];
end

% remove last point if it is the same as the first one
if sum(poly2(end, :) == poly2(1,:)) == 3
    poly2(end, :) = [];
end


function [ num_int, pint ] = plane_imp_triangle_int_add_3d ( p1, p2, dist1, ...
  dist2, num_int, pint )

%*****************************************************************************80
%
%% PLANE_IMP_TRIANGLE_INT_ADD_3D is a utility for plane/triangle intersections.
%
%  Discussion:
%
%    This routine is called to consider the value of the signed distance
%    from a plane of two nodes of a triangle.  If the two values
%    have opposite signs, then there is a point of intersection between
%    them.  The routine computes this point and adds it to the list.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 May 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real P1(3), P2(3), the coordinates of two vertices
%    of a triangle.
%
%    Input, real DIST1, DIST2, the signed distances of the
%    two vertices from a plane.
%
%    Input, integer NUM_INT, the number of intersection points.
%
%    Input, real PINT(3,NUM_INT), the intersection points.
%
%    Output, integer NUM_INT, the updated number of intersection points.
%
%    Output, real PINT(3,NUM_INT), the updated intersection points.
%
  dim_num = 3;

  if ( dist1 == 0.0 )
    num_int = num_int + 1;
    pint(1:dim_num,num_int) = p1(1:dim_num);
  elseif ( dist2 == 0.0 )
    num_int = num_int + 1;
    pint(1:dim_num,num_int) = p2(1:dim_num);
  elseif ( dist1 * dist2 < 0.0 )
    alpha = dist2 / ( dist2 - dist1 );
    num_int = num_int + 1;
    pint(1:dim_num,num_int) = alpha * p1(1:dim_num) + ( 1.0 - alpha ) * p2(1:dim_num);
  end

  return
end


%% FUNCTION Least_Trace
%   Trace-Norm Regularized Learning with Least Squares Loss.
%
%% OBJECTIVE
%   argmin_W { sum_i^t (0.5 * norm (Y{i} - X{i}' * W(:, i))^2)
%            + rho1 \|W\|_*}
%   where \|W\|_* = sum(svd(W, 0)) is the trace norm
%
%% INPUT
%   X: {n * d} * t - input matrix
%   Y: {n * 1} * t - output matrix
%   rho1: trace norm regularization parameter
%
%% OUTPUT
%   W: model: d * t
%   funcVal: function value vector.
%
%% LICENSE
%   This program is free software: you can redistribute it and/or modify
%   it under the terms of the GNU General Public License as published by
%   the Free Software Foundation, either version 3 of the License, or
%   (at your option) any later version.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU General Public License for more details.
%
%   You should have received a copy of the GNU General Public License
%   along with this program.  If not, see <http://www.gnu.org/licenses/>.
%
%   Copyright (C) 2011 - 2012 Jiayu Zhou and Jieping Ye
%
%   You are suggested to first read the Manual.
%   For any problem, please contact with Jiayu Zhou via jiayu.zhou@asu.edu
%
%   Last modified on June 3, 2012.
%
%% RELATED PAPERS
%
%   [1] Ji, S. and Ye, J. An Accelerated Gradient Method for Trace Norm Minimization, ICML 2009
%
%% RELATED FUNCTIONS
%   Logistic_Trace, init_opts

%% Code starts here
function [W, funcVal] = Least_Trace(X, Y, rho1, opts)

if nargin <3
    error('\n Inputs: X, Y, and rho1 should be specified!\n');
end
X = multi_transpose(X);

if nargin <4
    opts = [];
end

% initialize options.
opts=init_opts(opts);

if isfield(opts, 'rho_L2')
    rho_L2 = opts.rho_L2;
else
    rho_L2 = 0;
end

task_num  = length (X);
dimension = size(X{1}, 1);
funcVal = [];

% precomputation.
XY = cell(task_num, 1);
W0_prep = [];
for t_idx = 1: task_num
    XY{t_idx} = X{t_idx}*Y{t_idx};
    W0_prep = cat(2, W0_prep, XY{t_idx});
end

% initialize a starting point
if opts.init==2
    W0 = zeros(dimension, task_num);
elseif opts.init == 0
    W0 = W0_prep;
else
    if isfield(opts,'W0')
        W0=opts.W0;
        if (nnz(size(W0)-[dimension, task_num]))
            error('\n Check the input .W0');
        end
    else
        W0=W0_prep;
    end
end

bFlag=0; % this flag tests whether the gradient step only changes a little

Wz= W0;
Wz_old = W0;

t = 1;
t_old = 0;

iter = 0;
gamma = 1;
gamma_inc = 2;

while iter < opts.maxIter
    alpha = (t_old - 1) /t;

    Ws = (1 + alpha) * Wz - alpha * Wz_old;

    % compute function value and gradients of the search point
    gWs  = gradVal_eval(Ws);
    Fs   = funVal_eval  (Ws);

    while true
        [Wzp Wzp_tn] = trace_projection(Ws - gWs/gamma, 2 * rho1 / gamma);
        Fzp = funVal_eval  (Wzp);

        delta_Wzp = Wzp - Ws;
        r_sum = norm(delta_Wzp, 'fro')^2;
        %Fzp_gamma = Fs + trace(delta_Wzp' * gWs) + gamma/2 * norm(delta_Wzp, 'fro')^2;
        Fzp_gamma = Fs + sum(sum(delta_Wzp .* gWs)) + gamma/2 * norm(delta_Wzp, 'fro')^2;

        if (r_sum <=1e-20)
            bFlag=1; % this shows that, the gradient step makes little improvement
            break;
        end

        if (Fzp <= Fzp_gamma)
            break;
        else
            gamma = gamma * gamma_inc;
        end
    end

    Wz_old = Wz;
    Wz = Wzp;

    %funcVal = cat(1, funcVal, Fzp + rho1 * sum( svd(Wzp, 0) ));
    funcVal = cat(1, funcVal, Fzp + rho1 * Wzp_tn);

    if (bFlag)
        % fprintf('\n The program terminates as the gradient step changes the solution very small.');
        break;
    end

    % test stop condition.
    switch(opts.tFlag)
        case 0
            if iter>=2
                if (abs( funcVal(end) - funcVal(end-1) ) <= opts.tol)
                    break;
                end
            end
        case 1
            if iter>=2
                if (abs( funcVal(end) - funcVal(end-1) ) <=...
                        opts.tol* funcVal(end-1))
                    break;
                end
            end
        case 2
            if ( funcVal(end)<= opts.tol)
                break;
            end
        case 3
            if iter>=opts.maxIter
                break;
            end
    end

    iter = iter + 1;
    t_old = t;
    t = 0.5 * (1 + (1+ 4 * t^2)^0.5);

end

W = Wzp;

% private functions
    function [grad_W] = gradVal_eval(W)
        if opts.pFlag
            grad_W = zeros(size(W));
            parfor t_ii = 1:task_num
                XWi = X{t_ii}' * W(:,t_ii);
                XTXWi = X{t_ii}* XWi;
                grad_W(:, t_ii) = XTXWi - XY{t_ii};
                %grad_W = cat(2, grad_W, X{t_ii}*(X{t_ii}' * W(:,t_ii)-Y{t_ii}) );
            end
        else
            grad_W = [];
            for t_ii = 1:task_num
                XWi = X{t_ii}' * W(:,t_ii);
                XTXWi = X{t_ii}* XWi;
                grad_W = cat(2, grad_W, XTXWi - XY{t_ii});
                %grad_W = cat(2, grad_W, X{t_ii}*(X{t_ii}' * W(:,t_ii)-Y{t_ii}) );
            end
        end
        grad_W = grad_W + rho_L2 * 2 * W;
    end

    function [funcVal] = funVal_eval (W)
        funcVal = 0;
        if opts.pFlag
            parfor i = 1: task_num
                funcVal = funcVal + 0.5 * norm (Y{i} - X{i}' * W(:, i))^2;
            end
        else
            for i = 1: task_num
                funcVal = funcVal + 0.5 * norm (Y{i} - X{i}' * W(:, i))^2;
            end
        end
        funcVal = funcVal + rho_L2 * norm(W, 'fro')^2;
    end

end


% function x = synsq_cwt_iw(Tx, fs, opt)
%
% Inverse Synchrosqueezing transform of Tx with associated
% frequencies in fs.  This implements Eq. 5 of [1].
%
% 1. E. Brevdo, N.S. Fučkar, G. Thakur, and H-T. Wu, "The
% Synchrosqueezing algorithm: a robust analysis tool for signals
% with time-varying spectrum," 2011.
%
% Input:
%   Tx, fs: See help synsq_cwt_fw
%   opt: options structure (see help synsq_cwt_fw)
%      opt.type: type of wavelet used in synsq_cwt_fw
%
%      other wavelet options (opt.mu, opt.s) should also match
%      those used in synsq_cwt_fw
%
% Output:
%   x: reconstructed signal
%
% Example:
%   [Tx,fs] = synsq_cwt_fw(t, x, 32); % Synchrosqueezing
%   Txf = synsq_filter_pass(Tx, fs, -Inf, 1); % Pass band filter
%   xf = synsq_cwt_iw(Txf, fs);  % Filtered signal reconstruction
%
%---------------------------------------------------------------------------------
%    Synchrosqueezing Toolbox
%    Authors: Eugene Brevdo (http://www.math.princeton.edu/~ebrevdo/)
%---------------------------------------------------------------------------------
function x = synsq_cwt_iw(Tx, fs, opt)
    if nargin<3, opt = struct(); end
    if ~isfield(opt, 'type'), opt.type = 'morlet'; end

    % Find the admissibility coefficient Cpsi
    Css = synsq_adm(opt.type, opt);

    [na, N] = size(Tx);

    % Integration
    % Due to linear discretization of integral in log(fs), this becomes
    % a simple normalized sum.
    x = real(1/Css * ones(1,na) * Tx);
end


function [result]=sv_timeperdiff(mCatalog, fTimePeriod)
% function [result]=sv_timeperdiff(mCatalog, fTimePeriod)
% --------------------------------------------------------------------------------------
% Function to calculate absolute difference of number of events between
% two time periods of an earthquake catalog
% Author: J. Woessner
% woessner@seismo.ifg.ethz.ch
% last update: 08.07.02
%
% Incoming variables:
% mCatalog           : current earthquake catalog
% params.fTimePeriod : Time period in days
%
% Outgoing variable:
% result.dNdiffsumVal      : total difference of number of events in the two time periods
% result.dNdiffsumYearVal  : total difference of number of events in the two time periods normalized to a year
% result.dNdiffsumMonthVal : total difference of number of events in the two time periods normalized to a month
% result.dNdiff            : Difference of seismicity in 0.1 magnitude bins

% Init variable
result=[];

% Create the catalogs for two time periods
fStartTime = min(mCatalog(:,3));
fEndTime = max(mCatalog(:,3));

[result.mFirstCatalog, result.mSecondCatalog, result.fFirstPeriodExact, result.fSecondPeriodExact, result.fFirstPeriod,...
        result.fSecondPeriod] = ex_SplitCatalog(mCatalog, fSplitTime, 0, 100, 0, 100);

[dEv_val dMags dEv_valsum dEv_valsum_rev,  dMags_rev] =fcumulsum(result.mFirstCatalog);
[dEv_val2 dMags2 dEv_valsum2 dEv_valsum_rev2,  dMags_rev2] =fcumulsum(result.mSecondCatalog);

result.dNdiff = dEv_val2-dEv_val;
result.dNdiffYear=result.dNdiff/365;
result.dNdiffMonth = result.dNdiffYear/12;

result.dNdiffsum = cumsum(result.dNdiff');
result.dNdiffsumVal = result.dNdiffsum(length(result.dNdiffsum));
result.dNdiffsumYearVal = result.dNdiffsumVal/365;
result.dNdiffsumMonthVal = result.dNdiffsumYearVal/12;

result.dMags = dMags;
result.dMags = dMags2;
return


% SB2_PREPROCESSBASIS  Do basis matrix pre-processing for SPARSEBAYES
%
% [BASIS, SCALES] = SB2_PREPROCESSBASIS(BASIS)
%
% OUTPUT ARGUMENTS:
%
%	BASIS	NxM matrix of basis vectors appropriately pre-processed
%			(scaled to unit length per column)
%
%	SCALES	Vector of scaling factors applied
%
%
% INPUT ARGUMENTS:
%
%	BASIS	NxM matrix of basis vectors (one column per basis function)
%
% NOTES:
%
% Simply normalises the basis vectors to unit length, returning the
% original lengths so that the weights can be rescaled appropriately
% before returning to the user.
%

%
% Copyright 2009, Vector Anomaly Ltd
%
% This file is part of the SPARSEBAYES library for Matlab (V2.0).
%
% SPARSEBAYES 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.
%
% SPARSEBAYES 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 SPARSEBAYES in the accompanying file "licence.txt"; if not, write to
% the Free Software Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
% MA 02110-1301 USA
%
% Contact the author: m a i l [at] m i k e t i p p i n g . c o m
%
function [BASIS, Scales] = SB2_PreProcessBasis(BASIS)

%
[N,M]	= size(BASIS);
%
% Compute "lengths" of basis vectors (columns of BASIS)
%
Scales	= sqrt(sum(BASIS.^2));
%
% Work-around divide-by-zero inconvenience
%
Scales(Scales==0)	= 1;
%
% Normalise each basis vector to "unit length"
%
for m=1:M
  BASIS(:,m)	= BASIS(:,m) / Scales(m);
end


% ir_mri_al_p2_example.m
% Example of MRI SENSE reconstruction from under-sampled k-space data
% using the AL-P2 algorithm from Mar. 2011 T-MI paper.
% Sathish Ramani, circa 2011
% 2013-07-14, modified by Jeff Fessler

%% Test image - Shepp-Logan Phantom
if ~isvar('img')
	N = 256;
	if exist('phantom') % matlab with image processing toolbox
		img = phantom('Modified Shepp-Logan', N)';
		img = 256 * (img + 0.05);
	else
		img = ellipse_im(256);
		img = 256 * (img/max(img(:)) + 0.05);
	end
	[rs, cs] = size(img); Npix = rs*cs; mn = min(img(:)); mx = max(img(:));
	params.rs = rs;
	params.cs = cs;
	params.mn = mn;
	params.mx = mx;
	params.Npix = Npix;
	im plc 2 3
	im(1, img, 'true image')
end

%% sensitivity maps
if ~isvar('smap')
	smap = mri_sensemap_sim('nx', N, 'rcoil', 175, 'dx', 1);
	ncoils = size(smap, 3);
	params.ncoils = ncoils;
	im(2, abs(smap), '|sense maps|'), cbar
end

%% load sampling pattern
if ~isvar('SP')
	load('PDiskR256x256L1R2.252.25B0.8R1D1pctg20.2972.mat', 'SP', 'sampname');
	Ncentx = 8; % sample a window of 2*N around DC along x
	Ncenty = 8; % sample a window of 2*N around DC along y

	params.samp = sampname;
	SP = coverDC_SamplingMask(SP, Ncentx, Ncenty);
	lSP = length(find(SP>0)); % # of sampling points
	printm('Sampling %0.2f%% of k-space', lSP / Npix * 100)

	% Sampling pattern for the output of each coil
	SP3 = repmat(SP, [1 1 ncoils]);
	im(3, [-N/2:N/2-1], [-N/2:N/2-1], fftshift(SP), 'sampling pattern')
end

%% Data samples
if ~isvar('Data')
	SNR = 30; % SNR of noisy data
	smap_img = smap .* repmat(img, [1 1 ncoils]); % sensitivity-weighted images
	smap_Img = fft2(smap_img); % FFT of Sensitivity-weighted images

	% Add white Gaussian noise of desired SNR
	seedr = 0;
	stdnois = zeros(1, ncoils);
	noise_vr = zeros(rs, cs, ncoils);
	for ismap = 1:ncoils
		Dtemp = smap_Img(:,:,ismap);
		sigpow = sum(abs(Dtemp(:)).^2)/(rs*cs);
		stdnois(ismap) = sqrt(sigpow*10^(-SNR/10)); % Std. deviation of noise at ith coil
		randn('state', seedr + ismap);
		noise_realz = (randn(rs,cs) + j*randn(rs,cs))/sqrt(2);
		noise_vr(:, :, ismap) = noise_realz;
	end
	SQCStd = diag(stdnois);
	noisecorr = zeros(rs, cs, ncoils);
	for irK = 1 : ncoils % Apply the above correlation to the generated i.i.d noise to create correlated noise
		dump(1, 1, :) = SQCStd(irK, :);
		noisecorr(:, :, irK) = sum(repmat(dump, [rs cs 1]) .* noise_vr, 3);
	end
	fullData = smap_Img + noisecorr; % Noisy fully-sampled Data
	Data = fullData .* SP3; % Generate undersampled k-space data by applying a mask
	fftshift2 = @(x) fftshift(fftshift(Data, 1), 2);
	im(5, max(log(abs(fftshift2(Data))), 0), 'Data')
prompt
end

%% Simple iFFT / sum-of-squares recon of raw k-space data
if ~isvar('recon_sos')
	recon_sos = ifft2(Data);
	recon_sos = sqrt(sum(abs(recon_sos).^2, 3));
	params.recon_sos = recon_sos;
	im(4, recon_sos, 'SoS recon')
end

%% Initial Estimate = SoS + slight perturbation
if ~isvar('xini')
	randn('state', seedr);
	xini = recon_sos + (randn(rs,cs) + sqrt(-1) * randn(rs,cs))*0.001;
	errini = xini - img;
	errini = sqrt(sum(abs(errini(:)).^2)/Npix)/mx;
	xlabelf('nrmse %g dB', 20*log10(nrms(xini, img)))
	im(6, abs(xini-img), '|SoS error|')
prompt
end

%% Noise decorrelation in data and redefined
% (square root of inverse of noise correlation matrix weighted) sensitivity maps
if ~isvar('smap2')
	recon_F = Npix * ifft2(SP3 .* Data);
	recon_SF = sum(conj(smap) .* recon_F, 3);

	params.smap = smap;
	smap2 = sum(abs(smap).^2, 3); % S'S
%	im(smap2)
end

%% Penalty / regularizer / prior
if ~isvar('lod')
	params.Prior.PriorType{1} = 'TV'; % Type of penalty for wavelet coef.
	params.Prior.PriorType{2} = 'TV'; % Type of penalty for wavelet coef.

	% Type of sparsifying transform
	params.Operator = 'FD'; % finite differences
	% params.Operator = 'W'; % Redundant wavelet transform
	% params.Operator = 'WFD'; % Redundant wavelet transform & finite diff.

	% Wavelet Options
	params.Wavelet.redundancy = 'undecimated'; % Undecimated wavelet transform using wavelet filters corresponding to standard orthonormal wavelets
	params.Wavelet.wname = 'haar'; % undecimated Haar wavelets
	params.Wavelet.nlev = 2; % wavelet transform with 2 levels
	params.Wavelet.includeApprox = false; % exclude approximation level in the regularizer

%	todo: make it ok in octave
%	Uw = Gwave2('mask', true(rs,cs), 'nlevel', params.Wavelet.nlev)'; % trick

	dwtmode('per', 'nodisp'); % Period boundaries for wavelet implementation
	% params.Wavelet.redundancy = 'none'; % decimatSENSERecon_MRI029_Spiral6_23_2.1_SNR20_OpFD_PriorTV_Lam13.5_variablesed Orthonomal wavelet transform

	% Wavelet filters
	[lod, hid, lor, hir] = wfilters(params.Wavelet.wname);

	% Normalize filters so as to avoid a product with 0.5 during inverse undecimated wavelet transform
	params.Wavelet.lod = lod/sqrt(2);
	params.Wavelet.hid = hid/sqrt(2);
	params.Wavelet.lor = lor/sqrt(2);
	params.Wavelet.hir = hir/sqrt(2);
end

%% Parameters of the algorithms
if ~isvar('RR')
	% Regularization parameters for the wavelet and TV prior
	params.lambda = mean(stdnois .^ 2) * [1 0.1];
	params.precon = 0;
	params.betaD = 1e-10;
	params.xinf = zeros(rs,cs);
	params.xinfnorm = 1;
	params.dcosttol = 1e18;
	params.dxtol = 0;

	% Parameters for size of vectors
	[lzALP2 sALP2 eALP2 sr er] = get_Size_AuxVar(params);

	params.AL.lzALP2 = lzALP2;
	params.AL.sALP2 = sALP2;
	params.AL.eALP2 = eALP2;

	params.AL.sr = sr;
	params.AL.er = er;

	RR = compute_RR(params); % FFT of R^T * R
	mxRR = max(RR(:));
	mnRR = min(RR(:));
	params.AL.RR = RR;

	% Other setting
	params.figno = 1;
	params.subplot_img = 5;
	params.subplot_err = 6;
	params.maxitr = 400;
	params.maxitr_in = 1;
	params.dispfig = 1;
	params.dispitr = 1;
	params.dispitrnum = 10;
end

%% prepare to run AL-P2
if ~isvar('zALP2')
	mxsmap2 = max(smap2(:));
	mnsmap2 = min(smap2(:));
	condSS =  mxsmap2 / mnsmap2; % Condition number of S'S

	kapx = 0.9 * condSS;
	kapu0 = 24; % condition number of (FF + mu*I)
	kapu2 = 12; % condition number of (RR + nu2/n1*I)

	nu2 = (mxsmap2 - mnsmap2 * kapx) / (kapx - 1);
	mu = Npix / (kapu0 - 1); % min eig val of F'F = 0, while max eig val of F'F = N where the DFT matrix F is assumed to be un-normalized
	nu1 = (kapu2 - 1) * nu2 / (mxRR - mnRR * kapu2);
	nuratio = nu2 / nu1;

	% warn if any of the mu's is negative or less than eps
	if ((mu <= 0) || (nu1 <= 0) || (nu2 <= 0))
		error('At least one of the mu, nu''s is not positive');
	end

	% Inverse of some matrices required for solving sub-problems
	iPpmu = 1 ./ (Npix * SP3 + mu); % Freq. Response of (F'F + mu * I)^-1
	iSpnu2 = 1./(smap2 + nu2); % Inverse of (S'S + nu2 * I)
	iRpnu2nu1 = 1./(RR + nuratio); % Freq. Response of (R'R + nu2/nu1 * I)^-1

	params.AL.mu = mu;
	params.AL.nu1 = nu1;
	params.AL.nu2 = nu2;
	params.AL.iPpmu = iPpmu;
	params.AL.iSpnu2= iSpnu2;
	params.AL.iRpnu2nu1 = iRpnu2nu1;

	dALP2 = zeros(rs, cs, params.AL.lzALP2);
	zALP2 = dALP2;
end

%% Run AL-P2!
if ~isvar('x_est'), printm 'running al-p2'
	[x_est CALP2 TALP2 EALP2 ERRALP2] = SENSERecon_ALP2(img, ...
		SP3, Data, recon_F, dALP2, zALP2, xini, params);
	printm('Recon time = %g secs', sum(TALP2(:)))
	printm('Recon NRMSE = %g ', ERRALP2(end))
end


% test for line integral convolution

n = 180;
path(path, 'toolbox/');

rep = 'results/lic/';
if not(exist(rep))
    mkdir(rep);
end

options.bound = 'per';
%% generate a random irrotational vector field
sigma = 30;
v = perform_blurring(randn(n,n,2), sigma, options);
v = perform_vf_normalization(v);

options.histogram = 'gaussian';
options.histogram = 'linear';
options.verb = 1;
% size of the features
options.spot_size = 1.3;

%% original image
name = 'mandrill-color';
name = 'flowers';
name = 'rand';
options.dt = 0.5;
w_list = [4 6 8 10 12 14];
if strcmp(name, 'rand')
    sigma = 1.2;
    M0 = perform_blurring(randn(n), sigma, options);
    M0 = perform_histogram_equalization(M0, options.histogram);
else
    options.dt = 1.5;
    w_list = w_list * 2;
    options.histogram = [];
    M0 = load_image(name,n);
    M0 = rescale( crop(M0,n) );
end

%% iterated lic
M = M0;
w = 12;
for i=1:4
    options.M0 = M;
    M = perform_lic(v, w, options);
end

%% lic for increasing times
close all;
clf;
options.M0 = M0;
for i=1:min(length(w_list),6)
    M = perform_lic(v, w_list(i), options);
    imageplot(M, '', 2,3, i);
end

saveas(gcf, [rep name '-lic-results.png'], 'png');

return;

%% display with overlapping vector field
if size(M,3)==1
    figure; clf;
    sub = 4;
    plot_vf(v(1:sub:end, 1:sub:end,:), M);
    colormap gray(256);
    saveas(gcf, [rep, 'lic-vector-field.png'], 'png');
end


%% Copyright (C) 2014-2016, 2022 Colin B. Macdonald
%%
%% This file is part of OctSymPy.
%%
%% OctSymPy is free software; you can redistribute it and/or modify
%% it under the terms of the GNU General Public License as published
%% by the Free Software Foundation; either version 3 of the License,
%% or (at your option) any later version.
%%
%% This software 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 software; see the file COPYING.
%% If not, see <http://www.gnu.org/licenses/>.

%% -*- texinfo -*-
%% @documentencoding UTF-8
%% @defmethod  @@sym limit (@var{expr}, @var{x}, @var{a}, @var{dir})
%% @defmethodx @@sym limit (@var{expr}, @var{x}, @var{a})
%% @defmethodx @@sym limit (@var{expr}, @var{a})
%% @defmethodx @@sym limit (@var{expr})
%% Evaluate symbolic limits.
%%
%% The limit of @var{expr} as @var{x} tends to @var{a} from
%% @var{dir}.  @var{dir} can be @code{left} or @code{right}.
%%
%% Examples:
%% @example
%% @group
%% syms x
%% L = limit(sin(x)/x, x, 0)
%%   @result{} L = (sym) 1
%% L = limit(1/x, x, sym(inf))
%%   @result{} L = (sym) 0
%% L = limit(1/x, x, 0, 'left')
%%   @result{} L = (sym) -∞
%% L = limit(1/x, x, 0, 'right')
%%   @result{} L = (sym) ∞
%% @end group
%% @end example
%%
%% If @var{x} is omitted, @code{symvar} is used to determine the
%% variable.  If @var{a} is omitted, it defaults to 0.
%%
%% @var{dir} defaults to @code{right}.  Note this is different from
%% Matlab's Symbolic Math Toolbox which returns @code{NaN} for
%% @code{limit(1/x, x, 0)}
%% (and @code{+/-inf} if you specify @code{left/right}).  I'm not
%% sure how to get this nicer behaviour from SymPy.
%% FIXME: this is https://github.com/cbm755/octsympy/issues/74
%%
%% @seealso{@@sym/diff}
%% @end defmethod

function L = limit(f, x, a, dir)

  if (nargin > 4 || nargin < 1)
    print_usage ();
  end

  f = sym(f);
  if (nargin < 4)
    dir= 'right';
  end
  if (nargin == 2)
    a = x;
    x = symvar(f, 1);
  end
  if (nargin == 1)
    x = symvar(f, 1);
    a = 0;
  end

  switch (lower (dir))
    case {'left' '-'}
      pdir = '-';
    case {'right' '+'}
      pdir = '+';
    otherwise
      print_usage ();
  end

  if (isempty (x))
    L = f;
    return
  end

  L = elementwise_op ('lambda f, x, a, dir: f.limit(x, a, dir=dir)', ...
                      sym(f), sym(x), sym(a), pdir);
end

%!error limit (sym(1), 2, 3, 4, 5)

%!shared x, oo
%! syms x
%! oo = sym(inf);

%!assert (isa (limit(x, x, pi), 'sym'))

%!assert (isequal (limit(x, x, pi), sym(pi)))

%!assert (isequal (limit(sin(x)/x, x, 0), 1))

%!test
%! % left/right-hand limit
%! assert (isequal (limit(1/x, x, 0, 'right'), oo))
%! assert (isequal (limit(1/x, x, 0), oo))
%! assert (isequal (limit(1/x, x, 0, 'left'), -oo))
%! assert (isequal (limit(1/x, x, oo), 0))
%! assert (isequal (limit(sign(x), x, 0, 'left'), -1))
%! assert (isequal (limit(sign(x), x, 0, 'right'), 1))
%! assert (isequal (limit(sign(x), x, 0, '-'), -1))
%! assert (isequal (limit(sign(x), x, 0, '+'), 1))

%!test
%! % matrix
%! syms y
%! A = [x 1/x x*y];
%! B = sym([3 sym(1)/3 3*y]);
%! assert (isequal (limit(A, x, 3), B))

%!test
%! % omitting arguments
%! syms a
%! assert (isequal (limit(a), 0))
%! assert (isequal (limit(a*x+a+2), a+2))
%! assert (isequal (limit(a*x+a+2, 6), 7*a+2))

%!test
%! % constants
%! assert (isequal (limit(sym(6)), 6))
%! assert (isequal (limit(sym(6), 7), 6))
%! assert (isequal (limit([sym(6) sym(2)], 7), [6 2]))

%!test
%! % double constant, with sym limit
%! a = limit (6, sym(0));
%! assert (isa (a, 'sym'))
%! assert (isequal (a, sym(6)))


function [P,x] = polytope(X,options)
% polytope  Converts constraints to polytope object
%
% P     = polytope(F)
% [P,x] = polytope(F)
%
% P : polytope object (Requires the Multi-parametric Toolbox)
% x : sdpvar object defining the variables in the polytope P.H*x<=P.K
% F : Constraint object with linear inequalities
%
% See also sdpvar/polytope

if nargin < 2
    options = sdpsettings;
elseif isempty(options)
    options = sdpsettings;
end

[p,recoverdata,solver,diagnostic,F] = compileinterfacedata(X,[],[],[],options,0);

if any(p.K.q) || any(p.K.s) || any(p.variabletype)
  error('Polytope can only be applied to MILP-representable constraints.')
end

if any(p.K.f)
    try
        [P,x] = polyhedron(X,options);
        return
    catch
        disp('MPT does not support polytopes with empty interior')
        disp('Note that these equality constraints might have been generated internally by YALMIP')
        error('Functionality not yet supported')
    end
end

if isempty(p.binary_variables) && isempty(p.integer_variables)
    P = polytope(-p.F_struc(:,2:end),p.F_struc(:,1));
    x = recover(p.used_variables);
else
    nBin = length(p.binary_variables);
    [pBinary,removeEQ,removeLP] = extractOnly(p,p.binary_variables);
    p.F_struc = [p.F_struc(removeEQ,:);p.F_struc(p.K.f+removeLP,:)];
    p.K.f = length(removeEQ);
    p.K.l = length(removeLP);
    p.used_variables(p.binary_variables)=[];
    x = recover(p.used_variables);

    P = [];
    for i = 0:2^nBin-1
        comb = dec2decbin(i,nBin);
        if checkfeasiblefast(pBinary,comb(:),1e-6)
            pi = p;
            H = -p.F_struc(:,2:end);% Hx < K
            K = p.F_struc(:,1);
            K = K-H(:,p.binary_variables)*comb(:);
            H(:,p.binary_variables)=[];
            P = [P polytope(H,K)];
        end
    end
end

function pLP = extractLP(p)
pLP = p;
pLP.F_struc = pLP.F_struc(1:p.K.f+p.K.l,:);
pLP.K.q = 0;
pLP.K.s = 0;

function [pRed,removeEQ,removeLP] = extractOnly(p,these)
pRed = p;
p.F_struc(:,1+these) = 0;

removeEQ = find(any(p.F_struc(1:pRed.K.f,2:end),2));
removeLP = find(any(p.F_struc(1+pRed.K.f:end,2:end),2));
pRed.F_struc(pRed.K.f+removeLP,:)=[];
pRed.F_struc(removeEQ,:)=[];
pRed.K.f = pRed.K.f - length(removeEQ);
pRed.K.l = pRed.K.l - length(removeLP);
pRed.F_struc = pRed.F_struc(:,[1 1+these]);
pRed.lb = pRed.lb(these);
pRed.ub = pRed.ub(these);


function msm_to_mm_test10 ( )

%*****************************************************************************80
%
%% MSM_TO_MM_TEST10 tests MSM_TO_MM_ARRAY_REAL_SYMMETRIC.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 November 2008
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'MSM_TO_MM_TEST10\n' );
  fprintf ( 1, '  Convert an MSM to MM array real symmetric format.\n' );

  output_filename = 'msm_to_mm_test10.mm';

  a = r8mat_indicator ( 4, 4 );
  a = a + a';
%
%  Have MSM_TO_MM write the matrix to a file.
%
  msm_to_mm ( output_filename, a, 'array', 'real', 'symmetric' );

  return
end


% angle to the closest roi
function [data,units] = compute_angle2closestroi2(trx,n)

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

for i = 1:nflies,
  fly = flies(i);

  % roi closest to fly
  closestroi = trx(fly).closestroi2;

  % position of fly
  xnose_mm1 = trx(fly).xnose_mm;
  ynose_mm1 = trx(fly).ynose_mm;
  theta_mm1 = trx(fly).theta_mm;

  ROIdata=trx.roi2{n}.data{fly};

  % loop over all rois
  for j = 1:size(ROIdata,1),

    % frames where this roi is closest
    idx = find(closestroi == j);
    if isempty(idx), continue; end

    % angle to roi
    dx2 = ROIdata(j,1)-xnose_mm1(idx);
    dy2 = ROIdata(j,2)-ynose_mm1(idx);
    theta2 = atan2(dy2,dx2);

    % angle relative to fly's orientation
    data{i}(idx) = modrange(theta2 - theta_mm1(idx),-pi,pi);

  end
end

units = parseunits('rad');


function [sys,x0,str,ts]=ADRC_3(t,x,u,flag,h,TD,ESO,NLSEF,b0)

switch flag
    case 0
        [sys,x0,str,ts]=mdlInitializeSizes(h);
    case 2
        sys=mdlUpdate(x,u,h,TD,ESO,b0);
    case 3
        sys=mdlOutputs(x,NLSEF,b0);
    case 4,
        sys=mdlGetTimeOfNextVarHit(t,h);
    case {1,9}
        sys=[];
    otherwise
        error(['Unhandled flag=',num2str(flag)]);
end
function [sys,x0,str,ts]=mdlInitializeSizes(h)
    sizes=simsizes;
    sizes.NumContStates=0;
    sizes.NumDiscStates=7;
    sizes.NumOutputs=2;
    sizes.NumInputs=3;
    sizes.DirFeedthrough=1;
    sizes.NumSampleTimes=1;
    sys=simsizes(sizes);
    x0=[0;0;0;0;0;0;0];
    str=[];
    ts=[h 0];
function sys=mdlUpdate(x,u,h,TD,ESO,b0)
    e1=x(1)-u(1);
    fh=fhan(e1,x(2),TD(1),TD(2));
    sys(1)=x(1)+h*x(2);
    sys(2)=x(2)+h*x(3);
    sys(3)=x(3)+h*fh;
    e2=x(4)-u(2);
    sys(4)=x(4)+h*(x(5)-ESO(1)*e2);
    sys(5)=x(5)+h*(x(6)-ESO(2)*fal(e2,0.5,ESO(5)));
    sys(6)=x(6)+h*(x(7)-ESO(3)*fal(e2,0.25,ESO(5))+b0*u(3));
    sys(7)=x(7)+h*(-ESO(4)*fal(e2,0.125,ESO(5)));

%     fe=fal(e,0.5,Delta);
%     fe1=fal(e,0.25,Delta);
%     sys(1,1)=x(1)+h2*(x(2)-BB(1)*e);
%     sys(2,1)=x(2)+h2*(x(3)-BB(2)*fe+u(1));
%     sys(3,1)=x(3)+h2*(-BB(3)*fe1);
function sys=mdlOutputs(x,NLSEF,b0)
    e3=x(1)-x(4);
    e4=x(2)-x(5);
    e5=x(3)-x(6);
    %sys(1)=NLSEF(1)*e3+NLSEF(2)*e4+NLSEF(3)*e5-x(7)/b0;
    sys(1)=NLSEF(1)*fal(e3,0.5,NLSEF(4))+NLSEF(2)*fal(e4,0.75,NLSEF(4))+NLSEF(3)*fal(e5,1.5,NLSEF(4))-x(7)/b0;
    sys(2)=x(1);
function sys=mdlGetTimeOfNextVarHit(t,h)
    sys=t+h;

function y=fhan(x1,x2,r,h)
d=r*h;
d0=h*d;
y=x1+h*x2;
a0=sqrt(d^2+8*r*abs(y));
if abs(y)>d0
    a=x2+(a0-d)*sign(y)/2;
else
    a=x2+y/h;
end

if abs(a)>d
    y=-r*sign(a);
else
    y=-r*a/d;
end

function f=fal(e,a,d)
    if abs(e)<d
        f=e*d^(a-1);
    else f=(abs(e))^a*sign(e);
    end


function Q = ForwardStepIC2D(x2d, y2d, time)

% function Q = ForwardStepIC2D(x2d, y2d, time)
% Purpose: compute plane flow configuration

Globals2D;

gamma = 1.4;

rho = gamma*ones(Np,K);  p = ones(Np,K);

% M = |u|/c,  c = sqrt(gamma*p/rho)
rhou = rho.*(3*ones(Np, K)); rhov = zeros(Np, K);
Ener = p/(gamma-1.0) + (rhou.^2+rhov.^2)./(2*rho);

Q(:,:,1) = rho; Q(:,:,2) = rhou; Q(:,:,3) = rhov; Q(:,:,4) = Ener;
return


function [E,ER] = arap_energy(V,F,U,varargin)
  % ARAP_ENERGY This function is meant as a very human readable groundtruth for
  % evaluating ARAP energies.
  % For faster energy computation see `arap_gradient.m`
  %
  % Inputs:
  %    V  #V by dim list of mesh vertex rest positions
  %    F  #F by simplex list of element indices into V
  %    U  #V by dim list of deformed vertex positions
  %   Optional:
  %     'Energy' followed by 'spokes','spokes-and-rims', or 'elements'
  %       {'elements' or 'spokes-and-rims'} for tets or triangles respectively.
  % Outputs:
  %    E  scalar energy
  %    ER  #R list of "per-rotation" energy contributions
  %
  %
  % Known issues:
  % If E_G is the energy returned by arap_gradient, then
  % 'spokes-and-rims' energy E = 3 E_G
  % 'spokes' energy E = 2 E_G
  % 'elements' energy E = E_G
  %

  switch size(F,2)
  case 4
    energy = 'elements';
  case 3
    energy = 'spokes-and-rims';
  end
  % default values
  % Map of parameter names to variable names
  params_to_variables = containers.Map( {'Energy'}, ...
    {'energy'});
  v = 1;
  while v <= numel(varargin)
    param_name = varargin{v};
    if isKey(params_to_variables,param_name)
      assert(v+1<=numel(varargin));
      v = v+1;
      % Trick: use feval on anonymous function to use assignin to this workspace
      feval(@()assignin('caller',params_to_variables(param_name),varargin{v}));
    else
      error('Unsupported parameter: %s',varargin{v});
    end
    v=v+1;
  end

  dim = size(V,2);
  C = cotangent(V,F);

  n = size(V,1);
  m = size(F,1);
  switch energy
  case 'spokes'
    nr = size(V,1);
    edge_set = cell(nr,1);
    switch size(F,2)
    case 4
      AC = sparse( ...
        F(:,[2 3 1 4 4 4 3 1 2 1 2 3]),F(:,[3 1 2 1 2 3 2 3 1 4 4 4]), ...
        [C C],n,n);
    case 3
      AC = sparse(F(:,[2 3 1 3 1 2]),F(:,[3 1 2 2 3 1]),[C C],n,n);
    end
    % loop over vertices
    for r = 1:nr
      [Jr,~,Cr] = find(AC(:,r));
      Ir = repmat(r,numel(Jr),1);
      edge_set{r}.E = [Ir Jr];
      edge_set{r}.C = Cr;
    end
  case {'elements','spokes-and-rims'}
    nr = size(F,1);
    switch size(F,2)
    case 3
      I = F(:,[2 3 1]);
      J = F(:,[3 1 2]);
    case 4
      I = F(:,[2 3 1 4 4 4]);
      J = F(:,[3 1 2 1 2 3]);
    end
    edge_set = cell(nr,1);
    % loop over elements
    for r = 1:nr
      edge_set{r}.E = [I(r,:)' J(r,:)'];
      edge_set{r}.C = C(r,:);
    end
    if strcmp(energy,'spokes-and-rims')
      Fedge_set = edge_set;
      nr = size(V,1);
      edge_set = cell(nr,1);
      V2F = sparse( ...
        F,repmat(1:size(F,1),size(F,2),1)',repmat(1:size(F,2),size(F,1),1),n,m);
      % loop over vertices
      for r = 1:nr
        [~,Fr] = find(V2F(r,:));
        edge_set{r}.E = [];
        edge_set{r}.C = [];
        for f = Fr
          edge_set{r}.E = [edge_set{r}.E;Fedge_set{f}.E];
          edge_set{r}.C = [edge_set{r}.C Fedge_set{f}.C];
        end
      end

    end

  end

  ER = zeros(nr,1);
  % loop over rotations
  for r = 1:nr
    % get edges for this rotation
    Er = edge_set{r}.E;
    Cr = edge_set{r}.C;
    S = zeros(dim,dim);
    % loop over edges to build covariance matrix for rotation fitting
    for e = 1:size(Er,1)
      i = Er(e,1);
      j = Er(e,2);
      ev = V(j,:)-V(i,:);
      eu = U(j,:)-U(i,:);
      S = S + Cr(e) * eu'*ev;
    end
    R = fit_rotation(S);
    % Loop over edges to compute energy contribution
    for e = 1:size(Er,1)
      i = Er(e,1);
      j = Er(e,2);
      ev = V(j,:)-V(i,:);
      eu = U(j,:)-U(i,:);
      %ER(r) = ER(r) + 0.5 * Cr(e) * (eu - ev*R)*(eu - ev*R)';
      %ER(r) = ER(r) + 0.5 * Cr(e) * (eu*eu' - 2*ev*R*eu' + ev*R*R'*ev');
      ER(r) = ER(r) + 0.5 * Cr(e) * (eu*eu' - 2*ev*R*eu' + ev*ev');
    end
  end
  E = sum(ER);
end


function [x, infos] = incremental_mu_nmf(V, rank, in_options)
% Incremental non-negative matrix factorization (NMF) with outliers (INCNMF) algorithm.
%
% Inputs:
%       matrix      V
%       rank        rank
%       in_options  options
% Output:
%       w           solution of w
%       infos       information
%
% References:
%       S. S. Bucak, B. Gunsel,
%       "Incremental Subspace Learning via Non-negative Matrix Factorization,"
%       Pattern Recognition, 2009.
%
%
% This file is part of NMFLibrary.
%
% Created by H.Kasai on Feb. 12, 2017
%
% Change log:
%
%       Oct. 27, 2017 (Hiroyuki Kasai): Fixed algorithm.
%
%       May. 20, 2019 (Hiroyuki Kasai): Added initialization module.
%
%       Jul. 12, 2022 (Hiroyuki Kasai): Modified code structures.
%

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

    % set local options
    local_options = [];
    local_options.max_inneriter     = 1;
    local_options.online            = 0;
    local_options.tolcostdegrease   = 1e-8;
    local_options.alpha             = 0.5;
    local_options.beta              = 0.5;

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

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

    % initialize
    method_name = 'Incremental-MU';
    epoch = 0;
    grad_calc_count = 0;

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

    if options.online
        ht = H(:, end); %why
    else
        % Do nothing
    end
    A = V * H';
    B = H * H';

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

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

    % set start time
    start_time = tic();

    % main outer loop
    while true

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

        % main inner loop
        for t = 1 : options.batch_size : n - 1

            vt = V(:, t:t+options.batch_size -1);

            % Need to be considered more carefully
            if options.online
                % Do nothing (??)
            else
                ht = H(:, t:t+options.batch_size-1);
            end

            for j = 1 : options.max_inneriter

                % Compute new ht (column vector at t)
                ht = ht .* (W' * vt) ./ (W' * (W * ht) + 1e-9);
                ht = ht + (ht<eps) .* eps;

                % Compute new W
                vht = vt * ht';
                hht = ht * ht';
                A_tmp = options.beta * A + options.alpha * vht;
                B_tmp = options.beta * B + options.alpha * hht;
                W = W .* ( A_tmp ./ (W * B_tmp + 1e-9) ); % Add 1e-9 to avoid 0 in the denom.
                W = W + (W<eps) .* eps;

               if j > 1
                    oldobj = newobj;
                end
                newobj = ((sum(sum((vt-W*ht).^2)))/m);

                if options.verbose > 2
                    fprintf('\t[%d-%d-%d] %e\n', epoch, t, j, newobj);
                end

                if j > 1 && (oldobj-newobj)/newobj < options.tolcostdegrease
                    break;
                end

            end

            grad_calc_count = grad_calc_count + m * options.batch_size;

            % update A and B
            A = options.beta * A + options.alpha * vht;
            B = options.beta * B + options.alpha * hht;

            % update ht (columb of H at t)
            H(:,t:t+options.batch_size-1) = ht;

            if options.verbose > 2
                % measure cost
                f_val = nmf_cost(V, W, H, []);
                fprintf('%s: inner [%03d-%03d] %e\n', method_name, epoch, t, f_val);
            end
        end

        % measure elapsed time
        elapsed_time = toc(start_time);

        % update epoch
        epoch = epoch + 1;

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

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

    end

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

end


function  [fx,dfdx,dfdp] = f_Qlearning(x,P,u,in)
% // VBA toolbox //////////////////////////////////////////////////////////
%
% [fx,dfdx,dfdP] = f_Qlearning(x,P,u,in)
% Reinforcement-learning evolution function for a n-armed bandit task
%
% An RL agent learns by trial and error. A bandit task is such that, after
% each action, the agent receives a feedback (reward if positive,
% punishment if negative). The RL agent updates its action values as
% follows:
% V(chosen action) = V(chosen action) + alpha*(feedback-V(chosen action))
% V(unchosen action) = V(unchosen action)
% IN:
%	- x: action values (n x 1)
%	- P: learning rate (will be sigmoid transformed)
%	- u: (1) previous action
%        (2) feedback received for previous action
%	- in: [useless]
% OUT:
%   - fx: updated action values
%   - dfdx/dfdP: gradients for VBA inversion

% /////////////////////////////////////////////////////////////////////////

% Get parameter values
% =========================================================================
% Some of the model parameters can only take values within a certain range.
% However, the VBA toolbox can only perform estimation of unbounded values.
% To solve this, we transform the values passed by VBA to map them on the
% acceptable range for each parameter.

% learning rate
alpha = VBA_sigmoid(P); % [-Inf,Inf] -> [0 1]

% Apply delta-rule to update action values
% =========================================================================

% get experimental conditions
prevActionIdx = u(1)+1; % action 0 is first index
feedback = u(2);

% start with previous values
fx = x;

% udpdate previous action value
delta = feedback - x(prevActionIdx);
fx(prevActionIdx) = x(prevActionIdx) + alpha*delta; % update chosen value

% Compute evolution function's gradient
% =========================================================================
% This is not necessary, as the toolbox will approximate those gradients if
% needed. However, providing the analytical gradient can higly speed-up the
% inversion.

n = numel(x);

% derivative w.r.t hidden state
% -------------------------------------------------------------------------
dfdx = eye(n);
dfdx(prevActionIdx,prevActionIdx) = 1 - alpha;

% derivative w.r.t parameters
% -------------------------------------------------------------------------
dfdp = zeros(1,n);
dfdp(prevActionIdx) = alpha*(1-alpha)*delta;