function [pd, labels] = imPerimeterDensity(img, varargin)
% Perimeter density of a 2D binary structure, using Crofton formula.
%
%   Pv = imPerimeterDensity(IMG)
%
%   Example
%   imPerimeterDensity
%
%   See also
%     imPerimeter

% ------
% Author: David Legland
% e-mail: david.legland@inrae.fr
% Created: 2010-01-21,    using Matlab 7.9.0.529 (R2009b)
% Copyright 2010 INRA - Cepia Software Platform.

%% Pre-processing

% check image dimension
if ndims(img) ~= 2 %#ok<ISMAT>
    error('first argument should be a 2D image');
end

% in case of a label image, return a vector with a set of results
if ~islogical(img)
    % extract labels (considers 0 as background)
    labels = unique(img);
    labels(labels==0) = [];

    % allocate result array
    nLabels = length(labels);
    pd = zeros(nLabels, 1);

    props = regionprops(img, 'BoundingBox');

    % compute perimeter of each label considered as binary image
    for i = 1:nLabels
        label = labels(i);
        bin = imcrop(img, props(label).BoundingBox) == label;
        pd(i) = imPerimeterDensity(bin, varargin{:});
    end

    return;
end

%% Extract input arguments

% in case of binary image, compute only one label
labels = 1;

% default number of directions
nDirs = 4;

% default image resolution
delta = [1 1];

% parse parameter name-value pairs
while ~isempty(varargin)
    var = varargin{1};

    if isnumeric(var)
        % option is either number of directions or resolution
        if isscalar(var)
            nDirs = var;
        else
            delta = var;
        end
        varargin(1) = [];

    elseif ischar(var)
        if length(varargin) < 2
            error('Parameter name must be followed by parameter value');
        end

        if strcmpi(var, 'ndirs')
            nDirs = varargin{2};
        elseif strcmpi(var, 'resolution')
            delta = varargin{2};
        else
            error(['Unknown parameter name: ' var]);
        end

        varargin(1:2) = [];
    end
end

%% Compute perimeter within image, and normalize by area

% component area in image
p = imPerimeterEstimate(img, nDirs, delta);

% total area of image, without borders
totalArea = prod(size(img)-1) * prod(delta);

% compute perimeter density
pd = p / totalArea;


function ind = rhoInside(rho,minRho,maxRho)

minr = mod(minRho+1e-6,2*pi)-3e-6; % in [-2e-6,2*pi-3e-6]
maxr = mod(maxRho-1e-6,2*pi)+3e-6; % in [2e-6,2*pi+3e-6]

if minr < 0
  rho = mod(rho+1e-6,2*pi);
else
  rho = mod(rho-1e-6,2*pi);
end

if minr < maxr
  ind = rho > minr & rho < maxr;
else
  ind = rho > minr | rho < maxr;
end


function out=cot(x)

out=cos(x)./sin(x);


function [EB] = bit2EB(bit)
% Convert computery things from bits to exabytes.
% Chad A. Greene 2012
EB = bit*2^-63;


function IntegrateEuler(j)
% following Featherstone "Robot Dynamics Algorithm" p.103
% '04 May 8 s.k AIST
global uLINK Dtime

if j == 0 return; end
if j == 1
    [uLINK(j).p, uLINK(j).R] = SE3exp(j, Dtime);
    uLINK(j).vo = uLINK(j).vo + Dtime * uLINK(j).dvo;
    uLINK(j).w  = uLINK(j).w  + Dtime * uLINK(j).dw;
    %IntegrateSE3(j, Dtime);
else
    uLINK(j).q  = uLINK(j).q  + Dtime * uLINK(j).dq;
    uLINK(j).dq = uLINK(j).dq + Dtime * uLINK(j).ddq;
end

IntegrateEuler(uLINK(j).sister);
IntegrateEuler(uLINK(j).child);


function [distancemap]=surfacemap(Vertices,faces,Index)

%this function defines how many vertices each vertex is away from the
%specifc vertex with index "Index3".

%the first collumn gives the vertex indices, the second the number of
%vertices each vertex is distant from the Index vertix
%the third collumn gives the "weigth" for each, meaning the number of
%vertices of the same generation devided by the total number of vertices

faces(:,4)=1:length(faces);
distancemap(:,1)=1:length(Vertices(:,1));
distancemap(Index,2)=1;
distancemap(Index,3)=1-length(double(find(distancemap(:,2))))/length(Vertices(:,1));

a=2;
facestemp1(1,1)=Index;

while isempty(faces)==0

    indicesconnectedfaces=vertcat(find(double(ismember(faces(:,1),facestemp1))),find(double(ismember(faces(:,2),facestemp1))),find(double(ismember(faces(:,3),facestemp1))));
    connectedfaces=faces(indicesconnectedfaces,:);
    indicesvertices=unique(reshape(connectedfaces(:,1:3),3*length(connectedfaces(:,1)),1));
    distancemap(indicesvertices,2)=a;
    faces(indicesconnectedfaces,:)=[];

    indicesoldvertces=find(double(ismember(indicesvertices,facestemp1)));
    distancemap(indicesvertices(indicesoldvertces,:),2)=a-1;
    indicesvertices(indicesoldvertces,:)=[];
    facestemp1=indicesvertices;
    distancemap(indicesvertices,3)=1-length(double(find(distancemap(:,2))))/length(Vertices(:,1));
    a=a+1;
end


% Calculate border coverage for detected fields in a circular arena
%
% This function calculates firing map border coverage that is further used
% in calculation of a border score. This function must be used with recordings
% done in a circular environment. See USAGE for details.
%
%  USAGE
%   coverage = analyses.borderCoverageCircular(fieldsMap, <options>)
%   fieldsMap   2D binary matrix that repressents firing properties of the field.
%               fieldsMap must be a polar version of a regular firing rate map. This
%               means that x-axis should contain values in range 1-360, y-axis range
%               is 1 to radius of the circular arena.
%               fieldsMap should contain NaN for unvisited bins. Other possible values:
%               0 for zero firing rate, i.e. area that doesn't belong to a fields.
%               Any positive number marks an area of a field. For example, if there is
%               just a single field, then fieldsMap could consist of 3 values: NaNs, zeros,
%               and ones.
%   <options>   Optional list of property-value pairs (see table below)
%
%   ==============================================================================================
%    Properties    Values
%   ----------------------------------------------------------------------------------------------
%    'searchWidth'  If map is not perfect, but contains NaN values along borders, then
%                   search for border pixels can have NaNs. To mitigate this, we check
%                   searchWidth rows/columns near border and if the closest to the border pixel
%                   equals to NaN, we search for first non-NaN value in searchWidth rows/columns.
%                   This argument is optional and default value is 8 bins.
%    'walls'        Definition of walls along which the coverage is calculated. Provided by
%                   a 2D matrix Nx2. Each row defines 1 wall. Each row should have two values in degrees:
%                   1. angle at which wall starts.
%                   2. angle at which wall ends.
%                   Default value is [1 360], which represents a single wall around the whole circular
%                   environment. Note that you can not use 0 as an angle.
%   ==============================================================================================
%   coverage    Border coverage, ranges from 0 to 1.
%
function coverage = borderCoverageCircular(fieldsMap, varargin)
    coverage = 0;

    inp = inputParser;
    defaultSearchWidth = 8;
    defaultWalls = [1 360];

    checkSearchWidth = @(x) isnumeric(x) && isscalar(x) && (x > 0);
    checkWalls = @(x) size(x, 2) == 2 && all(x(:) > 0) && all(x(:) <= 360);

    addRequired(inp, 'fieldsMap', @(x) isnumeric(x) && size(x, 2) == 360);
    addParameter(inp, 'searchWidth', defaultSearchWidth, checkSearchWidth);
    addParameter(inp, 'walls', defaultWalls, checkWalls);

    parse(inp, fieldsMap, varargin{:});

    walls = inp.Results.walls;
    searchWidth = inp.Results.searchWidth;
    fieldsMap(fieldsMap > 1) = 1; % make it binary

    for i = 1:size(walls, 1)
        wall = walls(i, :);

        aux_map = fieldsMap(end-searchWidth+1:end, wall(1):wall(2));
        [covered, norm] = wall_field(aux_map);
        coverage = max([covered/norm, coverage]);
    end
end

% 'covered' pixels will have distance to border 0.
% Essentially we need to calculate number of elements,
% that equal to zero and take NaNs into account.
function [covered, norm] = wall_field(map)
    lx = size(map, 2);
    map = flipud(map);

    D = bwdist(map);
    nanIndices = find(isnan(map(1, :)));
    numNans = length(nanIndices);
    for i = 1:numNans
        testColumn = nanIndices(i);
        nonNan = find(~isnan(map(:, testColumn)), 1, 'first');
        if ~isempty(nonNan)
            numNans = numNans - 1;
            D(1, testColumn) = D(nonNan, testColumn);
        end
    end

    norm = lx - numNans;
    covered = nansum(D(1, :) == 0) - numNans;
end


function localangle = point_angle(bw, seed, R, start, NeighborNum)

% This function computes the angle of seed from the start point
if (nargin == 4)
    NeighborNum =3;
end

[M, N]= size(bw);

% image(y,x) <==>(x-1)*M + y
seedy = mod(seed, M);
if (seedy==0) seedy = M; end
seedx = 1 + (seed - seedy)/M;
starty = mod(start, M);
if (starty==0) starty = M; end
startx = 1 + (start - starty)/M;

dy = 1 + R + starty - seedy;
dx = 1 + R + startx - seedx;

% set the region border from 1:8*R
region = zeros(2*R+1);
region(R+1:-1:1,end) = (1:R+1)';
region(1,end-1:-1:2)= R+2:3*R;
region(1:end,1)= (3*R+1:5*R+1)';
region(end,2:end-1) = 5*R+2:7*R;
region(end:-1:R+2,end) = (7*R+1:8*R)';

startidx = region(dy, dx);
anglevec = point_anglevec(bw, seed, R);

if (startidx==0)
    localangle = [];
    return;
end

if (anglevec(startidx)==0)
    localangle = [];
    return;
end

localangle = findangle(anglevec, startidx);
localnum = prod(size(localangle));

if (localnum > NeighborNum)
    localangle = localangle(1:NeighborNum);
end
if (localnum < NeighborNum)
    localangle = [localangle, zeros(1, NeighborNum - localnum )];
end


% eeg_lat2point() - convert latencies in time units relative to the
%                   time locking event of an eeglab() data epoch to
%                   latencies in data points (assuming concatenated epochs).
% Usage:
%       >> [newlat] = eeg_lat2point( lat_array, epoch_array,...
%                                 srate, timelimits, timeunit);
% Inputs:
%   lat_array   - latency array in 'timeunit' units (see below)
%   epoch_array - epoch number for each latency
%   srate       - data sampling rate in Hz
%   timelimits  - [min max] epoch timelimits in 'timeunit' units (see below)
%   timeunit    - time unit relative to seconds. Default is 1 = seconds.
%
% Outputs:
%   newlat      - converted latency values in points assuming concatenated
%                 data epochs (see eeglab() event structure)
%
% Author: Arnaud Delorme, CNL / Salk Institute, 2 Mai 2002
%
% See also: eeg_point2lat(), eeglab()

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

function newlat = eeg_lat2point( lat_array, epoch_array, srate, timewin, timeunit);

if nargin <4
    help eeg_lat2point;
    return;
end;
if nargin <5
	timeunit = 1;
end;

if length(lat_array) ~= length(epoch_array)
	if length(epoch_array)~= 1
		disp('eeg_lat2point: latency and epochs must have the same length'); return;
	else
		epoch_array = ones(1,length(lat_array))*epoch_array;
	end;
end;
if length(timewin) ~= 2
    disp('eeg_lat2point: timelimits must have length 2'); return;
end;
if iscell(epoch_array)
	epoch_array = [ epoch_array{:} ];
end;
if iscell(lat_array)
	lat_array = [ lat_array{:} ];
end

timewin = timewin*timeunit;
pnts = (timewin(2)-timewin(1))*srate+1;
newlat  = (lat_array*timeunit-timewin(1))*srate+1 + (epoch_array-1)*pnts;


classdef RWMOP8 < PROBLEM
% <multi> <real> <constrained>
% Car side impact design problem

