text:
%DEMO_REGRESSION_MEANF  Regression problem demonstration for GP model with a
%                       mean function
%
%  Description
%    The regression problem consist of a data with one input
%    variable and one output variable with Gaussian noise. The
%    problem is modelled with Full GP model with gaussian
%    likelihood and a specified mean function. The mean function m
%    is a weighted sum of some basis functions h, where
%
%                   m=h'*Beta
%
%    and we have set a gaussian prior for the weights Beta
%
%                   Beta ~ N(b,B)
%
%    Inference is done according to Rasmussen and Williams (2006)
%    p. 27-29.
%
%    In this demonstration the data is from a function:
%
%         y = 2 + x + x^2 + 4*cos(x)*sin(x) + epsilon
%
%    and we define the base functions to be:
%
%       h1(x)=x^2, h2(x)=x, h3(x)=2
%
%    References:
%    Rasmussen, C. E. and Williams, C. K. I. (2006). Gaussian
%    Processes for Machine Learning. The MIT Press.

% Copyright (c) 2010 Tuomas Nikoskinen

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

% Create the data
 x=[-2:0.6:2]';
 res=4*cos(x).*sin(x)+0.4*randn(size(sin(x)));
 y= 2 + x + x.^2 + res;
%---------------

gpcf = gpcf_sexp('lengthScale', [0.5], 'magnSigma2', .5);
lik = lik_gaussian('sigma2', 0.4^2);

% Initialize base functions for GP's mean function.
gpmf1 = gpmf_constant('prior_mean',.3,'prior_cov',1);
gpmf2 = gpmf_linear('prior_mean',.3,'prior_cov',1);
gpmf3 = gpmf_squared('prior_mean',.3,'prior_cov',1);

% Initialize gp structure
gp = gp_set('lik', lik, 'cf', gpcf, 'meanf', {gpmf1,gpmf2,gpmf3});

% Set the options for the optimization
opt=optimset('TolFun',1e-3,'TolX',1e-3,'DerivativeCheck','on');
% Optimize with the scaled conjugate gradient method
gp=gp_optim(gp,x,y,'opt',opt);

% Predictions
xt=[-3:0.1:3]';
[Eft, Varft] = gp_pred(gp, x, y, xt);

% PLOT THE DATA

figure
m=plot(xt,Eft,'k','lineWidth',2);
hold on
plot(xt,Eft+2*sqrt(Varft),'k--')
hold on
m95=plot(xt,Eft-2*sqrt(Varft),'k--');
hold on
hav=plot(x,y, 'ro','markerSize',6,'MarkerFaceColor','r');
hold on
h=plot(xt,2+xt+xt.^2+4*cos(xt).*sin(xt),'b--','lineWidth',2);
hold on
mmmean=plot(xt,2+xt+xt.^2,'r--','lineWidth',1);
legend([m m95 h mmmean hav],'prediction','95%','f(x)','mean function','observations');
xlabel('input x')
ylabel('output y')
title('GP regression with a mean function')

author: fieldtrip

repo: fieldtrip

save_path: github-repos/MATLAB/fieldtrip-fieldtrip


text:
function value = p04_i4 ( action, name, value )

%*****************************************************************************80
%
%% P04_I4 sets or gets I4 parameters for problem 04.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 March 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, string ACTION,
%    'D' to set a parameter to its default value.
%    'S' to set a parameter,
%    'G' to get a parameter,
%    'I' to increment a parameter.
%
%    Input, string NAME, the name of the variable.
%    '#' is the number of calls to the integrand routine.
%
%    Input/output, integer VALUE.
%    * If ACTION = 'I', then VALUE is an input quantity, and is the
%      new value to be added to NAME.
%    * If ACTION = 'S', then VALUE is an input quantity, and is the
%      new value to be assigned to NAME.
%    * If ACTION = 'G', then VALUE is an output quantity, and is the
%      current value of NAME.
%
  persistent calls;

  if ( action == 'D' | action == 'd' )

    if ( name == '#' | name == '*' )
      calls = 0;
    end

  elseif ( action == 'G' | action == 'g' )

    if ( name == '#' )
      value = calls;
    else
      fprintf ( 1, '\n' );
      fprintf ( 1, 'P04_I4 - Fatal error!\n' );
      fprintf ( 1, '  Unrecognized name = "%s".\n', name );
      error ( 'P04_I4 - Fatal error!' );
    end

  elseif ( action == 'I' | action == 'i' )

    if ( name == '#' )
      calls = calls + value;
    else
      fprintf ( 1, '\n' );
      fprintf ( 1, 'P04_I4 - Fatal error!\n' );
      fprintf ( 1, '  Unrecognized name = "%s".\n', name );
      error ( 'P04_I4 - Fatal error!' );
    end

  elseif ( action == 'S' | action == 's' )

    if ( name == '#' )
      calls = value;
    else
      fprintf ( 1, '\n' );
      fprintf ( 1, 'P04_I4 - Fatal error!\n' );
      fprintf ( 1, '  Unrecognized name = "%s".\n', name );
      error ( 'P04_I4 - Fatal error!' );
    end

  else

    fprintf ( 1, '\n' );
    fprintf ( 1, 'P04_I4 - Fatal error!\n' );
    fprintf ( 1, '  Unrecognized action = "%s".\n', action );
    error ( 'P04_I4 - Fatal error!' );

  end

  return
end

author: johannesgerer

repo: jburkardt-m


text:
function [points,weights] = get_spherical_grid(number,conf)
%GET_SPHERICAL_GRID spherical grid points and weights
%
%   Usage: [points,weights] = get_spherical_grid(number,conf)
%
%   Input parameters:
%       number  - number of grid points
%       conf    - configuration struct (see SFS_config)
%
%   Output parameters:
%       points  - grid points
%       weights - integration weights for the grid points
%
%   GET_SPHERICAL_GRID(number,conf) returns the points and weights for a grid on
%   a sphere. The type of grid is specified by conf.secondary_sources.grid.
%   For available grids, have a look at https://github.com/sfstoolbox/data.
%   It expects the grid files at SFS_basepath/data/spherical_grids. If the
%   desired file is not available on the hard disk, the function tries to
%   download it directly from github.
%   For conf.secondary_sources.grid='gauss' the grid positions are calculated
%   after Ahrens (2012), p. 121 (see also: Rafaely (2015), p. 64)
%
%   See also: secondary_source_positions,
%       weights_for_points_on_a_sphere_rectangle
%
%   References:
%       Ahrens (2012) - "Analytic Methods of Sound Field Synthesis", Springer,
%       ISBN 978-3-642-25743-8
%
%       Rafaely (2015) - "Fundamentals of Spherical Array Processing", Springer,
%       ISBN 978-3-662-45664-4

%*****************************************************************************
% The MIT License (MIT)                                                      *
%                                                                            *
% Copyright (c) 2010-2019 SFS Toolbox Developers                             *
%                                                                            *
% 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, including without limitation *
% the rights  to use, copy, modify, merge,  publish, distribute, sublicense, *
% and/or  sell copies of  the Software,  and to permit  persons to whom  the *
% Software is furnished to do so, 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, EXPRESS OR *
% IMPLIED, INCLUDING BUT  NOT LIMITED TO THE  WARRANTIES OF MERCHANTABILITY, *
% FITNESS  FOR A PARTICULAR  PURPOSE AND  NONINFRINGEMENT. IN NO EVENT SHALL *
% THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER *
% LIABILITY, WHETHER  IN AN  ACTION OF CONTRACT, TORT  OR OTHERWISE, ARISING *
% FROM,  OUT OF  OR IN  CONNECTION  WITH THE  SOFTWARE OR  THE USE  OR OTHER *
% DEALINGS IN THE SOFTWARE.                                                  *
%                                                                            *
% The SFS Toolbox  allows to simulate and  investigate sound field synthesis *
% methods like wave field synthesis or higher order ambisonics.              *
%                                                                            *
% https://sfs.readthedocs.io                            sfstoolbox@gmail.com *
%*****************************************************************************

%% ===== Checking input parameters =======================================
nargmin = 2;
nargmax = 2;
narginchk(nargmin,nargmax);
isargpositivescalar(number);
isargstruct(conf);

%% ===== Configuration ===================================================
spherical_grid = conf.secondary_sources.grid;

%% ===== Main ============================================================
filename = sprintf('%06.0fpoints.mat',number);
basepath = get_sfs_path();

if strcmp('equally_spaced_points',spherical_grid)
    % Check if we have a squared number of points, because only for those values
    % are equally spaced points grids available.
    if mod(number,sqrt(number))~=0
        error('%s: number has to be a squared number.',upper(mfilename));
    end
    file = [basepath '/data/spherical_grids/equally_spaced_points/' filename];
    url = ['https://raw.githubusercontent.com/sfstoolbox/data/master/' ...
           'spherical_grids/equally_spaced_points/' filename];
    % Download file if not present
    if ~exist(file,'file')
        download_file(url,file);
    end
    tmp = load(file,'-ascii');
    points = tmp(:,1:3);
    weights = tmp(:,4);
elseif strcmp('fabian',spherical_grid)
    % Here we have only one number of secondary sources available
    if number~=11345
        error('%s: this grid is only available for 11345 sources.', ...
            upper(mfilename));
    end
    file = [basepath '/data/spherical_grids/fabian/' filename];
    url = ['https://raw.githubusercontent.com/sfstoolbox/data/master/' ...
           'spherical_grids/fabian/' filename];
    % Download file if not present
    if ~exist(file,'file')
        download_file(url,file);
    end
    tmp = load(file,'-ascii');
    points = tmp(:,1:3);
    weights = tmp(:,4);
elseif strcmp('gauss',spherical_grid)
    % The number of secondary sources needs to be 2,8,18,32, ... ,
    % see Ahrens (2012)
    if mod(number,sqrt(number/2))~=0
        error(['%s: the number of secondary sources needs to be ', ...
            '2*n^2 for a gauss grid.'],upper(mfilename));
    end
    number = sqrt(number/2);
    % Get gauss points and weights
    [p,w] = legpts(number);
    % Sampling points along azimuth
    PHI = linspace(0,2*pi,2*number+1);
    % Remove the last one, because phi=0 and phi=2pi are the same
    PHI = PHI(1:end-1);
    % Sampling points along elevation
    THETA = acos(p)-pi/2;
    % Get grid points
    [phi,theta] = meshgrid(PHI,THETA);
    [~,weights] = meshgrid(PHI,w);
    r = ones(size(phi));
    % Convert to cartesian
    [points(:,1) points(:,2) points(:,3)] = sph2cart(phi(:),theta(:),r(:));
    % Normalize integration weights
    weights = weights(:)*pi/number;
else
    error(['%s: the given spherical grid is not available, have a look at ' ...
        'https://github.com/sfstoolbox/data for avialable grids.'], ...
        upper(mfilename));
end

author: sfstoolbox

repo: sfs-matlab


text:
function [pvec, pstruct] = tapas_ehgf_transp(r, ptrans)
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2012-2020 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

pvec    = NaN(1,length(ptrans));
pstruct = struct;

l = r.c_prc.n_levels;

pvec(1:l)         = ptrans(1:l);                           % mu_0
pstruct.mu_0      = pvec(1:l);
pvec(l+1:2*l)     = exp(ptrans(l+1:2*l));                  % sa_0
pstruct.sa_0      = pvec(l+1:2*l);
pvec(2*l+1:3*l)   = ptrans(2*l+1:3*l);                     % rho
pstruct.rho       = pvec(2*l+1:3*l);
pvec(3*l+1:4*l-1) = exp(ptrans(3*l+1:4*l-1));              % ka
pstruct.ka        = pvec(3*l+1:4*l-1);
pvec(4*l:5*l-1)   = ptrans(4*l:5*l-1);                     % om
pstruct.om        = pvec(4*l:5*l-1);
pvec(5*l)         = exp(ptrans(5*l));                      % pi_u
pstruct.pi_u      = pvec(5*l);

end

author: translationalneuromodeling

repo: tapas


text:
function output = calllmirank(interfacedata)

% Retrieve needed data
options = interfacedata.options;
F_struc = interfacedata.F_struc;
c       = interfacedata.c;
K       = interfacedata.K;
x0      = interfacedata.x0;
ub      = interfacedata.ub;
lb      = interfacedata.lb;

% Bounded variables converted to constraints
if ~isempty(ub)
    [F_struc,K] = addStructureBounds(F_struc,K,ub,lb);
end

pars = options.lmirank;
pars.fid = double(options.verbose);

if ~options.warmstart
    interfacedata.options.verbose =  max(0,interfacedata.options.verbose - 1);
    initialsolver = eval(['@' interfacedata.solver.initialsolver.call]);
    start = K.l;
    b = zeros(size(F_struc,2)-1,1);
    for j=1:length(K.s)
        if K.s(j)~=K.rank(j)
            ind = find(speye(K.s(j)));
            for i=1:size(F_struc,2)-1
                b(i,1) = b(i,1) + sum(F_struc(start+ind,1+i));
            end
        end
        start=start+K.s(j)^2;
    end
    interfacedata.c = b;
    interfacedata.solver = interfacedata.solver.initialsolver;
    output = feval(initialsolver,interfacedata);
    if output.problem ~= 1
        options.warmstart = 1;
        x0 = output.Primal;
    else
        return;
    end
end

if options.savedebug
    At = -F_struc(:,2:end);
    C  = F_struc(:,1);
    save lmirankdebug C At K pars x0
end

if options.showprogress;showprogress(['Calling ' interfacedata.solver.tag],options.showprogress);end
solvertime = tic;
if options.warmstart
    [y,info] = lmirank(-F_struc(:,2:end),F_struc(:,1),K,pars,x0);
else
    [y,info] = lmirank(-F_struc(:,2:end),F_struc(:,1),K,pars);
end
solvertime = toc(solvertime);
x = y;

switch info.solved
    case 1
        problem = 0;
    otherwise
        problem = 11;
end

% Save ALL data sent to solver
if options.savesolverinput
    solverinput.At = -F_struc(:,2:end);
    solverinput.c = F_struc(:,1);
    solverinput.K = K;
    solverinput.pars = pars;
    solverinput.x0 = x0;
else
    solverinput = [];
end

% Save ALL data from the solution?
if options.savesolveroutput
    solveroutput.y = y;
    solveroutput.info = info;
else
    solveroutput = [];
end

% Standard interface
output = createOutputStructure(x,[],[],problem,interfacedata.solver.tag,solverinput,solveroutput,solvertime);

author: yalmip

repo: YALMIP

save_path: github-repos/MATLAB/yalmip-YALMIP


text:
function [] = prints(a,b)
%
%
%     Print Simulink model as EPS-file to the current working directory
%
%     Syntax: PRINTS('FILENAME','<OPTION>')
%
%     Filename must be given without extension, e.g. 'MyFile'.
%
%     Options: p = Portrait, l = Landscape
%     No option returns the default 'Portrait' paperorientation.
%
%     Written by Bjřrn R. Sřrensen (1997)
%     Narvik Institute of Technology
%     N-8505 Norway
%     E-mail: brs@hin.no
%
%     Tested on versions 5.0-5.3 of Matlab (simulink 3.0)
%
%
filename=[a];
if nargin==2
   if b=='p'
      % Set 'Paper orientation' 'Portrait'
      set_param(filename, 'PaperOrientation','Portrait');
   end;
   if b=='l'
      % Set 'Paper orientation' 'Landscape'
      set_param(filename, 'PaperOrientation','Landscape');
   end;
end;
if nargin==1
   set_param(filename, 'PaperOrientation','Portrait');
end;
eval(['print -deps -s ', filename]);

author: Sable

repo: mcbench-benchmarks


text:
% A demo of Bayesian CP factorization for image completion
% Written by Qibin Zhao 2014 RIKEN BSI
%
% In this demo, we provide two algorithms including BCPF_IC and BCPF_MP.
% BCPF_IC is a Bayesian CP for image completion; BCPF_MP is a Bayesian CP
% using mixture priors, which is particularly useful for natural image
% completion. For algorithm settings, please refer to the detailed help by
% >> help BCPF_MP

% The experimental data can be tested with
% 1) Different image files
% 2) Observation rate (1-missing rate)
% The predictive image can be online visualized during model learning.
% The performance of RSE, PSNR, SSIM, Time Cost are evaluated and reported.

close all; clear all;
randn('state',1); rand('state',1); %#ok<RAND>
%% Load image data
filename='./TestImages/peppers.bmp';    % Image file
ObsRatio = 0.1;                      % Observation rate

X = double(imread(filename));
DIM = size(X);

Omega = randperm(prod(DIM));
Omega = Omega(1:round(ObsRatio*prod(DIM)));
O = zeros(DIM);
O(Omega) = 1;
Y = O.*X;

% plot images
subplot = @(m,n,p) subtightplot (m, n, p, [0.01 0.01], [0.01 0.01], [0.01 0.01]);
row =1; col =2;
figure;
subplot(row,col,1);
imshow(uint8(X));
subplot(row,col,2);
imshow(uint8(Y));
drawnow;

% Initialization
TimeCost = zeros(2,1);
RSElist = zeros(2,3);
PSNRlist = zeros(2,1);
SSIMlist = zeros(2,1);
RankEst = zeros(2,1);

%% BCPF for low-rank images completion
tStart = tic;
fprintf('------Bayesian CP factorization for Image Completion---------- \n');
[model] = BCPF_IC(Y, 'obs', O, 'init', 'rand', 'maxRank', 100, 'maxiters', 20, ...
    'tol', 1e-4, 'dimRed', 1, 'verbose', 2);
X_FBCP = double(model.X);
RSElist(1,1) = perfscore(X_FBCP, X);
RSElist(1,2) = perfscore(X_FBCP(O==1), X(O==1));
RSElist(1,3) = perfscore(X_FBCP(O==0), X(O==0));

X_FBCP(O==1) = X(O==1);
PSNRlist(1) = PSNR_RGB(X_FBCP,X);
SSIMlist(1) = ssim_index(rgb2gray(uint8(X_FBCP)),rgb2gray(uint8(X)));
RankEst(1) = model.TrueRank;
TimeCost(1) = toc(tStart);
% figure; imshow(uint8(X_FBCP)); title('FBCP','FontWeight','bold'); drawnow;

%% BCPF-MP (mixture priors) for natural images
if ~isempty(strfind(filename,'facade.bmp'))
    nd=0.1;    % low-rank structural images
else
    nd=1;      % natural images
end
tStart = tic;
fprintf('------Bayesian CP with Mixture Priors for Image Completion---------- \n');
[model] = BCPF_MP(Y, 'obs', O, 'init', 'rand', 'maxRank', 100, 'maxiters', 30, ...
    'tol', 1e-4, 'dimRed', 1, 'verbose', 2, 'nd', nd);
X_FBCPS = double(model.X);

RSElist(2,1) = perfscore(X_FBCPS, X);
RSElist(2,2) = perfscore(X_FBCPS(O==1), X(O==1));
RSElist(2,3) = perfscore(X_FBCPS(O==0), X(O==0));

X_FBCPS(O==1) = X(O==1);
PSNRlist(2) = PSNR_RGB(X_FBCPS,X);
SSIMlist(2) = ssim_index(rgb2gray(uint8(X_FBCPS)),rgb2gray(uint8(X)));
RankEst(2) = model.TrueRank;
TimeCost(2) = toc(tStart);
% figure; imshow(uint8(X_FBCPS)); title('FBCP-MP','FontWeight','bold'); drawnow;

%%
RankEst
RSElist
PSNRlist
SSIMlist
TimeCost

author: andrewssobral


text:
function [DEM] = spm_ADEM(DEM)
% Dynamic expectation maximisation:  Active inversion
% FORMAT DEM   = spm_ADEM(DEM)
%
% DEM.G  - generative process
% DEM.M  - recognition  model
% DEM.C  - causes
% DEM.U  - prior expectation of causes
%__________________________________________________________________________
%
% This implementation of DEM is the same as spm_DEM but integrates both the
% generative process and model inversion in parallel. Its functionality is
% exactly the same apart from the fact that confounds are not accommodated
% explicitly.  The generative model is specified by DEM.G and the veridical
% causes by DEM.C; these may or may not be used as priors on the causes for
% the inversion model DEM.M (i.e., DEM.U = DEM.C).  Clearly, DEM.G does not
% require any priors or precision components; it will use the values of the
% parameters specified in the prior expectation fields.
%
% This routine is not used for model inversion per se but to simulate the
% dynamical inversion of models.  Critically, it includes action
% variables a - that couple the model back to the generative process
% This enables active inference (c.f., action-perception) or embodied
% inference.
%
% hierarchical models M(i)
%--------------------------------------------------------------------------
%   M(i).g  = y(t)  = g(x,v,P)    {inline function, string or m-file}
%   M(i).f  = dx/dt = f(x,v,P)    {inline function, string or m-file}
%
%   M(i).pE = prior expectation of p model-parameters
%   M(i).pC = prior covariances of p model-parameters
%   M(i).hE = prior expectation of h hyper-parameters (cause noise)
%   M(i).hC = prior covariances of h hyper-parameters (cause noise)
%   M(i).gE = prior expectation of g hyper-parameters (state noise)
%   M(i).gC = prior covariances of g hyper-parameters (state noise)
%   M(i).Q  = precision components (input noise)
%   M(i).R  = precision components (state noise)
%   M(i).V  = fixed precision (input noise)
%   M(i).W  = fixed precision (state noise)
%   M(i).xP = precision (states)
%
%   M(i).m  = number of inputs v(i + 1);
%   M(i).n  = number of states x(i)
%   M(i).l  = number of output v(i)
%   M(i).k  = number of action a(i)

% hierarchical process G(i)
%--------------------------------------------------------------------------
%   G(i).g  = y(t)  = g(x,v,a,P)    {inline function, string or m-file}
%   G(i).f  = dx/dt = f(x,v,a,P)    {inline function, string or m-file}
%
%   G(i).pE = model-parameters
%   G(i).U  = precision (action)
%   G(i).V  = precision (input noise)
%   G(i).W  = precision (state noise)
%
%   G(1).R  = restriction or rate matrix for action [default: 1];
%   G(i).aP = precision (action)   [default: exp(-2)]
%
%   G(i).m  = number of inputs v(i + 1);
%   G(i).n  = number of states x(i)
%   G(i).l  = number of output v(i)
%   G(i).k  = number of action a(i)
%
%
% Returns the following fields of DEM
%--------------------------------------------------------------------------
%
% true model-states - u
%--------------------------------------------------------------------------
%   pU.x    = true hidden states
%   pU.v    = true causal states v{1} = response (Y)
%   pU.C    = prior covariance: cov(v)
%   pU.S    = prior covariance: cov(x)
%
% model-parameters - p
%--------------------------------------------------------------------------
%   pP.P    = parameters for each level
%
% hyper-parameters (log-transformed) - h,g
%--------------------------------------------------------------------------
%   pH.h    = cause noise
%   pH.g    = state noise
%
% conditional moments of model-states - q(u)
%--------------------------------------------------------------------------
%   qU.a    = Action
%   qU.x    = Conditional expectation of hidden states
%   qU.v    = Conditional expectation of causal states
%   qU.z    = Conditional prediction errors (v)
%   qU.C    = Conditional covariance: cov(v)
%   qU.S    = Conditional covariance: cov(x)
%
% conditional moments of model-parameters - q(p)
%--------------------------------------------------------------------------
%   qP.P    = Conditional expectation
%   qP.C    = Conditional covariance
%
% conditional moments of hyper-parameters (log-transformed) - q(h)
%--------------------------------------------------------------------------
%   qH.h    = Conditional expectation (cause noise)
%   qH.g    = Conditional expectation (state noise)
%   qH.C    = Conditional covariance
%
% F         = log evidence = log marginal likelihood = negative free energy
%__________________________________________________________________________
%
% spm_ADEM implements a variational Bayes (VB) scheme under the Laplace
% approximation to the conditional densities of states (u), parameters (p)
% and hyperparameters (h) of any analytic nonlinear hierarchical dynamic
% model, with additive Gaussian innovations.  It comprises three
% variational steps (D,E and M) that update the conditional moments of u, p
% and h respectively
%
%                D: qu.u = max <L>q(p,h)
%                E: qp.p = max <L>q(u,h)
%                M: qh.h = max <L>q(u,p)
%
% where qu.u corresponds to the conditional expectation of hidden states x
% and causal states v and so on.  L is the ln p(y,u,p,h|M) under the model
% M. The conditional covariances obtain analytically from the curvature of
% L with respect to u, p and h.
%
% The D-step is embedded in the E-step because q(u) changes with each
% sequential observation.  The dynamical model is transformed into a static
% model using temporal derivatives at each time point.  Continuity of the
% conditional trajectories q(u,t) is assured by a continuous ascent of F(t)
% in generalised co-ordinates.  This means DEM can deconvolve online and
% represents an alternative to Kalman filtering or alternative Bayesian
% update procedures.
%
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_ADEM.m 7679 2019-10-24 15:54:07Z spm $

% check model, data, priors and unpack
%--------------------------------------------------------------------------
DEM   = spm_ADEM_set(DEM);
M     = DEM.M;
G     = DEM.G;
C     = DEM.C;
U     = DEM.U;

% check whether to print
%--------------------------------------------------------------------------
try
    db = DEM.db;
catch
    db = 1;
end

% find or create a DEM figure
%--------------------------------------------------------------------------
if db
    Fdem = spm_figure('GetWin','DEM');
end

% ensure embedding dimensions are compatible
%--------------------------------------------------------------------------
G(1).E.n = M(1).E.n;
G(1).E.d = M(1).E.n;

% order parameters (d = n = 1 for static models) and checks
%==========================================================================
d    = M(1).E.d + 1;                      % embedding order of q(v)
n    = M(1).E.n + 1;                      % embedding order of q(x)
s    = M(1).E.s;                          % smoothness - s.d. (bins)