%------------------------------- Reference --------------------------------
% A. Kumar, G. Wu, M. Ali, Q. Luo, R. Mallipeddi, P. Suganthan, and S. Das,
% A benchmark-suite of real-world constrained multi-objective optimization
% problems and some baseline results, Swarm and Evolutionary Computation,
% 2021, 67: 100961.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    methods
        %% Initialization
        function Setting(obj)
            obj.M        = 3;
            obj.D        = 7;
            obj.lower    = [0.5,0.45,0.5,0.5,0.875,0.4,0.4];
            obj.upper    = [1.5,1.35,1.5,1.5,2.625,1.2,1.2];
            obj.encoding = ones(1,obj.D);
        end
        %% Evaluate multiple solutions
        function Population = Evaluation(obj,varargin)
            x  = varargin{1};
            x1 = x(:,1);
            x2 = x(:,2);
            x3 = x(:,3);
            x4 = x(:,4);
            x5 = x(:,5);
            x6 = x(:,6);
            x7 = x(:,7);
            VMBP = 10.58-0.674.*x1.*x2-0.67275.*x2;
            VFD  = 16.45-0.489.*x3.*x7-0.843.*x5.*x6;
            % Objective function
            f(:,1) = 1.98+4.9.*x1.*6.67.*x2+6.98.*x3+4.01.*x4+1.78.*x5+1e-5.*x6+2.73.*x7;
            f(:,2) = 4.72-0.5.*x4-0.19.*x2.*x3;
            f(:,3) = 0.5.*(VMBP+VFD);
            % Constraints
            g(:,1) = -1+1.16-0.3717.*x2.*x4-0.0092928.*x3;
            g(:,2) = -0.32+0.261-0.0159.*x1.*x2-0.06486.*x1-0.019.*x2.*x7+0.0144.*x2.*x5+0.0154464.*x6;
            g(:,3) = -0.32+0.74-0.61.*x2-0.031296.*x3-0.031872.*x7+0.227.*x2.^2;
            g(:,4) = -0.32+0.214+0.00817.*x5-0.045195.*x1-0.0135168.*x1+0.03099.*x2.*x6-0.018.*x2.*x7+0.007176.*x3+0.023232.*x3-0.00364.*x5.*x6-0.018.*x2.^2;
            g(:,5) = -32+33.86+2.95.*x3-5.057.*x1.*x2-3.795.*x2-3.4431.*x7+1.45728;
            g(:,6) = -32+28.98+3.818.*x3-4.2.*x1.*x2+1.27296.*x6-2.68065.*x7;
            g(:,7) = -32+46.36-9.9.*x2-4.4505.*x1;
            g(:,8) = f(:,2)-4;
            g(:,9) = VMBP - 9.9;
            Population = SOLUTION(varargin{1},f,g,varargin{2:end});
            obj.FE     = obj.FE + length(Population);
        end
         %% Generate a point for hypervolume calculation
        function R = GetOptimum(obj,~)
            R = [9.2596587e+01   4.0000000e+00   1.2699733e+01];
        end
    end
end


function ConvHDRtoStack(fmtIn, fmtOut, bSampling, ldr_gamma)
%
%        ConvHDRtoStack(fmtIn, fmtOut, bSampling, ldr_gamma)
%
%
%        For example:
%           ConvLDRtoLDR('hdr', 'jpg', 2.2);
%
%        This lines tonemaps all the .hdr files in the folder using the
%        Reinhard et al.'s operator and it saves them as .jpg files using
%        gamma 2.2
%
%        Input:
%           -fmtIn: an input string represeting the LDR format of the images
%           to be converted. This can be: 'hdr', 'pfm'
%           -fmtOut: an input string represeting the LDR format of
%           converted images. This can be: 'jpeg', 'jpg', 'png', etc.
%           -bSampling: using or not histogram sampling
%           -ldr_gamma: the encoding gamma for the LDR images. The default
%           value is 2.2
%
%        Output:
%           -ret: a boolean value, true or 1 if the method succeeds
%
%     Copyright (C) 2012-15  Francesco Banterle
%
%     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/>.
%

if(~exist('bSampling', 'var'))
    bSampling = 0;
end

if(~exist('ldr_gamma', 'var'))
    ldr_gamma = 2.2;
end

lst = dir(['*.', fmtIn]);

for i=1:length(lst)
    disp(lst(i).name);

    tmp_name = lst(i).name;
    img = hdrimread(tmp_name);
    L = lum(img);
    L = imresize(L, 0.5, 'bilinear');

    if(bSampling)
        fstops = ExposureHistogramSampling(L, 8, 2);
    else
        minL = round(log2(min(L(:))));
        maxL = round(log2(max(L(:))));
        fstops = -maxL:-minL;
    end

    for j=1:length(fstops)
        disp(fstops(j));
        img_exp_j = GammaTMO(img, ldr_gamma, fstops(j));

        bSkip = 0;
        if(~bSampling)
            tImg1 = ClampImg(round(255 * img_exp_j) / 255, 0, 1);
            val = mean(tImg1(:)); %mean value

            if((val < 0.1) || (val > 0.9))
                bSkip = 1;
            end

        end

        if(~bSkip)
            tmp_name_we = RemoveExt(tmp_name);
            tmp_name_out = [tmp_name_we, '_fstop_', num2str(j), '.', fmtOut];
            imwrite(img_exp_j, tmp_name_out);
        end
    end
end

end


function [traj, infStates] = tapas_hgf_whichworld(r, p, varargin)
% Calculates the trajectories of the agent's representations under the HGF
%
% This function can be called in two ways:
%
% (1) tapas_hgf_whichworld(r, p)
%
%     where r is the structure generated by tapas_fitModel and p is the parameter vector in native space;
%
% (2) tapas_hgf_whichworld(r, ptrans, 'trans')
%
%     where r is the structure generated by tapas_fitModel, ptrans is the parameter vector in
%     transformed space, and 'trans' is a flag indicating this.
%
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2013 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/>.

% Check whether we have a configuration structure
if ~isfield(r,'c_prc')
    error('tapas:hgf:ConfigRequired', 'Configuration required: before calling tapas_hgf_whichworld, tapas_hgf_whichworld_config has to be called.');
end

% Transform paramaters back to their native space if needed
if ~isempty(varargin) && strcmp(varargin{1},'trans');
    p = tapas_hgf_whichworld_transp(r, p);
end

% Number of worlds
nw = r.c_prc.nw;

% Bernoulli parameters that characterize
% worlds (column vector)
bp = [0.85; 0.15];

% Unpack parameters
mu2_0 = p(1:nw);
sa2_0 = p(nw+1:2*nw);
mu3_0 = p(2*nw+1);
sa3_0 = p(2*nw+2);
ka    = p(2*nw+3);
om    = p(2*nw+4);
th    = p(2*nw+5);
m     = p(2*nw+6);
phi   = p(2*nw+7);

% Add dummy "zeroth" trial
u = [0; r.u(:,1)];

% Number of trials (including prior)
n = length(u);

% Initialize updated quantities

% Representations
mu1 = NaN(n,nw);
pi1 = NaN(n,nw);
mu2 = NaN(n,nw);
pi2 = NaN(n,nw);
mu3 = NaN(n,1);
pi3 = NaN(n,1);

% Other quantities
mu1hat = NaN(n,nw);
pi1hat = NaN(n,nw);
mu2hat = NaN(n,nw);
pi2hat = NaN(n,nw);
mu3hat = NaN(n,1);
pi3hat = NaN(n,1);
v2     = NaN(n,1);
w2     = NaN(n,nw);
da1    = NaN(n,nw);
da2    = NaN(n,nw);

% Representation priors
% Note: first entries of the other quantities remain
% NaN because they are undefined and are thrown away
% at the end; their presence simply leads to consistent
% trial indices.
mu1(1,:) = tapas_sgm(mu2_0, 1);
pi1(1,:) = 1./(mu1(1,:).*(1-mu1(1,:)));
mu2(1,:) = mu2_0;
pi2(1,:) = 1./sa2_0;
mu3(1)   = mu3_0;
pi3(1)   = 1/sa3_0;