% number of states and parameters - generative model
%--------------------------------------------------------------------------
nY   = size(C,2);                         % number of samples
nl   = size(M,2);                         % number of levels
nv   = sum(spm_vec(M.m));                 % number of v (causal states)
nx   = sum(spm_vec(M.n));                 % number of x (hidden states)
ny   = M(1).l;                            % number of y (inputs)
nc   = M(end).l;                          % number of c (prior causes)
nu   = nv*d + nx*n;                       % number of generalised states

% number of states and parameters - generative process
%--------------------------------------------------------------------------
gr   = sum(spm_vec(G.l));                 % number of v (outputs)
ga   = sum(spm_vec(G.k));                 % number of a (active states)
gx   = sum(spm_vec(G.n));                 % number of x (hidden states)
gy   = G(1).l;                            % number of y (inputs)
na   = ga;                                % number of a (action)

% number of iterations
%--------------------------------------------------------------------------
try, nE = M(1).E.nE; catch, nE = 16; end
try, nM = M(1).E.nM; catch, nM = 8;  end
try, dt = M(1).E.dt; catch, dt = 1;  end

% initialise regularisation parameters
%--------------------------------------------------------------------------
te = 2;                                   % log integration time for E-Step
global t

% precision (roughness) of generalised fluctuations
%--------------------------------------------------------------------------
iV    = spm_DEM_R(n,s);
iG    = spm_DEM_R(n,s);

% time-delay operators (absorb motor delays into motor gain matrix)
%--------------------------------------------------------------------------
try
    nG = norm(iG);
    iG = iG*spm_DEM_T(n,-M(1).Ta);
    iG = iG*nG/norm(iG);
end
try
    Ty = spm_DEM_T(n,-M(1).Ty);
    Ty = kron(Ty,speye(ny,ny));
end

% precision components Q{} requiring [Re]ML estimators (M-Step)
%==========================================================================
Q     = {};
for i = 1:nl
    q0{i,i} = sparse(M(i).l,M(i).l); %#ok<AGROW>
    r0{i,i} = sparse(M(i).n,M(i).n);
end
Q0    = kron(iV,spm_cat(q0));
R0    = kron(iV,spm_cat(r0));
for i = 1:nl
    for j = 1:length(M(i).Q)
        q          = q0;
        q{i,i}     = M(i).Q{j};
        Q{end + 1} = blkdiag(kron(iV,spm_cat(q)),R0);
    end
    for j = 1:length(M(i).R)
        q          = r0;
        q{i,i}     = M(i).R{j};
        Q{end + 1} = blkdiag(Q0,kron(iV,spm_cat(q)));
    end
end

% and fixed components P
%--------------------------------------------------------------------------
Q0    = kron(iV,spm_cat(spm_diag({M.V})));
R0    = kron(iV,spm_cat(spm_diag({M.W})));
Qp    = blkdiag(Q0,R0);
nh    = length(Q);                           % number of hyperparameters
iR    = [zeros(1,ny),ones(1,nv),ones(1,nx)]; % for empirical priors
iR    = kron(speye(n,n),diag(iR));

% restriction or rate matrices - in terms of precision
%--------------------------------------------------------------------------
q0{1} = G(1).U;
Q0    = kron(iG,spm_cat(q0));
R0    = kron(iG,spm_cat(r0));
iG    = blkdiag(Q0,R0);

% restriction or rate matrices - in terms of dE/da
%--------------------------------------------------------------------------
try
    R         = sparse(sum(spm_vec(G.l)),na);
    R(1:ny,:) = G(1).R;
    R         = kron(spm_speye(n,1,0),R);
catch
    R = 1;
end

% fixed priors on action (a)
%--------------------------------------------------------------------------
try
    aP = G(1).aP;
catch
    aP = exp(-2);
end

% fixed priors on states (u)
%--------------------------------------------------------------------------
xP    = spm_cat(spm_diag({M.xP}));
Px    = kron(iV(1:n,1:n),speye(nx,nx)*exp(-8) + xP);
Pv    = kron(iV(1:d,1:d),speye(nv,nv)*exp(-8));
Pa    = spm_speye(na,na)*aP;
Pu    = spm_cat(spm_diag({Px Pv}));

% hyperpriors
%--------------------------------------------------------------------------
ph.h  = spm_vec({M.hE M.gE});             % prior expectation of h
ph.c  = spm_cat(spm_diag({M.hC M.gC}));   % prior covariances of h
qh.h  = ph.h;                             % conditional expectation
qh.c  = ph.c;                             % conditional covariance
ph.ic = spm_inv(ph.c);                    % prior precision

% priors on parameters (in reduced parameter space)
%==========================================================================
pp.c  = cell(nl,nl);
qp.p  = cell(nl,1);
for i = 1:(nl - 1)

    % eigenvector reduction: p <- pE + qp.u*qp.p
    %----------------------------------------------------------------------
    qp.u{i}   = spm_svd(M(i).pC);                    % basis for parameters
    M(i).p    = size(qp.u{i},2);                     % number of qp.p
    qp.p{i}   = sparse(M(i).p,1);                    % initial qp.p
    pp.c{i,i} = qp.u{i}'*M(i).pC*qp.u{i};            % prior covariance

    try
        qp.e{i} = qp.p{i} + qp.u{i}'*(spm_vec(M(i).P) - spm_vec(M(i).pE));
    catch
        qp.e{i} = qp.p{i};                           % initial qp.e
    end

end
Up    = spm_cat(spm_diag(qp.u));

% initialise and augment with confound parameters B; with flat priors
%--------------------------------------------------------------------------
np    = sum(spm_vec(M.p));                  % number of model parameters
pp.c  = spm_cat(pp.c);
pp.ic = spm_inv(pp.c);

% initialise conditional density q(p) (for D-Step)
%--------------------------------------------------------------------------
qp.e  = spm_vec(qp.e);
qp.c  = sparse(np,np);

% initialise cell arrays for D-Step; e{i + 1} = (d/dt)^i[e] = e[i]
%==========================================================================
qu.x      = cell(n,1);
qu.v      = cell(n,1);
qu.a      = cell(1,1);
qu.y      = cell(n,1);
qu.u      = cell(n,1);
pu.v      = cell(n,1);
pu.x      = cell(n,1);
pu.z      = cell(n,1);
pu.w      = cell(n,1);

[qu.x{:}] = deal(sparse(nx,1));
[qu.v{:}] = deal(sparse(nv,1));
[qu.a{:}] = deal(sparse(na,1));
[qu.y{:}] = deal(sparse(ny,1));
[qu.u{:}] = deal(sparse(nc,1));
[pu.v{:}] = deal(sparse(gr,1));
[pu.x{:}] = deal(sparse(gx,1));
[pu.z{:}] = deal(sparse(gr,1));
[pu.w{:}] = deal(sparse(gx,1));

% initialise cell arrays for hierarchical structure of x[0] and v[0]
%--------------------------------------------------------------------------
qu.x{1}   = spm_vec({M(1:end - 1).x});
qu.v{1}   = spm_vec({M(1 + 1:end).v});
qu.a{1}   = spm_vec({G.a});
pu.x{1}   = spm_vec({G.x});
pu.v{1}   = spm_vec({G.v});

% derivatives for Jacobian of D-step
%--------------------------------------------------------------------------
Dx    = kron(spm_speye(n,n,1),spm_speye(nx,nx,0));
Dv    = kron(spm_speye(d,d,1),spm_speye(nv,nv,0));
Dc    = kron(spm_speye(d,d,1),spm_speye(nc,nc,0));
Da    = kron(spm_speye(1,1,1),sparse(na,na));
Du    = spm_cat(spm_diag({Dx,Dv}));
Dq    = spm_cat(spm_diag({Dx,Dv,Dc,Da}));

Dx    = kron(spm_speye(n,n,1),spm_speye(gx,gx,0));
Dv    = kron(spm_speye(n,n,1),spm_speye(gr,gr,0));
Dp    = spm_cat(spm_diag({Dv,Dx,Dv,Dx}));
dfdw  = kron(speye(n,n),speye(gx,gx));
dydv  = kron(speye(n,n),speye(gy,gr));

% and null blocks
%--------------------------------------------------------------------------
dVdc  = sparse(d*nc,1);

% gradients and curvatures for conditional uncertainty
%--------------------------------------------------------------------------
dWdu  = sparse(nu,1);
dWduu = sparse(nu,nu);

% preclude unnecessary iterations
%--------------------------------------------------------------------------
if ~np && ~nh, nE = 1; end

% create innovations (and add causes)
%--------------------------------------------------------------------------
[z,w]  = spm_DEM_z(G,nY);
z{end} = C + z{end};
a      = {G.a};
Z      = spm_cat(z(:));
W      = spm_cat(w(:));
A      = spm_cat(a(:));

% Iterate DEM
%==========================================================================
F      = -Inf;
for iE = 1:nE

    % get time and clear persistent variables in evaluation routines
    %----------------------------------------------------------------------
    tic; clear spm_DEM_eval

    % E-Step: (with embedded D-Step)
    %======================================================================

    % [re-]set accumulators for E-Step
    %----------------------------------------------------------------------
    dFdp  = zeros(np,1);
    dFdpp = zeros(np,np);
    EE    = sparse(0);
    ECE   = sparse(0);
    EiSE  = sparse(0);
    qp.ic = sparse(0);
    Hqu.c = sparse(0);

    % [re-]set precisions using [hyper]parameter estimates
    %----------------------------------------------------------------------
    iS    = Qp;
    for i = 1:nh
       iS = iS + Q{i}*exp(qh.h(i));
    end

    % precision for empirical priors
    %----------------------------------------------------------------------
    iP    = iR*iS*iR;

    % [re-]set states & their derivatives
    %----------------------------------------------------------------------
    try
        qu = qU(1);
        pu = pU(1);
    end

    % D-Step: (nY samples)
    %======================================================================
    for iY = 1:nY

        % time (GLOBAL variable for non-automomous systems)
        %------------------------------------------------------------------
        t      = iY/nY;

        % pass action to pu.a (external states)
        %==================================================================
        try, A = spm_cat({qU.a qu.a}); end

        % derivatives of responses and random fluctuations
        %------------------------------------------------------------------
        pu.z = spm_DEM_embed(Z,n,iY);
        pu.w = spm_DEM_embed(W,n,iY);
        pu.a = spm_DEM_embed(A,n,iY);
        qu.u = spm_DEM_embed(U,n,iY);

        % evaluate generative process
        %------------------------------------------------------------------
        [pu,dg,df] = spm_ADEM_diff(G,pu);

        % and pass response to qu.y
        %==================================================================
        for i = 1:n
            y       = spm_unvec(pu.v{i},{G.v});
            qu.y{i} = y{1};
        end

        % sensory delays
        %------------------------------------------------------------------
        try, qu.y = spm_unvec(Ty*spm_vec(qu.y),qu.y); end

        % evaluate generative model
        %------------------------------------------------------------------
        [E,dE] = spm_DEM_eval(M,qu,qp);

        % conditional covariance [of states {u}]
        %------------------------------------------------------------------
        qu.c   = spm_inv(dE.du'*iS*dE.du + Pu);
        pu.c   = spm_inv(dE.du'*iP*dE.du + Pu);
        Hqu.c  = Hqu.c + spm_logdet(qu.c);

        % save at qu(t)
        %------------------------------------------------------------------
        qE{iY} = E;
        qC{iY} = qu.c;
        pC{iY} = pu.c;
        qU(iY) = qu;
        pU(iY) = pu;

        % and conditional precision
        %------------------------------------------------------------------
        if nh
            ECEu  = dE.du*qu.c*dE.du';
            ECEp  = dE.dp*qp.c*dE.dp';
        end

        % uncertainty about parameters dWdv, ... ; W = ln(|qp.c|)
        %==================================================================
        if np
            for i = 1:nu
                CJp(:,i)   = spm_vec(qp.c*dE.dpu{i}'*iS);
                dEdpu(:,i) = spm_vec(dE.dpu{i}');
            end
            dWdu  = CJp'*spm_vec(dE.dp');
            dWduu = CJp'*dEdpu;
        end

        % tensor products for Jacobian (generative process)
        %------------------------------------------------------------------
        Dgda  = kron(spm_speye(n,1,1),dg.da);
        Dgdv  = kron(spm_speye(n,n,1),dg.dv);
        Dgdx  = kron(spm_speye(n,n,1),dg.dx);
        dfda  = kron(spm_speye(n,1,0),df.da);
        dfdv  = kron(spm_speye(n,n,0),df.dv);
        dfdx  = kron(spm_speye(n,n,0),df.dx);

        dgda  = kron(spm_speye(n,1,0),dg.da);
        dgdx  = kron(spm_speye(n,n,0),dg.dx);

        % change in error w.r.t. action
        %------------------------------------------------------------------
        Dfdx  = 0;
        for i = 1:n
            Dfdx = Dfdx + kron(spm_speye(n,n,-i),df.dx^(i - 1));
        end

        % dE/da with restriction (R)
        %------------------------------------------------------------------
        dE.dv = dE.dy*dydv;
        dE.da = dE.dv*((dgda + dgdx*Dfdx*dfda).*R);

        % first-order derivatives
        %------------------------------------------------------------------
        dVdu  = -dE.du'*iS*E - Pu*spm_vec({qu.x{1:n} qu.v{1:d}}) - dWdu/2;
        dVda  = -dE.da'*iG*E - Pa*spm_vec( qu.a{1:1});

        % and second-order derivatives
        %------------------------------------------------------------------
        dVduu = -dE.du'*iS*dE.du - Pu - dWduu/2 ;
        dVdaa = -dE.da'*iG*dE.da - Pa;
        dVduv = -dE.du'*iS*dE.dv;
        dVduc = -dE.du'*iS*dE.dc;
        dVdua = -dE.du'*iS*dE.da;
        dVdav = -dE.da'*iG*dE.dv;
        dVdau = -dE.da'*iG*dE.du;
        dVdac = -dE.da'*iG*dE.dc;

        % D-step update: of causes v{i}, and hidden states x(i)
        %==================================================================

        % states and conditional modes
        %------------------------------------------------------------------
        p     = {pu.v{1:n} pu.x{1:n} pu.z{1:n} pu.w{1:n}};
        q     = {qu.x{1:n} qu.v{1:d} qu.u{1:d} qu.a{1:1}};
        u     = [p q];

        % gradient
        %------------------------------------------------------------------
        dFdu  = [                              Dp*spm_vec(p);
                 spm_vec({dVdu; dVdc; dVda}) + Dq*spm_vec(q)];

        % Jacobian (variational flow)
        %------------------------------------------------------------------
        dFduu = spm_cat(...
                {Dgdv  Dgdx Dv   []   []       []    Dgda;
                 dfdv  dfdx []   dfdw []       []    dfda;
                 []    []   Dv   []   []       []    [];
                 []    []   []   Dx   []       []    [];
                 dVduv []   []   []   Du+dVduu dVduc dVdua;
                 []    []   []   []   []       Dc    []
                 dVdav []   []   []   dVdau    dVdac dVdaa});

        % update states q = {x,v,z,w} and conditional modes
        %==================================================================
        du    = spm_dx(dFduu,dFdu,dt);
        u     = spm_unvec(spm_vec(u) + du,u);

        % and save them
        %------------------------------------------------------------------
        pu.v(1:n) = u((1:n));
        pu.x(1:n) = u((1:n) + n);
        qu.x(1:n) = u((1:n) + n + n + n + n);
        qu.v(1:d) = u((1:d) + n + n + n + n + n);
        qu.a(1:1) = u((1:1) + n + n + n + n + n + d + d);

        % Gradients and curvatures for E-Step: W = tr(C*J'*iS*J)
        %==================================================================
        if np
            for i = 1:np
                CJu(:,i)   = spm_vec(qu.c*dE.dup{i}'*iS);
                dEdup(:,i) = spm_vec(dE.dup{i}');
            end
            dWdp  = CJu'*spm_vec(dE.du');
            dWdpp = CJu'*dEdup;

            % Accumulate; dF/dP = <dL/dp>, dF/dpp = ...
            %--------------------------------------------------------------
            dFdp  = dFdp  - dWdp/2  - dE.dp'*iS*E;
            dFdpp = dFdpp - dWdpp/2 - dE.dp'*iS*dE.dp;
            qp.ic = qp.ic           + dE.dp'*iS*dE.dp;

        end

        % accumulate SSE
        %------------------------------------------------------------------
        EiSE = EiSE + E'*iS*E;

        % and quantities for M-Step
        %------------------------------------------------------------------
        if nh
            EE  = E*E'+ EE;
            ECE = ECE + ECEu + ECEp;
        end

        if nE == 1

            % evaluate objective function (F)
            %======================================================================
            J(iY) = - trace(E'*iS*E)/2  ...            % states (u)
                    + spm_logdet(qu.c)  ...            % entropy q(u)
                    + spm_logdet(iS)/2;                % entropy - error
        end

    end % sequence (nY)

    % augment with priors
    %----------------------------------------------------------------------
    dFdp   = dFdp  - pp.ic*qp.e;
    dFdpp  = dFdpp - pp.ic;
    qp.ic  = qp.ic + pp.ic;
    qp.c   = spm_inv(qp.ic);

    % E-step: update expectation (p)
    %======================================================================

    % update conditional expectation
    %----------------------------------------------------------------------
    dp     = spm_dx(dFdpp,dFdp,{te});
    qp.e   = qp.e + dp;
    qp.p   = spm_unvec(qp.e,qp.p);

    % M-step - hyperparameters (h = exp(l))
    %======================================================================
    mh     = zeros(nh,1);
    dFdh   = zeros(nh,1);
    dFdhh  = zeros(nh,nh);
    for iM = 1:nM

        % [re-]set precisions using [hyper]parameter estimates
        %------------------------------------------------------------------
        iS    = Qp;
        for i = 1:nh
            iS = iS + Q{i}*exp(qh.h(i));
        end
        S     = spm_inv(iS);
        dS    = ECE + EE - S*nY;

        % 1st-order derivatives: dFdh = dF/dh
        %------------------------------------------------------------------
        for i = 1:nh
            dPdh{i}        =  Q{i}*exp(qh.h(i));
            dFdh(i,1)      = -trace(dPdh{i}*dS)/2;
        end

        % 2nd-order derivatives: dFdhh
        %------------------------------------------------------------------
        for i = 1:nh
            for j = 1:nh
                dFdhh(i,j) = -trace(dPdh{i}*S*dPdh{j}*S*nY)/2;
            end
        end

        % hyperpriors
        %------------------------------------------------------------------
        qh.e  = qh.h  - ph.h;
        dFdh  = dFdh  - ph.ic*qh.e;
        dFdhh = dFdhh - ph.ic;

        % update ReML estimate of parameters
        %------------------------------------------------------------------
        dh    = spm_dx(dFdhh,dFdh);
        qh.h  = qh.h + dh;
        mh    = mh   + dh;

        % conditional covariance of hyperparameters
        %------------------------------------------------------------------
        qh.c  = -spm_inv(dFdhh);

        % convergence (M-Step)
        %------------------------------------------------------------------
        if (dFdh'*dh < 1e-2) || (norm(dh,1) < exp(-8)), break, end

    end % M-Step

    % evaluate objective function (F)
    %======================================================================
    L   = - trace(EiSE)/2  ...               % states (u)
        - trace(qp.e'*pp.ic*qp.e)/2  ...     % parameters (p)
        - trace(qh.e'*ph.ic*qh.e)/2  ...     % hyperparameters (h)
        + Hqu.c/2             ...            % entropy q(u)
        + spm_logdet(qp.c)/2  ...            % entropy q(p)
        + spm_logdet(qh.c)/2  ...            % entropy q(h)
        - spm_logdet(pp.c)/2  ...            % entropy - prior p
        - spm_logdet(ph.c)/2  ...            % entropy - prior h
        + spm_logdet(iS)*nY/2 ...            % entropy - error
        - n*ny*nY*log(2*pi)/2;

    % if F is increasing, save expansion point and derivatives
    %----------------------------------------------------------------------
    if L > F(end) || iE < 3

        % save model-states (for each time point)
        %==================================================================
        for t = 1:length(qU)

            % states
            %--------------------------------------------------------------
            a     = spm_unvec(qU(t).a{1},{G.a});
            v     = spm_unvec(pU(t).v{1},{G.v});
            x     = spm_unvec(pU(t).x{1},{G.x});
            z     = spm_unvec(pU(t).z{1},{G.v});
            w     = spm_unvec(pU(t).w{1},{G.x});
            for i = 1:nl
                try
                    PU.v{i}(:,t) = spm_vec(v{i});
                    PU.z{i}(:,t) = spm_vec(z{i});
                end
                try
                    PU.x{i}(:,t) = spm_vec(x{i});
                    PU.w{i}(:,t) = spm_vec(w{i});
                end
                try
                    QU.a{i}(:,t) = spm_vec(a{i});
                end
            end

            % conditional modes
            %--------------------------------------------------------------
            v     = spm_unvec(qU(t).v{1},{M(1 + 1:end).v});
            x     = spm_unvec(qU(t).x{1},{M(1:end - 1).x});
            z     = spm_unvec(qE{t}(1:(ny + nv)),{M.v});
            w     = spm_unvec(qE{t}((1:nx) + (ny + nv)*n),{M.x});
            for i = 1:(nl - 1)
                if M(i).m, QU.v{i + 1}(:,t) = spm_vec(v{i}); end
                if M(i).l, QU.z{i}(:,t)     = spm_vec(z{i}); end
                if M(i).n, QU.x{i}(:,t)     = spm_vec(x{i}); end
                if M(i).n, QU.w{i}(:,t)     = spm_vec(w{i}); end
            end
            QU.v{1}(:,t)  = spm_vec(qU(t).y{1}) - spm_vec(z{1});
            QU.z{nl}(:,t) = spm_vec(z{nl});

            % and conditional covariances
            %--------------------------------------------------------------
            i       = (1:nx);
            QU.S{t} = qC{t}(i,i);
            PU.S{t} = pC{t}(i,i);
            i       = (1:nv) + nx*n;
            QU.C{t} = qC{t}(i,i);
            PU.C{t} = pC{t}(i,i);
        end

        % save conditional densities
        %------------------------------------------------------------------
        B.QU  = QU;
        B.PU  = PU;
        B.qp  = qp;
        B.qh  = qh;

        % decrease regularisation
        %------------------------------------------------------------------
        F(iE) = L;
        te    = min(te + 1,8);

    else

        % otherwise, return to previous expansion point and break
        %------------------------------------------------------------------
        QU    = B.QU;
        PU    = B.PU;
        qp    = B.qp;
        qh    = B.qh;

        % increase regularisation
        %------------------------------------------------------------------
        F(iE) = F(end);
        te    = min(te - 1,0);

    end

    % report and break if convergence
    %======================================================================
    if db
        figure(Fdem)
        spm_DEM_qU(QU)
        if np
            subplot(nl,4,4*nl)
            bar(full(Up*qp.e))
            xlabel({'parameters';'{minus prior}'})
            axis square, grid on
        end
        if length(F) > 2
            subplot(nl,4,4*nl - 1)
            plot(F - F(1))
            xlabel('updates')
            title('log-evidence')
            axis square, grid on
        end
        drawnow

        % report (EM-Steps)
        %------------------------------------------------------------------
        str{1} = sprintf('ADEM: %i (%i)',iE,iM);
        str{2} = sprintf('F:%.4e',full(L - F(1)));
        str{3} = sprintf('p:%.2e',full(dp'*dp));
        str{4} = sprintf('h:%.2e',full(mh'*mh));
        str{5} = sprintf('(%.2e sec)',full(toc));

        fprintf('%-16s%-16s%-14s%-14s%-16s\n',str{:})
    end

    if (norm(dp,1) < exp(-8)) && (norm(mh,1) < exp(-8)), break, end

end

% assemble output arguments
%==========================================================================

% conditional moments of model-parameters (rotated into original space)
%--------------------------------------------------------------------------
qP.P   = spm_unvec(Up*qp.e + spm_vec(M.pE),M.pE);
qP.C   = Up*qp.c*Up';
qP.V   = spm_unvec(diag(qP.C),M.pE);

% conditional moments of hyper-parameters (log-transformed)
%--------------------------------------------------------------------------
qH.h   = spm_unvec(qh.h,{{M.hE} {M.gE}});
qH.g   = qH.h{2};
qH.h   = qH.h{1};
qH.C   = qh.c;
qH.V   = spm_unvec(diag(qH.C),{{M.hE} {M.gE}});
qH.W   = qH.V{2};
qH.V   = qH.V{1};

% Fill in DEM with response and its causes
%--------------------------------------------------------------------------
DEM.pP.P = {G.pE};            % parameters encoding process

DEM.M  = M;                   % generative model
DEM.U  = U;                   % causes
DEM.Y  = PU.v{1};             % response

DEM.pU = PU;                  % prior moments of model-states
DEM.qU = QU;                  % conditional moments of model-states
DEM.qP = qP;                  % conditional moments of model-parameters
DEM.qH = qH;                  % conditional moments of hyper-parameters

DEM.F  = F;                   % [-ve] Free energy
try
    DEM.J  = J;               % [-ve] Free energy (over samples)
end

author: spm


text:
function edge = createEdge3d(varargin)
%CREATEEDGE3D Create an edge between two 3D points, or from a 3D line.
%
%   E = createEdge3d(P1, P2)
%   Creates the 3D edge joining the two points P1 and P2.
%
%   E = createEdge3d(LIN)
%   Creates the 3D edge with same origin and same direction vector as the
%   3D line LIN.
%
%   Example
%     p1 = [1 1 1];
%     p2 = [3 4 5];
%     edge = createEdge3d(p1, p2);
%     edgeLength3d(edge)
%     ans =
%         5.3852
%
%   See also
%     edges3d, drawEdge3d, clipEdge3d, edgelength3d

% ------
% Author: David Legland
% E-mail: david.legland@inra.fr
% Created: 2018-08-29, using Matlab 9.4.0.813654 (R2018a)
% Copyright 2018-2022 INRA - Cepia Software Platform

if nargin == 1
    % Only one input parameter. Assumes it corresponds to a 3D line with
    % 6 params.
    var = varargin{1};

    if size(var, 2) ~= 6
        error('single input must have 6 columns');
    end

    % converts 3D line into 3D edge
    edge = zeros(size(var));
    edge(:, 1:3) = var(:, 1:3);
    edge(:, 4:6) = edge(:, 1:3) + var(:,4:6);

elseif nargin == 2
    % 2 input parameters correspond to two 3D points

    % extract the two arguments
    v1 = varargin{1};
    v2 = varargin{2};

    if size(v1, 2) ~= 3 || size(v2, 2) ~= 3
        error('Input points must be arrays with 3 columns');
    end

    % first input parameter is first point, and second input is the
    % second point. Allows multiple points.
    n1 = size(v1, 1);
    n2 = size(v2, 1);
    if n1 == n2
        edge = [v1 v2];
    elseif n1 == 1 || n2 == 1
        edge = [repmat(v1, n2, 1) repmat(v2, n1, 1)];
    end

else
    error('Wrong number of arguments in ''%s''', mfilename);
end

author: mattools

repo: matGeom

save_path: github-repos/MATLAB/mattools-matGeom


text:
function value = r8_mop ( i )

%*****************************************************************************80
%
%% R8_MOP returns the I-th power of -1 as an R8 value.
%
%  Modified:
%
%    07 November 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer I, the power of -1.
%
%    Output, real R8_MOP, the I-th power of -1.
%
  if ( mod ( i, 2 ) == 0 )
    value = + 1.0;
  else
    value = - 1.0;
  end

  return
end


text:
%% Copyright (C) 2016 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 besselj (@var{alpha}, @var{x})
%% Symbolic Bessel function of the first kind.
%%
%% Example:
%% @example
%% @group
%% syms n x
%% A = 2*besselj(n,x)
%%   @result{} A = (sym) 2⋅besselj(n, x)
%% diff(A)
%%   @result{} (sym) besselj(n - 1, x) - besselj(n + 1, x)
%% @end group
%% @end example
%%
%% @seealso{@@sym/bessely, @@sym/besseli, @@sym/besselk}
%% @end defmethod

function J = besselj(n, x)

  if (nargin ~= 2)
    print_usage ();
  end

  J = elementwise_op ('besselj', sym(n), sym(x));

end

%!test
%! X = [1 2 3; 4 5 6];
%! ns = [sym(0) 1 -2; sym(1)/2 -sym(3)/2 pi];
%! n = double(ns);
%! A = double(besselj(ns, X));
%! B = besselj(n, X);
%! assert (all (all (abs (A - B) < 50*eps*abs(A))))

%!test
%! % roundtrip
%! syms x
%! A = besselj(2, 10);
%! q = besselj(2, x);
%! h = function_handle(q);
%! B = h(10);
%! assert (abs (A - B) <= eps*abs(A))

%!error besselj(sym('x'))

repo: octsympy


text:
function boundary=compute_boundary(face, options)

% compute_boundary - compute the vertices on the boundary of a 3D mesh
%
%   boundary=compute_boundary(face);
%
%   Copyright (c) 2007 Gabriel Peyre

options.null = 0;
verb = getoptions(options, 'verb', 1);

if size(face,1)<size(face,2)
    face=face';
end

nvert=max(max(face));
nface=size(face,1);

A=sparse(nvert,nvert);
for i=1:nface
    if verb
        progressbar(i,nface);
    end
    f=face(i,:);
    A(f(1),f(2))=A(f(1),f(2))+1;
    A(f(1),f(3))=A(f(1),f(3))+1;
    A(f(3),f(2))=A(f(3),f(2))+1;
end
A=A+A';

for i=1:nvert
    u=find(A(i,:)==1);
    if ~isempty(u)
        boundary=[i u(1)];
        break;
    end
end

s=boundary(2);
i=2;
while(i<=nvert)
    u=find(A(s,:)==1);
    if length(u)~=2
        warning('problem in boundary');
    end
    if u(1)==boundary(i-1)
        s=u(2);
    else
        s=u(1);
    end
    if s~=boundary(1)
        boundary=[boundary s];
    else
        break;
    end
    i=i+1;
end

if i>nvert
    warning('problem in boundary');
end

%%% OLD %%%
function v = compute_boundary_old(faces)

nvert = max(face(:));
ring = compute_vertex_ring( face );

% compute boundary
v = -1;
for i=1:nvert   % first find a starting vertex
    f = ring{i};
    if f(end)<0
        v = i;
        break;
    end
end
if v<0
    error('No boundary found.');
end
boundary = [v];
prev = -1;
while true
    f = ring{v};
    if f(end)>=0
        error('Problem in boundary');
    end
    if f(1)~=prev
        prev = v;
        v = f(1);
    else
        prev = v;
        v = f(end-1);
    end
    if ~isempty( find(boundary==v) )
        % we have reach the begining of the boundary
        if v~=boundary(1)
            warning('Begining and end of boundary doesn''t match.');
        else
            break;
        end
    end
    boundary = [boundary,v];
end

author: gpeyre

repo: matlab-toolboxes


text:
function obj2off(objFname, offFname)

if ~exist('objFname','var')
    objFname = 'example/model.obj';
end

if ~exist('offFname','var')
    offFname = 'example/model.off';
end

OBJ=read_wobj(objFname);

faces = [];
for i=1:length(OBJ.objects)
    if strcmp(OBJ.objects(i).type,'f')
        faces = [faces; OBJ.objects(i).data.vertices];
    end
end

% check duplicates
for i=1:size(faces,1)
    faces(i,:) = sort(faces(i,:));
end
faces = unique(faces,'rows');

vertices = OBJ.vertices;

%mesh2off(offFname, faces,vertices);
mesh2offbinary(offFname, faces,vertices);

author: jianxiongxiao

repo: ProfXkit


text:
function latLonEnd=directRhumbProbGen(latLonStart,azimuth,dist,height,useHeightApprox,a,f,numSteps4Circ)
%%DIRECTRHUMBLINEPROBGEN Given a starting location near the surface of the
%                    reference ellipsoid as well as a constant heading a
%                    distance to travel, determine the location of a vessel
%                    traveling at the constant heading after traveling the
%                    distance. A constant heading trajectory is a rhumb
%                    line (loxodrome) and is usually not the shortest path
%                    between two points. This function is general in that
%                    it will also solve the direct rhumb problem at a fixed
%                    height above the reference ellipsoid, if desired.
%
%INPUTS: latLonStart A 2X1 vector of the starting ellipsoidal latitude
%                    (North) and longitude (East) in radians.
%            azimuth The constant heading that is to be traveled in
%                    radians East of North.
%               dist The distance that is to be traveled on a constant
%                    heading course.
%             height The heights above the reference ellipsoid at which the
%                    trajectory should be determined. If this parameter
%                    is omitted, then the default value of 0 is used.
%    useHeightApprox If true, and the height is not zero, then a distance
%                    scaling height approximation is used. An equatorial
%                    trajectory traveling a distance dist at a height h
%                    above the reference ellipsoid will only travel
%                    a/(a+h)*dist over the surface. If useHeightApprox is
%                    true, then this scaling applied to any trajectry
%                    (including non-equatorial). If useHeightApprox is
%                    false, a slow iterative optimization used. The default
%                    value if omitted or an empty matrix is passed is
%                    false. This parameter is ignored if height=0.
%                  a The semi-major axis of the reference ellipsoid. If
%                    this argument is omitted, the value in
%                    Constants.WGS84SemiMajorAxis is used.
%                  f The flattening factor of the reference ellipsoid. If
%                    this argument is omitted, the value in
%                    Constants.WGS84Flattening is used.
%      numSteps4Circ If height!=0, and useHeightApprox=false, then an
%                    algorithm propagating a state in ECEF coordinates
%                    around the curved Earth is used. This parameter
%                    determines the number of steps that would be needed
%                    for a target that circumnavigates the globe around the
%                    equator. The default value if this parameter is not
%                    provided is 2000. A value of 6000 appears to be about
%                    the best number for overall precision. Reducing the
%                    number of steps will speed up the function. This
%                    parameter is not used if height=0.
%
%OUTPUTS: latLonEnd The ellipsoidal latitude and longitude that one will be
%                    after starting at latLonStart and traveling a distance
%                    of dist at a constant heading on azimuth.
%
%For zero-altitude, the directRhumbProb function is called. The solution
%for a non-zero height is based on [1]. The function is significantly
%faster if a zero hight is used or if useHeightApprox=true, in which case
%the approximation described in [2] is used.
%
%The algorithm is generally the opposite of indirectRhumbProblem, except
%when a starting or ending point is at a geographic pole, as discussed in
%the comments to the indirectRhumbProblem function.
%
%REFERENCES:
%[1] D. F. Crouse, "Simulating aerial targets in 3D accounting for the
%    Earth's curvature," Journal of Advances in Information Fusion, vol.
%    10, no. 1, Jun. 2015.
%[2] D. F. Crouse, "Worldwide Ground Target State Propagation," in
%    Proceedings of the 23rd International Conference on Information
%    Fusion, 6-9 Jul. 2020.
%
%August 2019 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<8||isempty(numSteps4Circ))
%This is the number of steps that would be taken to travel a distance of
%2*pi*a (circumnavigate the globe the long way). This is a design parameter
%that should be large enough to ensure that the results are accurate.
%However, if it is too large, finite precision errors will accumulate.
    numSteps4Circ=2000;
end

if(nargin<7||isempty(f))
    f=Constants.WGS84Flattening;
end

if(nargin<6||isempty(a))
    a=Constants.WGS84SemiMajorAxis;
end

if(nargin<5||isempty(useHeightApprox))
    useHeightApprox=false;
end

if(nargin<4||isempty(height))
    height=0;
end

if(useHeightApprox)
    %Scale the distance based on the height and then solve the rumb problem
    %with the scaled distance.
    dist=a/(a+height)*dist;
    height=0;
end

if(height==0)
    latLonEnd=directRhumbProblem(latLonStart,azimuth,dist,0,a,f);
    return
else
    %If the height is not zero, then use the algorithm for propagating a
    %level, non-maneuvering dynamic model along a rhumb line forward in
    %time.

    %This is the number of Runge-Kutta steps on a curved Earth that will be
    %performed.
    numSteps=ceil(numSteps4Circ*(dist/(2*pi*a)));
    stepSize=1/numSteps;

    %We have to calculate the initial location and heading in CARTESIAN
    %ECEF coordinates.
    xyzInit=ellips2Cart([latLonStart;height],a,f);

    %The initial heading in the LOCAL East-North-Up tangent-plane
    %coordinate system for level flight.
    uh=[sin(azimuth);cos(azimuth);0];

    %The speed of the target is such that it travels a distance of dist in
    %1 unit of time.
    speed=dist;

    %The initial target state with the position in the GLOBAL ECEF
    %coordinate system and the velocity in a LOCAL flat-Earth coordinate
    %system.
    xInit=[xyzInit;speed*uh];

    aDyn=@(x,t)aPoly(x,t,3);%A constant velocity model.

    %When traveling along a rhumb-line, the local coordinate system is
    %known at all times: it is the local ENU coordinate system. The local
    %basis vectors are deterministially known at all places on the Earth.
    uDet=@(x,t)getENUAxes(Cart2Ellipse(x(1:3),[],a,f));
    xList=RungeKCurvedAtTimes(xInit,[],[0;1],aDyn,uDet,stepSize,4);
    plhEnd=Cart2Ellipse(xList(1:3,end),[],a,f);
    latLonEnd=plhEnd(1:2);%The Cartesian location at the end of the
                          %trajectory.
end
end

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

author: USNavalResearchLaboratory

repo: TrackerComponentLibrary


text:
function t = islower(c)
%ISLOWER True for lowercase letters.
%
%   For a string C, ISLOWER(C) is 1 for lowercase letters and 0 otherwise.
%
%   See also: ISALNUM, ISALPHA, ISASCII, ISDIGIT, ISPRTCHR, ISUPPER,
%   ISXDIGIT.

%   Author:      Peter J. Acklam
%   Time-stamp:  2002-03-03 13:51:03 +0100
%   E-mail:      pjacklam@online.no
%   URL:         http://home.online.no/~pjacklam

   error(nargchk(1, 1, nargin));

   t = isletter(c) & (c == lower(c));

author: CovertLab

repo: WholeCell

save_path: github-repos/MATLAB/CovertLab-WholeCell


text:
function r82row_sort_quick_a_test ( )

%*****************************************************************************80
%
%% R82ROW_SORT_QUICK_A_TEST tests R82ROW_SORT_QUICK_A.
%
%  Discussion:
%
%    An R82ROW is a (2,N) array of R8's.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 April 2009
%
%  Author:
%
%    John Burkardt
%
  n = 12;

  b = 0.0;
  c = 10.0;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'R82ROW_SORT_QUICK_A_TEST\n' );
  fprintf ( 1, '  R82ROW_SORT_QUICK_A sorts an R82ROW\n' );
  fprintf ( 1, '  using quick sort.\n' );
  fprintf ( 1, '  Using initial random number seed = %d\n', seed );

  [ a, seed ] = r8mat_uniform_ab ( 2, n, b, c, seed );
%
%  Give a few elements the same first component.
%
  a(1,3) = a(1,5);
  a(1,4) = a(1,12);
%
%  Give a few elements the same second component.
%
  a(2,6) = a(2,1);
  a(2,2) = a(2,9);
%
%  Make two entries equal.
%
  a(1:2,7) = a(1:2,11);

  r82row_print ( n, a, '  Before rearrangement:' );

  a = r82row_sort_quick_a ( n, a );

  r82row_print ( n, a, '  Sorted array:' );

  return
end


text:
%compute velmag_tail
function [data,units]=compute_velmagtail(trx,n)
larvae=trx.exp2flies{n};
numlarvae=numel(larvae);
velmagtail=cell(1,numlarvae);
for i=1:numlarvae
    larva=larvae(i);
    velmagtail{1,i}=bsxfun(@hypot,trx(larva).dxtail_mm,trx(larva).dytail_mm);
end

units=parseunits('mm/s');
data=velmagtail;

author: kristinbranson

repo: JAABA


text:
function obj = extras_autonn_custom_fn(block, inputs, params)
% EXTRAS_AUTONN_CUSTOM_FN autonn custom layer converter for extra
% layers
%
% Copyright (C) 2017 Samuel Albanie
% Licensed under The MIT License [see LICENSE.md for details]

  switch class(block)
    case 'dagnn.Normalize'
      obj = Layer.create(@vl_nnscalenorm, {inputs{1}, params{1}}) ;
    case 'dagnn.Permute'
      obj = Layer.create(@permute, {inputs{1}, block.order}) ;
    case 'dagnn.Flatten'
      obj = Layer.create(@vl_nnflatten, {inputs{1}, block.axis}) ;
    case 'dagnn.GlobalPooling'
      obj = Layer.create(@vl_nnglobalpool, inputs(1)) ;
    case 'dagnn.Reshape'
      obj = Layer.create(@vl_nnreshape, {inputs{1}, block.shape}) ;
    case 'dagnn.Max'
      obj = Layer.create(@vl_nnmax, [{numel(inputs)}, inputs]) ;
    case 'dagnn.Crop'
      obj = vl_nncrop_wrapper(inputs{1}, inputs{2}, block.crop) ;
    case 'dagnn.Axpy'
      obj = Layer.create(@vl_nnaxpy, inputs(1:3)) ;
    case 'dagnn.Interp'
      obj = Layer.create(@vl_nninterp, {inputs{1}, block.shrinkFactor, ...
                        block.zoomFactor, 'padBeg', block.padBeg, ...
                        'padEnd', block.padEnd}) ;
    case 'dagnn.SoftMaxTranspose'
      obj = Layer.create(@vl_nnsoftmaxt, {inputs{1}, 'dim', block.dim}) ;
    case 'dagnn.Scale'
      args = {'numInputDer', 3} ; hasBias = block.hasBias ; % simplify interface
      if hasBias, ins = inputs(1:3) ; else, ins = [inputs(1:2) {[]}] ; end
      obj = Layer.create(@vl_nnscale, [ins {'size', block.size}], args{:}) ;
    case 'dagnn.PriorBox'
      obj = Layer.create(@vl_nnpriorbox, {inputs{1}, inputs{2}, ...
                                  'aspectRatios', block.aspectRatios, ...
                                  'pixelStep', block.pixelStep, ...
                                  'variance', block.variance, ...
                                  'minSize', block.minSize, ...
                                  'maxSize', block.maxSize, ...
                                  'offset', block.offset, ...
                                  'flip', block.flip, ...
                                  'clip', block.clip}, ...
                                  'numInputDer', 0) ;
    case 'dagnn.MultiboxDetector'
      numClasses = double(block.numClasses) ; % fixes weird bug
      obj = Layer.create(@vl_nnmultiboxdetector, ...
                                 {inputs{1}, inputs{2}, inputs{3}...
                                 'numClasses', numClasses, ...
                                 'nmsThresh', block.nmsThresh}, ...
                                 'numInputDer', 0) ;
    case 'SpatialSoftMax'
      obj = Layer.create(@vl_nnspatialsoftmax, inputs) ;
    otherwise, keyboard
  end

repo: MFT


text:
%% Unstructured Grid
% This examples shows how to use ConstructProjector2D on unstructured
% grids. Two sets of points are generated randomly.
clear;clc;
%% Initializing Part I
disp('- Initializing')
rSource=2*pi;
rDestination=rSource*1.05; % This slightly bigger radius causes some points
                           % on destination grid being outside of the
                           % source grid; hence, on those points it is
                           % actually extrapolation and not interpolation.

ns=300;
nd=50;

nPoly=4; % We gonna fit a fourth order polynomial,
         % i.e. sum_{n,m=0}^{n,m=nPoly} x^n*y^m
nInterp=35; % nPoly=4 requires 25 points. However,
            % we are going to use 35 points. This would be
            % the least-square fit of the surface.
            % Note that: min(nInterp)=(nPoly+1)^2. Otherwise it won't work.
%% Generating the Source grid
disp('- Generating the Source grid')
xs=rand(ns,1)*2*rSource-rSource;
ys=rand(ns,1)*2*rSource-rSource;

%% Generating the Destination Grid
disp('- Generating the Destination Grid')
xd=rand(nd,1)*2*rDestination-rDestination;
yd=rand(nd,1)*2*rDestination-rDestination;

figure
plot(xs,ys,'b.');
hold on
plot(xd,yd,'k.');
axis tight
axis square
legend('Source Grid','Destination Grid')
title('Distribution of the points')

%% Generating the interpolant
disp('- Generating the interpolant')
P=ConstructProjector2D(xs,ys,xd,yd,nPoly,nInterp);

%% Generating sum data
disp('- Generating sum data and interpolating')
F1=@(x,y) (sin(sqrt(x.^2+y.^2)));
F2=@(x,y) (sin(x).*cos(y));
F3=@(x,y) (exp(-sqrt(x.^2+y.^2)));
F4=@(x,y,x0,y0) (exp(-sqrt((x-x0).^2+(y-y0).^2)));

z=zeros(ns,4);
z(:,1)=F1(xs,ys);
z(:,2)=F2(xs,ys);
z(:,3)=F3(xs,ys);
z(:,4)=F4(xs,ys,0,0);

z_Analytic=zeros(nd,4);
z_Analytic(:,1)=F1(xd,yd);
z_Analytic(:,2)=F2(xd,yd);
z_Analytic(:,3)=F3(xd,yd);
z_Analytic(:,4)=F4(xd,yd,0,0);

% Interpolating
z_interp=P*z;

% Note that since the points are randomly generated the RMSE would change
% each time. Depending on how the points are distributed you may get very
% good or very bad RMSE.
RMSE=sqrt(mean( (z_interp-z_Analytic).^2 ));

%% plotting
figure
for i=1:4
  subplot(2,2,i)
  plot(z_interp(:,i),'b.');
  hold on
  plot(z_Analytic(:,i),'r.');
  legend('Interpolated','Analytic');
  title(['RMSE= ' num2str(RMSE(i))]);
end


text:
function results = mapRxnsViaMets(model,mnx,mapRxns)
%mapRxnsViaMets  Map model reactions to MNX database via metabolite MNXIDs.
%
% mapRxnsViaMets searches an MNX database structure for reactions that
% share the same set of metabolites as reactions in model, and returns the
% corresponding MNX reaction IDs and reaction equations. The mapping is
% performed by looking for any MNX reactions that contain the same set of
% metabolite MNX IDs as a model reaction, meaning that STOICHIOMETRY
% COEFFICIENTS ARE IGNORED.
%
% This function can also handle model metabolites associated with multiple
% MNX IDs. In this case, only one of the multiple met IDs for a particular
% metabolite need to match a metabolite in the corresponding MNX reaction.
%
% USAGE:
%
%   results = mapRxnsViaMets(model,mnx,mapRxns)
%
% INPUT:
%
%   model    Model structure.
%
%   mnx      (Optional, will be generated if not provided) MNX database
%            structure, generated using the following command:
%            mnx = buildMNXmodel('both');
%
%   mapRxns  (Optional, Default = all reactions) A logical vector of the
%            same size as model.rxns, indicating which reactions should be
%            mapped. Reactions corresponding to mapRxns entries that are
%            FALSE will be skipped.
%
% OUTPUT:
%
%   results  A results structure with the following fields:
%
%       rxnMNXID: A column cell array containing the MNX IDs mapped to each
%                 reaction. For reactions to which multiple MNX IDs were
%                 mapped, the corresponding rxnMNXID entry will be a nested
%                 cell of MNX IDs.
%
%      rxnMNXeqn: A column cell array containing the reaction equations
%                 from the MNX database that correspond to the MNX ID(s)
%                 mapped to each model reaction.
%
%          notes: A column of strings providing information on if and how
%                 each model reaction was mapped to any MXN ID(s).
%

% handle input arguments
if nargin < 3
    mapRxns = true(size(model.rxns));
elseif ~isequal(size(mapRxns),size(model.rxns))
    error('mapRxns input must have same dimensions as model.rxns');
end
if nargin < 2
    mnx = [];
end

% initialize results structure
results.rxnMNXID = {};
results.rxnMNXeqn = {};
results.notes = repmat({''},size(model.rxns));
results.notes(~mapRxns) = {'IGNORED'};

%% Prepare model and MNX structures for processing

fprintf('Pre-processing model and MNX structures... ');

% if the model.metMNXID field contains multiple columns, convert it to a
% single column of nested cells
if size(model.metMNXID,2) > 1
    model.metMNXID = nestCell(model.metMNXID,true);
elseif all(cellfun(@ischar,model.metMNXID)) && any(contains(model.metMNXID,';'))
    empty_inds = cellfun(@isempty,model.metMNXID);
    model.metMNXID = cellfun(@(id) strsplit(id,';\s*','DelimiterType','RegularExpression'),model.metMNXID,'UniformOutput',false);
    model.metMNXID(empty_inds) = {''};  % to deal with cells that should be empty, but are recognized as non-empty
end

% generate binary (logical) version of S-matrix indicating which mets are
% involved in each rxn
S = (full(model.S) ~= 0);

% do not try to map model rxns that contain a metabolite without an MNXID
met_noID = cellfun(@isempty,model.metMNXID);
rxn_noID = any(S(met_noID,:),1)';
results.notes(mapRxns & rxn_noID) = {'SKIPPED: one or more mets lacking MNXID'};
mapRxns(rxn_noID) = false;

% Create an additional field in model that lists all possible metMNXIDs
% associated with each reaction.
for i = 1:length(model.rxns)
    model.rxnMetsMNX{i,1} = unique(horzcat(model.metMNXID{S(:,i)}));
end

% For faster processing later on, remove all rxns in the MNX structure that
% contain at least one metabolite that is not present in the model.

% get list of all metMNXIDs in the model (for reactions to be mapped)
met_inds = any(S(:,mapRxns),2);
allMetMNXIDs = unique(horzcat(model.metMNXID{met_inds}))';
allMetMNXIDs(cellfun(@isempty,allMetMNXIDs)) = [];

% load MNX database structure if not provided as input
if isempty(mnx)
    fprintf('MNX database structure not provided. It will be loaded.\n\n');
    mnx = buildMNXmodel('both');
end

% flatten mnx.rxnMets (single column -> multi column cell array)
mnx.rxnMets = flattenCell(mnx.rxnMets,true);

% find MNX rxns with mets that aren't present in model
mismatches = ~ismember(mnx.rxnMets,allMetMNXIDs);
mismatches(cellfun(@isempty,mnx.rxnMets)) = false;
del_rxns = any(mismatches,2);

% restore mnx.rxnMets to its original format of a column of nested cells
mnx.rxnMets = nestCell(mnx.rxnMets,true);

% remove reactions from MNX structure
nrxns = length(mnx.rxns);
f = fields(mnx);
for i = 1:length(f)
    if iscell(mnx.(f{i})) && size(mnx.(f{i}),1) == nrxns
        mnx.(f{i})(del_rxns,:) = [];
    end
end

% initialize rxn MNX IDs
rxnMNXID = repmat({''},size(model.rxns));

fprintf('Done.\n');

%% STAGE 1: Find exact matches to MNX reactions that are balanced

fprintf('Searching for exact, balanced rxn matches... ');

% subset MNX structure to include only the balanced reactions
mnx_bal = mnx;
unbal = ~ismember(mnx_bal.rxnBalanced,'true');
mnx_bal.rxns(unbal) = [];
mnx_bal.rxnMets(unbal) = [];

% map reactions
rxnMNXID(mapRxns) = findMNXrxns(model,mnx_bal,mapRxns);

% find newly mapped rxns
newMapped = mapRxns & ~cellfun(@isempty,rxnMNXID);
results.notes(newMapped) = {'MATCH: exact'};
mapRxns(newMapped) = false;  % update to ignore rxns that are now mapped

% clear MNX structure to free up some memory
clear mnx_bal

fprintf('Done.\n');

%% STAGE 2: Ignore protons (H+) and water (H2O) in reaction equations

fprintf('Searching for matches, ignoring balance status, protons (H+), and water (H2O)... ');

% remove H+ and H2O from all model reaction equations
rem_ind = ismember(model.metFormulas,{'H2O','H'});
model.S(rem_ind,:) = 0;

% also remove from MNX reaction equations
rem_ind = ismember(mnx.metFormulas,{'H2O','H'});
rem_ind(contains(mnx.metNames,{'(.)','deuterium','tritium','hydride'},'IgnoreCase',true)) = false;  % exclude other variants
rem_IDs = mnx.mets(rem_ind);
mnx.rxnMets = flattenCell(mnx.rxnMets,true);  % flatten cell
mnx.rxnMets(ismember(mnx.rxnMets,rem_IDs)) = {''};  % remove IDs
mnx.rxnMets = nestCell(mnx.rxnMets,true);  % re-nest cell

% map reactions
rxnMNXID(mapRxns) = findMNXrxns(model,mnx,mapRxns);

% find newly mapped rxns
newMapped = mapRxns & ~cellfun(@isempty,rxnMNXID);
results.notes(newMapped) = {'MATCH: ignored H+ and H2O'};

fprintf('Done.\n');

%% Finalize output

fprintf('Finalizing output... ');

results.notes(cellfun(@isempty,rxnMNXID) & mapRxns) = {'NO MATCHES FOUND'};

% retrieve MNX reaction equations for all mapped MNXIDs
rxnMNXeqn = repmat({''},size(model.rxns));
for i = 1:length(rxnMNXID)
    if ~isempty(rxnMNXID{i})
        rxnMNXeqn{i} = mnx.rxnEqnNames(ismember(mnx.rxns,rxnMNXID{i}));
    end
end

% add data to results structure
results.rxnMNXID = rxnMNXID;
results.rxnMNXeqn = rxnMNXeqn;

fprintf('Done.\n');

end

%% Additional functions

function rxnIDs = findMNXrxns(model,mnx,mapRxns)
%Finds MNX rxns with same set of mets as model rxns.

% convert stoich matrix to logicals, where all non-zero coeffs are TRUE
S = (full(model.S) ~= 0);

% exclude reactions that are not to be mapped
S(:,~mapRxns) = [];
model.rxns(~mapRxns) = [];
model.rxnMetsMNX(~mapRxns) = [];

% get number of mets in each reaction, for model and MNX
Nmets_per_rxn_model = sum(S,1)';
Nmets_per_rxn_mnx = cellfun(@numel,mnx.rxnMets);

% initialize output
rxnIDs = repmat({''},size(model.rxns));

% iterate through model reactions, searching for matching MNX reactions
h = waitbar(0,'Scanning reactions...');
for i = 1:length(model.rxns)

    waitbar(i/length(model.rxns),h);

    % find MNX rxns that have the same number of mets as the model rxn
    match = (Nmets_per_rxn_mnx == Nmets_per_rxn_model(i));
    if ~any(match)
        continue
    end

    % find MNX rxns whose mets match at least one of those associated with the model rxn
    match(match) = cellfun(@(m) all(ismember(m,model.rxnMetsMNX{i})),mnx.rxnMets(match));

    % now check if each of the mets in the model rxn are contained in the MNX rxns
    met_inds = find(S(:,i));
    for j = 1:length(met_inds)

        % stop checking metabolites if no further matches remain
        if ~any(match)
            break
        end

        % remove matching MNX rxns that don't contain the current model met
        match(match) = cellfun(@(m) any(ismember(model.metMNXID{met_inds(j)},m)),mnx.rxnMets(match));
    end

    % obtain IDs of all MNX rxns with a matching set of mets as the model rxn
    rxnIDs{i} = mnx.rxns(match);

end
close(h);

end

author: SysBioChalmers

repo: Human-GEM


text:
function [matlabbatch, indValidContrasts] = tapas_physio_check_prepare_job_contrasts(fileSPM, ...
    model, SPM, indReportPhysContrasts, namesPhysContrasts)
% adapts contrast generator for physiogical regressors to actual SPM-file
% (directory & design matrix columns)
%
%
% IN
%   fileSpm
%   model
%   SPM
%   indReportPhysContrasts  [1, nContrasts] vector of phys contrast ids to be
%                           reported, in their default order
%   indValidContrasts       contrasts ids that could be queried with existing
%                           physIO model
%   namesPhysContrasts      if contrasts shall be named differently,
%                           entered here
% Author: Lars Kasper
% Created: 2014-01-21
% Copyright (C) 2014 TNU, Institute for Biomedical Engineering, University of Zurich and ETH Zurich.
%
% This file is part of the PhysIO toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

if nargin < 6
    namesPhysContrasts = tapas_physio_get_contrast_names_default();
end

if ~exist('SPM', 'var'), load(fileSPM); end

[colPhys, colCard, colResp, colMult, colHRV, colRVT, colRois, colMove, colAll] = ...
    tapas_physio_check_get_regressor_columns(SPM, model);
con{1} = colPhys;
con{2} = colCard;
con{3} = colResp;
con{4} = colMult;
con{5} = colHRV;
con{6} = colRVT;
con{7} = colRois;
con{8} = colMove;
con{9} = colAll;

pathCodePhysIO = fileparts(mfilename('fullpath'));
load(fullfile(pathCodePhysIO,'tapas_physio_check_job_contrasts.mat'));
matlabbatch{1}.spm.stats.con.spmmat{1} = fileSPM;

% execute computation only for non-empty contrasts;
indValidContrasts = intersect(indReportPhysContrasts, ...
    find(~cellfun(@isempty, con)));
nContrasts = numel(indValidContrasts);
for c = 1:nContrasts
    iC = indValidContrasts(c);
    F{c} = zeros(max(con{iC}));
    F{c}(sub2ind(size(F{c}),con{iC}, con{iC})) = 1;
    matlabbatch{1}.spm.stats.con.consess{c}.fcon.convec{1} = F{c};

    matlabbatch{1}.spm.stats.con.consess{c}.fcon.name = ...
            namesPhysContrasts{indValidContrasts(c)};
end

% remove additional contrasts in matlabbatch
matlabbatch{1}.spm.stats.con.consess(nContrasts+1:end) = [];


text:
%dout=A*din+b;
function [A b]=cp_affine(dout,din)
nvin=size(din,1);
nvout=size(dout,1);
ndat=size(din,2);

data=[din;ones(1,ndat)];
mx_a=(dout*data')/(data*data');

A=mx_a(1:nvout,1:nvin);
b=mx_a(1:nvout,nvin+1);

repo: ekfmonoslam


text:
function [cost,grad] = softICACost(theta, x, params)

% unpack weight matrix
W = reshape(theta, params.numFeatures, params.n);

% % project weights to norm ball (prevents degenerate bases)
Wold = W;
W = l2rowscaled(W, 1);

% Forward Prop
h = W*x;
r = W'*h;

% Sparsity Cost
K = sqrt(params.epsilon + h.^2);
sparsity_cost = params.lambda * sum(sum(K));
K = 1./K;

% Reconstruction Loss and Back Prop
diff = (r - x);
reconstruction_cost = 0.5 * sum(sum(diff.^2));
outderv = diff;

% compute the cost comprised of: 1) sparsity and 2) reconstruction
cost = sparsity_cost + reconstruction_cost;

% Backprop Output Layer
W2grad = outderv * h';

% Baclprop Hidden Layer
outderv = W * outderv;
outderv = outderv + params.lambda * (h .* K);

W1grad = outderv * x';
Wgrad = W1grad + W2grad';

% % unproject gradient for minFunc
grad = l2rowscaledg(Wold, W, Wgrad, 1);
grad = grad(:);

repo: REST


text:
close all
clear

%--------------------------------------iMPC-DCBF------------------------------------------------
xo = 0;
yo = 0;
r = 1;
N = 24;%Number of horizon
K1 = 1000;%Maximum iteration times, jmax
K = 1000;%Maximum iteration times, jmax
dt = 0.1;
gamma1 = 4;
gamma2 = 4;
nsim = 100;%Total time steps
Itera_x = [];%This is to store iterative convergence of location x
Itera_y = [];%This is to store iterative convergence of location y
Itera_theta = [];%This is to store iterative convergence of orientation theta
Itera_v = [];%This is to store iterative convergence of speed v

% Constraints

umin = [-7; -5; -inf; -inf;];
umax = [7; 5; Inf; Inf];
xmin = [-10; -10; -10; -10];
xmax = [ 10;  10;  10;  10];

% Objective function
Q = diag([10 10 10 10]);
QN = Q;
R = 1 * eye(4);
R(3,3) = 1000;
R(4,4) = 1000;
% Initial and reference states
x0 = [-3 0 0 0];
xr = [3; 0.01; 0; 0];
ur = [0; 0; 1; 1];

% Dynamic system initialization
BB = [0 0 0 0;0 0 0 0;dt 0 0 0;0 dt 0 0];

% Convex MPC frame
[nx, nu] = size(BB);
% Initialize states set (x00,x01,,,x0N）
x_0 = [];
x0 = x0';
u_0 = zeros(nu, N);
u0 = zeros(nu, 1);% we may need to change this for a better warm-up start
impc = zeros(nx, nsim + 1);
impc(:, 1) = x0;
x0new = x0;
for i=1 : (N+1)
    x_0 = [x_0 x0new];
    x0new = [dt*x0new(4)*cos(x0new(3))+x0new(1);dt*x0new(4)*sin(x0new(3))+x0new(2);dt*u0(1)+x0new(3);dt*u0(2)+x0new(4)];
end

abc = tangentline(x_0, r);
Acbf = cbfmat(abc, nx, nu, dt, gamma1, x0);
lcbf = -abc(3, :);
lcbf(1) = [];
lcbf = lcbf';
ucbf = inf * ones(N, 1);
A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0);
lcbf2 = getlcbf2(abc, dt, gamma1, gamma2);
ucbf2 = inf * ones(N-1, 1);

% Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
% - quadratic objective
P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
% - linear objective
q = [repmat(-Q*xr, N, 1); -QN*xr; repmat(-R*ur, N, 1)];
% - linear dynamics
Ax = getax(x_0, dt, N, nx);
Bu = kron([sparse(1, N); speye(N)], BB);
Cx = getcx(x_0, u_0, dt, N, nx);
Aeq = [Ax, Bu];
leq = [-x0; -Cx];
ueq = leq;
% - input and state constraints
Aineq = speye((N+1)*nx + N*nu);
lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];

% - OSQP constraints
A = [Aeq; Aineq; Acbf; A2cbf];%Highest order mcbf=2
l = [leq; lineq; lcbf; lcbf2];
u = [ueq; uineq; ucbf; ucbf2];

% Create an OSQP object
prob = osqp;
% Setup workspace
prob.setup(P, q, A, l, u, 'warm_start', true);
% Solve
res = prob.solve();

% Check solver status
if ~strcmp(res.info.status, 'solved')
   error('OSQP did not solve the problem!')
end
ctrlx = res.x(1:(N+1)*nx);
ctrlu = res.x((N+1)*nx+1:(N+1)*nx+N*nu);

for j = 1 : (K1-1)
    storagex = ctrlx;
    storageu = ctrlu;
    x_0 = trans(x0, ctrlx, nx, N+1);
    u_0 = transu(ctrlu, nu, N);
    abc = tangentline(x_0, r);
    Acbf = cbfmat(abc, nx, nu, dt, gamma1, x0);
    lcbf = -abc(3, :);
    lcbf(1) = [];
    lcbf = lcbf';
    ucbf = inf * ones(N, 1);
    A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0);
    lcbf2 = getlcbf2(abc, dt, gamma1, gamma2);
    ucbf2 = inf * ones(N-1, 1);
    % Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
    % - quadratic objective
    P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
    % - linear objective
    q = [repmat(-Q*xr, N, 1); -QN*xr; repmat(-R*ur, N, 1)];
    % - linear dynamics
    Ax = getax(x_0, dt, N, nx);
    Bu = kron([sparse(1, N); speye(N)], BB);
    Cx = getcx(x_0, u_0, dt, N, nx);
    Aeq = [Ax, Bu];
    leq = [-x0; -Cx];
    ueq = leq;
    % - input and state constraints
    Aineq = speye((N+1)*nx + N*nu);
    lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
    uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];

    % - OSQP constraints
    A = [Aeq; Aineq; Acbf; A2cbf];%Highest order mcbf=2
    l = [leq; lineq; lcbf; lcbf2];
    u = [ueq; uineq; ucbf; ucbf2];
    % Create an OSQP object
    prob = osqp;
    % Setup workspace
    prob.setup(P, q, A, l, u, 'warm_start', true);
    % Solve
    res = prob.solve();

    % Check solver status
    if ~strcmp(res.info.status, 'solved')
       error('OSQP did not solve the problem!')
    end
    ctrlx = res.x(1:(N+1)*nx);
    ctrlu = res.x((N+1)*nx+1:(N+1)*nx+N*nu);
    testx = (storagex - ctrlx)'*(storagex - ctrlx);
    testu = (storageu - ctrlu)'*(storageu - ctrlu);
    test = (testx)/(storagex'*storagex);
    if (test)^(0.5)<=10^(-2)&& (testx/((N+1)*nx))^(0.5)<=10^(-4)%Convergence criterion
      break
    end
end % Move for the first step
ctrl = ctrlu(1:nu);
x0 = [dt*x0(4)*cos(x0(3))+x0(1);dt*x0(4)*sin(x0(3))+x0(2);dt*ctrl(1)+x0(3);dt*ctrl(2)+x0(4)];
ctrlu = rewrite(ctrlu, nu, N);
x_0 = newinit(x0, ctrlu, nx, nu, N, dt);
u_0 = transu(ctrlu, nu, N);
impc(:, 2) = x0;
storagex = ctrlx;
storageu = ctrlu;

for i = 1 : (nsim-1)

   for j = 1 : K
    abc = tangentline(x_0, r);
    Acbf = cbfmat(abc, nx, nu, dt, gamma1, x0);
    lcbf = -abc(3, :);
    lcbf(1) = [];
    lcbf = lcbf';
    ucbf = inf * ones(N, 1);
    A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0);
    lcbf2 = getlcbf2(abc, dt, gamma1, gamma2);
    ucbf2 = inf * ones(N-1, 1);
    % Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
    % - quadratic objective
    P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
    % - linear objective
    q = [repmat(-Q*xr, N, 1); -QN*xr; repmat(-R*ur, N, 1)];
    % - linear dynamics
    Ax = getax(x_0, dt, N, nx);
    Bu = kron([sparse(1, N); speye(N)], BB);
    Cx = getcx(x_0, u_0, dt, N, nx);
    Aeq = [Ax, Bu];
    leq = [-x0; -Cx];
    ueq = leq;
    % - input and state constraints
    Aineq = speye((N+1)*nx + N*nu);
    lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
    uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];

    % - OSQP constraints
    A = [Aeq; Aineq; Acbf; A2cbf];%Highest order mcbf=2
    l = [leq; lineq; lcbf; lcbf2];
    u = [ueq; uineq; ucbf; ucbf2];
    % Create an OSQP object
    prob = osqp;
    % Setup workspace
    prob.setup(P, q, A, l, u, 'warm_start', true);
    % Solve
    res = prob.solve();

    % Check solver status
    if ~strcmp(res.info.status, 'solved')
       error('OSQP did not solve the problem!')
    end
    ctrlx = res.x(1:(N+1)*nx);
    ctrlu = res.x((N+1)*nx+1:(N+1)*nx+N*nu);
    if i == 6 %Get several open-loop trajectories at different iterations predicted at t = 6
     Itera_x = [Itera_x; ctrlx(1:4:(N*nx)+1)'];
     Itera_y = [Itera_y; ctrlx(2:4:(N*nx)+2)'];
     Itera_theta = [Itera_theta; ctrlx(3:4:(N*nx)+3)'];
     Itera_v = [Itera_v; ctrlx(4:4:(N*nx)+4)'];
    end
    x0 = ctrlx(1:nx);
    x_0 = trans(x0, ctrlx, nx, N+1);
    u_0 = transu(ctrlu, nu, N);
    testx = (storagex - ctrlx)'*(storagex - ctrlx);
    testu = (storageu - ctrlu)'*(storageu - ctrlu);
    test = (testx)/(storagex'*storagex);
    if (test)^(0.5)<=10^(-2)&& (testx/((N+1)*nx))^(0.5)<=10^(-4)%Convergence criterion
      break
    end
    storagex = ctrlx;
    storageu = ctrlu;
   end
   ctrl = ctrlu(1:nu);
   x0 = [dt*x0(4)*cos(x0(3))+x0(1);dt*x0(4)*sin(x0(3))+x0(2);dt*ctrl(1)+x0(3);dt*ctrl(2)+x0(4)];
   ctrlu = rewrite(ctrlu, nu, N);
   x_0 = newinit(x0, ctrlu, nx, nu, N, dt);
   u_0 = transu(ctrlu, nu, N);
   impc(:, i+2) = x0;
end

save('trajectory','impctra');%Close-loop 2D-trajectory where tsim=100
save('iteration_x','Itera_x');
save('iteration_y','Itera_y');
save('iteration_theta','Itera_theta');
save('iteration_v','Itera_v');

% Linerize the CBF constraints (get a, b, c for lines)
function abc = tangentline(xy, r)% x and y from initialize states set, abc are coeeficients for linear equation a*x+b*y+c=0
[xx, ~] = size(xy);%xx=2,yy=N+1
xy(xx,:) = []; % this part should be changed for other case
xy((xx-1),:) = []; % this part should be changed other case
[xx, yy] = size(xy);%xx=2,yy=N+1
xyjiao = zeros(xx, yy);%intersection points
for i = 1 : xx
    for j = 1 : yy
        xyjiao(i, j) = r * xy(i, j) * (1 / (xy(:, j)' * xy(:, j)))^(0.5);%calculate coordinates of intersection points
    end
end
cc = -r^2 * ones(1, yy);
abc = [xyjiao; cc];
end

% Get CBF constraints matrix
function Acbf = cbfmat(abc, nx, nu, dt, gamma, x0)
[~, yy] = size(abc);
Acbfx = zeros((yy-1), yy*nx);
Acbfu = zeros((yy-1), (yy-1)*nu);
for i = 1 : (yy-1)
    Acbfx(i, (i*nx)+1) = abc(1, (i+1));
    Acbfx(i, (i*nx)+2) = abc(2, (i+1));
end
for i = 1 : (yy-1)
    Acbfu(i, ((i-1)*nu+3)) = - (1 - dt * gamma)^(i) * (abc(1, 1) * x0(1, 1) + abc(2, 1) * x0(2, 1) + abc(3, 1));
end
Acbf = [Acbfx Acbfu];
end

% Transfer vector x into matrix
function res = trans(x0, vector, nxx, nyy)%nxx=nx,nyy=N+1
 res = zeros(nxx, nyy);
 res(:,1) = x0;
 for i = 1 : (nyy -1)
     res(:,i+1)= vector(((i)*nxx+1):(i+1)*nxx);
 end
end

% Transfer vector u into matrix
function resu = transu(vector, nxx, nyy)%nxx=nu,nyy=N
 resu = zeros(nxx, nyy);
 for i = 1 : (nyy)
     resu(:,i)= vector(((i-1)*nxx+1):(i)*nxx);
 end
end

% Rewrite u vector
function reu = rewrite(vector, nu, N)
append = vector((N-1)*nu+1:N*nu);
vector(1:nu) = [];
reu = [vector;append];
end

% Get new x_0
function x_0 = newinit(x0, ctrlu, nx, nu, N, dt)
 x_0 = zeros(nx, N+1);
 x_0(:, 1) = x0;
 for i=1 : N
    u0 = ctrlu((i-1)*nu+1:i*nu);
    x0 = [dt*x0(4)*cos(x0(3))+x0(1);dt*x0(4)*sin(x0(3))+x0(2);dt*u0(1)+x0(3);dt*u0(2)+x0(4)];
    x_0(:, i + 1) = x0;
 end
end

% Get AA matrix
function AA = getaa(x0, dt)
 AA = [1 0 -x0(4)*sin(x0(3))*dt cos(x0(3))*dt;0 1 x0(4)*cos(x0(3))*dt sin(x0(3))*dt;0 0 1 0;0 0 0 1];
end
% Get CC matrix
function CC = getcc(x0, x1, u0, dt)
 CC = [x0(4)*sin(x0(3))*x0(3)*dt-x0(4)*cos(x0(3))*dt+x1(1)-x0(1);-x0(4)*cos(x0(3))*x0(3)*dt-x0(4)*sin(x0(3))*dt+x1(2)-x0(2);-u0(1)*dt+x1(3)-x0(3);-u0(2)*dt+x1(4)-x0(4)];
end
% Get Ax matrix
function Ax = getax(x_0, dt, N, nx)
x0 = x_0(:,1);
AA = getaa(x0, dt);
Ax = kron(speye(N+1), -speye(nx)) + kron(sparse(diag(ones(N, 1), -1)), AA);
for i = 1 : (N-1)
   x0 = x_0(:,i+1);
   AA = getaa(x0, dt);
   Ax(nx*(i+1)+1:nx*(i+1)+nx,nx*i+1:nx*i+nx) = AA;
end
end
% Get Cx matrix
function Cx = getcx(x_0, u_0, dt, N, nx)
Cx = zeros(N*nx, 1);
for i = 1 : N
    u0 = u_0(:,i);
    x0 = x_0(:,i);
    x1 = x_0(:,i+1);
    CC = getcc(x0, x1, u0, dt);
    Cx((i-1)*nx+1:(i-1)*nx+nx) = CC;
end
end
% Get A2cbf
function A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0)
[~, yy] = size(abc);
Acbfx2 = zeros((yy-2), yy*nx);
Acbfx22 = zeros((yy-2), yy*nx);
Acbfu2 = zeros((yy-2), (yy-1)*nu);
for i = 1 : (yy-2)
    Acbfx2(i, (i*nx)+1) = (gamma1-1/dt)*abc(1, (i+1));
    Acbfx2(i, (i*nx)+2) = (gamma1-1/dt)*abc(2, (i+1));
    Acbfx2(i, ((i+1)*nx)+1) = (1/dt)*abc(1, (i+2));
    Acbfx2(i, ((i+1)*nx)+2) = (1/dt)*abc(2, (i+2));
end
for i = 1 : (yy-2)
    Acbfx22(i, (1*nx)+1) = -(1-dt*gamma2)^(i)/dt*abc(1, (1+1));
    Acbfx22(i, (1*nx)+2) = -(1-dt*gamma2)^(i)/dt*abc(2, (1+1));
end
Acbfx2 = Acbfx2 + Acbfx22;
for i = 1 : (yy-2)
    Acbfu2(i, ((i-1)*nu+4)) = - (1 - dt * gamma2)^(i)*(gamma1-1/dt)*(abc(1, 1) * x0(1, 1) + abc(2, 1) * x0(2, 1) + abc(3, 1));
end
A2cbf = [Acbfx2 Acbfu2];
end
% Get lcbf2
function lcbf2 = getlcbf2(abc, dt, gamma1, gamma2)
[~, yy] = size(abc);
lcbf2 = zeros((yy-2),1);
for i = 1 : (yy-2)
    lcbf2(i, 1) = -abc(3, (i+2))/dt-(gamma1-1/dt)*abc(3, (i+1))+(1 - dt * gamma2)^(i)*abc(3, (1+1))/dt;
end
end

author: HybridRobotics

repo: NMPC-DCLF-DCBF


text:
%% febioOutputView
% Below is a demonstration of the features of the |febioOutputView| function

%%
clear; close all; clc;

%% Syntax
% |[varargout]=febioOutputView(pathName,febio_spec);|

%% Description
% UNDOCUMENTED
%% Examples
%
%%
%
% <<gibbVerySmall.gif>>
%
% _*GIBBON*_
% <www.gibboncode.org>
%
% _Kevin Mattheus Moerman_, <gibbon.toolbox@gmail.com>
%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% 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/>.

author: gibbonCode

repo: GIBBON

save_path: github-repos/MATLAB/gibbonCode-GIBBON


text:
function varargout = expexpintinv(varargin)

switch class(varargin{1})

    case 'double'
        z = varargin{1};
        varargout{1} = exp(1./z).*expint(1./z);

    case 'sdpvar'
        varargout{1} = InstantiateElementWise(mfilename,varargin{:});

    case 'char'

        operator = CreateBasicOperator('concave','increasing','positive','callback');
        operator.derivative = @derivative;
        operator.range = [0 inf];
        operator.domain = [0 inf];

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

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

function d = derivative(z);
d = (-1./z.^2).*exp(1./z).*expint(1./z)+exp(1./z).*(-z.*exp(-1./z)).*(-1./z.^2);


text:
clc; clear all; close all;

%% plot scalp topographies of ERDs
%% get converted data

dd='dir';
filelist={'subj1','subj2'};

Result=zeros(length(filelist),1);
Result_Std=zeros(length(filelist),1);

for i=1:length(filelist)
    [cnt,mrk,mnt]=eegfile_loadMatlab([dd filelist{i}]);

    band=[8 25];
    %% Define channel layout

    grd=sprintf('C3,Cz,C4');
    mnt=mnt_setGrid(mnt,grd);

    %% band pass filter to the continuous EEG
    [b,a]=butter(5, band/cnt.fs*2);
    cnt_flt=proc_channelwise(cnt,'filtfilt',b,a);

    %% cutout segments (short-time windows) from the continuous signals
    epo=makeEpochs(cnt_flt,mrk,[-500 4000]);
    epo=proc_rectifyChannels(epo);
    epo=proc_movingAverage(epo,200,'centered');
    epo=proc_baseline(epo, [-500 0]);
    erd=proc_average(epo);

    scalpEvolutionPlusChannel(erd,mnt,'C4',1000:750:4000);
end

author: PatternRecognition

repo: OpenBMI