% Pass through representation update loop
for k = 2:1:n
    if not(ismember(k-1, r.ign))

        %%%%%%%%%%%%%%%%%%%%%%
        % Effect of input u(k)
        %%%%%%%%%%%%%%%%%%%%%%

        % 1st level
        % ~~~~~~~~~
        % Predictions
        mu1hat(k,:) = tapas_sgm(mu2(k-1,:), 1);

        % Precisions of predictions
        pi1hat(k,:) = 1./(mu1hat(k,:).*(1 -mu1hat(k,:)));

        % Updates (simply applying Bayes' theorem)

        % Likelihood of outcome u(k)
        llh = bp.^u(k).*(1-bp).^(1-u(k));

        % Marginal likelihood of outcome
        mllh = mu1hat(k,:)*llh;

        % Posterior for each world
        mu1(k,:) = mu1hat(k,:).*llh'./mllh;

        % Precision of posterior
        pi1(k,:) = 1./(mu1(k,:).*(1-mu1(k,:)));

        % Prediction errors
        da1(k,:) = mu1(k,:) -mu1hat(k,:);

        % 2nd level
        % ~~~~~~~~~
        % Predictions
        mu2hat(k,:) = mu2(k-1,:);

        % Precisions of predictions
        pi2hat(k,:) = 1./(1./pi2(k-1,:) +exp(ka *mu3(k-1) +om));

        % Updates
        pi2(k,:) = pi2hat(k,:) +1./pi1hat(k,:);

        mu2(k,:) = mu2hat(k,:) +1./pi2(k,:) .*da1(k,:);

        % Volatility prediction errors
        da2(k,:) = (1./pi2(k,:) +(mu2(k,:) -mu2hat(k,:)).^2) .*pi2hat(k,:) -1;

        % 3rd level
        % ~~~~~~~~~
        % Predictions
        mu3hat(k) = mu3(k-1) +phi *(m -mu3(k-1));

        % Precision of prediction
        pi3hat(k) = 1/(1/pi3(k-1) +th);

        % Weighting factors
        v2(k)   = exp(ka *mu3(k-1) +om);
        w2(k,:) = v2(k) *pi2hat(k,:);

        % Updates
        pi3(k) = pi3hat(k) +1/nw*sum(1/2 *ka^2 *w2(k,:) .*(w2(k,:) +(2 *w2(k,:) -1) .*da2(k,:)));

        if pi3(k) <= 0
            error('tapas:hgf:NegPostPrec', 'Negative posterior precision. Parameters are in a region where model assumptions are violated.');
        end

        mu3(k) = mu3hat(k) +sum(1/2 *1/pi3(k) *ka *w2(k,:) .*da2(k,:));

    else
        mu1(k,:) = mu1(k-1,:);
        pi1(k,:) = pi1(k-1,:);
        mu2(k,:) = mu2(k-1,:);
        pi2(k,:) = pi2(k-1,:);
        mu3(k)   = mu3(k-1);
        pi3(k)   = pi3(k-1);

        mu1hat(k,:) = mu1hat(k-1,:);
        pi1hat(k,:) = pi1hat(k-1,:);
        mu2hat(k,:) = mu2hat(k-1,:);
        pi2hat(k,:) = pi2hat(k-1,:);
        mu3hat(k)   = mu3hat(k-1);
        pi3hat(k)   = pi3hat(k-1);
        v2(k)       = v2(k-1);
        w2(k,:)     = w2(k-1,:);
        da1(k,:)    = da1(k-1,:);
        da2(k,:)    = da2(k-1,:);
    end
end

% Implied learning rates at the first level
sgmmu2 = tapas_sgm(mu2, 1);
lr1    = diff(sgmmu2)./da1(2:n,:);
lr1(da(2:n,1)==0) = 0;

% Remove representation priors
mu1(1,:)  = [];
pi1(1,:)  = [];
mu2(1,:)  = [];
pi2(1,:)  = [];
mu3(1)    = [];
pi3(1)    = [];

% Remove other dummy initial values
mu1hat(1,:) = [];
pi1hat(1,:) = [];
mu2hat(1,:) = [];
pi2hat(1,:) = [];
mu3hat(1,:) = [];
pi3hat(1)   = [];
v2(1)       = [];
w2(1,:)     = [];
da1(1,:)    = [];
da2(1,:)    = [];

% Create result data structure
traj = struct;

traj.mu = NaN(n-1,3,nw);
traj.mu(:,1,:) = mu1;
traj.mu(:,2,:) = mu2;
traj.mu(:,3,1) = mu3;

traj.sa = NaN(n-1,3,nw);
traj.sa(:,1,:) = 1./pi1;
traj.sa(:,2,:) = 1./pi2;
traj.sa(:,3,1) = 1./pi3;

traj.muhat = NaN(n-1,3,nw);
traj.muhat(:,1,:) = mu1hat;
traj.muhat(:,2,:) = mu2hat;
traj.muhat(:,3,1) = mu3hat;

traj.sahat = NaN(n-1,3,nw);
traj.sahat(:,1,:) = 1./pi1hat;
traj.sahat(:,2,:) = 1./pi2hat;
traj.sahat(:,3,1) = 1./pi3hat;

traj.v       = v2;
traj.w       = w2;

traj.da = NaN(n-1,2,nw);
traj.da(:,1,:) = da1;
traj.da(:,2,:) = da2;

% Updates with respect to prediction
traj.ud = traj.mu -traj.muhat;

% Psi (precision weights on prediction errors)
psi        = NaN(n-1,3,nw);
psi(:,2,:) = 1./pi2;
psi(:,3,:) = diag(1./pi3) *pi2hat;
traj.psi   = psi;

% Epsilons (precision-weighted prediction errors)
epsi        = NaN(n-1,3,nw);
epsi(:,2,:) = squeeze(psi(:,2,:)) .*da1;
epsi(:,3,:) = squeeze(psi(:,3,:)) .*da2;
traj.epsi   = epsi;

% Full learning rate (full weights on prediction errors)
wt        = NaN(n-1,3,nw);
wt(:,1,:) = lr1;
wt(:,2,:) = psi(:,2,:);
wt(:,3,:) = 1/2 *ka *diag(1/pi3) *w2;
traj.wt   = wt;

% Create matrices for use by the observation model
infStates = NaN(n-1,3,nw,4);
infStates(:,:,:,1) = traj.muhat;
infStates(:,:,:,2) = traj.sahat;
infStates(:,:,:,3) = traj.mu;
infStates(:,:,:,4) = traj.sa;

return;


% absolute value of smoothed change in orientation
function [data,units] = compute_abssmoothdtheta(trx,n)

flies = trx.exp2flies{n};
nflies = numel(flies);
data = cell(1,nflies);
for i = 1:nflies,
  fly = flies(i);
  data{i} = abs(trx(fly).smoothdtheta);
end
units = parseunits('rad/s');


function pde = constant_fun(bm,bp,r,x0,y0,z0,coef)
%% USAGE: polynomial solution for Poisson equation
%  Last Modified: 02/21/2020 by Xu Zhang

%% PDE Structure
pde = struct('intf',@intf,...
    'exactu1',@exactu1,'exactu2',@exactu2,'exactu3',@exactu3,...
    'um1',@um1,'um2',@um2,'um3',@um3,'up1',@up1,'up2',@up2,'up3',@up3,...
    'Dxu',@Dxu,'Dxum',@Dxum,'Dxup',@Dxup,'Dyu',@Dyu,...
    'Dyum',@Dyum,'Dyup',@Dyup,'Dzu',@Dzu,'Dzum',@Dzum,'Dzup',@Dzup,...
    'f1',@f1,'f2',@f2,'f3',@f3,...
    'fm1',@fm1,'fm2',@fm2,'fm3',@fm3,...
    'fp1',@fp1,'fp2',@fp2,'fp3',@fp3,...
    'A',@A,'Am',@Am,'Ap',@Ap,'one',@one);

pde.bm = bm;
pde.bp = bp;
%% interface function
    function u = intf(x,y,z)
        u = ((x-x0).^2 + (y-y0).^2 + (z-z0).^2).^(1/2)/r-1;
    end

%% exact solution
    function u = exactu1(x,y,z)
        u = um1(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = up1(x(id),y(id),z(id));
    end
    function u = exactu2(x,y,z)
        u = um2(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = up2(x(id),y(id),z(id));
    end
    function u = exactu3(x,y,z)
        u = um3(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = up3(x(id),y(id),z(id));
    end
    function u = um1(x,y,z)
        u = coef*ones(size(x));
    end
    function u = um2(x,y,z)
        u = coef*ones(size(x));
    end
    function u = um3(x,y,z)
        u = coef*ones(size(x));
    end
    function u = up1(x,y,z)
        u = coef*ones(size(x));
    end
    function u = up2(x,y,z)
        u = coef*ones(size(x));
    end
    function u = up3(x,y,z)
        u = coef*ones(size(x));
    end

%% Boundary Function
    function u = gD1(x,y,z)
        u = exactu1(x,y,z);
    end
    function u = gD2(x,y,z)
        u = exactu2(x,y,z);
    end
    function u = gD3(x,y,z)
        u = exactu3(x,y,z);
    end
%% Derivative of the exact solution
    function u = Dxu(x,y,z)
        u = Dxum(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Dxup(x(id),y(id),z(id));
    end
    function u = Dyu(x,y,z)
        u = Dyum(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Dyup(x(id),y(id),z(id));
    end
    function u = Dzu(x,y,z)
        u = Dzum(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Dzup(x(id),y(id),z(id));
    end
    function u = Dxum(x,y,z)
        u = zeros(size(x));
    end
    function u = Dyum(x,y,z)
        u = zeros(size(x));
    end
    function u = Dzum(x,y,z)
        u = zeros(size(x));
    end
    function u = Dxup(x,y,z)
        u = zeros(size(x));
    end
    function u = Dyup(x,y,z)
        u = zeros(size(x));
    end
    function u = Dzup(x,y,z)
        u = zeros(size(x));
    end

    function u = Duker(x,y,z)
        u = zeros(size(x));
    end

%% right hand side function
    function u = f1(x,y,z)
        u = fm1(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = fp1(x(id),y(id),z(id));
    end
    function u = f2(x,y,z)
        u = fm2(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = fp2(x(id),y(id),z(id));
    end
    function u = f3(x,y,z)
        u = fm3(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = fp3(x(id),y(id),z(id));
    end

    function u = fm1(x,y,z)
        u = coef*ones(size(x));
    end
    function u = fm2(x,y,z)
        u = coef*ones(size(x));
    end
    function u = fm3(x,y,z)
        u = coef*ones(size(x));
    end
    function u = fp1(x,y,z)
        u = coef*ones(size(x));
    end
    function u = fp2(x,y,z)
        u = coef*ones(size(x));
    end
    function u = fp3(x,y,z)
        u = coef*ones(size(x));
    end

%% Diffusion coefficient function
    function u = A(x,y,z)
        u = Am(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Ap(x(id),y(id),z(id));
    end
    function u = Am(x,y,z)
        u = bm*ones(size(x));
    end
    function u = Ap(x,y,z)
        u = bp*ones(size(x));
    end

%% Other function
    function u = one(x,y,z)
        u = ones(size(x));
    end
end


function colorTest()

%   colorTest() creates a test image showing the color encoding scheme

%   According to the c++ source code of Daniel Scharstein
%   Contact: schar@middlebury.edu

%   Author: Deqing Sun, Department of Computer Science, Brown University
%   Contact: dqsun@cs.brown.edu
%   $Date: 2007-10-31 20:22:10 (Wed, 31 Oct 2006) $

% Copyright 2007-2010, Brown University, Providence, RI. USA
%
%
%                         All Rights Reserved
%
% Permission to use, copy, modify, and distribute this software and its
% documentation for any purpose other than its incorporation into a
% commercial product is hereby granted without fee, provided that the
% above copyright notice appear in all copies and that both that
% copyright notice and this permission notice appear in supporting
% documentation, and that the name of the author and Brown University not be used in
% advertising or publicity pertaining to distribution of the software
% without specific, written prior permission.
%
% THE AUTHOR AND BROWN UNIVERSITY DISCLAIM ALL WARRANTIES WITH REGARD TO THIS SOFTWARE,
% INCLUDING ALL IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR ANY
% PARTICULAR PURPOSE.  IN NO EVENT SHALL THE AUTHOR OR BROWN UNIVERSITY BE LIABLE FOR
% ANY SPECIAL, INDIRECT OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
% WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
% ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
% OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.

%% test color pattern of Daniel's c++ code

truerange = 1;
height = 151;
width  = 151;
range = truerange * 1.04;

s2 = round(height/2);

[x y] = meshgrid(1:width, 1:height);

u = x*range/s2 - range;
v = y*range/s2 - range;

img = computeColor(u/truerange, v/truerange);

img(s2,:,:) = 0;
img(:,s2,:) = 0;

figure;
imshow(img);
title('test color pattern');
pause; close;

% test read and write flow
F(:,:,1) = u;
F(:,:,2) = v;
writeFlowFile(F, 'colorTest.flo');
F2 = readFlowFile('colorTest.flo');

u2 = F2(:,:,1);
v2 = F2(:,:,2);

img2 = computeColor(u2/truerange, v2/truerange);

img2(s2,:,:) = 0;
img2(:,s2,:) = 0;

figure; imshow(img2);
title('saved and reloaded test color pattern');
pause; close;

% color encoding scheme for optical flow
img = computeColor(u/range/sqrt(2), v/range/sqrt(2));

img(s2,:,:) = 0;
img(:,s2,:) = 0;

figure;
imshow(img);
title('optical flow color encoding scheme');
pause; close;


function A10 = synA10min(A11, A00, cmax)
%-----------------------------------------------------------------------------
%
% For each point of colour 10 this function assigns the minimum value at the
% neighbouring gridpoints of colours 11 and 00.
%
% Design and implementation by:
% Dr. Paul M. de Zeeuw <Paul.de.Zeeuw@cwi.nl>  http://homepages.cwi.nl/~pauldz/
% Last Revision: December 7, 2001.
% (c) 1998-2002 Stichting CWI, Amsterdam
%-----------------------------------------------------------------------------
[n00, m00]=size(A00);
[n11, m11]=size(A11);
n10=n11;
m10=m00;
%[n10, m10]=size(A10);
if     m10 == m11
  S=min(A11, stripR(extL(A11, cmax)));
elseif m10 == m11+1
  S=min(extL(A11, cmax), extR(A11, cmax));
else
  disp([' size A11 = ' int2str(size(A11)) ' size A00 = ' int2str(size(A00))]);
  error(' synA10min - A11 and A00 do not match ');
end
if     n10 == n00
  T=min(A00, stripU(extD(A00, cmax)));
elseif n10 == n00-1
  T=min(stripD(A00), stripU(A00));
else
  disp([' size A11 = ' int2str(size(A11)) ' size A00 = ' int2str(size(A00))]);
  error(' synA10min - A11 and A00 do not match ');
end
%Note: all(size(S) == size(T)) & all(size(S) == [n10 m10]) always holds.
A10=min(S, T);
%-----------------------------------------------------------------------------


%% FACE_POOL runs the example interactively with MATLABPOOL.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 June 2010
%
%  Author:
%
%    Gene Cliff, John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'FACE_POOL\n' );
  fprintf ( 1, '  Evaluate FACE_FUN directly with MATLABPOOL.\n' );
%
%  Load the MAT file containing the grid coordinates as the variable
%  "Grid" and the element connectivity as the variable "e_conn".
%
  load GRID
%
%  We will be looking for tetrahedrons which have one face lying
%  on a plane defining one side of the box.  To define this box
%  we must specify:
%
%  II, the index of the "I" variable which is constant on the plane.
%  If the constant variable is X, then II is 1, and so on.
%
%  VAL_II, the value of the constant "I" variable.
%
%  XJ_LB, XJ_UB, the upper and lower bounds for the "J" nonconstant variable.
%
%  XK_LB, XK_UB, the upper and lower bounds for the "K" nonconstant variable.
%
%  The "J" and "K" variables are the first and second variables to follow
%  the "I" variable, assuming that we are counting with wrap around.
%  There are thus three possibilities for I, J and K:
%
%    I  J  K
%   -- -- --
%    1  2  3
%    2  3  1
%    3  1  2
%
%  Our choices below specify that we are looking at the plane Z = 3.0,
%  and more specifically, the box in that plane for which
%  -1.0 <= X <= +1.0, and -1.0 <= Y <= +1.0.
%
  ii = 3;
  val_ii = 3.0;
  xj_lb = -1.0;
  xj_ub = +1.0;
  xk_lb = -1.0;
  xk_ub = +1.0;
%
%  Call FACE_FUN to compute a list of the triangular faces of the
%  tetrahedral elements which lie on the boundary plane.
%
%  Get 4 "labs" to work on the job.
%
  matlabpool open local 4
  F_conn = face_fun ( Grid, e_conn, ii, val_ii, xj_lb, xj_ub, xk_lb, xk_ub );
  matlabpool close

  face_num = size ( F_conn, 2 );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Number of triangles on boundary is %d\n', face_num );
%
%  Display the surface triangulation.
%
  i_pl = setdiff ( 1:3, ii );

  triplot ( F_conn', Grid(:, i_pl(1)), Grid(:, i_pl(2)) );

  if ( ii == 1 )
    title ( 'Triangular faces on X boundary plane' );
    xlabel ( '- Y axis -' );
    ylabel ( '- Z axis -' );
  elseif ( ii == 2 )
    xlabel ( '- X axis -' );
    title ( 'Triangular faces on Y boundary plane' );
    ylabel ( '- Z axis -' );
  elseif ( ii == 3 )
    xlabel ( '- X axis -' );
    ylabel ( '- Y axis -' );
    title ( 'Triangular faces on Z boundary plane' );
  end

  axis equal
  axis tight


function [Estimation_X] = REKF_propagate(Estimation_X, OdometryFromThis2Next, Sigma_ODO )

% retrieve v and w
v = OdometryFromThis2Next(1:3);
w = OdometryFromThis2Next(4:6);

NumberOfLandmarks = size(Estimation_X.landmarks, 2);
%Jrw = jaco_r(-w);
Jrw=eye(3);
ExpMinusM = so3_exp(-w);

temp = repmat({Estimation_X.orientation}, NumberOfLandmarks+2,1 );
A = blkdiag(temp{:});
 A(4:6,1:3)=skew(Estimation_X.position)*Estimation_X.orientation;

if NumberOfLandmarks>0
   for i=1:NumberOfLandmarks
    A(6+3*i-2:6+3*i,1:3)=skew(Estimation_X.landmarks(1:3,i))*Estimation_X.orientation;
   end
end

%B1=[-Jrw  zeros(3,3); -skew(v)*Jrw -eye(3)];

B1=[eye(3)  zeros(3,3); zeros(3,3) eye(3)];

B=[B1; sparse(3*NumberOfLandmarks,6)];
B=sparse(B);

adA=A*B;

odoCov=diag([w.^2;v.^2])*Sigma_ODO^2;

% final update the covariance
Estimation_X.cov = Estimation_X.cov+ adA*odoCov*adA';

% update position and orientation
Estimation_X.position = Estimation_X.position+Estimation_X.orientation*v;
Estimation_X.orientation = Estimation_X.orientation*so3_exp(w);

end


function [Ta,Ka,Ba,Tg,Kg,Bg,Tm2a,Bm,Vm,mag_strength]=ImuCalibration_Gesture(data)
% input data raw IMU data from mpu9250
% data :time accelerometer  gyroscope   magnetometer
%  cal_acc=Ta*Ka*(raw_acc+Ba)
%  cal_gyro=Tg*Kg*(raw_gyro+Bg)
%  cal_mag=Tm2a*(raw_mag+Bm)
%
% author  Zhang Xin

[~,fix_point,rotation]=FindFixData(data,30);

[Ta,Ka,Ba]=ICRA2014_acc(fix_point);

Bg=-mean(fix_point(:,4:6),1)';

n=size(rotation,1);

rotation{n+1}=Ta;
rotation{n+2}=Ka;
rotation{n+3}=Ba;
rotation{n+4}=Bg;

[Tg,Kg]=ICRA_2014_gyro(rotation);

%[Tm2a,Bm,Vm]=mag2acc_matrix(fix_point,Ta,Ka,Ba);

[Tm2a,Bm,Vm,mag_strength]=Cal_mag4acc_frame(rotation,fix_point,Tg,Kg);

Set_Bias_Gyro=[0.1,-0.4,1.5];

See_Gesture( data,Ta,Ka,Ba,Tg,Kg,Bg,Tm2a,Bm,Vm,Set_Bias_Gyro);

end


function varargout = rank(varargin)
%RANK      Rank of a SPHEREFUN.
%   RANK(F) produces an estimate of the rank of the approximant F.
%
%   RANK(F, TOL) is the number of singular values of F greater than TOL/N, where
%   N is the first singular value of F.
%
% See also SPHEREFUN/LENGTH.

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

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

end


function f = makeimagestack(m,wantnorm,addborder,csize,bordersize)

% function f = makeimagestack(m,wantnorm,addborder,csize,bordersize)
%
% <m> is a 3D matrix.  if more than 3D, we reshape to be 3D.
%   we automatically convert to double format for the purposes of this function.
% <wantnorm> (optional) is
%   0 means no normalization
%   [A B] means normalize and threshold values such that A and B map to 0 and 1.
%   X means normalize and threshold values such that X percentile
%     from lower and upper end map to 0 and 1.  if the X percentile
%     from the two ends are the same, then map everything to 0.
%   -1 means normalize to 0 and 1 using -max(abs(m(:))) and max(abs(m(:)))
%   -2 means normalize to 0 and 1 using 0 and max(m(:))
%   -3 means normalize to 0 and 1 using min(m(:)) and max(m(:))
%   default: 0.
% <addborder> (optional) is
%    0 means do not add border
%    1 means add border at the right and bottom of each image.
%      the border is assigned the maximum value.
%    2 means like 1 but remove the final borders at the right and bottom.
%   -1 means like 1 but assign the border the middle value instead of the max.
%   -2 means like 2 but assign the border the middle value instead of the max.
%    j means like 1 but assign the border a value of 0.
%  2*j means like 2 but assign the border a value of 0.
%  NaN means plot images into figure windows instead of returning a matrix.
%      each image is separated by one matrix element from surrounding images.
%      in this case, <wantnorm> should not be 0.
%    default: 1.
% <csize> (optional) is [X Y], a 2D matrix size according
%   to which we concatenate the images (row then column).
%   default is [], which means try to make as square as possible
%   (e.g. for 16 images, we would use [4 4]).
%   special case is -1 which means use [1 size(m,3)].
%   another special case is [A 0] or [0 A] in which case we
%   set 0 to be the minimum possible to fit all the images in.
% <bordersize> (optional) is number of pixels in the border in the case that
%   <addborder> is not NaN.  default: 1.
%
% if <addborder> is not NaN, then return a 3D matrix.  the first two dimensions
% contain images concatenated together, with any extra slots getting filled
% with the minimum value.  the third dimension contains additional sets of images
% (if necessary).
%
% if <addborder> is NaN, then make a separate figure window for each set of images.
% (actually, we create new figure windows only for sets after the first set.  so the
% we attempt to draw the first set in the current figure window.)  in each figure window,
% we plot individual images using imagesc scaled to the range [0,1].
% we return <f> as [].
%
% example:
% a = randn(10,10,12);
% imagesc(makeimagestack(a,-1));
% imagesc(makeimagestack(a,-1,NaN));

% input
if ~exist('wantnorm','var') || isempty(wantnorm)
  wantnorm = 0;
end
if ~exist('addborder','var') || isempty(addborder)
  addborder = 1;
end
if ~exist('csize','var') || isempty(csize)
  csize = [];
end
if ~exist('bordersize','var') || isempty(bordersize)
  bordersize = 1;
end

% calc
nrows = size(m,1);
ncols = size(m,2);

% make double if necessary
m = double(m);
wantnorm = double(wantnorm);

% make <m> 3D if necessary
m = reshape(m,size(m,1),size(m,2),[]);

% find range, normalize
if length(wantnorm)==2
  m = normalizerange(m,0,1,wantnorm(1),wantnorm(2));
  mn = 0;
  mx = 1;
elseif wantnorm==0
  mn = nanmin(m(:));
  mx = nanmax(m(:));
elseif wantnorm==-1
  m = normalizerange(m,0,1,-max(abs(m(:))),max(abs(m(:))));
  mn = 0;
  mx = 1;
elseif wantnorm==-2
  m = normalizerange(m,0,1,0,max(m(:)));
  mn = 0;
  mx = 1;
elseif wantnorm==-3
  m = normalizerange(m,0,1,min(m(:)),max(m(:)));
  mn = 0;
  mx = 1;
else
  rng = range_outlier(m(:),wantnorm);
  if rng(2)==rng(1)
    m = zeros(size(m));  % avoid error from normalizerange.m
  else
    m = normalizerange(m,0,1,rng(1),rng(2));
  end
  mn = 0;
  mx = 1;
end
md = (mn+mx)/2;

% number of images
numim = size(m,3);

% calculate csize if necessary
if isempty(csize)
  rows = floor(sqrt(numim));  % MAKE INTO FUNCTION?
  cols = ceil(numim/rows);
  csize = [rows cols];
elseif isequal(csize,-1)
  csize = [1 numim];
elseif csize(1)==0
  csize(1) = ceil(numim/csize(2));
elseif csize(2)==0
  csize(2) = ceil(numim/csize(1));
end

% calc
chunksize = prod(csize);
numchunks = ceil(numim/chunksize);

% convert to cell vector, add some extra matrices if necessary
m = splitmatrix(m,3);
m = [m repmat({repmat(mn,size(m{1}))},1,numchunks*chunksize-numim)];

% figure case
if isnan(addborder)

  for p=1:numchunks
    if p ~= 1
      drawnow; figure;
    end
    hold on;
    for q=1:chunksize
      xx = linspace(1+(ceil(q/csize(1))-1)*(ncols+1),ncols+(ceil(q/csize(1))-1)*(ncols+1),ncols);
      yy = linspace(1+(mod2(q,csize(1))-1)*(nrows+1),nrows+(mod2(q,csize(1))-1)*(nrows+1),nrows);
      imagesc(xx,yy,m{(p-1)*chunksize+q},[0 1]);
    end
    axis equal;
    set(gca,'YDir','reverse');
  end
  f = [];

% matrix case
else

  % add border?
  if imag(addborder) || addborder
    for p=1:length(m)
      m{p}(end+(1:bordersize),:) = choose(imag(addborder),0,choose(addborder > 0,mx,md));
      m{p}(:,end+(1:bordersize)) = choose(imag(addborder),0,choose(addborder > 0,mx,md));
    end
  end

  % combine images
  f = [];
  for p=1:numchunks
    temp = m((p-1)*chunksize + (1:chunksize));
    f = cat(3,f,cell2mat(reshape(temp,csize)));
  end

  % remove final?
  if abs(addborder)==2
    f(end-bordersize+1:end,:,:) = [];
    f(:,end-bordersize+1:end,:) = [];
  end

end


function c = camGraphics(csize,euler)

%CAMGRAPHICS Create a camera graphics 3D object.
%   CAMGRAPHICS creates a solid representing a camera. The objective is
%   heading towards the camera optical axis and is located at camera
%   position. Initial configuration is: position at origin and optical
%   axis aligned with world's Z axis.
%
%   The result is a structure with fields:
%       .vert0  the vertices in camera frame.
%       .vert   the vertices in world frame.
%       .faces  the definition of faces.
%
%   Fields .vert and .faces are used to draw the object via the PATCH
%   command. Further object repositionning is accomplished with DRAWOBJECT.
%
%   CAMGRAPHICS(SIZE) allows for choosing the camera size. Default is 0.1.
%
%   CAMGRAPHICS(SIZE,EULER) admits a different orientation to the default
%   one. To make the camera look in the X-axis direction, use EULER =
%   [-pi/2 0 -pi/2].
%
%   See also PATCH, SET, DRAWOBJECT.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

if nargin < 2
    euler = zeros(3,1);
    if nargin < 1
        csize = 0.1;
    end
end

d = .6; % objective diameter
f = .5; % objective length
h = .8; % body height
w = 1;  % body width
t = .3; % body thickness

% objective vertices
a         = (0:pi/4:2*pi-pi/4)';
na        = length(a);
circle    = d/2*[cos(a),sin(a)];
objective = [circle,zeros(na,1);circle,f*ones(na,1)];

% body vertices
rect      = [1 1;-1 1;-1 -1;1 -1]*diag([w h])/2;
body      = [rect,zeros(4,1);rect,-t*ones(4,1)];

% rotation matrix
R = e2R(euler);

% vertices in camera frame
vert0     = [body;objective]*R';

% graphics structure - vertices and faces
c.vert0   = csize*vert0;
c.vert    = c.vert0;
c.faces   = [ ...
    01 02 03 04
    05 06 07 08
    01 02 06 05
    02 03 07 06
    03 04 08 07
    04 01 05 08
    09 10 18 17
    10 11 19 18
    11 12 20 19
    12 13 21 20
    13 14 22 21
    14 15 23 22
    15 16 24 23
    16 09 17 24];

% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

%---------------------------------------------------------------------
%
%   This file is part of SLAMTB, a SLAM toolbox for Matlab.
%
%   SLAMTB 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.
%
%   SLAMTB 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 SLAMTB.  If not, see <http://www.gnu.org/licenses/>.
%
%---------------------------------------------------------------------

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


function m = computem (L, w, W, K, Phi)

  % Get the number of documents.
  D = length(L);

  % Initialize the counts "m".
  m = zeros(W,K);

  % Repeat for every document, then for every word in the document.
  is = 0;
  for d = 1:D
    is = is(end) + (1:L(d));
    for i = is
      j      = w(i);
      m(j,:) = m(j,:) + Phi(:,i)';
    end
  end


function [hr] = yr2hr(yr)
% Convert time from Julian years (365.25 days) to hours.
% Chad Greene 2012
hr = yr*8766;


function vw = replaceROIQuadrilateralCoords(vw,corners)
%
% vw = replaceROIQuadrilateralCoords(vw,corners)
%
% Author:  BW,AAB
% Purpose:
%   Replace the coordinates in the currently selected roi with a
%   quadrilateral region defined by the corners.

curSlice =viewGet(vw, 'Current Slice');

% is this a flat view with potential rotations/flips?
% if so, we need to rotate the corners to reflect how they would be
% displayed given the current settings
if strcmp(vw.viewType, 'Flat')
	% we need to do some reorienting to get the corners into the standard
	% view coordinate system, then rotate them:
	cornerCoords = [fliplr(corners) repmat(curSlice, [size(corners, 1) 1])]';
	cornerCoords = (rotateCoords(vw, cornerCoords, 0));
	corners = cornerCoords([2 1],:)';
end

% Create a binary image with 1s within this quadrilateral
polyIm = roipoly(vw.ui.image,corners(:,1),corners(:,2));

% % markPoly returns an image with 1's marking the polygon
% dims=size(vw.ui.image);
% polyIm = markPoly(dims);

% Compute image coordinates
polyImIndices = find(polyIm);
polyImCoords = indices2Coords(polyImIndices,size(polyIm));

% Add curSlice as 3rd row to get volume coordinates
polyCoords = [polyImCoords; curSlice*ones(1,size(polyImCoords,2))];

% Do an (inverse) rotation if necessary
if (strcmp(vw.viewType,'Flat'))
    polyCoords = (rotateCoords(vw,polyCoords,1));
end

% Convert coords to canonical frame of reference
polyCoords = curOri2CanOri(vw,polyCoords);

vw.ROIs(vw.selectedROI).coords = polyCoords;

return;


function s=shuffle(x,c)
%Syntax: s=shuffle(x,c)
%______________________
%
% Makes c shuffled surrogates of a time series x.
%
% s is the shuffled time series.
% x is the original time series.
% c is the number of surrogates.
%
%
% References:
%
% Theiler J, Galdrikian B, Longtin A, Eubank S, Farmer D J (1992): Using
% Surrogate Data to Detect Nonlinearity in Time Series. In Nonlinear Modeling
% and Forecasting, eds. Casdagli M & Eubank S. 163-188. Addison-Wesley
%
% Theiler J, Eubank S,Galdrikian B, Longtin A,  Farmer D J (1992): Testing
% for nonlinearity in time series: the method of surrogate data. Physica D
% 58: 77-94
%
%
% Alexandros Leontitsis
% Department of Education
% University of Ioannina
% 45110 - Dourouti
% Ioannina
% Greece
%
% University e-mail: me00743@cc.uoi.gr
% Lifetime e-mail: leoaleq@yahoo.com
% Homepage: http://www.geocities.com/CapeCanaveral/Lab/1421
%
% 12 Apr 2001.

if nargin<1 | isempty(x)==1
   error('You should provide a time series.');
else
   % x must be a vector
   if min(size(x))>1
      error('Invalid time series.');
   end
   x=x(:);
   % N is the time series length
   N=length(x);
end

if nargin<2 | isempty(c)==1
   c=1;
else
   % c must be scalar
   if sum(size(c))>2
      error('c must be scalar.');
   end
   % c must be greater or equal than 1
   if c<1
      error('c must be greater or equal than 1.');
   end
end

for i=1:c

    % Make a random vector with indices up to n
    s1=randperm(N);

    % Shuffle the original x with respect to s1
    s(:,i)=x(s1);
end


%% brute_force_tune
% Code to test the performance of various tuning parameters
% Works sorta like RANSAC I guess?
% Adam Werries 2016, see Apache 2.0 license.

k_max = 50;
% Specify ranges
accel_bias_PSD = logspace(-10,-4,100);
gyro_bias_PSD = logspace(-10,-4,100);
% Repeat arrays
accel_bias_PSD = repmat(accel_bias_PSD, [1 k_max]);
gyro_bias_PSD = repmat(gyro_bias_PSD, [1 k_max]);
% Generate random selections of each vector
num_items = length(accel_bias_PSD);
accel_bias_i = randperm(num_items);
gyro_bias_i = randperm(num_items);

rms_error_filter = Inf*ones(1,num_items);
max_error_filter = Inf*ones(1,num_items);
parfor i = 1:num_items
    fprintf('Iteration: %d, ABias: %08.5e, GBias: %08.5e\n', i, accel_bias_PSD(accel_bias_i(i)), gyro_bias_PSD(gyro_bias_i(i)));
    temp_conf = LC_KF_config;
    temp_conf.accel_bias_PSD = accel_bias_PSD(accel_bias_i(i));
    temp_conf.gyro_bias_PSD = gyro_bias_PSD(gyro_bias_i(i));
    [out_profile,out_IMU_bias_est,out_KF_SD] = Loosely_coupled_INS_GNSS(init_cond, filter_time, epoch, lla, gps, imu, temp_conf, est_IMU_bias);
    xyz = out_profile(:,2:4);
    if ~any(any(isnan(xyz))) && ~any(any(isinf(xyz)))
        llh = ecef2lla(xyz);
        [x,y] = deg2utm(llh(:,1),llh(:,2));
        x = x-min_x;
        y = y-min_y;
%         h = -llh(:,3);
%         distance = ((ground_truth_full(:,1)-x).^2 + (ground_truth_full(:,2)-y).^2 + (ground_truth_full(:,3)-h).^2).^0.5;
        distance = ((ground_truth_full(:,1)-x).^2 + (ground_truth_full(:,2)-y).^2).^0.5;
        rms_error_filter(i) = rms(distance);
        max_error_filter(i) = max(distance);
    end
end

[minmax, i] = min(max_error_filter);
fprintf('\nBest max: %08.4f, rms is %08.4f\n', minmax, rms_error_filter(i));
fprintf('Best iteration for max: %d, ABias: %08.5e, GBias: %08.5e\n', i, accel_bias_PSD(accel_bias_i(i)), gyro_bias_PSD(gyro_bias_i(i)));
[minrms, i] = min(rms_error_filter);
fprintf('Best rms: %08.4f, max is %08.4f\n', minrms, max_error_filter(i));
fprintf('Best iteration for rms: %d, ABias: %08.5e, GBias: %08.5e\n', i, accel_bias_PSD(accel_bias_i(i)), gyro_bias_PSD(gyro_bias_i(i)));
[minrms, i] = min((rms_error_filter+max_error_filter)/2);
fprintf('Best average of RMS and max: %08.4f, rms is  %08.4f, max is %08.4f\n', minrms, rms_error_filter(i), max_error_filter(i));
fprintf('Best iteration for rms: %d, ABias: %08.5e, GBias: %08.5e\n', i, accel_bias_PSD(accel_bias_i(i)), gyro_bias_PSD(gyro_bias_i(i)));


% efficiencyOFexpDesigns2 : OVERLAPPING EVENTS
% ER-fMRI data analysis
% script:
% throughout we assume TR=2.

loadDataYes=1; % =1 -> loads existing data file

pluginCorrYes=0;
daleYes=1; % 1-usual Dale efficiency. 0-Fisher

% event matrices are defined based on vectors with the average
% energy removed

cutEndPad=0; %cutting reduces efficiency!
nReps=10000; %n-2;
n2=24; % assumed # TRs to cover HDR fn
nVals=1;
pwRange=6:10; %7

if ~loadDataYes,

rndEffAll=[];
mEffAll=[];

tic
for pwr=pwRange,
% creating event vectors
ms=m2bin([mseq(2,pwr,0,1)])'; % fixing to the length of 2^n
n=length(ms)  % scan duration in TRs
if pluginCorrYes
  b=[0.406;0.8825]; % parameters from SPG fMRI data
  fittedACorr=autocorrFnct(b,1:n+(n2-1)*rem(cutEndPad+1,2));
  Cninv=inv(toeplitz(fittedACorr));
else
  Cninv=eye(n+(n2-1)*rem(cutEndPad+1,2));
end;

mEff=zeros(1,n-1);

shift=2^(pwr-1);
%shift1=2^(pwr-2)*3;
%XXXXXXXXXXXXXXXXXXXXX
for cycle=1:(2^pwRange(1))-2,
  mEvent=ms; %row 1
  mEvent=[mEvent; [ms(shift+1:end) ms(1:shift)]];%row 3
  mEvent=[mEvent; [ms(cycle+1:end) ms(1:cycle)]];%row 2

  % defining event matrix as convolution matrix
  %eventMatrix=makeEventMtrx(mEvent-(ones(length(mEvent),1)*...
  %sum(mEvent'))'/length(mEvent),n2);

  eventMatrix=makeEventMtrx(mEvent,n2);

  if cutEndPad, eventMatrix=eventMatrix(1:n,:); end;
  eventMatrix=eventMatrix-ones(size(eventMatrix,1),1)* ...
    sum(eventMatrix)/size(eventMatrix,1);

  if daleYes,
    designEff=1/trace(inv(eventMatrix'*Cninv*eventMatrix));
  else
    designEff=trace(eventMatrix'*Cninv*eventMatrix);
  end;

  mEff(cycle)=designEff;
end;

%mProbab=sum(mEvent'); mProbab=mean(mProbab)/n;
plot(mEff,'k','LineWidth',2);
set(gca,'FontSize',12);
xlabel('Cyclical shift of event vector #3','FontSize',16)
ylabel('Efficiency','FontSize',16)
%axis([0 n 0 1])
set(gca,'Position',[.2,.15,.7,.7]);
set(gcf,'PaperPosition',[1,1,5,4]);
%print -dpsc2 cyclingOverlapping3ev
drawnow;
pause

foo=max(mEff);
mEffAll=[mEffAll, foo(1)];

% for simply randomized designs:
nVals=size(mEvent,1);
nOnes=0.5;
overlapYes=1;

rndEff=[];
for k=1:nReps;
   %nOnes=k/(n-1);

	rndEvent=balancedRnd(n,nVals,nOnes,overlapYes);
	%if k==1, sum(rndEvent'), end;
        eventMatrix=makeEventMtrx(rndEvent-(ones(length(rndEvent),1)*sum(rndEvent'))'/length(rndEvent),n2);
   if cutEndPad, eventMatrix=eventMatrix(1:n,:); end;
        if daleYes,
	  designEff=1/trace(inv(eventMatrix'*Cninv*eventMatrix));
	else
	  designEff=trace(eventMatrix'*Cninv*eventMatrix);
	end;

	rndEff=[rndEff, designEff];
end;
rndEffAll=[rndEffAll;rndEff];
end;
toc;

else
  eval(['load dataEff',num2str(nVals),'ev',num2str(nReps),'reps',num2str(n2),'nh']);
end %fi loadDataYes

nn=2.^[pwRange]-1;

figure(1); clf;
%[b,a]=hist(rndEff,20);
%bar(a,b/nReps); hold on;
%plot(max(mEff),0,'k*');

loglog(nn,mEffAll,'r*');hold on;

% theoretical max:
%theoMax=(nn+1)/n2/4;
%loglog(nn,theoMax,'g+');hold on;
p99=max(rndEffAll'); %prctile(rndEffAll',99.9);
p00=min(rndEffAll'); %prctile(rndEffAll',0.1);
med=median(rndEffAll');
loglog(nn,med,'k.-','LineWidth',2);
loglog([nn;nn],[p00;p99],'k')
loglog([nn*.93;nn*1.1],[p00;p00],'k')
loglog([nn*.93;nn*1.1],[p99;p99],'k')

axis([50 10^3*1.2 10^-.5 10^1*3]);
%set(gca,'YTick',[0.003 .01 .1 .3 1 3 10 30]);
set(gca,'XTick',[2^6 2^7 2^8 2^9 2^10]);
set(gca,'LineWidth',2,'FontSize',12);

xlabel('Sequence length','FontSize',12);
ylabel('Efficiency','FontSize',12);
title(['With Overlap: n_e=',num2str(nVals),' p=',num2str(nOnes),' n_h=',num2str(n2)]);

set(gca,'Position',[.2,.15,.7,.7]);
set(gcf,'PaperPosition',[1,1,4,3]);
eval(['print -dpsc2 eff',num2str(nVals),'ev',num2str(nReps),'reps',num2str(n2),'nh_over']);
disp(['print -dpsc2 eff',num2str(nVals),'ev',num2str(nReps),'reps',num2str(n2),'nh_over']);

figure(2); clf;
semilogx(nn,mEffAll./max(rndEffAll'),'-ok','LineWidth',2); hold on;
semilogx(nn,mEffAll./med,':+k','LineWidth',2)
set(gca,'Position',[.2,.15,.7,.7]);
xlabel('Sequence length','FontSize',12);
ylabel('m-seq/random efficiency ratio','FontSize',12);
title(['With overlap: n_e=',num2str(nVals),' p=',num2str(nOnes),' n_h=',num2str(n2)]);
set(gca,'Position',[.2,.15,.7,.7]);
set(gcf,'PaperPosition',[1,1,4,3]);
set(gca,'XTick',[2^6 2^7 2^8 2^9 2^10]);
set(gca,'LineWidth',2,'FontSize',12);
axis([50 10^3*1.2 .9 2.5]);
eval(['print -dpsc2 eff',num2str(nVals),'ev',num2str(nReps),'reps',num2str(n2),'nhRATIO']);
disp(['print -dpsc2 eff',num2str(nVals),'ev',num2str(nReps),'reps',num2str(n2),'nhRATIO']);

eval(['save dataEff',num2str(nVals),'ev',num2str(nReps),'reps',num2str(n2),'nh rndEffAll p99 p00 med mEffAll']);
disp(['save dataEff',num2str(nVals),'ev',num2str(nReps),'reps',num2str(n2),'nh rndEffAll p99 p00 med mEffAll']);


function [Gal] = G2Gal(G)
% Convert acceleration from average acceration due to Earth's gravity to
% galileos
% Chad A. Greene 2012
Gal = G*9.80665e+2;
end


function [x,e,obj,err,iter] = tracelassoR(A,b,lambda,opts)

% Solve the trace Lasso regularized minimization problem by M-ADMM
%
% min_{x,e} loss(e)+lambda*||A*Diag(x)||_*, s.t. Ax+e=b
% loss(e) = ||e||_1 or 0.5*||e||_2^2
% ---------------------------------------------
% Input:
%       A       -    d*n matrix
%       b       -    d*1 vector
%       opts    -    Structure value in Matlab. The fields are
%           opts.loss       -   'l1' (default): loss(e) = ||e||_1
%                               'l2': loss(e) = 0.5*||e||_2^2
%           opts.tol        -   termination tolerance
%           opts.max_iter   -   maximum number of iterations
%           opts.mu         -   stepsize for dual variable updating in ADMM
%           opts.max_mu     -   maximum stepsize
%           opts.rho        -   rho>=1, ratio used to increase mu
%           opts.DEBUG      -   0 or 1
%
% Output:
%       x       -    n*1 vector
%       e       -    d*1 vector
%       obj     -    objective function value
%       err     -    residual
%       iter    -    number of iterations
%
% version 1.0 - 18/06/2016
%
% Written by Canyi Lu (canyilu@gmail.com)
%

tol = 1e-8;
max_iter = 500;
rho = 1.1;
mu = 1e-4;
max_mu = 1e10;
DEBUG = 0;
loss = 'l1';

if ~exist('opts', 'var')
    opts = [];
end
if isfield(opts, 'loss');        loss = opts.loss;            end
if isfield(opts, 'tol');         tol = opts.tol;              end
if isfield(opts, 'max_iter');    max_iter = opts.max_iter;    end
if isfield(opts, 'rho');         rho = opts.rho;              end
if isfield(opts, 'mu');          mu = opts.mu;                end
if isfield(opts, 'max_mu');      max_mu = opts.max_mu;        end
if isfield(opts, 'DEBUG');       DEBUG = opts.DEBUG;          end

[d,n] = size(A);
x = zeros(n,1);
Z = zeros(d,n);
e = zeros(d,1);
Y1 = e;
Y2 = Z;

Atb = A'*b;
AtA = A'*A;
invAtA = (AtA+diag(diag(AtA)))\eye(n);

iter = 0;
for iter = 1 : max_iter
    xk = x;
    ek = e;
    Zk = Z;
    % first super block {Z,e}
    [Z,nuclearnorm] = prox_nuclear(A*diag(x)-Y2/mu,lambda/mu);
    if strcmp(loss,'l1')
        e = prox_l1(b-A*x-Y1/mu,1/mu);
    elseif strcmp(loss,'l2')
        e = mu*(b-A*x-Y1/mu)/(1+mu);
    else
        error('not supported loss function');
    end
    % second super block {x}
    x = invAtA*(-A'*(Y1/mu+e)+Atb+diagAtB(A,Y2/mu+Z));
    dY1 = A*x+e-b;
    dY2 = Z-A*diag(x);
    chgx = max(abs(xk-x));
    chge = max(abs(ek-e));
    chgZ = max(max(abs(Zk-Z)));
    chg = max([chgx chge chgZ max(abs(dY1(:))) max(abs(dY2(:)))]);
    if DEBUG
        if iter == 1 || mod(iter, 10) == 0
            obj = comp_loss(e,loss)+lambda*nuclearnorm;
            err = sqrt(norm(dY1,'fro')^2+norm(dY2,'fro')^2);
            disp(['iter ' num2str(iter) ', mu=' num2str(mu) ...
                    ', obj=' num2str(obj) ', err=' num2str(err)]);
        end
    end

    if chg < tol
        break;
    end
    Y1 = Y1 + mu*dY1;
    Y2 = Y2 + mu*dY2;
    mu = min(rho*mu,max_mu);
end
obj = comp_loss(e,loss)+lambda*nuclearnorm;
err = sqrt(norm(dY1,'fro')^2+norm(dY2,'fro')^2);

function v = diagAtB(A,B)
% A, B - d*n matrices
% v = diag(A'*B), n*1 vector

n = size(A,2);
v = zeros(n,1);
for i = 1 : n
   v(i) = A(:,i)'*B(:,i);
end


function [ know, x ] = p37_sol ( n )

%*****************************************************************************80
%
%% P37_SOL returns the solution for problem 37.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 October 2011
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the problem.  This value
%    is only needed for those problems with variable N.
%
%    Output, integer KNOW.
%    If KNOW is 0, then the solution is not known.
%    If KNOW is positive, then the solution is known, and is returned in X.
%
%    Output, real X(N), the solution, if known.
%
  know = 1;

  x = pi * ones ( n, 1 );

  return
end


clear all
close all

disp('Synthetic data from nonlinear ODE model');
disp('defined in Ramsay et al. (2007)');
disp('based on Van der Pol oscillator and which');
disp('reduces to Fitzhugh-Nagumo for certain parameters');

sigma_e=0.01;
[M,U] = mci_ramsay_struct(sigma_e);

% Use higher integration tolerances than by default
M.reltol=1e-3;
M.abstol=1e-5;

rand_init=0;
if rand_init
    P=spm_normrnd(M.vpE,M.pC,1);
else
    P=[log(0.2) log(0.2)]';
end

tic;
Y = mci_ramsay_gen(P,M,U);
toc

mci_plot_outputs(M,Y);

%mcmc.inference='vl';
%mcmc.inference='langevin';
%mcmc.maxits=16;
%mcmc.verbose=1;

mcmc.inference='ais';
mcmc.anneal='geometric';
mcmc.prop='lmc';
mcmc.nprop=1;
mcmc.J=16;
mcmc.maxits=8;
mcmc.rec_traj=1;

post = spm_mci_post (mcmc,M,U,Y,P);

if ~strcmp(mcmc.inference,'vl')
    spm_mci_diag(post);

    stats = spm_mci_mvnpost (post,'ESS')
    stats = spm_mci_mvnpost (post,'thinning')

    for j=1:length(P),
        spm_mci_quantiles (post,j,0);
    end
end

load ramsay-surface
figure
surf(S.x,S.y,L);
xlabel(S.name{1});
ylabel(S.name{2});

figure;
imagesc(S.pxy(1,:),S.pxy(2,:),L);
axis xy
hold on
xlabel(S.name{1});
ylabel(S.name{2});
hold on
ms=10;
plot(M.pE(1),M.pE(2),'wo','MarkerSize',ms);
plot(P(1),P(2),'w+','MarkerSize',ms);
if ~strcmp(mcmc.inference,'vl')
    j=post.ind;
    plot(post.P(1,j),post.P(2,j),'w.');
end
plot(post.Ep(1),post.Ep(2),'kx','MarkerSize',ms);

if strcmp(mcmc.inference,'ais')
    % plot individual trajectories
    figure
    xlim([-2 0]);
    ylim([-2 0]);
    hold on
    for i=1:mcmc.maxits,
        xx=squeeze(post.traj(i,1,:));
        yy=squeeze(post.traj(i,2,:));
        plot(xx,yy,'k-');
        plot(xx(end),yy(end),'kx','MarkerSize',ms);
        xlabel(S.name{1});
        ylabel(S.name{2});
        disp('Press space to see more trajectories');
        pause

    end
end


function varargout = chol( f, varargin )
%CHOL    Cholesky factorization of a SEPARABLEAPPROX.
%
% R = CHOL( F ), if F is a nonnegative definite SEPARABLEAPPROX then this
% returns an upper triangular quasimatrix so that R'*R is a
% decomposition of F. If F is not nonnegative definite then an error is thrown.
%
% L = CHOL(F, 'lower'), if F is a nonnegative definite SEPARABLEAPPROX then this
% produces a lower triangular quasimatrix so that L*L' is a decomposition of F.
% If F is not nonnegative definite then an error is thrown.
%
% [R, p] = CHOL( F ), with two outputs never throwns an error message. If F is
% nonnegative definite then p is 0 and R is the same as above. If F is
% symmetric but negative definite or semidefinite then p is a positive
% integer such that R has p columns and R'*R is a rank p nonnegative definite
% SEPARABLEAPPROX that approximates F.
% This is particular useful when F is nonnegative definite, but rounding errors
% have perturbed it to be semidefinite.
%
% [L, p] = CHOL(F, 'lower') same as above but the first argument is lower
% triangular.
%
% For more information about the factorization:
% A. Townsend and L. N. Trefethen, Continuous analogues of matrix
% factorizations, Proc. Royal Soc. A., 2015.
%
% See also LU, and QR.

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

% Empty check:
if ( isempty( f ) )
    varargout = cell(1, nargout);
    return
end

% Is the separableApprox on a square domain?:
if ( ~domainCheck(f.cols, f.rows) )
    error('CHEBFUN:SEPARABLEAPPROX:chol:domain', ...
        'SEPARABLEAPPROX is not on a square domain.');
end

% Get rank of f:
k = length( f );

% All the pivots should be on the y = x line.
PivLoc = f.pivotLocations;

if ( isempty( PivLoc ) )
    % For some reason (probably because f was made with sampling data, rather
    % than a handle) f didn't have any pivot information. Make it now:
    f = compose(f, @plus, 0 );
    varargout = {chol( f )};
    return
end

% Find the first pivot location is off-diagonal:
Diagk = find( PivLoc(:,1) ~= PivLoc(:,2), 1, 'first');
k = min( k, Diagk );

% Check that the pivots are positive:
Posk = find( pivots( f ) < 0, 1, 'first');
k = min(k, Posk);

% Were all the pivots nonnegative and the locations on the diagonal?
posdef = ( k == length( f ) );

% Return an error if the function is not nonnegative definite:
if ( nargout < 2 && ~isempty( posdef ) && ~posdef )
    error('CHEBFUN:SEPARABLEAPPROX:chol:definite', ...
        'SEPARABLEAPPROX is not nonnegative definite.');
end

% Get the CDR decomposition (already computed by constructor):
[C, D, R] = cdr( f );

% Return an error if the function is not symmetric:
dom = f.domain;
r = 0.0192475;   % arbitrary point in [-1,1]
s = -.34756987;  % arbitrary point in [-1,1]
r = diff(dom(1:2))/2*r + mean(dom(1:2));
s = diff(dom(3:4))/2*s + mean(dom(3:4));
symTest = abs(feval(f,r,s) - feval(f,s,r)) < 1e2*eps;
if ( ~symTest )
    error('CHEBFUN:SEPARABLEAPPROX:chol:symmetric', ...
        'The SEPARABLEAPPROX must be a symmetric function.');
end

% How many terms are posdef:
p = k;
if ( isempty( p ) )
    p = length( f );
end
% Extract out posdef part:
C = C( :, 1:p );
D = D( 1:p, 1:p );
R = R( :, 1:p );

% Balance scaling:
C = C * sqrt( D );
R = R * sqrt( D );

% Output to user:
if ( ( nargout < 2 ) && ( nargin == 1 ) )
    varargout = { R.' };
elseif ( ( nargout < 2 ) && strcmpi( varargin{ 1 }, 'lower') )
    varargout = { C };
elseif ( ( nargout == 2 ) && ( nargin == 1 ) )
    varargout = { R.', p };
elseif ( ( nargout == 2 ) && strcmpi( varargin{ 1 }, 'lower') )
    varargout = { C, p };
end

end


% NMF-ALS: NMF solved by Alternating Least Squares
% process_video('NMF', 'NMF-ALS', 'dataset/demo.avi', 'output/demo_NMF-ALS.avi');
alg_path_aux = fullfile(lrs_conf.nmf_path,'NMF-DTU-Toolbox');
addpath(genpath(alg_path_aux));

M = sparse(M);

% als: Alternating least squares.
[W, H] = nmf(M,1,'als');

L = W * H;
S = M - L;

rmpath(genpath(alg_path_aux));


function showSpaceImages(img, x, y, scaling, limits)
%
% Displays images organized in a 2D space
%
% input:
%   img: array [nrows ncols 3 nimages]
%   x,y  = coordinate for each image

if nargin<5
    x1 = min(x(:));
    y1 = min(y(:));
    x2 = max(x(:));
    y2 = max(y(:));
else
    x1 = limits(1);
    x2 = limits(2);
    y1 = limits(3);
    y2 = limits(4);
end

%xa = x - min(x(:));
%ya = y - min(y(:));

if nargin<4
    scaling = 2;
end

%S = max(xa);
%xa = (0.975-0.05*scaling)*xa/S+0.025;
%S = max(ya);
%ya = (0.975-0.05*scaling)*ya/S+0.025;

xa = (0.975-0.05*scaling)*(x - x1)/(x2-x1) + 0.05*scaling;
ya = (0.975-0.05*scaling)*(y - y1)/(y2-y1) + 0.05*scaling;

figure
for n = length(xa):-1:1
    n
    h=axes('position', [xa(n) ya(n) .05*scaling .05*scaling]);
    image(uint8(img(:,:,:,n)), 'parent', h)
    axis('off'); axis('equal')
end


%
% Extract variable names from any number of polynomial strings
%
% Syntax:  (pvar is the shortened alias for GetVariables)
%          >> z = GetVariables(f)
%          >> z = GetVariables(f,g,h)
%
%  to extract variables from a cell array f of polynomial strings:
%          >> z = GetVariables(f{:})
%
%  Input:   f --- (string) polynomial
% Output:   z --- (cell)   variable names of f
%
% Example:  >> f = '-2*x + 9*x*y^2 - 9*x^2*y^2 + 8*x^3*y^2';
%           >> g = '5 - x^3*z';
%           >> z = GetVariables(f,g)
%           z =
%               'x'    'y'    'z'


classdef prtBrvDiscreteStickBreaking < prtBrv & prtBrvVbOnline

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Properties required by prtAction
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    properties (SetAccess = private)
        name = 'Discrete Stick Breaking Bayesian Random Variable';
        nameAbbreviation = 'BRVSB';
    end

    properties (SetAccess = protected)
        isSupervised = false;
        isCrossValidateValid = true;
    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Methods for prtBrv
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    methods

        function self = estimateParameters(self, x)
            self = conjugateUpdate(self, self, x);
        end

        function y = predictivePdf(self, x)
            y = exp(predictiveLogPdf(self, x));
        end

        function y = predictiveLogPdf(self, x)
            %%%% FIXME
            % The true predictive here is a product of beta-binomials
            % Since that isn't implemented yet we use the average
            % variational loglikelihood

            y = conjugateVariationalAverageLogLikelihood(self, x);
        end

        function val = getNumDimensions(self)
            val = size(self.model.beta,1);
        end

        function self = initialize(self, x)
            x = self.parseInputData(x);
            if ~self.model.isValid
                self.model = self.model.defaultParameters(size(x,2));
            end
        end

        % Optional methods
        %------------------------------------------------------------------
        function kld = conjugateKld(obj, priorObj)
            kld = obj.model.kld(priorObj.model);
        end

        function s = posteriorMeanStruct(obj)
            s.probabilities = exp(obj.model.expectedValueLogProbabilities);
        end

        function val = plotLimits(self)
            val = [0 length(self(1).model.lambda)+1 0 length(self(1).model.lambda)+1];
        end
    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Methods for prtBrvVb
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    methods
        function [self, training] = vbBatch(self,x)
            % Since we are purely conjugate we actually don't need vbBatch
            % However we must implement it.
            self = conjugateUpdate(self,x);
            training = struct([]);
        end
    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Methods for prtBrvVbOnline
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    methods
        function self = vbOnlineInitialize(self, x) %#ok<INUSD>
            randDraw = rand(1,self.nDimensions);
            randDraw = randDraw./sum(randDraw);

            self = self.conjugateUpdate(self, randDraw);
        end

        function [self, training] = vbOnlineUpdate(self, priorSelf, x, training, prevSelf, learningRate, D) %#ok<INUSL>
            x = self.parseInputData(x);

            [self.model, training] = self.model.vbOnlineWeightedUpdate(priorObj.model, sum(x,1), [], lambda, D, prevObj.model);
        end
    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Methods for prtBrvMembershipModel
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    methods
        % Don't actually inherit from prtBrvMembershipModel but these two
        % methods are abstracted there
        function self = conjugateUpdate(self, prior, x)
            x = parseInputData(self,x);
            self.model = self.model.conjugateUpdate(prior.model,x);
        end

        function obj = weightedConjugateUpdate(obj, priorObj, x, weights)
            x = parseInputData(self,x);
            obj.model = obj.model.conjugateUpdate(priorObj.model,bsxfun(@times,x,weights));
        end
    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Methods for prtBrvVbOnlineMembershipModel
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    methods
        function [obj, training] = vbOnlineWeightedUpdate(obj, priorObj, x, weights, lambda, D, prevObj)
            x = obj.parseInputData(x);
            if ~isempty(weights)
                x = bsxfun(@times,x,weights);
            end

            [obj.model, training] = obj.model.vbOnlineWeightedUpdate(priorObj.model, sum(x,1), [], lambda, D, prevObj.model);
        end
    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Properties for prtBrvDiscreteStickBreaking use
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    properties
        model = prtBrvDiscreteStickBreakingHierarchy;
    end
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    % Methods for prtBrvDiscreteStickBreaking use
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    methods
        function obj = prtBrvDiscreteStickBreaking(varargin)
            if nargin < 1
                return
            end
            obj = obj.constructorInputParse(varargin{:});
        end

        function val = expectedLogMean(obj)
            val = obj.model.expectedValueLogProbabilities(:)';
        end

        function model = modelDraw(obj)
            model.probabilities = draw(obj.model);
        end
    end

    methods (Hidden)
        function x = parseInputData(self,x) %#ok<MANU>
            if isnumeric(x)
                return
            elseif prtUtilIsSubClass(class(x),'prtDataSetBase')
                x = x.getObservations();
            else
                error('prt:prtBrvDiscreteStickBreaking:parseInputData','prtBrvDiscreteStickBreaking requires a prtDataSet or a numeric 2-D matrix');
            end
        end
    end
end


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

function [y,m] = getPathsAsian(S0, r, sigma, dt, NSim, Nr)
% path retrieval for Asian options

if length(S0) > 1
    lenS = length(S0);             % for using this with different starting values
    R = exp((r - sigma^2/2) * dt ...
    + sigma * sqrt(dt) * randn(NSim,Nr,lenS));   % random noise + drift
    tmp = zeros(NSim,1,lenS);
    tmp(:,1,:) = repmat(log(S0)',NSim,1);
    y = exp(cumsum([tmp, R], 2));    % path set simple example
else
    R = exp((r - sigma^2/2) * dt ...
    + sigma * sqrt(dt) * randn(NSim,Nr));
    y = cumprod([S0*ones(NSim,1), R], 2); %S0 in Spalte 1 -> Indexverschiebung

end

tmp = repmat(1:Nr, NSim, 1); %Hilfsmatrix
m = [y(:,1) cumsum(y(:,2:end), 2) ./ tmp];

end


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

%------------------------------- Reference --------------------------------
% G. Wu, R. Mallipeddi, and P. N. Suganthan, Problem definitions and
% evaluation criteria for the CEC 2017 competition on constrained real-
% parameter optimization, National University of Defense Technology, China,
% 2016.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

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


%% housekeeping
clear
close all
clc
%% bring in the data
[num,txt] = xlsread('Smets_Wouters_data.xlsx');
vnames=strrep(txt,' ','');
date_start='1947q3';
data=ts(date_start,num(:,2:end),vnames);
data=pages2struct(data);
figure('name','observed data')
for ii=1:numel(vnames)
    subplot(3,3,ii)
    v=vnames{ii};
    plot(data.(v),'linewidth',2)
    title(v)
end
xrotate(45)

%% Set the RFVAR
endo_aliases=struct('robs','interest rates',...
    'dy','GDP growth',...
    'labobs','hours worked',...
    'pinfobs','inflation',...
    'dw','wages growth',...
    'dc','Consumption growth',...
    'dinve','Investment growth');

endog=fieldnames(endo_aliases);

nlags=3;

const=true;

exog={};

rv=rfvar(endog,exog,nlags,const); % formerly redfvar

%% Estimate the reduced-form VAR

rv1=estimate(rv,data);%,{db.LGDP.start,db.LGDP.finish}

%% identification setup

shock_aliases=struct('mp','monetary policy',...
    'ad','aggreg. demand',...
    'as','aggreg. supply',...
    'ls','labor supply');

structural_shocks=fieldnames(shock_aliases);

ident_restr={
    %     'dy{inf}@mp',0
    %     'dy{inf}@ad',0
    %     'dy{inf}@as',0
    %     'dy{inf}@ls',0
    'robs{0}@mp','+'
    'dy@mp','-'
    'pinfobs@mp','-'
    'robs{0}@ad','+'
    'pinfobs@ad','+'
    'dy@ad','+'
    %     'robs@as','-'
    'dy@as','+'
    'pinfobs@as','-'
    'robs@ls','-'
    'dy@ls','+'
    'labobs@ls','+'
    %     'pinfobs@ls','-'
    'dw@ls','-'
    };

agnostic=true;

max_trials=6000;

[Rfunc,ident]=identification(rv1,ident_restr,structural_shocks,...
        agnostic,max_trials);

%% Impulse responses

myirfs=irf(rv1,structural_shocks,40,[],Rfunc);

%% Plot the IRFs for one particular rotation

for ishock=1:numel(structural_shocks)

    shock=structural_shocks{ishock};

    figure('name',['Orthogonalized responses to a ',...
        shock_aliases.(shock),' shock']);

    for ivar=1:numel(endog)

        vname=endog{ivar};

        subplot(3,3,ivar)

        plot(myirfs.(shock).(vname),'linewidth',2)

        title(endo_aliases.(vname))

    end

end


function estimation_results = notREKF_SLAM(data, NumberOfSteps)
% R-EKF SLAM

% load pre-given data: odometry and observations
if nargin < 1
    load('./data.mat');
end

data_matrix = data.state;

odom_sigma = data.odom_sigma;
obsv_sigma = data.obsv_sigma;

% odoCov = data.odom_cov;   % constant variable
% obsCov = data.obse_cov;   % constant variable

%%%%%%%%%%%%%%%%%%%% Estimation_X is used to save the state in each step %%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%% In every step, all elements of Estimation_X will be changed %%%%%%%%%%%%
Estimation_X.orientation = data.poses.orientation(1:3,1:3);
Estimation_X.position    = data.poses.position(:,1);
Estimation_X.cov         = sparse(6,6);
Estimation_X.landmarks   = [];       % the landmarks observed until this step (included), 4*N format, the 4-th row is the index
Estimation_X0.IndexObservedNew=[];
Estimation_X0.IndexObservedAlreadyThis=[];
%%%%%%%%%%%%%%%%%%%% Estimation_X is used to save the state in each step %%%%%%%%%%%%%%%%%%%%

% Initialize
if nargin < 2
    NumberOfSteps = max(data_matrix(:,4));  % step instead of pose,  hence, it does not include pose 0
elseif NumberOfSteps > max(data_matrix(:,4))
    NumberOfSteps = max(data_matrix(:,4));
end
estimation_results = cell(1, NumberOfSteps+1);
estimation_results{1} = Estimation_X;
row_idx = (data_matrix(:, end) <= NumberOfSteps+1);
data_matrix = data_matrix(row_idx, :);

for i = 0:NumberOfSteps
    IndexOfCurrentStepInDataMatrix = find(data_matrix(:,4) == i);
    m = size(IndexOfCurrentStepInDataMatrix, 1);
    if ( mod(i, 50) == 0 )
        disp(['Processing pose ', int2str(i)]);
    end
    % det(Estimation_X.cov)
    if i==NumberOfSteps-1
    a=1;
    end

    if i ~= NumberOfSteps
        OdometryFromThis2Next = data_matrix(IndexOfCurrentStepInDataMatrix(m-5):IndexOfCurrentStepInDataMatrix(m),1);
        if m > 6
            CameraMeasurementThis = [ data_matrix( IndexOfCurrentStepInDataMatrix(1): IndexOfCurrentStepInDataMatrix(m-6) , 1 ),...
                                      data_matrix( IndexOfCurrentStepInDataMatrix(1): IndexOfCurrentStepInDataMatrix(m-6) , 3 )];
           [Estimation_X] = notREKF_update(Estimation_X, CameraMeasurementThis, obsv_sigma );
        end

        estimation_results{i+1} = Estimation_X;

%        propagation using odometry info
        [Estimation_X] = notREKF_propagate(Estimation_X, OdometryFromThis2Next, odom_sigma );

    else
        a=2;
        if m > 6
            CameraMeasurementThis = [ data_matrix( IndexOfCurrentStepInDataMatrix(1): IndexOfCurrentStepInDataMatrix(end) , 1 ) , ...
                                      data_matrix( IndexOfCurrentStepInDataMatrix(1): IndexOfCurrentStepInDataMatrix(end) , 3 )];
            [Estimation_X] = notREKF_update(Estimation_X, CameraMeasurementThis, obsv_sigma );
        end
        estimation_results{i+1} = Estimation_X;
    end
end
clearvars -except estimation_results


function foff=comp_warpedfoff(fc,bw,fs,L,freqtoscale,scaletofreq,do_symmetric)
%COMP_WARPEDFOFF  foff for warped filters

fcwasnegative = fc < 0;

if fcwasnegative && do_symmetric
   fc = -fc;
   fcscale = freqtoscale(fc);
   foff = -floor(scaletofreq(fcscale+.5*bw)/fs*L)+1;
else
   fcscale = freqtoscale(fc);
   foff = floor(scaletofreq(fcscale-.5*bw)/fs*L)+1;
end


function gT = simwhiteKernDiagGradX(kern, t)

% SIMWHITEKERNDIAGGRADX Gradient of SIM-WHITE kernel's diagonal w.r.t. t.
% FORMAT
% DESC computes the gradient of the diagonal of the SIM-White (Single Input
% Motif - White) kernel matrix with respect to the elements of the input
% column vector given in t.
% ARG kern : the kernel structure for which gradients are being computed.
% ARG t : the input data in the form of a design matrix.
% RETURN gT : the gradients of the diagonal with respect to each element
% of t. The returned matrix has the same dimensions as t.
%
% SEEALSO : simwhiteKernParamInit, kernDiagGradX, simwhitekernGradX
%
% COPYRIGHT : David Luengo, 2009

% KERN

if (kern.isStationary == true)
    gT = zeros(size(t));
else
    gT = kern.variance * (kern.sensitivity^2) * exp(-2*kern.decay*t);
end


function [OffDec,OffVel] = Operator_LMOCSO(Problem,Loser,Winner,Rate)
% The competitive swarm optimizer of LMOCSO

%  Copyright (C) 2021 Xu Yang
%  Xu Yang <xuyang.busyxu@qq.com> or <xuyang369369@gmail.com>

    %% Parameter setting
    LoserDec  = Loser.decs;
    WinnerDec = Winner.decs;
    [N,D]     = size(LoserDec);
	LoserVel  = Loser.adds(zeros(N,D));
    WinnerVel = Winner.adds(zeros(N,D));

    %% Competitive swarm optimizer
    r1     = repmat(rand(N,1),1,D);
    r2     = repmat(rand(N,1),1,D);

    OffVel = r1.*LoserVel + r2.*(WinnerDec-LoserDec);
    OffDec = LoserDec + OffVel;

    if Problem.FE/Problem.maxFE < Rate
        LoserVel1 = rand(N,D);
        OffVel1 = r1.*LoserVel1 + r2.*(WinnerDec-LoserDec);
        OffDec1 = LoserDec + OffVel1 + r1.*(OffVel1-LoserVel1);

        OffDec = [OffDec;OffDec1];
        OffVel = [OffVel;OffVel1];
    end

    %% Add the winners
    OffDec = [OffDec;WinnerDec];
    OffVel = [OffVel;WinnerVel];

    %% Polynomial mutation
    [N,D] = size(OffDec);
    Lower = repmat(Problem.lower,N,1);
    Upper = repmat(Problem.upper,N,1);
    disM  = 20;
    Site  = rand(N,D) < 1/D;
    mu    = rand(N,D);
    temp  = Site & mu<=0.5;
    OffDec       = max(min(OffDec,Upper),Lower);
    OffDec(temp) = OffDec(temp)+(Upper(temp)-Lower(temp)).*((2.*mu(temp)+(1-2.*mu(temp)).*...
                   (1-(OffDec(temp)-Lower(temp))./(Upper(temp)-Lower(temp))).^(disM+1)).^(1/(disM+1))-1);
    temp  = Site & mu>0.5;
    OffDec(temp) = OffDec(temp)+(Upper(temp)-Lower(temp)).*(1-(2.*(1-mu(temp))+2.*(mu(temp)-0.5).*...
                   (1-(Upper(temp)-OffDec(temp))./(Upper(temp)-Lower(temp))).^(disM+1)).^(1/(disM+1)));
end


function bd = mne_block_diag(A,n);
%
%   function bd = mne_block_diag(A,n)
%
%   Make or extract a sparse block diagonal matrix
%
%   If A is not sparse, then returns a sparse block diagonal "bd", diagonalized from the
%   elements in "A".
%   "A" is ma x na, comprising bdn=(na/"n") blocks of submatrices.
%   Each submatrix is ma x "n", and these submatrices are
%   placed down the diagonal of the matrix.
%
%   If A is already sparse, then the operation is reversed, yielding a block
%   row matrix, where each set of n columns corresponds to a block element
%   from the block diagonal.
%
%   Routine uses NO for-loops for speed considerations.

%
% Principal Investigators and Developers:
% ** Richard M. Leahy, PhD, Signal & Image Processing Institute,
%    University of Southern California, Los Angeles, CA
% ** John C. Mosher, PhD, Biophysics Group,
%    Los Alamos National Laboratory, Los Alamos, NM
% ** Sylvain Baillet, PhD, Cognitive Neuroscience & Brain Imaging Laboratory,
%    CNRS, Hopital de la Salpetriere, Paris, France
%
% Copyright (c) 2005 BrainStorm by the University of Southern California
% This software distributed  under the terms of the GNU General Public License
% as published by the Free Software Foundation. Further details on the GPL
% license can be found at http://www.gnu.org/copyleft/gpl.html .
%
% FOR RESEARCH PURPOSES ONLY. THE SOFTWARE IS PROVIDED "AS IS," AND THE
% UNIVERSITY OF SOUTHERN CALIFORNIA AND ITS COLLABORATORS DO NOT MAKE ANY
% WARRANTY, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO WARRANTIES OF
% MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE, NOR DO THEY ASSUME ANY
% LIABILITY OR RESPONSIBILITY FOR THE USE OF THIS SOFTWARE.
%
% Author: John C. Mosher 1993 - 2004
%
%
% Modifications for mne Matlab toolbox
%
%   Matti Hamalainen
%   2006
%   Revision 1.2  2006/04/23 15:29:40  msh
%   Added MGH to the copyright
%
%   Revision 1.1  2006/04/18 20:44:46  msh
%   Added reading of forward solution.
%   Use length instead of size when appropriate
%
%

if(~issparse(A)),        % then make block sparse
    [ma,na] = size(A);
    bdn = na/n;             % number of submatrices

    if(bdn - fix(bdn)),
        error('Width of matrix must be even multiple of n');
    end

    tmp = reshape([1:(ma*bdn)]',ma,bdn);
    i = zeros(ma*n,bdn);
    for iblock = 1:n,
        i((iblock-1)*ma+[1:ma],:) = tmp;
    end

    i = i(:);             % row indices foreach sparse bd

    j = [1:na];
    j = j(ones(ma,1),:);
    j = j(:);             % column indices foreach sparse bd

    bd = sparse(i,j,A(:));

else                 % already is sparse, unblock it

    [mA,na] = size(A);        % matrix always has na columns
    % how many entries in the first column?
    bdn = na/n;            % number of blocks
    ma = mA/bdn;            % rows in first block

    % blocks may themselves contain zero entries.  Build indexing as above
    tmp = reshape([1:(ma*bdn)]',ma,bdn);
    i = zeros(ma*n,bdn);
    for iblock = 1:n,
        i((iblock-1)*ma+[1:ma],:) = tmp;
    end

    i = i(:);             % row indices foreach sparse bd

    j = [0:mA:(mA*(na-1))];
    j = j(ones(ma,1),:);
    j = j(:);

    i = i + j;

    bd = full(A(i));     % column vector
    bd = reshape(bd,ma,na);    % full matrix
end

return


function [P] = prolongation(CV,CF,V,varargin)
  % PROLONGATION Build a linear prolongation operator taking solutions on a
  % coarse mesh (CV,CF) and prolongating (upsampling) them onto vertices of a
  % fine mesh (V). Useful for multigrid and embedded mesh deformations.
  %
  % P = prolongation(CV,CF,V)
  %
  % Inputs:
  %   CV  #CV by dim list of coarse mesh vertex positions
  %   CF  #CF by dim+1 list of coarse mesh element indices into CV
  %   V  #V by dim list of fine mesh vertex positions
  %   Optional:
  %     'Extrapolation'  followed by name of method to use for points in V
  %       lying outside (CV,CF). One of the following:
  %         {'linear'}   finds closest element and uses barycentric coordinates
  %           (with some negative weights). This is linearly precise.
  %         'contstant'  finds closest point and uses its barycentric
  %           coordinates (all non-negtive since it lies on a facet). This is
  %           only constantly precise.
  % Outputs:
  %   P  #V by #CV prolongation matrix so that X = P * CX prolongates a
  %     solution CX on the coarse mesh to a solution X on the fine mesh.
  %

  [I] = in_element_aabb(CV,CF,V);
  I0 = find(I==0);
  extrapolation = 'linear';
  % Map of parameter names to variable names
  params_to_variables = containers.Map( ...
    {'Extrapolation'}, ...
    {'extrapolation'});
  v = 1;
  while v <= numel(varargin)
    param_name = varargin{v};
    if isKey(params_to_variables,param_name)
      assert(v+1<=numel(varargin));
      v = v+1;
      % Trick: use feval on anonymous function to use assignin to this workspace
      feval(@()assignin('caller',params_to_variables(param_name),varargin{v}));
    else
      error('Unsupported parameter: %s',varargin{v});
    end
    v=v+1;
  end

  switch size(CF,2)
  case 4
    [BF,BJ,BK] = boundary_faces(CF);
    BV = V;
    % Extrapolate
    % Snap to boudnary and use snapped points interpolation
    [~,BI,BVI0] = point_mesh_squared_distance(V(I0,:),CV,BF);
    I(I0) = BJ(BI);
    switch extrapolation
    case 'linear'
      % Leave BV
    case 'constant'
      BV(I0,:) = BVI0;
    otherwise
      error('Unknown extrapolation (%s)',extrapolation);
    end
    [IV] = max(I,[],2);
    % recover barycentric coordinates (interpolation & extrapolation)
    B = barycentric_coordinates( ...
      BV,CV(CF(IV,1),:),CV(CF(IV,2),:),CV(CF(IV,3),:),CV(CF(IV,4),:));
    %% Snap to closest vertex
    %B(any(B<0,2),:) = bsxfun(@eq,max(B(any(B<0,2),:),[],2),B(any(B<0,2),:));
  case 3
    % Dirty trick to make V,CV 3D
    BV = V;
    if ~isempty(I0)
      pV = V;
      pCV = CV;
      pBV = BV;
      pV(:,end+1:3) = 0;
      pCV(:,end+1:3) = 0;
      [~,I(I0),pBVI0] = point_mesh_squared_distance(pV(I0,:),pCV,CF);
      switch extrapolation
      case 'linear'
        % Leave BV
      case 'constant'
        BV(I0,:) = pBVI0(:,1:2);
      otherwise
        error('Unknown extrapolation (%s)',extrapolation);
      end
    end
    [IV] = max(I,[],2);
    B = barycentric_coordinates( ...
      BV,CV(CF(IV,1),:),CV(CF(IV,2),:),CV(CF(IV,3),:));
  end

  P = sparse( ...
    repmat(1:size(V,1),size(CF,2),1)', ...
    CF(IV,:), ...
    B, ...
    size(V,1),size(CV,1));
end


function [str] = printDVManeuversMFMSToTextbox(hDvManInfoText,waypoints,dVDepartVectNTW,deltaVVectNTW,eOrbit,orbitsIn,paddLen,form)
%UNTITLED4 Summary of this function goes here
%   Detailed explanation goes here

    hRule = getHRule();
    str = {};

    sumDv = 0;
    for(i=1:length(waypoints)-1) %#ok<*NO4LP>
        if(i==1)
            dvVect = dVDepartVectNTW;
            ta = eOrbit(8);
        else
            dvVect = deltaVVectNTW(:,i-1);
            ta = orbitsIn(i-1,6);
        end

        str{end+1} = ['Burn Information to Depart ', cap1stLetter(waypoints{i}.name)];
        str{end+1} = hRule;
        str{end+1} = [paddStr('Total Delta-V = ',paddLen), num2str(norm(dvVect), form), ' km/s'];
        str{end+1} = [paddStr('Prograde Delta-V = ',paddLen), num2str(1000*dvVect(1), form), ' m/s'];
        str{end+1} = [paddStr('Orbit Normal Delta-V = ',paddLen), num2str(1000*dvVect(2), form), ' m/s'];
        str{end+1} = [paddStr('Radial Delta-V = ',paddLen), num2str(1000*dvVect(3), form), ' m/s'];
        str{end+1} = '---------------------';
        str{end+1} = [paddStr('Burn True Anomaly = ',paddLen), num2str(rad2deg(ta), form), ' deg'];
        str{end+1} = hRule;

        sumDv = sumDv + norm(dvVect);
    end

    str{end+1} = [paddStr('Total Mission Delta-V = ',paddLen), num2str(sumDv, form), ' km/s'];

    set(hDvManInfoText,'String',str);
end


function visualizeData( X, k, IDX, types, C )
% Project high dim. data unto principal components (PCA) for visualization.
%
% Optionally IDX can be specified to indicate different classes for the
% points; in this case points in different classes are displayed using
% different colors. Up to 12 types are handled (for technical reasons
% involving plot), any cluster with a label>12 is assigned the label 12.
%
% USAGE
%  visualizeData( X, k, [IDX], [types], [C] )
%
% INPUTS
%  X       - column vector of data - N vectors of dimension p (X is Nxp)
%  k       - dimension to which to reduce data (2 or 3)
%  IDX     - [] cluster membership [see kmeans2.m]
%  types   - [] cell array of length ntypes of text labels for each type
%  C       - [] cluster centers (Kxp)
%
% OUTPUTS
%
% EXAMPLE
%  X = [randn(100,5); randn(100,5)+4];
%  C = [mean(X(1:100,:)); mean(X(101:200,:))];
%  IDX = [ones(100,1); 2*ones(100,1)];
%  visualizeData( X, 2, IDX, {'type1','type2' }, C);
%
% See also KMEANS2, DEMOCLUSTER
%
% Piotr's Computer Vision Matlab Toolbox      Version 2.0
% Copyright 2014 Piotr Dollar.  [pdollar-at-gmail.com]
% Licensed under the Simplified BSD License [see external/bsd.txt]

if( nargin<3 || isempty(IDX) ); IDX=[]; end
if( nargin<4 || isempty(types) ); types=[]; end
if( nargin<5 || isempty(C) ); C=[]; end

% apply PCA if necessary
if( size(X,2)~= k )
  [ U, mu ] = pca( X' );
  X = pcaApply( X', U, mu, k )';
  if(~isempty(C)); C = pcaApply( C', U, mu, k )'; end
end

%%% get k
k = size(X,2);
if( k==1 ); X = [X zeros(size(X))]; k = 2; end
if( k>3 ); error( 'k must be <= 3'); end

%%% show points
if( isempty(IDX) )
  if( k==2 )
    plot( X(:,1), X(:,2), '.' );
  elseif( k==3 )
    plot3( X(:,1), X(:,2), X(:,3), '.' );
  end;

else
  IDX(IDX>12)=12;  m=max(IDX);

  if( k==2)
    % plot points
    R = cell(1,3*m+3);
    for i=1:m;
      R((3*i-2):(3*i)) = {X(IDX==i,1), X(IDX==i,2), '.'};
    end;
    R((3*m+1):(3*m+3)) = {X(IDX==-1,1), X(IDX==-1,2), 'k.'};
    plot( R{:} );

    % plot centers
    if( ~isempty(C) )
      R=cell(1,3*m);
      for i=1:m;  R((3*i-2):(3*i)) = {C(i,1), C(i,2), 'x'}; end
      hold('on');  plot( R{:}, 'MarkerSize', 30 );  hold('off');
    end;

  elseif( k==3 )
    % plot points
    R = cell(1,4*m+4);
    for i=1:m;
      R((4*i-3):(4*i)) = {X(IDX==i,1), X(IDX==i,2), X(IDX==i,3), '.'};
    end;
    R((4*m+1):(4*m+4)) = {X(IDX==-1,1), X(IDX==-1,2), X(IDX==-1,3), 'k.'};
    plot3( R{:} );

    % plot centers
    if( ~isempty(C) )
      R=cell(1,4*m);
      for i=1:m;  R((4*i-3):(4*i)) = {C(i,1), C(i,2), C(i,3), 'x'}; end
      hold('on'); plot3( R{:}, 'MarkerSize', 30 );  hold('off');
    end
  end
end
axis('equal');

%%% show legend if types is provided
if(~isempty(types));  legend(types); end


%BUCKYTUMBLE shows tumbling Bucky Ball
%   BUCKYTUMBLE -- display the Bucky Ball, slowly tumbling in space
%   Needs perms.m

%   Bill McKeeman

function buckytumble
  gr = (1+sqrt(5))/2;                  % golden ration
  d = @(a,b) a + b*gr;                 % vertex function

  bb = perms(...                       % Bucky Ball vertices
    [d(0,0), d(0,3), d(1,0)            % truncated icosahedron
     d(1,0), d(0,2), d(2,1)
     d(2,0), d(0,1), d(1,2)]/2, 'cycles', 'signs', 'unique');
  [s,f] = edges(bb);                   % start,finish

  mx = max(abs(bb(:)))*1.2;            % frame the picture
  mz = max(abs(bb(3,:)));              % z scale
  clip = @(vec) min(max(vec,0),1);     % black and white
  axis([-mx mx -mx mx]);               % fix the axes
  axis equal
  axis off
  bg = .8*[1 1 1];                     % background grey
  set(gcf, 'color', bg);

  hold on
  tumble;                              % plot it
  hold off

  return;

  % compute the edges of a polyhedron
  function [s,f] = edges(p)
    m = size(p,1);
    d = inf(m);                        % distance matrix
    for i=1:m
      for j=i+1:m
        seg = p(i,:)-p(j,:);           % vertex pairs
        d(i,j) = sqrt(seg*seg');       % distance between
      end
    end
    es = min(d(:));                    % nearest neighbors
    TOL = es/10000;
    [s,f] = find(abs(d-es)<TOL);       % compensate for roundoff
  end

  % tumble until stopped with ^C
  function tumble
    nd = size(bb,2);
    a = rand(nd)/25;                   % about 1 degree
    p = bb;                            % rotatable vertex set
    for reps = 1:intmax
      dr = ndrotate(a);
      for i=1:100                      % 100, then change direction
        cla;                           % clear previous
        fromInfinity(p);               % new edges
        drawnow;
        p = p*dr;                      % new position
        pause(0.03);                   % leave some cycles
      end
      a = a + (rand-0.5)/100;          % change direction
    end
  end

  % plot 2-D shadow of edges
  function fromInfinity(p)
    for k = 1:numel(s)                 % all edges
      e1 = [p(s(k),1)  p(f(k),1)];     % x end
      e2 = [p(s(k),2)  p(f(k),2)];     % y end
      z = p(s(k),3) + p(f(k),3);       % 2*mx : -2*mx
      h = ((z+2*mz)/mz)/4;             %    1 : 0
      h = 1-h;                         %    0 : 1
      c = clip([h h h]);               % black is nearest
      w = 2-h;
      plot(e1, e2, 'color', c, 'linewidth', w);
    end
  end

  % turn angles into orthogonal matrix
  function res = ndrotate(angles)
    [m,n] = size(angles);
    res = eye(n);
    for i=1:m
      for j=1:n
        if i ~= j && angles(i,j) ~= 0
          tmp = eye(n);
          tmp(i,i) = cos(angles(i,j));
          tmp(j,j) = tmp(i,i);
          tmp(i,j) = -sin(angles(i,j));
          tmp(j,i) = -tmp(i,j);
          res = res*tmp;
        end
      end
    end
  end
end