function [index]=elementdof(node,nnel,ndof)
%----------------------------------------------------------
%  Purpose:
%     Compute system dofs associated with each element
%
%  Synopsis:
%     [index]=elementdof(node,nnel,ndof)
%
%  Variable Description:
%     index - system dof vector associated with element "iel"
%     iel - element number whose system dofs are to be determined
%     node - nodes associated with the element "iel"
%     nnel - number of nodes per element
%     ndof - number of dofs per node
%-----------------------------------------------------------

   k=0;
   for i=1:nnel
     start = (node(i)-1)*ndof;
       for j=1:ndof
         k=k+1;
         index(k)=start+j;
       end
   end


function out = spm_series_align(job)
% Longitudinal registration of image series
% FORMAT out = spm_series_align(job)
%__________________________________________________________________________
% Copyright (C) 2012-2019 Wellcome Trust Centre for Neuroimaging

% John Ashburner
% $Id: spm_series_align.m 7563 2019-04-01 10:39:24Z guillaume $

N = numel(job.vols);
tim = job.times(:);
if numel(tim) ~= N
    error('Incompatible numbers of times and scans.');
end
if any(abs(diff(tim)) > 50)
    error('Time differences should be in years.');
end

if numel(job.noise)==1
    noise = repmat(job.noise,[N,1]);
elseif numel(job.noise) ~= N
    error('Incompatible numbers of noise estimates and scans.');
else
    noise = job.noise(:);
end
for i=find(~isfinite(noise(:)))'
    % Make an estimate of the scanner noise
    noise(i,1) = spm_noise_estimate(job.vols{i});
    fprintf('Estimated noise sd for "%s" = %g\n', job.vols{i}, noise(i,1));
end
prec   = noise.^(-2);

bparam    = [0 0 job.bparam];
wparam0   = job.wparam;

midtim = median(tim);
tim    = tim - midtim;
wparam = kron(wparam0,1./(abs(tim)+1/365));
sparam = round(3*abs(tim)+2);
Nii    = nifti(strvcat(job.vols));

output = {};
if job.write_avg, output = [output, {'wavg'}]; end
if job.write_jac, output = [output, {'wjac'}]; end
if job.write_div, output = [output, {'wdiv'}]; end
if job.write_def, output = [output, {'wdef'}]; end

out = spm_groupwise_ls(Nii, output, prec, wparam, bparam, sparam);


%% Read shape

[X,T] = readOff('../data/meshes/cat1.off');
M = getMeshData(X,T,10); % compute 10 LB eigenfunctions for fun

%% Design two distributions

blurTime = .0001; % if this gets too small, distances get noisy
blurSteps = 10; % was 3

frontVtx = [18962];% 15966];
backVtx = [22553];%26142

p0 = zeros(M.numVertices,1);
for i=1:length(frontVtx)
    p0(frontVtx(i)) = .5 / M.areaWeights(frontVtx(i));
end
p0 = blurOnMesh(p0,M,blurTime,blurSteps);
showDescriptor(M,p0);

p1 = zeros(M.numVertices,1);
for i=1:length(backVtx)
    p1(backVtx(i)) = .5 / M.areaWeights(backVtx(i));
end
p1 = blurOnMesh(p1,M,blurTime,blurSteps);
showDescriptor(M,p1);

%% Set up Gaussian blur function for barycenter

blurTime = .00015; % if this gets too small, distances get noisy
blurSteps = 10;

blur = @(x) applySymmetricKernel(x,M,blurTime,blurSteps); % faster than pre-factored?
blurTranspose = @(x) blur(x);

%% Take the barycenter

p = [p0 p1];
p(p<1e-10) = 1e-10;
nFunctions = 2;

maxEntropy = max(-sum(p.*log(p).*repmat(M.areaWeights,1,2),1));

entropyLimits = [maxEntropy (maxEntropy+1) (maxEntropy+2) (maxEntropy+3) inf];
nEntropies = length(entropyLimits);

euclideanBarycenter = sum(p,2)/nFunctions;
alpha = [1 1];

options = [];
options.niter = 100; % diminishing returns after that...
%options.unit_area_projection = 1;

barycenter = zeros(M.numVertices,nEntropies);
parfor i=1:length(entropyLimits)
    barycenter(:,i) = convolutionalBarycenter(p,alpha,M.areaWeights,blur,blurTranspose,entropyLimits(i),options);
end

save entropyTest.mat

%%

for i=1:nEntropies
    showDescriptor(M,barycenter(:,i));
end

%% Write meshes

clear full
for i=1:nEntropies
    e = entropyLimits(i);
    e = full(e);
    name = sprintf('entropy_barycenter_%g.obj',e);
    writeTexturedObj(name, M, barycenter(:,i), 'scalar_function.mtl');
end

%% Write boundary

writeTexturedObj('p0.obj', M, p0, 'scalar_function.mtl');
writeTexturedObj('p1.obj', M, p1, 'scalar_function.mtl');


function arx=lpcbwexp(ar,bw)
%LPCBWEXP expand formant bandwidths of LPC filter ARX=(AR,BW)
%minimum bandwidth will be BW*fs where fs is the sampling frequency
%the radius of each pole will be multiplied by R=exp(-BW*pi)
% To set the maximum pole radius to R use BW=-log(R)/PI.

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

[nf,p1]=size(ar);
k=exp(-pi*(0:p1-1)*bw);
arx=ar.*k(ones(nf,1),:);


%% BernoulliParticleFilterX Demo
% ----------------------
% * This script demonstrates the process of configuring an running a
%   BernoulliParticleFilterX object to perform single-target state estimation.
%
% * A toy single-target scenario is considered, for a target that generates
%   regular reports of it's position with the possibility of clutter and
%   missed detections
%
%% Extract the GroundTruth data from the example workspace
load('single-target-tracking.mat');
NumIter = size(TrueTrack.Trajectory,2);

%% Models

lambdaV = 5; % Expected number of clutter measurements over entire surveillance region
V = 10^2;     % Volume of surveillance region (10x10 2D-grid)
V_bounds = [0 10 0 10]; % [x_min x_max y_min y_max]
P_D = 0.5;

% Instantiate a Transitionamic model
transition_model = ConstantVelocityX('NumDims',2,'VelocityErrVariance',0.0001);

% Instantiate an Observation model
measurement_model = LinearGaussianX('NumMeasDims',2,'NumStateDims',4,'MeasurementErrVariance',0.02,'Mapping',[1 3]);

% Instantiate a clutter model
clutter_model = PoissonRateUniformPositionX('ClutterRate',lambdaV,'Limits',[V_bounds(1:2);V_bounds(3:4)]);

% Instantiate birth model
birth_model = DistributionBasedBirthModelX('Distribution', UniformDistributionX([V_bounds(1:2); ...
                                                                                [-0.1 0.1 ];...
                                                                                V_bounds(3:4);...
                                                                                [-0.1 0.1 ]]),...
                                           'BirthIntensity', 0.00005);
% Instantiate detection model
detection_model = ConstantDetectionProbabilityX('DetectionProbability',P_D);

% Compile the State-Space model
model = StateSpaceModelX(transition_model,...
                       measurement_model,...
                       'Clutter',clutter_model,...
                       'Birth', birth_model,...
                       'Detection', detection_model);

%% Simulation
% Data Simulator
dataSim = SingleTargetMeasurementSimulatorX(model);

% Simulate some measurements from ground-truth data
MeasurementScans = dataSim.simulate(TrueTrack);
measurements = [MeasurementScans.Vectors];

%% Initiation

% Setup prior assuming we have some intuition of the target's initial
% position
priorParticles = TrueTrack.Trajectory(1).Vector ...
                 + model.Measurement.finv(model.Measurement.random(4000));
priorWeights = ones(1,4000)/4000;
config.Model = model;
config.StatePrior = ParticleStateX(priorParticles,priorWeights./sum(priorWeights));
config.StatePrior.Metadata.ExistenceProbability = 0.5;
config.BirthScheme = {'Expansion', 5000};
config.SurvivalProbability = 0.99;

% Initiate a track using the generated prior
track = TrackX(config.StatePrior, TagX(1));

%% Estimation
% Instantiate a filter objects
filter = BernoulliParticleFilterX(config);
figure;
for t = 2:NumIter

    % Provide filter with the new measurement
    MeasurementList = MeasurementScans(t);

    % Perform filtering
    filter.MeasurementList = MeasurementList;
    filter.predict();
    filter.update();

    % Log the data
    track.Trajectory(end+1) = filter.StatePosterior;

    clf;
    hold on;
    measurements = [MeasurementScans(1:t).Vectors];
    meas = measurement_model.finv(measurements);
    true_means = [TrueTrack.Trajectory(1:t).Vector];
    track_means = [track.Trajectory(1:t).Mean];
    plot(true_means(1,1:t), true_means(3,1:t),'.-k', track_means(1,1:t), track_means(3,1:t), 'b-', meas(1,:), meas(3,:), 'rx');
    plotgaussellipse(track.Trajectory(t).Mean([1,3],1), ...
                     track.Trajectory(t).Covar([1,3],[1,3]),...
                     'Color','g');
    legend('GroundTrouth','Estimated Mean','Measurements', 'Estimated Covariance');
    xlabel("x coordinate (m)");
    ylabel("y coordinate (m)");
    axis([2 9 1 9]);
    drawnow();
end


function z=rpsola(x,y,t)
  % The solution of a Riemann problem (continuous)
  z=zeros(size(x));
  lne=0.25*t;
  lm=-0.5*t;
  lM=0.5*t;

  ind=find((x>=lM)&(y>=lM));
  z(ind)=0.5;

  ind=find((x<lM)&(x>=lne)&(y>=lne)&(y>=x));
  z(ind)=x(ind)/t;

  ind=find((x>=lne)&(y<lM)&(y>=lne)&(y<x));
  z(ind)=y(ind)/t;

  ind=find(((x>=lne)&(y<lne))|((x<lne)&(y>=lne)));
  z(ind)=0.25;

  ind=find((x<lne)&(x>=lm)&(x>=y));
  z(ind)=x(ind)/t;

  ind=find((y<lne)&(y>=lm)&(x<y));
  z(ind)=y(ind)/t;

  ind=find((x<=lm)&(y<=lm));
  z(ind)=-0.5;


function [PB] = GB2PB(GB)
% Convert computery things from gigabytes to petabytes.
% Chad A. Greene 2012
PB = GB*2^-20 ;


function [data,units] = compute_min_dwing_angle_out(trx,n)

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

  data{i} = min(-diff(trx(fly).wing_anglel),diff(trx(fly).wing_angler)) ./ trx(fly).dt;

end
units = parseunits('rad/s');


function [ vS ] = RecSignalFreqSamples( vF, mF, vFIdx )
% ----------------------------------------------------------------------------------------------- %
%[ estFreq ] = EstimateSineFreqKay( vX, samplingFreq, estType )
% Estimates the frequency of a single Real Harmonic signal with arbitrary
% phase.
% Input:
%   - vX                -   Input Samples.
%                           The vector to be optimized. Initialization of
%                           the iterative process.
%                           Structure: Vector (numSamples X 1).
%                           Type: 'Single' / 'Double'.
%                           Range: (-inf, inf).
%   - paramLambda       -   Parameter Lambda.
%                           The L1 Regularization parameter.
%                           Structure: Scalar.
%                           Type: 'Single' / 'Double'.
%                           Range: (0, inf).
%   - numIterations     -   Number of Iterations.
%                           Number of iterations of the algorithm.
%                           Structure: Scalar.
%                           Type: 'Single' / 'Double'.
%                           Range {1, 2, ...}.
% Output:
%   - estFreq           -   Number of Iterations.
%                           Number of iterations of the algorithm.
%                           Structure: Scalar.
%                           Type: 'Single' / 'Double'.
%                           Range {1, 2, ...}.
% References
%   1.  Steven Kay - A Fast and Accurate Single Frequency Estimator.
% Remarks:
%   1.  It would work with complex numbers (Harmonic Signla) by changing:
%       vX(ii) * vX(ii + 1) -> vX(ii)' * vX(ii + 1).
%   2.  fds
% Known Issues:
%   1.  C
% TODO:
%   1.  D
% Release Notes:
%   -   1.0.000     09/08/2021  Royi Avital
%       *   First release version.
% ----------------------------------------------------------------------------------------------- %

% arguments
%     vX (:, 1) {mustBeNumeric, mustBeReal}
%     samplingFreq (1, 1) {mustBeNumeric, mustBeReal, mustBePositive}
%     estType (1, 1) {mustBeNumeric, mustBeReal, mustBePositive, mustBeInteger, mustBeMember(estType, [1, 2])} = 2
% end

numSamples = size(mF, 2);

mFR = real(mF);
mFI = imag(mF);

vS = real(SolveBasisPursuitLp001([mFR(vFIdx, :); mFI(vFIdx, :)], [real(vF); imag(vF)]));
% vSRec = real(SolveBasisPursuitLp002(mF(vFIdx, :), vF));

% vS = complex(vSRec(1:numSamples), vSRec((numSamples + 1):end));

end


function [sma, ecc, inc, raan, arg, tru] = vect_getKeplerFromState(rVect,vVect,gmu, varargin)
% getKeplerFromState() returns Keplerian orbital elements when provided
% with the state (cartesian position vector, cartesian velocity vector) of
% a spacecraft or celestial body.
%
%INPUTS
% rVect - a 3x1 vector that contains the x,y,z components of the orbiting
% body's current position relative to the central body.  Units: [km]
% vVect - a 3x1 vector that contains the x,y,z components of the orbiting
% body's current velocity vector relative to the central body.  Units: [km/sec]
% muCB - the gravitational parameter of the central body.  Units: km^3/s^2
%
%OUTPUTS
% sma - semi-major axis of the orbit.  Units: [km]
% ecc - eccentricity of the orbit.  Units: dimensionless
% inc - inclination angle of the orbit.  Units: radian
% raan - Longitude of ascending node of the orbit.  Units: radian
% arg - Argument of periapse of the orbit.  Units: radian.
% tru - Current true anomaly of the spacecraft/body in the orbit.
% Units: radian
if(~isempty(varargin))
    consistencyCheck = varargin{1};
else
    consistencyCheck = true;
end

numRV = size(rVect,2);

rVect = reshape(rVect,3,numRV);
vVect = reshape(vVect,3,numRV);

if(isscalar(gmu))
    gmu = gmu * ones(1,numRV);
end

try
    [sma, ecc, inc, raan, arg, tru] = vect_getKeplerFromState_Alg_mex(rVect,vVect,gmu(:)');
catch
    [sma, ecc, inc, raan, arg, tru] = vect_getKeplerFromState_Alg(rVect,vVect,gmu(:)');
end

bool = ecc<1.0;

tru(bool)=AngleZero2Pi(tru(bool));
tru(~bool) = angleNegPiToPi(tru(~bool));

tol = 1E-6;
if(consistencyCheck)
    [rVect2,~]=vect_getStatefromKepler(sma, ecc, inc, raan, arg, tru, gmu, false);
    rDang = real(acos(dot(rVect,rVect2)./(sqrt(sum(abs(rVect).^2,1)) .* sqrt(sum(abs(rVect2).^2,1)) )));
    if(all(rDang < tol))
        return;
    end

    [rVect3,~]=vect_getStatefromKepler(sma, ecc, inc, raan-rDang, arg+rDang, tru, gmu, false);
    rDang2 = real(acos(dot(rVect,rVect3)./(sqrt(sum(abs(rVect).^2,1)) .* sqrt(sum(abs(rVect3).^2,1)) )));
    if(any(rDang2 < tol))
        raan(rDang2 < tol) = raan(rDang2 < tol) - rDang(rDang2 < tol);
        arg(rDang2 < tol)  = arg(rDang2 < tol)  + rDang(rDang2 < tol);
        raan = AngleZero2Pi(raan);
        arg = AngleZero2Pi(arg);

        if(all(rDang2 < tol))
            return;
        end
    end

    [rVect4,~]=vect_getStatefromKepler(sma, ecc, inc, raan+rDang, arg-rDang, tru, gmu, false);
    rDang3 = real(acos(dot(rVect,rVect4)./(sqrt(sum(abs(rVect).^2,1)) .* sqrt(sum(abs(rVect4).^2,1)) )));
    if(any(rDang3 < tol))
        raan(rDang3 < tol) = raan(rDang3 < tol) + rDang(rDang3 < tol);
        arg(rDang3 < tol)  = arg(rDang3 < tol)  - rDang(rDang3 < tol);
        raan = AngleZero2Pi(raan);
        arg = AngleZero2Pi(arg);

        if(all(rDang3 < tol))
            return;
        end
    end

    [rVect5,~]=vect_getStatefromKepler(sma, ecc, inc, raan+rDang, arg+rDang, tru, gmu, false);
    rDang4 = real(acos(dot(rVect,rVect5)./(sqrt(sum(abs(rVect).^2,1)) .* sqrt(sum(abs(rVect5).^2,1)) )));
    if(any(rDang4 < tol))
        raan(rDang4 < tol) = raan(rDang4 < tol) + rDang(rDang4 < tol);
        arg(rDang4 < tol)  = arg(rDang4 < tol) + rDang(rDang4 < tol);
        raan = AngleZero2Pi(raan);
        arg = AngleZero2Pi(arg);

        if(all(rDang4 < tol))
            return;
        end
    end

    [rVect6,~]=vect_getStatefromKepler(sma, ecc, inc, raan-rDang, arg-rDang, tru, gmu, false);
    rDang5 = real(acos(dot(rVect,rVect6)./(sqrt(sum(abs(rVect).^2,1)) .* sqrt(sum(abs(rVect6).^2,1)) )));
    if(any(rDang5 < tol))
        raan(rDang5 < tol) = raan(rDang5 < tol) - rDang(rDang5 < tol);
        arg(rDang5 < tol)  = arg(rDang5 < tol) - rDang(rDang5 < tol);
        raan = AngleZero2Pi(raan);
        arg = AngleZero2Pi(arg);

        if(all(rDang5 < tol))
            return;
        end
    end

    [rVect7,~]=vect_getStatefromKepler(sma, ecc, inc, raan, arg, tru-rDang, gmu, false);
    rDang6 = real(acos(dot(rVect,rVect7)./(sqrt(sum(abs(rVect).^2,1)) .* sqrt(sum(abs(rVect7).^2,1)) )));
    if(any(rDang6 < tol))
        tru(rDang6 < tol) = tru(rDang6 < tol) - rDang(rDang6 < tol);

        if(all(rDang6 < tol))
            return;
        end
    end

    [rVect8,~]=vect_getStatefromKepler(sma, ecc, inc, raan, arg, tru+rDang, gmu, false);
    rDang7 = real(acos(dot(rVect,rVect8)./(sqrt(sum(abs(rVect).^2,1)) .* sqrt(sum(abs(rVect8).^2,1)) )));
    if(any(rDang7 < tol))
        tru(rDang7 < tol) = tru(rDang7 < tol) + rDang(rDang7 < tol);

        if(all(rDang7 < tol))
            return;
        end
    end
end


function value = t_polynomial_value ( n, x )

%*****************************************************************************80
%
%% T_POLYNOMIAL_VALUE: returns the single value T(n,x).
%
%  Discussion:
%
%    In cases where calling T_POLYNOMIAL is inconvenient, because it returns
%    a vector of values for multiple arguments X, this simpler interface
%    may be appropriate.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 April 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the polynomial.
%
%    Input, real X, the argument of the polynomial.
%
%    Output, real VALUE, the value of T(n,x).
%
  m = 1;

  v_vec = t_polynomial ( m, n, x );

  value = v_vec(n+1);

  return
end


function newS = align_shape( TransM , S )

newS = zeros(size(S));

x = S(1:2:end);
y = S(2:2:end);

xy = [x'; y'; ones(1, size(x,1))];
newXY = [TransM ; [0 0 1]]\xy;

newS(1:2:end) = newXY(1,:);
newS(2:2:end) = newXY(2,:);

end


function [p,CI] = compareAUC(X1,X2,Y)
% -------------------------------------------------------------------------
% function [p,CI] = compareAUC(X1,X2,Y)
% -------------------------------------------------------------------------
% DESCRIPTION:
% This function calculates the significance (p-value) for the difference
% between two CORRELATED AUCs (i.e. computed from the same samples)
% computed for two different prediction situations. The implementation of
% this function is based on ref. [1].
% -------------------------------------------------------------------------
% REFERENCE:
% [1] DeLong, E. R., DeLong, D. M. & Clarke-Pearson, D. L. Comparing the
%     areas under two or more correlated receiver operating characteristic
%     curves: a nonparametric approach. Biometrics 44, 837–845 (1988).
% -------------------------------------------------------------------------
% INPUTS:
% 1. X1: [N X 1] vector of probabilities of outcome Y for situation 1.
% 2. X2: [N X 1] vector of probabilities of outcome Y for situation 2.
% 3. Y:  [N X 1] vector defining if each nth intance has the outcome or not
%        (1's or 0's), where N is the total number of patients.
% -------------------------------------------------------------------------
% OUTPUTS:
% 1. p: p-value for the difference in AUC.
% 2. CI: 95 % confidence interval for the difference in AUCs (diff), to be
%    subsequently applied as diff - CI and diff + CI.
% -------------------------------------------------------------------------
% AUTHOR(S): Martin Vallieres <mart.vallieres@gmail.com>
% -------------------------------------------------------------------------
% HISTORY:
% - Creation: January 2017
% -------------------------------------------------------------------------
% STATEMENT:
% This file is part of <https://github.com/mvallieres/radiomics/>,
% a package providing MATLAB programming tools for radiomics analysis.
% --> Copyright (C) 2015-2017  Martin Vallieres
%
%    This package 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 package 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 package.  If not, see <http://www.gnu.org/licenses/>.
% -------------------------------------------------------------------------

% SANITY CHECKS
if numel(X1) ~= numel(Y) || numel(X2) ~= numel(Y)
    error('The number of instances in vectors X1, X2, and Y must be the same')
end
Y = logical(Y);

% PARAMETERS OR INTEREST
sigma = 1.96; % For 95 % confidence interval
L = [1,-1]; % Contrast vector. We are always comparing 2 variables at a time, not more. This is hardcoded for now and should stay as is.

% INITIALIZATION
X = [X1,X2]; % Combining the variables into a single matrix. We are always comparing 2 variables at a time, not more.
nVar = size(X,2); % Number of variables we compare (referred to "r" in ref. [1], so here, "v" --> "r" in ref. [1])
nInst = numel(Y); nPos = sum(Y); nNeg = nInst - nPos; % Total number of instances, number of positive instances ("nPos" --> "m" in ref. [1]) and number of negative instances ("nNeg" --> "n" in ref. [1]), respectively
Vpos = zeros(nPos,nVar); Vneg = zeros(nNeg,nVar); % Matrices "Vpos" --> "V10" and "Vneg" --> "V01" in ref. [1], respectively.
auc = zeros(1,nVar); % AUC estimates ("auc" --> "theta" in ref. [1])

% COMPUTATION OF AUC ESTIMATES ("auc" --> "theta"), VPOS ("Vpos" --> "V10") and VNEG ("Vneg" --> "V01")
Xpos = X(Y,:); Xneg = X(~Y,:); % Separating positive and negative instances ("Xpos" --> "X" and Xneg --> "Y" in ref. [1]. Here, our "Y" refers to the outcomes, or targets, to defined if an instance as the condition or not)
for v = 1:nVar
    for i = 1:nPos
        val = Xpos(i,v);
        Vpos(i,v) = sum(Xneg(:,v) < val) + 0.5*sum(Xneg(:,v) == val);
    end
    for i = 1:nNeg
        val = Xneg(i,v);
        Vneg(i,v) = sum(Xpos(:,v) > val) + 0.5*sum(Xpos(:,v) == val);
    end
end
auc = sum(Vpos)/(nNeg*nPos);
Vpos = Vpos/nNeg; Vneg = Vneg/nPos;

% COMPUTATION OF COVARIANCE MATRICES ("Spos" --> "S10" and "Sneg" --> "S01" in ref. [1])
Spos = (1/nPos) * ((Vpos'*Vpos) - nPos*(auc*auc'));
Sneg = (1/nNeg) * ((Vneg'*Vneg) - nNeg*(auc*auc'));
S = (1/nPos)*Spos + (1/nNeg)*Sneg;

% COMPUTATION OF CHI-SQUARE STATISTICS
LSL = L*S*L';
val_chi2 = (auc*L')*(inv(LSL))*(L*auc')*sqrt(2);
df_chi2 = rank(LSL);
p = 1-chi2cdf(val_chi2,df_chi2);
CI = sigma * sqrt(LSL);

end


function [W] = hpe2W(hpe)
% Convert power from electrical horsepower to watts.
% Chad A. Greene 2012
W = hpe*746;


classdef SOP_F21 < PROBLEM
% <single> <real> <expensive/none>
% Shekel's family

%------------------------------- Reference --------------------------------
% X. Yao, Y. Liu, and G. Lin, Evolutionary programming made faster, IEEE
% Transactions on Evolutionary Computation, 1999, 3(2): 82-102.
%------------------------------- 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
        %% Default settings of the problem
        function Setting(obj)
            obj.M = 1;
            obj.D = 4;
            obj.lower    = zeros(1,obj.D);
            obj.upper    = zeros(1,obj.D) + 10;
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,PopDec)
            a = [4 4 4 4;1 1 1 1;8 8 8 8;6 6 6 6;3 7 3 7];
            c = [0.1;0.2;0.2;0.4;0.4];
            PopObj = zeros(size(PopDec,1),1);
            for i = 1 : size(PopDec,1)
                PopObj(i) = -sum(1./(sum((repmat(PopDec(i,:),5,1)-a).^2,2)+c));
            end
        end
        %% Generate the minimum objective value
        function R = GetOptimum(obj,N)
            R = -10.16;
        end
    end
end


% Infrared Small-Target Detection Using Multiscale Gray Difference Weighted Image Entropy
clearvars;
close all;
clc;
wflag = 1;
for kk = 1
    fold = '.\data\';% 27 images
    try
        img = imread([fold, num2str(kk), '.jpg']);
    catch
        img = imread([fold, num2str(kk), '.bmp']);
    end
    img = img(:,:,1);
    re = mgdwe(img);
    bw = bwfunc(re);
    if wflag
        refold = '.\';
        if exist(refold,'dir')==0
            mkdir(refold);
        end
        imwrite(uint8(img), [refold, num2str(kk), '1.png']);
        imwrite(uint8( mat2gray(re) * 255), [refold, num2str(kk), '2.png']);
        imwrite(bw, [refold, num2str(kk), '3.png']);
    end
end


function [xi,yi] = snakeinterp(x,y,dmax,dmin)
%SNAKEINTERP  Interpolate the snake adaptively
%   [xi,yi] = snakeinterp(x,y,dmax,dmin)
%
%   dmax: the maximum distance between two snake points
%   dmin: the maximum distance between two snake points
%   d(i,i+1)>dmax, then a new point is added between i and i+1
%   d(i,i+1)<dmin, then either i or i+1 is removed
%
%   NOTE: the spacing of original curve must be close to the
%         range defined by dmax and dmin. For arbitrary spacing,
%         try snakeinterp1.
%
%   See also SNAKEINTERP1

%    there is a bug in the program for points removal

%   Chenyang Xu and Jerry L. Prince, 4/1/95, 6/17/97
%   Copyright (c) 1995-97 by Chenyang Xu and Jerry L. Prince
%   Image Analysis and Communications Lab, Johns Hopkins University
    xi=x;
    yi=y;
% convert to column vector
x = x(:); y = y(:);

N = length(x);

d = abs(x([2:N 1])- x(:)) + abs(y([2:N 1])- y(:));

% remove the points which distance to neighbor points is shorter than dmin
IDX = (d<dmin);

idx = find(IDX==0);
x = x(idx);
y = y(idx);

N = length(x);

if N>0

    d = abs(x([2:N 1])- x(:)) + abs(y([2:N 1])- y(:));

    IDX = (d>dmax);

    z = snakeindex(IDX);

    p = 1:N+1;

    xi = interp1(p,[x;x(1)],z');
    yi = interp1(p,[y;y(1)],z');

    N = length(xi);
    d = abs(xi([2:N 1])- xi(:)) + abs(yi([2:N 1])- yi(:));

    while (max(d)>dmax),

        IDX = (d>dmax);
        z = snakeindex(IDX);

        p = 1:N+1;

        xi = interp1(p,[xi;xi(1)],z');
        yi = interp1(p,[yi;yi(1)],z');

        N = length(xi);
        d = abs(xi([2:N 1])- xi(:)) + abs(yi([2:N 1])- yi(:));
    end

end


function asa066_test02 ( )

%*****************************************************************************80
%
%% TEST02 tests NORMP.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST02:\n' );
  fprintf ( 1, '  Compare tabulated values of the normal\n' );
  fprintf ( 1, '  Cumulative Density Function against values\n' );
  fprintf ( 1, '  computed by NORMP.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '         X        CDF                       CDF' );
  fprintf ( 1, '                    DIFF\n' );
  fprintf ( 1, '               (tabulated)                 (NORMP)\n' );
  fprintf ( 1, '\n' );

  upper = 0;
  n_data = 0;

  while ( 1 )

    [ n_data, x, fx ] = normal_01_cdf_values ( n_data );

    if ( n_data == 0 )
      break
    end

    [ p, q, pdf ] = normp ( x );
    fx2 = p;

    fprintf ( 1, '  %10.4e  %24.16e  %24.16e  %10.4e\n', ...
    x, fx, fx2, abs ( fx - fx2 ) );

  end

  return
end


function out = EN_SampEn(y,M,r,preProcessHow)
% EN_SampEn     Sample Entropy of a time series
%
% SampEn(m,r), using code from PhysioNet.
%
% Uses a compiled C version of the code if available, otherwise uses a (slower)
% Matlab implementation (which can actually be faster for shorter time series
% due to overheads of reading/writing to disk)
%
% The publicly-available PhysioNet Matlab code, sampenc (renamed here to
% RN_sampenc) is available from:
% http://www.physionet.org/physiotools/sampen/matlab/1.1/sampenc.m
%
% cf. "Physiological time-series analysis using approximate entropy and sample
% entropy", J. S. Richman and J. R. Moorman, Am. J. Physiol. Heart Circ.
% Physiol., 278(6) H2039 (2000)
%
% This function can also calculate the SampEn of successive increments of time
% series, i.e., we using an incremental differencing pre-processing, as
% used in the so-called Control Entropy quantity:
%
% "Control Entropy: A complexity measure for nonstationary signals"
% E. M. Bollt and J. Skufca, Math. Biosci. Eng., 6(1) 1 (2009)
%
%---INPUTS:
% y, the input time series
% M, the embedding dimension
% r, the threshold
% preProcessHow [opt], (i) 'diff1', incremental differencing (as per 'Control Entropy').

% ------------------------------------------------------------------------------
% Copyright (C) 2020, Ben D. Fulcher <ben.d.fulcher@gmail.com>,
% <http://www.benfulcher.com>
%
% If you use this code for your research, please cite the following two papers:
%
% (1) B.D. Fulcher and N.S. Jones, "hctsa: A Computational Framework for Automated
% Time-Series Phenotyping Using Massive Feature Extraction, Cell Systems 5: 527 (2017).
% DOI: 10.1016/j.cels.2017.10.001
%
% (2) B.D. Fulcher, M.A. Little, N.S. Jones, "Highly comparative time-series
% analysis: the empirical structure of time series and their methods",
% J. Roy. Soc. Interface 10(83) 20130048 (2013).
% DOI: 10.1098/rsif.2013.0048
%
% This function 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/>.
% ------------------------------------------------------------------------------

% Check y is a column vector:
if isempty(y)
    error('Must input a valid data vector');
end
if size(y,1)==1
    y = y';
end

% Embedding dimension:
if nargin < 2
    M = 2;
end

% Tolerance:
if nargin < 3
    r = 0.1*std(y);
end

if nargin < 4
    preProcessHow = ''; % don't apply any preprocessing
end

%-------------------------------------------------------------------------------
% Can specify to first apply an incremental differencing of the time series
% thus yielding the 'Control Entropy':
% "Control Entropy: A complexity measure for nonstationary signals"
% E. M. Bollt and J. Skufca, Math. Biosci. Eng., 6(1) 1 (2009)
if ~isempty(preProcessHow)
    y = BF_PreProcess(y,preProcessHow);
end

% ------------------------------------------------------------------------------
% Use the physionet code to calculate the Sample Entropy using these parameters:
% ------------------------------------------------------------------------------
% Check if a compiled C version exists:
% [a,b] = system('which sampen');

sampEn = sampen_mex(y',M+1,r);
sampEn = sampEn(1:M+1); % always that extra one for the M=0

%-------------------------------------------------------------------------------
% This is dangerous because it could lead to inconsistent implementations across runs
%-------------------------------------------------------------------------------
% warning('No mex file found: using a slower native Matlab implementation instead');
% No mex version available; use (much slower) Matlab implementation
% if isempty(b) || length(y) < 3000 % faster to run within Matlab
% sampEn = PN_sampenc(y,M+1,r,false);
%     fprintf('Using compiled C code~~~\n')
%     % http://www.physionet.org/physiotools/sampen/c/
%     % (use Makefile in Toolboxes/Physionet/ to run make, then make install)
%
%     % Run compiled C code:
%     filePath = BF_WriteTempFile(y);
%     command = sprintf('sampen -m %u -r %f < %s',M,r,filePath);
%     [~,res] = system(command);
%     fprintf(1,'%s\n',res)
%     s = textscan(res,'%[^\n]'); s = s{1};
%     sampEn = zeros(M,1);
%     for i = 1:M
%         [~,params] = regexp(s{i},'\((\S+)\)','tokens','match');
%         params = regexp(params{1}(2:end-1),',','split');
%         [~,result] = regexp(s{i},'= (\S+)','tokens','match');
%         result = str2num(result{1}(3:end));
%         sampEn(i) = result;
%     end
% end
% end

% ------------------------------------------------------------------------------
% Compute outputs from the code
% ------------------------------------------------------------------------------
for i = 1:M+1
    % Sample entropy:
    out.(sprintf('sampen%u',i-1)) = sampEn(i);

    % Quadratic sample entropy (QSE), Lake (2006):
    % (allows better comparison across r values)
    out.(sprintf('quadSampEn%u',i-1)) = sampEn(i) + log(2*r);

    % COSEn (Lake and Moorman, 2011), doesn't really make sense in general;
    % especially for z-scored series!:
    % out.(sprintf('COSEn%u',i)) = sampEn(i) + log(2*r) - log(mean(y));
end

if M > 1
    out.meanchsampen = mean(diff(sampEn));
end

end


function msm_to_mm_test07 ( )

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

  output_filename = 'msm_to_mm_test07.mm';

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

  return
end


function fg = mk_fgraph(G, node_sizes, factors, varargin)
% MK_FGRAPH Make a factor graph
% fg = mk_fgraph(G, node_sizes, factors, ...)
%
% A factor graph is a bipartite graph, with one side containing variables,
% and the other containing functions of (subsets of) these variables.
% For details, see "Factor Graphs and the Sum-Product Algorithm",
%  F. Kschischang and B. Frey and H-A. Loeliger,
%  IEEE Trans. Info. Theory, 2001
%
% G(i,j) = 1 if there is an arc from variable i to factor j
%
% node_sizes(i) is the number of values node i can take on,
%   or the length of node i if i is a continuous-valued vector.
%
% 'factors' is the list of factors (kernel functions)
%
% The list below gives optional arguments [default value in brackets].
%
% equiv_class - equiv_class(i)=j  means factor node i gets its params from factors{j} [1:F]
% discrete - the list of nodes which are discrete random variables [1:N]
%
% e.g., fg = mk_fgraph(G, [2 2], {bnet.CPD{1},bnet.CPD{2}}, 'discrete', [1 2])

fg.G = G;
fg.node_sizes = node_sizes;
fg.factors = factors;
[fg.nvars fg.nfactors] = size(G);

% default values for parameters
fg.equiv_class = 1:fg.nfactors;
fg.dnodes = 1:fg.nvars;

if nargin >= 4
  args = varargin;
  nargs = length(args);
  for i=1:2:nargs
    switch args{i},
     case 'equiv_class', fg.equiv_class = args{i+1};
     case 'discrete',    fg.dnodes = args{i+1};
     otherwise,
      error(['invalid argument name ' args{i}]);
    end
  end
end

% so that determine_pot_type will work...
fg.utility_nodes = [];
%fg.decision_nodes = [];
%fg.chance_nodes = fg.nvars;

fg.dom = cell(1, fg.nfactors);
for f=1:fg.nfactors
  fg.dom{f} = find(G(:,f));
end
fg.dep = cell(1, fg.nvars);
for x=1:fg.nvars
  fg.dep{x} = find(G(x,:));
end
fg.cnodes = mysetdiff(1:fg.nvars, fg.dnodes);


function im = ibwt(decomp)
% function im = ibwt(decomp)
%
% BWT reconstruction
% Version 1.2
%
% Arguments:
%  decomp: A square BWT decomposition, as produced by bwt.m
%
% Result:
%  im: An image
%
% Citation:
%  Willmore B, Prenger RJ, Wu MC and Gallant JL (2008). The Berkeley
%  Wavelet Transform: A biologically-inspired orthogonal wavelet transform.
%  Neural Computation 20:6, 1537-1564
%
% The article is available at:
%  <http://dx.doi.org/10.1162/neco.2007.05-07-513>
%
% Copyright (c) 2008 Ben Willmore
%
% Permission is hereby granted, free of charge, to any person
% obtaining a copy of this software and associated documentation
% files (the "Software"), to deal in the Software without
% restriction, including without limitation the rights to use,
% copy, modify, merge, publish, distribute, sublicense, and/or sell
% copies of the Software, and to permit persons to whom the
% Software is furnished to do so, subject to the following
% conditions:
%
% The above copyright notice and this permission notice shall be
% included in all copies or substantial portions of the Software.
%
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
% EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
% OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
% NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
% HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
% WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
% FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
% OTHER DEALINGS IN THE SOFTWARE.

sz = size(decomp);

if (length(sz) ~= 2) || (sz(1) ~= sz(2))
  disp('Input must be square');
  im = nan;
  return;
end

sz = sz(1);

numlevels = log(sz)/log(3);

if ( (numlevels-floor(numlevels)) > abs(numlevels)*eps )
  disp('Input side length must be a power of 3');
  im = nan;
  return;
end

im = zeros(sz);

for level = 1:numlevels
  ssz = size(decomp,1);
  decomp_thislevel = decomp;

  if (ssz>3)
    decomp_thislevel(end-ssz/3+1:end,1:ssz/3) = 0;
  end

  im_thislevel = ibwt_onelevel(decomp_thislevel)/sz*ssz*((3^level)^2);
  idx = ceil((1:sz)*ssz/sz);
  im = im + im_thislevel(idx,idx); % upsample by sz/ssz times

  if (ssz>1)
    decomp = decomp(end-ssz/3+1:end,1:ssz/3);
  end
end


function err_shape = error_depth(param_dist,xcn,xpn,R,T,X_shape,ind);

X_new = depth_compute(xcn,xpn,[param_dist],R,T);

N_pt_calib = size(xcn,2);

% UnNormalized shape extraction:

X_shape2 = X_new;
X_shape2 = X_shape2 - (X_shape2(:,1)*ones(1,N_pt_calib));

% map the second vector at [1;0;0]:

omu = -cross([1;0;0],X_shape2(:,2));
omu = acos((dot([1;0;0],X_shape2(:,2)))/norm(X_shape2(:,2)))*(omu / norm(omu));
Ru = rodrigues(omu);

X_shape2 = Ru* X_shape2;

omu2 = -cross([0;1;0],[0;X_shape2(2:3,ind)]);
omu2 = acos((dot([0;1;0],[0;X_shape2(2:3,ind)]))/norm([0;X_shape2(2:3,ind)]))*(omu2 / norm(omu2));
Ru2 = rodrigues(omu2);

X_shape2 = Ru2* X_shape2;

% Error:

err_shape = X_shape2(:,2:end) - X_shape(:,2:end);

err_shape = err_shape(:);

%err_depth = Z_new - Z_ref;


function [rVectECEF, vVectECEF, REci2Ecef] = getFixedFrameVectFromInertialVect(ut, rVectECI, bodyInfo, varargin)
% %getFixedFrameVectFromInertialVect Summary of this function goes here
% %   Detailed explanation goes here
%
    inputs = bodyInfo.getFixedFrameFromInertialFrameInputsCache();
    [rVectECEF, vVectECEF, REci2Ecef] = getFixedFrameVectFromInertialVect_alg(ut, rVectECI, inputs{:}, varargin{:});
end

% %getFixedFrameVectFromInertialVect Summary of this function goes here
% %   Detailed explanation goes here
%
%     if(~isempty(varargin))
%         vVectECI = varargin{1};
%     else
%         vVectECI = [NaN;NaN;NaN];
%     end
%
%     numElems = length(ut);
%
%     spinAngle = getBodySpinAngle(bodyInfo, ut);
%
%     cSA = reshape(cos(spinAngle),1,1,numElems);
%     sSA = reshape(sin(spinAngle),1,1,numElems);
%     zero = zeros(1,1,numElems);
%     one = zero + 1;
%
% %     R = [cos(spinAngle) -sin(spinAngle) 0;
% %          sin(spinAngle) cos(spinAngle) 0;
% %          0 0 1];
%
%     R = [cSA, -sSA, zero;
%          sSA, cSA, zero;
%          zero, zero, one];
%
% %     rVectECI = reshape(rVectECI,3,1);
%     rVectECI = reshape(rVectECI,3,1,numElems);
%
%     REci2Ecef = permute(R,[2,1,3]); %ND transpose
% %     rVectECEF = REci2Ecef * rVectECI;
%     rVectECEF = mtimesx(REci2Ecef, rVectECI);
%     rVectECEF = reshape(rVectECEF,3,numElems);
%
%     if(~any(isnan(vVectECI)))
%         rotRateRadSec = 2*pi/bodyInfo.rotperiod;
%         omegaRI = repmat([0;0;rotRateRadSec],1,1,numElems);
%         vVectECI = reshape(vVectECI,3,1,numElems);
%
% %         vVectECEF = REci2Ecef*(vVectECI - cross(omegaRI, rVectECI));
%         vVectECEF = mtimesx(REci2Ecef, (vVectECI - cross(omegaRI, rVectECI)));
%         vVectECEF = reshape(vVectECEF,3,numElems);
%     else
%         vVectECEF = repmat([NaN;NaN;NaN],1,numElems);
%     end


function log_b = binomial_coeff(n_max)
    % Compute log binomial coefficients
    log_b = {};
    for n = 0:n_max
        Ks = 0:n;
        log_b{n+1} = gammaln(n+1) - gammaln(Ks+1) - gammaln(n-Ks+1);
    end
end


% capillary pressure curve for drainage
% Written by Ali A. Eftekhari
% TBD, probably not required in the new IMPES formulation
function res=dpc_drain(sw, pce, swc, labda, pc_max)
sw0=swc+(1-labda*log(pc_max/pce)+sqrt((-1+labda*log(pc_max/pce))^2+...
      4*swc/(1-swc)))/2*(1-swc);
res = (sw>sw0).*(-1.0/((1-swc)*labda)*pce*((sw-swc)/(1-swc)).^(-1.0/labda-1))+...
    (0.0<=sw).*(sw<=sw0).*(-1.0/((1-swc)*labda)*pce*((sw0-swc)/(1-swc)).^(-1.0/labda-1))+...
    (sw<0)*0.0;
end


function [F, subF, Data] = ObjFun(Track, UAV)
%OBJFUN 目标函数、适应度函数（一个agent的）

% 多目标优化权重设置（必需为行向量）
weight = [ 0.05, 0.05, 0.1, 0.7, 0.7 ]; % 默认权重

% 表达式系数调整（将各项指标无量纲化）
p1 = 1;   % 燃料项（已除以最大航程）
p21 = 1; % 高度项
p22 = 1; % 低度项
p31 = 1.2; % 雷达威胁
p32 = 1.1; % 其余威胁
p4 = 1.2;   % 时间同步项
p5 = 1;   % 碰撞项

% 进行航迹检测
report = TrackDetect(Track, UAV);  % Track 为 struck结构

% 燃油（论文3）
ZZ = sum(UAV.limt.L);
MaxL_mt = ZZ(2);
f_o = p1 * report.L_mt / MaxL_mt;

% 高度（论文3）
dim = UAV.PointDim;
if dim < 3
    f_h = 0;
else
    f_h = 0;
    for i = 1 : UAV.num
        Hmax = UAV.limt.h(i, 2);
        Hmin = UAV.limt.h(i, 1);
        for k = 1 : UAV.PointNum(i)
            z = Track.P{i}(3, k);
            if z>Hmax
                fk = p21 * (z - Hmax);
            elseif z >= Hmin
                fk = 0;
            else
                fk = p22 * (Hmin - z);
            end
            f_h = f_h + fk;
        end
    end
end

% 威胁（论文2）
% 注意：只适应球或圆区域，不适合圆柱区域
O_r = UAV.Menace.radar(: ,1:end-1);             %雷达
O_o = UAV.Menace.other(: ,1:end-1);            %导弹，火炮，气象等
f_t = 0;  % 威胁代价
for i = 1 : UAV.num
    for k = 1 : UAV.PointNum(i)
        P = Track.P{i}(:, k)' ;  % 转置成 1*dim
        for m = 1 : size(O_r, 1)
            fk = p31 / (norm(P - O_r(m, :)))^4;
            f_t = f_t + fk;
        end
        for m = 1 : size(O_o, 1)
            fk = p32 / norm(P - O_o(m, :));
            f_t = f_t + fk;
        end
    end
end

% 同步（论文1）
f_m = 0;
for i = 1 : UAV.num
    Li = report.L(i);
    tmax = Li / UAV.limt.v(i,1);
    tmin = Li / UAV.limt.v(i,2);
    ti = report.time(i);
    tc = UAV.tc;
    if tc <= tmax && tc >= tmin
        fk = 0;
    else
        fk = p4 * abs(ti - tc);
    end
    f_m = f_m + fk;
end

% 碰撞（论文1）
f_c = p5 * report.col_times;

% 目标函数分量（必需是列向量，否则无法聚类）
subF = [ f_o; f_h; f_t; f_m; f_c ]; % 5*1

% 加权目标函数
F = weight * subF ;

% 输出信息
Data.ProbPoint = report.ProbPoint;      % 所有有问题的点
Data.AngleProb = report.AngleProb;    % 不满足角度约束的点
Data.TrajProb = report.TrajProb;           % 不满足最小航迹间隔的点
Data.Threat = report.Threat;                  % 受威胁的点

Data.L = report.L;                                      % 每个无人机的航程
Data.t = report.time;                                 % 每个无人机的时间
Data.c = report.col_times;                         % 所有无人机总碰撞次数

end


% TD | Tucker-ALS | Tucker Decomposition solved by Alternating Least Squares
% process_video('TD', 'Tucker-ALS', 'dataset/demo.avi', 'output/demo_Tucker-ALS.avi');

r = [size(T,1) size(T,2) 1];
A = double(T);
L = double(tucker_als(T,r));
S = A - L;


%% Radar Plan Position Indicator(PPI) Simulation

% Created by
% R.Vivek
% MIT (Anna University)

%% Description
% This function displays the Radar PPI plot for detection and ...
% tracking of a single target approaching at a specified angle.

%%

function PPI_plot(dist,Angle,Max_Range)

%% "dist" is an array containing the distance (in meters) of the target

%% "Angle" is the angle (in radians) with respect to the radar transceiver
%  at which the target is approaching

%% "Max_Range" is the maximum detectable range of the radar

%% -------------------------------------------------------------------------

angle=repmat(Angle,1,(length(dist)));

for j=1:length(dist)
    for i=(2*pi):-(pi/180):(pi/180)
        [x,y]=(pol2cart(i,55));
        x=ceil(x);
        z=complex(x,y);
        h=compass(z);
        title('Radar PPI Plot');
        ph=findall(gca,'type','patch');
        set(ph,'facecolor',[0,0.5,0]);
        set( h, 'Color', [0.5,1,0] );
        set( h, 'LineWidth', 4 );
        for k = 1:length(h)
            a = get(h(k), 'xdata');
            b = get(h(k), 'ydata');
            set(h(k), 'xdata', a(1:2), 'ydata', b(1:2)')
        end

        if(j>1)
            if(angle(j-1)<i)
                hold on
                plothandle=polar(angle(j-1),dist(j-1),'o');
                set(plothandle,'markerfacecolor',[0,1,0],'markeredgecolor',[0,1,0]);
                get( plothandle ) ;
                set( plothandle, 'Color', [ 0.5, 1, 0 ] );
                set ( plothandle, 'LineWidth', 2 );
                hold off
            end
        end
        if(dist(j)<=Max_Range)
        if(angle(j)>=i)
            hold on
            plothandle=polar(angle(j),dist(j),'o');
            set(plothandle,'markerfacecolor',[0,1,0],'markeredgecolor',[0,1,0])
            get( plothandle ) ;
            set( plothandle, 'Color', [ 0.5, 1, 0 ] );
            set ( plothandle, 'LineWidth', 2 );
            hold off
        end
        end
        pause(1*10^-6);
        end
    end

end


function [w0, t] = adw_fan(ob, wi, mask, Phi, varargin)
% function [w0, phi, t] = adw_fan(ob, wi)
% Compute the angular-dependent weighting for 3rd gen fan-beam
% w0(\Phi) = w(x0, y0, \Phi)
%         for fully corrected penalty:
%          = w(s',\beta')*J(s')|\phi'=\Phi + ...
%            w(s',\beta')*J(s')|\phi'=\Phi-pi
% see fessler chapter 3.5.3
%
% in:
%    ob         a struct or object
%               including system parameters, such as pixel-size etc.
%    wi         cov(yi)
%    varargin   default: only compute phi' = Phi
%               'all': compute phi' = Phi + phi' = Phi- pi
% out:
%    w0     angular-dependent weighting
%    t      computation time
%
% Copyright 2005-4-07, Yingying Zhang, The University of Michigan

all = logical(0);
if ~isempty(varargin)
    arg = varargin{1};
    if isempty(arg)
			%	do nothing for empty arguments

	elseif ~ischar(arg)
        error 'unknown non-string argument?'
    elseif streq(arg, 'all')
        all = logical(1);
    else
        error 'unknown string argument?'
    end
end

% default argument
ob.Df = 0; % 3rd generation CT scanner

% image domain pixel locations
% only compute within mask

yflip = (-1); % since the top is positive in y direction %
% x = ([1:ob.nx] - (ob.nx+1)/2)* ob.dx;
% y = ([1:ob.ny] - (ob.ny+1)/2)* ob.dx * yflip;

x = ([0:ob.nx-1] - ob.nx/2)* ob.dx;
y = ([0:ob.ny-1] - ob.ny/2)* ob.dx * yflip;

[x y] = ndgrid(x, y);

rr = sqrt(x.^2 + y.^2);
theta = atan2(y,x); % use it for our purpose since atan will have pi flip

r0 = rr(mask(:));
phi0 = theta(mask(:));

clear rr theta

% dummy variables in freq. domain
phi = Phi;

% if ~isempty(varargin) & isnumeric(varargin{1})
%     phi = varargin{1};
% end

% sinogram domain locations
ob.beta = ([0:ob.na-1]'/ob.na * ob.orbit ...
    + ob.orbit_start) * pi / 180;
ob.s = ([-(ob.nb-1)/2:(ob.nb-1)/2]' ...
			- ob.offset_s) * ob.ds;
ob.gamma = ob.s / ob.dis_src_det;
[ss, beta] = meshgrid(ob.s,ob.beta); % meshgrid and ndgrid reverse

% loop over \phi_i
Dcf = ob.dis_src_det + ob.Df;
ratio_fcf = ob.Df/Dcf;
% w0 = zeros(ob.nx, ob.ny, ob.na);
w0 = zeros([size(r0,1) length(phi)]);

tic
for ia = 1:length(phi)
    % -------------------------
    % for \phi' = \phi
    % -------------------------

    % compute new variables in (3.5.2)
    r_p = r0 .* cos(phi(ia) - phi0); % use ob.beta, not outer_sum(ob.gamma, ob.beta)
%                                         since evaluate at \phi_p = \phi(view angle)
    s_p = Dcf.*(asin(r_p./ob.dis_src_iso) - asin(ratio_fcf.*r_p./ob.dis_src_iso));
    beta_p = phi(ia) - asin(r_p./ob.dis_src_iso);

    % compute Jacobian determinant (2.8.9)
    Jacob = ob.dis_src_det ./ sqrt(ob.dis_src_iso.^2 - r_p.^2);

%     % find corresponding wi: nearest neighbor method
%     s_cen = (ob.nb - 1)/2 + ob.offset_s; % for nb = even % <-----(yy) plus:(to me account for the right index) or (hugo) minus?
%     s_loc = round(s_p./ob.ds + s_cen);
%     s_loc = min(s_loc, ob.nb);
%     s_loc = max(s_loc, 1);
%     beta_loc = round((mod(beta_p,2*pi)-ob.orbit_start)./(2*pi/ob.na)) + 1;
%     beta_loc = min(beta_loc, ob.na);
%     beta_loc = max(beta_loc, 1);
%
%     % find the corresponding index in column-stack wi
%     loc = 1 + (s_loc(:)-1) + (beta_loc(:)-1)*ob.nb;

    % compute w0 at \phi' = \phi
%     w0(:,ia) =wi(loc(:)) .* Jacob;
    si = min(s_p, ob.s(end)); si = max(si, ob.s(1));
    betai = min(beta_p, ob.beta(end)); betai = max(betai, ob.beta(1));
    w0(:,ia) = interp2(ss, beta, wi.', si(:), betai(:)) .* Jacob;

    clear s_loc beta_loc loc
    % -------------------------
    % for \phi' = \phi - pi
    % -------------------------
    if all
        phi_pi = phi(ia) - pi;
        r_p = r0 .* cos(phi_pi - phi0);
        s_p = Dcf.*(asin(r_p./ob.dis_src_iso) - asin(ratio_fcf.*r_p./ob.dis_src_iso));
        beta_p = phi_pi - asin(r_p./ob.dis_src_iso);

% %         Jacob_p again: not needed since same
%         Jacob_p = abs(ob.dis_src_iso*cos(s_p./ob.dis_src_det) - ...
%             ob.source_offset*sin(s_p./ob.dis_src_det))./ob.dis_src_det;
%
%         % find corresponding wi: nearest neighbor method
%         s_loc = round(s_p./ob.ds + s_cen);
%         s_loc = min(s_loc, ob.nb);
%         s_loc = max(s_loc, 1);
%
%         beta_loc = round((mod(beta_p,2*pi)-ob.orbit_start)./(2*pi/ob.na)) + 1;
%         beta_loc = min(beta_loc, ob.na);
%         beta_loc = max(beta_loc, 1);
%
%         % find the corresponding index in column-stack wi
%         loc = 1 + (s_loc(:)-1) + (beta_loc(:)-1)*ob.nb;
%
%         % compute w0 at \phi' = \phi - pi and sum with \phi' = \phi
%         w0(:,ia) = w0(:,ia) + wi(loc(:)).*Jacob;

        si = min(s_p, ob.s(end)); si = max(si, ob.s(1));
        betai = min(beta_p, ob.beta(end)); betai = max(betai, ob.beta(1));
        w0(:,ia) = w0(:,ia) + interp2(ss, beta, wi.', si(:), betai(:)) .* Jacob;
        clear s_loc beta_loc loc
    end

%     % angle_dependent blur
%     b0(:,:,ia) = (ob.ray_spacing./Dcf) .* sqrt(ob.dis_src_iso^2 + r0^2 + 2 .* ob.dis_src_iso .* Dcf .* cos(phi(ia)-phi0));
end

t = toc;
sprintf('angular weighting computation time %.4f', t)


classdef DNSGAII < ALGORITHM
 % <multi> <real/integer/label/binary/permutation> <constrained/none> <dynamic>
 % Dynamic NSGA-II
 % type ---   1 --- 1. Mutation based reinitialization 2. Random reinitialization
 % zeta --- 0.2 --- Ratio of reinitialized solutions

%------------------------------- Reference --------------------------------
% K. Deb, U. Bhaskara Rao N., and S. Karthik, Dynamic multi-objective
% optimization and decision-making using modified NSGA-II: A case study on
% hydro-thermal power scheduling, Proceedings of the International
% Conference on Evolutionary Multi-Criterion Optimization, 2007, 803-817.
%------------------------------- 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
        function main(Algorithm,Problem)
            %% Parameter setting
            [type,zeta] = Algorithm.ParameterSet(1,0.2);
            % Reset the number of saved populations (only for dynamic optimization)
            Algorithm.save = sign(Algorithm.save)*inf;

            %% Generate random population
            Population = Problem.Initialization();
            [~,FrontNo,CrowdDis] = EnvironmentalSelection(Population,Problem.N);
            % Archive for storing all populations before each change
            AllPop = [];

            %% Optimization
            while Algorithm.NotTerminated(Population)
                if Changed(Problem,Population)
                    % Save the population before the change
                    AllPop = [AllPop,Population];
                    % React to the change
                    [Population,FrontNo,CrowdDis] = Reinitialization(Problem,Population,type,zeta);
                end
                MatingPool = TournamentSelection(2,Problem.N,FrontNo,-CrowdDis);
                Offspring  = OperatorGA(Problem,Population(MatingPool));
                [Population,FrontNo,CrowdDis] = EnvironmentalSelection([Population,Offspring],Problem.N);
                if Problem.FE >= Problem.maxFE
                    % Return all populations
                    Population = [AllPop,Population];
                    [~,rank]   = sort(Population.adds(zeros(length(Population),1)));
                    Population = Population(rank);
                end
            end
        end
    end
end


function spm_slice_timing(P, sliceorder, refslice, timing, prefix)
% Correct differences in slice acquisition times
% FORMAT spm_slice_timing(P, sliceorder, refslice, timing, prefix)
% P           - char array of image filenames
%               can also be a cell array of the above (multiple subjects).
% sliceorder  - slice acquisition order, a vector of integers, each
%               integer referring the slice number in the image file
%               (1=first), and the order of integers representing their
%               temporal acquisition order
%               OR vector containig the acquisition time for each slice
%               in milliseconds
% refslice    - slice for time 0
%               OR time in milliseconds for the reference slice
% timing      - additional information for sequence timing
%               timing(1) = time between slices
%                         = TA / (nslices - 1)
%               timing(2) = time between last slices and next volume
%                         = TR - TA
%               OR timing = [0 TR] when previous inputs are specified in
%               milliseconds
% prefix      - filename prefix for corrected image files, defaults to 'a'
%__________________________________________________________________________
%
%   Note: The sliceorder arg that specifies slice acquisition order is
%   a vector of N numbers, where N is the number of slices per volume.
%   Each number refers to the position of a slice within the image file.
%   The order of numbers within the vector is the temporal order in which
%   those slices were acquired.
%
%   To check the order of slices within an image file, use the SPM Display
%   option and move the crosshairs to a voxel co-ordinate of z=1.  This
%   corresponds to a point in the first slice of the volume.
%
%   The function corrects differences in slice acquisition times.
%   This routine is intended to correct for the staggered order of
%   slice acquisition that is used during echoplanar scanning. The
%   correction is necessary to make the data on each slice correspond
%   to the same point in time. Without correction, the data on one
%   slice will represent a point in time as far removed as 1/2 the TR
%   from an adjacent slice (in the case of an interleaved sequence).
%
%   This routine "shifts" a signal in time to provide an output
%   vector that represents the same (continuous) signal sampled
%   starting either later or earlier. This is accomplished by a simple
%   shift of the phase of the sines that make up the signal.
%
%   Recall that a Fourier transform allows for a representation of any
%   signal as the linear combination of sinusoids of different
%   frequencies and phases. Effectively, we will add a constant
%   to the phase of every frequency, shifting the data in time.
%
%   Shifter - This is the filter by which the signal will be convolved
%   to introduce the phase shift. It is constructed explicitly in
%   the Fourier domain. In the time domain, it may be described as
%   an impulse (delta function) that has been shifted in time the
%   amount described by TimeShift.
%
%   The correction works by lagging (shifting forward) the time-series
%   data on each slice using sinc-interpolation. This results in each
%   time series having the values that would have been obtained had
%   the slice been acquired at the same time as the reference slice.
%
%   To make this clear, consider a neural event (and ensuing hemodynamic
%   response) that occurs simultaneously on two adjacent slices. Values
%   from slice "A" are acquired starting at time zero, simultaneous to
%   the neural event, while values from slice "B" are acquired one
%   second later. Without corection, the "B" values will describe a
%   hemodynamic response that will appear to have began one second
%   EARLIER on the "B" slice than on slice "A". To correct for this,
%   the "B" values need to be shifted towards the Right, i.e., towards
%   the last value.
%
% Written by Darren Gitelman at Northwestern U., 1998
%
% Based (in large part) on ACQCORRECT.PRO from G. Aguirre and E. Zarahn
% at U. Penn.
%
% Modified by R. Henson, C. Buechel and J. Ashburner, FIL, to
% handle different reference slices and memory mapping.
%
% Modified by M. Erb, at U. Tuebingen, 1999, to ask for non-continuous
% slice timing and number of sessions.
%
% Modified by R. Henson for more general slice order and SPM2.
%
% Modified by A. Hoffmann, M. Woletz and C. Windischberger from Medical
% University of Vienna, Austria, to handle multi-band EPI sequences.
%__________________________________________________________________________
% Copyright (C) 1998-2014 Wellcome Trust Centre for Neuroimaging

% Darren Gitelman et al.
% $Id: spm_slice_timing.m 6130 2014-08-01 17:41:18Z guillaume $

SVNid = '$Rev: 6130 $';

%-Say hello
%--------------------------------------------------------------------------
SPMid = spm('FnBanner',mfilename,SVNid);

%-Parameters & Arguments
%==========================================================================
if nargin < 4, error('Not enough input arguments.'); end
if nargin < 5, prefix = 'a'; end

if ~iscell(P), P = {P}; end
nsubjects = numel(P);

% Acquisition order: 1=first slice in image
% Reference slice: 1=first slice in image, in Analyze format, slice 1 = bottom
% TR: Interscan interval (TR) {secs}
% TA: Acquisition Time (TA) {secs} [Def: TR-TR/nslices], TA <= TR
% timing(2) = TR - TA, time between last slices and next volume
% timing(1) = TA / (nslices -1), time between slices

Vin     = spm_vol(P{1}(1,:));
nslices = Vin(1).dim(3);

TR      = (nslices-1)*timing(1)+timing(2);
fprintf('%-40s: %30s\n','Number of slices is...',num2str(nslices))      %-#
fprintf('%-40s: %30s\n','Time to Repeat (TR) is...',num2str(TR))        %-#

if ~isequal(1:nslices,sort(sliceorder))
    if ~all(sliceorder >= 0 & sliceorder <= TR*1000)
        error('Input is neither slice indices nor slice times.');
    end
    unit = 'slice times (ms)';
else
    if ~ismember(refslice,sliceorder)
        error('Reference slice should contain a slice index.');
    end
    unit = 'slice indices';
end
fprintf('%-40s: %30s\n','Parameters are specified as...',unit)          %-#

if nslices ~= numel(sliceorder)
    error('Mismatch between number of slices and length of ''sliceorder'' vector.');
end

%-Slice timing correction
%==========================================================================
for subj = 1:nsubjects
    Vin   = spm_vol(P{subj});
    nimgo = numel(Vin);
    nimg  = 2^(floor(log2(nimgo))+1);
    if Vin(1).dim(3) ~= nslices
        error('Number of slices differ: %d vs %d.', nslices, Vin(1).dim(3));
    end

    % Create new header files
    Vout  = Vin;
    for k=1:nimgo
        Vout(k).fname  = spm_file(Vin(k).fname, 'prefix', prefix);
        if isfield(Vout(k),'descrip')
            desc = [Vout(k).descrip ' '];
        else
            desc = '';
        end
        Vout(k).descrip = [desc 'acq-fix ref-slice ' num2str(refslice)];
    end
    Vout = spm_create_vol(Vout);

    % Set up [time x voxels] matrix for holding image info
    slices = zeros([Vout(1).dim(1:2) nimgo]);
    stack  = zeros([nimg Vout(1).dim(1)]);

    task = sprintf('Correcting acquisition delay: session %d', subj);
    spm_progress_bar('Init',nslices,task,'planes complete');

    % Compute shifting amount from reference slice and slice order
    if isequal(unit,'slice times (ms)')
        % Compute time difference between the acquisition time of the
        % reference slice and the current slice by using slice times
        % supplied in sliceorder vector
        shiftamount = (sliceorder - refslice) / (1000 * TR);
    else
        rslice      = find(sliceorder==refslice);
        [Y, I]      = sort(sliceorder);
        shiftamount = (I - rslice) * timing(1) / TR;
    end

    % For loop to perform correction slice by slice
    for k = 1:nslices

        % Read in slice data
        B  = spm_matrix([0 0 k]);
        for m=1:nimgo
            slices(:,:,m) = spm_slice_vol(Vin(m),B,Vin(1).dim(1:2),1);
        end

        % Set up shifting variables
        len     = size(stack,1);
        phi     = zeros(1,len);

        % Check if signal is odd or even -- impacts how Phi is reflected
        %  across the Nyquist frequency. Opposite to use in pvwave.
        OffSet  = 0;
        if rem(len,2) ~= 0, OffSet = 1; end

        % Phi represents a range of phases up to the Nyquist frequency
        % Shifted phi 1 to right.
        for f = 1:len/2
            phi(f+1) = -1*shiftamount(k)*2*pi/(len/f);
        end

        % Mirror phi about the center
        % 1 is added on both sides to reflect Matlab's 1 based indices
        % Offset is opposite to program in pvwave again because indices are 1 based
        phi(len/2+1+1-OffSet:len) = -fliplr(phi(1+1:len/2+OffSet));

        % Transform phi to the frequency domain and take the complex transpose
        shifter = [cos(phi) + sin(phi)*sqrt(-1)].';
        shifter = shifter(:,ones(size(stack,2),1)); % Tony's trick

        % Loop over columns
        for i=1:Vout(1).dim(2)

            % Extract columns from slices
            stack(1:nimgo,:) = reshape(slices(:,i,:),[Vout(1).dim(1) nimgo])';

            % Fill in continous function to avoid edge effects
            for g=1:size(stack,2)
                stack(nimgo+1:end,g) = linspace(stack(nimgo,g),...
                    stack(1,g),nimg-nimgo)';
            end

            % Shift the columns
            stack = real(ifft(fft(stack,[],1).*shifter,[],1));

            % Re-insert shifted columns
            slices(:,i,:) = reshape(stack(1:nimgo,:)',[Vout(1).dim(1) 1 nimgo]);
        end

        % Write out the slice for all volumes
        for p = 1:nimgo
            Vout(p) = spm_write_plane(Vout(p),slices(:,:,p),k);
        end
        spm_progress_bar('Set',k);
    end
    spm_progress_bar('Clear');
end

fprintf('%-40s: %30s\n','Completed',spm('time'))                        %-#


function x = r83p_sl ( n, a_lu, b, job, work2, work3, work4 )

%*****************************************************************************80
%
%% R83P_SL solves a R83P system.
%
%  Discussion:
%
%    The R83P storage format stores a periodic tridiagonal matrix as
%    a 3 by N array, in which each row corresponds to a diagonal, and
%    column locations are preserved.  The matrix value
%    A(1,N) is stored as the array entry A(3,N), and the matrix value
%    A(N,1) is stored as the array entry A(1,1).
%
%    The linear system must have been factored by R83P_FA.
%
%  Example:
%
%    Here is how a R83P matrix of order 5 would be stored:
%
%      A51 A12 A23 A34 A45
%      A11 A22 A33 A44 A55
%      A21 A32 A43 A54 A15
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 March 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%    N must be at least 3.
%
%    Input, real A_LU(3,N), the LU factors from R83P_FA.
%
%    Input, real B(N), the right hand side of the linear system.
%
%    Input, integer JOB, specifies the system to solve.
%    0, solve A * x = b.
%    nonzero, solve A' * x = b.
%
%    Input, real WORK2(N-1), WORK3(N-1), WORK4, factor data from R83P_FA.
%
%    Output, real X(N), the solution to the linear system.
%
  x(1:n) = b(1:n);

  if ( job == 0 )
%
%  Solve A1 * X1 = B1.
%
    x(1:n-1) = r83_np_sl ( n-1, a_lu, x, job );
%
%  X2 = B2 - A3 * X1
%
    x(n) = x(n) - a_lu(1,1) * x(1) - a_lu(3,n-1) * x(n-1);
%
%  Solve A4 * X2 = X2
%
    x(n) = x(n) / work4;
%
%  X1 := X1 - inverse ( A1 ) * A2 * X2.
%
    x(1:n-1) = x(1:n-1) - work2(1:n-1) * x(n);

  else
%
%  Solve A1' * X1 = B1.
%
    x(1:n-1) = r83_np_sl ( n-1, a_lu, x, job );
%
%  X2 := X2 - A2' * B1
%
    x(n) = x(n) - a_lu(3,n) * x(1) - a_lu(1,n) * x(n-1);
%
%  Solve A4 * X2 = X2.
%
    x(n) = x(n) / work4;
%
%  X1 := X1 - transpose ( inverse ( A1 ) * A3 ) * X2.
%
    x(1:n-1) = x(1:n-1) - work3(1:n-1) * x(n);

  end

  return
end


function [matching, objective] = greedy_matching_rowwise(iou_matrix)
  assert(size(iou_matrix, 1) <= size(iou_matrix, 2));
  n = size(iou_matrix, 1);
  matching = zeros(n, 1);
  objective = 0;
  for i = 1:n
    % find max element int matrix
 % [max_per_row, max_col_per_row] = max(iou_matrix, [], 2);
%       size(max_per_row)
%       size( max_col_per_row)
   [max_per_row, max_col_per_row] = max(iou_matrix')
    [max_iou,row] = max(max_per_row);
    if max_iou == -inf
      break
    end

    objective = objective + max_iou;
    col = max_col_per_row(row);
    matching(row) = col;
    iou_matrix(row,:) = -inf;
    iou_matrix(:,col) = -inf;
  end
end


function F = acotd(F, varargin)
%ACOTD   Inverse cotangent of a CHEBFUN, result in degrees.
%   ACOTD(F) computes the inverse cotangent (in degrees) of the CHEBFUN F.
%
%   ACOTD(F, PREF) does the same but uses the CHEBFUNPREF object PREF when
%   computing the composition.
%
% See also COTD, ACOT.

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

% Call the compose method:
F = compose(F, @acotd, varargin{:});

end


function [ value, x ] = randlc_jump ( x, k )

%*****************************************************************************80
%
%% RANDLC_JUMP returns the K-th element of a uniform pseudorandom sequence.
%
%  Discussion:
%
%    The sequence uses the linear congruential generator:
%
%      X(K+1) = A * X(K)  mod 2^46
%
%    The K-th element, which can be represented as
%
%      X(K) = A^K * X(0)  mod 2^46
%
%    is computed directly using the binary algorithm for exponentiation.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 March 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    David Bailey, Eric Barszcz, John Barton, D Browning, Robert Carter,
%    Leonardo Dagum, Rod Fatoohi,
%    Samuel Fineberg, Paul Frederickson, Thomas Lasinski, Robert Schreiber,
%    Horst Simon, V Venkatakrishnan, Sisira Weeratunga,
%    The NAS Parallel Benchmarks,
%    RNR Technical Report RNR-94-007,
%    March 1994.
%
%    Donald Knuth,
%    The Art of Computer Programming,
%    Volume 2, Seminumerical Algorithms,
%    Third Edition,
%    Addison Wesley, 1997,
%    ISBN: 0201896842,
%    LC: QA76.6.K64.
%
%  Parameters:
%
%    Input, uint64 X, the initial seed (with index 0).
%
%    Input, integer K, the index of the desired value.
%
%    Output, real VALUE, the K-th value in the sequence.
%
%    Output, uint64 X, the K-th seed.
%
  persistent a;
  persistent a1;
  persistent a2;
  persistent ks;
  persistent r23;
  persistent r46;
  persistent t23;
  persistent t46;
%
%  If this is the first call, compute
%
%    R23 = 2 ^ -23,
%    R46 = 2 ^ -46,
%    T23 = 2 ^ 23,
%    T46 = 2 ^ 46.
%
%  These are computed in loops, rather than by merely using the power operator,
%  in order to insure that the results are exact on all systems.
%
  if ( isempty ( ks ) )

    r23 = 1.0;
    r46 = 1.0;
    t23 = 1.0;
    t46 = 1.0;

    for i = 1 : 23
      r23 = 0.5 * r23;
      t23 = 2.0 * t23;
    end

    for i = 1 : 46
      r46 = 0.5 * r46;
      t46 = 2.0 * t46;
    end

    a = 1220703125.0;
%
%  Break A into two parts such that A = 2^23 * A1 + A2.
%
    t1 = r23 * a;
    a1 = floor ( t1 );
    a2 = a - t23 * a1;

    ks = 1;

  end

  if ( k < 0 )

    fprintf ( 1, '\n' );
    fprintf ( 1, 'RANDLC_JUMP - Fatal error!\n' );
    fprintf ( 1, '  K < 0.\n' );
    error ( 'RANDLC_JUMP - Fatal error!' )

  elseif ( k == 0 )

%
%  Find M so that K < 2^M.
%
  else

    m = 1;
    twom = 2;
    while ( twom <= k )
      twom = twom * 2;
      m = m + 1;
    end

    b = a;
    b1 = a1;
    b2 = a2;

    for i = 1 : m

      j = floor ( k / 2 );
%
%  Replace X by A * X, if appropriate.
%
      if ( 2 * j ~= k )

        t1 = r23 * x;
        x1 = floor ( t1 );
        x2 = x - t23 * x1;

        t1 = b1 * x2 + b2 * x1;
        t2 = floor ( r23 * t1 );
        z = t1 - t23 * t2;

        t3 = t23 * z + b2 * x2;
        t4 = floor ( r46 * t3 );
        x = t3 - t46 * t4;

      end
%
%  Replace A by A * A mod 2^46.
%
      t1 = r23 * b;
      x1 = floor ( t1 );
      x2 = b - t23 * x1;

      t1 = b1 * x2 + b2 * x1;
      t2 = floor ( r23 * t1 );
      z = t1 - t23 * t2;

      t3 = t23 * z + b2 * x2;
      t4 = floor ( r46 * t3 );
      b = t3 - t46 * t4;
%
%  Update A1, A2.
%
      t1 = r23 * b;
      b1 = floor ( t1 );
      b2 = b - t23 * b1;

      k = j;

    end

  end

  value = r46 * x;

  return
end


function [A, C, Q, R, initx, initV, LL] = ...
    learn_kalman(data, A, C, Q, R, initx, initV, max_iter, diagQ, diagR, ARmode, constr_fun, varargin)
% LEARN_KALMAN Find the ML parameters of a stochastic Linear Dynamical System using EM.
%
% [A, C, Q, R, INITX, INITV, LL] = LEARN_KALMAN(DATA, A0, C0, Q0, R0, INITX0, INITV0) fits
% the parameters which are defined as follows
%   x(t+1) = A*x(t) + w(t),  w ~ N(0, Q),  x(0) ~ N(init_x, init_V)
%   y(t)   = C*x(t) + v(t),  v ~ N(0, R)
% A0 is the initial value, A is the final value, etc.
% DATA(:,t,l) is the observation vector at time t for sequence l. If the sequences are of
% different lengths, you can pass in a cell array, so DATA{l} is an O*T matrix.
% LL is the "learning curve": a vector of the log lik. values at each iteration.
% LL might go positive, since prob. densities can exceed 1, although this probably
% indicates that something has gone wrong e.g., a variance has collapsed to 0.
%
% There are several optional arguments, that should be passed in the following order.
% LEARN_KALMAN(DATA, A0, C0, Q0, R0, INITX0, INITV0, MAX_ITER, DIAGQ, DIAGR, ARmode)
% MAX_ITER specifies the maximum number of EM iterations (default 10).
% DIAGQ=1 specifies that the Q matrix should be diagonal. (Default 0).
% DIAGR=1 specifies that the R matrix should also be diagonal. (Default 0).
% ARMODE=1 specifies that C=I, R=0. i.e., a Gauss-Markov process. (Default 0).
% This problem has a global MLE. Hence the initial parameter values are not important.
%
% LEARN_KALMAN(DATA, A0, C0, Q0, R0, INITX0, INITV0, MAX_ITER, DIAGQ, DIAGR, F, P1, P2, ...)
% calls [A,C,Q,R,initx,initV] = f(A,C,Q,R,initx,initV,P1,P2,...) after every M step. f can be
% used to enforce any constraints on the params.
%
% For details, see
% - Ghahramani and Hinton, "Parameter Estimation for LDS", U. Toronto tech. report, 1996
% - Digalakis, Rohlicek and Ostendorf, "ML Estimation of a stochastic linear system with the EM
%      algorithm and its application to speech recognition",
%       IEEE Trans. Speech and Audio Proc., 1(4):431--442, 1993.

%    learn_kalman(data, A, C, Q, R, initx, initV, max_iter, diagQ, diagR, ARmode, constr_fun, varargin)
if nargin < 8, max_iter = 10; end
if nargin < 9, diagQ = 0; end
if nargin < 10, diagR = 0; end
if nargin < 11, ARmode = 0; end
if nargin < 12, constr_fun = []; end
verbose = 1;
thresh = 1e-4;

if ~iscell(data)
  N = size(data, 3);
  data = num2cell(data, [1 2]); % each elt of the 3rd dim gets its own cell
else
  N = length(data);
end

N = length(data);
ss = size(A, 1);
os = size(C,1);

alpha = zeros(os, os);
Tsum = 0;
for ex = 1:N
  %y = data(:,:,ex);
  y = data{ex};
  T = length(y);
  Tsum = Tsum + T;
  alpha_temp = zeros(os, os);
  for t=1:T
    alpha_temp = alpha_temp + y(:,t)*y(:,t)';
  end
  alpha = alpha + alpha_temp;
end

previous_loglik = -inf;
loglik = 0;
converged = 0;
num_iter = 1;
LL = [];

% Convert to inline function as needed.
if ~isempty(constr_fun)
  constr_fun = fcnchk(constr_fun,length(varargin));
end

while ~converged & (num_iter <= max_iter)

  %%% E step

  delta = zeros(os, ss);
  gamma = zeros(ss, ss);
  gamma1 = zeros(ss, ss);
  gamma2 = zeros(ss, ss);
  beta = zeros(ss, ss);
  P1sum = zeros(ss, ss);
  x1sum = zeros(ss, 1);
  loglik = 0;

  for ex = 1:N
    y = data{ex};
    T = length(y);
    [beta_t, gamma_t, delta_t, gamma1_t, gamma2_t, x1, V1, loglik_t] = ...
	Estep(y, A, C, Q, R, initx, initV, ARmode);
    beta = beta + beta_t;
    gamma = gamma + gamma_t;
    delta = delta + delta_t;
    gamma1 = gamma1 + gamma1_t;
    gamma2 = gamma2 + gamma2_t;
    P1sum = P1sum + V1 + x1*x1';
    x1sum = x1sum + x1;
    %fprintf(1, 'example %d, ll/T %5.3f\n', ex, loglik_t/T);
    loglik = loglik + loglik_t;
  end
  LL = [LL loglik];
  if verbose, fprintf(1, 'iteration %d, loglik = %f\n', num_iter, loglik); end
  %fprintf(1, 'iteration %d, loglik/NT = %f\n', num_iter, loglik/Tsum);
  num_iter =  num_iter + 1;

  %%% M step

  % Tsum =  N*T
  % Tsum1 = N*(T-1);
  Tsum1 = Tsum - N;
  A = beta * inv(gamma1);
  %A = (gamma1' \ beta')';
  Q = (gamma2 - A*beta') / Tsum1;
  if diagQ
    Q = diag(diag(Q));
  end
  if ~ARmode
    C = delta * inv(gamma);
    %C = (gamma' \ delta')';
    R = (alpha - C*delta') / Tsum;
    if diagR
      R = diag(diag(R));
    end
  end
  initx = x1sum / N;
  initV = P1sum/N - initx*initx';

  if ~isempty(constr_fun)
    [A,C,Q,R,initx,initV] = feval(constr_fun, A, C, Q, R, initx, initV, varargin{:});
  end

  converged = em_converged(loglik, previous_loglik, thresh);
  previous_loglik = loglik;
end

%%%%%%%%%

function [beta, gamma, delta, gamma1, gamma2, x1, V1, loglik] = ...
    Estep(y, A, C, Q, R, initx, initV, ARmode)
%
% Compute the (expected) sufficient statistics for a single Kalman filter sequence.
%

[os T] = size(y);
ss = length(A);

if ARmode
  xsmooth = y;
  Vsmooth = zeros(ss, ss, T); % no uncertainty about the hidden states
  VVsmooth = zeros(ss, ss, T);
  loglik = 0;
else
  [xsmooth, Vsmooth, VVsmooth, loglik] = kalman_smoother(y, A, C, Q, R, initx, initV);
end

delta = zeros(os, ss);
gamma = zeros(ss, ss);
beta = zeros(ss, ss);
for t=1:T
  delta = delta + y(:,t)*xsmooth(:,t)';
  gamma = gamma + xsmooth(:,t)*xsmooth(:,t)' + Vsmooth(:,:,t);
  if t>1 beta = beta + xsmooth(:,t)*xsmooth(:,t-1)' + VVsmooth(:,:,t); end
end
gamma1 = gamma - xsmooth(:,T)*xsmooth(:,T)' - Vsmooth(:,:,T);
gamma2 = gamma - xsmooth(:,1)*xsmooth(:,1)' - Vsmooth(:,:,1);

x1 = xsmooth(:,1);
V1 = Vsmooth(:,:,1);


function [gains]=lookup_gain_in_table(G,a_post,a_priori,a_post_range,a_priori_range,step);
% function [gains]=lookup_gain_in_table(G,a_post,a_priori,a_post_range,a_priori_range,step);
% This function selects the right gain value from the table G, given
% vectors with a priori and a posteriori SNRs
%
% INPUT variables:
% G: Matrix with gain values for speech DFT or magnitude estimation,
% evaluated at all combinations of a priori and a posteriori SNR in the
% input variables Rksi and Rgam.
%
%
% a_priori: Array of "a priori" SNR (SNRprior) values for which values
% have to be selected from the gain table   NOTE: The values must be in dBs.
% a_post: Array of "a posteriori" SNR (SNRpost) values for which values
% have to be selected from the gain table  NOTE: The values must be in dBs.
%
% a_post_range: The range of "a posteriori" SNR values
%
% a_priori_range: The range of "a priori" SNR values
%
% step: step is the stepsize in db's in the table
%
% OUTPUT variables:
% gains: Matrix with gain values that are selected from the gain table G
%
%
% Copyright 2007: Delft University of Technology, Information and
% Communication Theory Group. The software is free for non-commercial use.
% This program comes WITHOUT ANY WARRANTY.
%
% Last modified: 22-11-2007.

a_prioridb=round(10*log10(a_priori)/step)*step;
a_postdb=round(10*log10(a_post)/step)*step;
[Ia_post]=min(max(min(a_post_range),a_postdb), max(a_post_range));
Ia_post=Ia_post-min(a_post_range)+1;
Ia_post=Ia_post/step;
[Ia_priori]=min(max(min(a_priori_range),a_prioridb), max(a_priori_range));
Ia_priori=Ia_priori-min(a_priori_range)+1;
Ia_priori=Ia_priori/step;

gains=G(Ia_priori+(Ia_post-1)*length(G(:,1)));


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Author: Eugenio Alcala Baselga
% Date: 02/06/2018
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

classdef automatic_kinematic_control

    properties(Constant)
        V_vec           = [1 18];  % Case of State FEedback LPV kinematic control
%         V_vec           = [2 18]; % Case of MPC kinematic control

        Theta_err_vec   = [deg2rad(-5) deg2rad(5)];
        W_vec           = [-1.417 1.417];
        gamma = 0.01;
        Q               = [ 2    0     0;
                            0    3     0;
                            0    0    20];
        R               = [ 0.5   0;
                            0   0.001];
    end

    methods
    end

end


% Copyright (C) 2009 International Business Machines
% All Rights Reserved.
% This code is published under the Eclipse Public License.
%
% $Id: hs071_c.c 699 2006-04-05 21:05:18Z andreasw $
%
% Author:  Andreas Waechter               IBM    2009-04-02
%
% This file is part of the Ipopt tutorial.  It is a version with
% mistakes for the matlab implemention of the coding exercise problem
% (in AMPL formulation):
%
% param n := 4;
%
% var x {1..n} <= 0, >= -1.5, := -0.5;
%
% minimize obj:
%   sum{i in 1..n} (x[i]-1)^2;
%
% subject to constr {i in 2..n-1}:
%   (x[i]^2+1.5*x[i]-i/n)*cos(x[i+1]) - x[i-1] = 0;
%
% The constant term "i/n" in the constraint is supposed to be input data

function [x, info] = TutorialMatlab

  % Size of the problem
  n = 5;

  % Problem data
  a = ((1:n)/n)';

  % Starting point
  x0 = -0.5*ones(n,1);

  % Lower and upper bounds for the variables
  options.lb = -1.5*ones(n,1);
  options.ub = zeros(n,1);

  % Constraint bounds
  options.cl = zeros(n-2,1);
  options.cu = zeros(n-2,1);

  % Set the Ipopt options
  % options.ipopt.mu_strategy = 'adaptive';
  options.ipopt.derivative_test = 'second-order';
  options.ipopt.max_iter = 10;

  % Set the callback functions
  funcs.objective         = @eval_f;
  funcs.constraints       = @eval_g;
  funcs.gradient          = @eval_grad_f;
  funcs.jacobian          = @eval_jac_g;
  funcs.jacobianstructure = @eval_jac_g_struct;
  funcs.hessian           = @eval_hess;
  funcs.hessianstructure  = @eval_hess_struct;

  [x info] = ipopt(x0, funcs, options);

  % End of main function
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % Evaluate value of objective function
  function f = eval_f(x)

    tmp = (x - 1).^2;
    f = sum(tmp);

  end

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % Evaluate gradient of objective function
  function df = eval_grad_f(x)

    df = (x - 1);

  end

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % Evaluate value of constraint bodies
  function g = eval_g(x)

    g = (x(2:n-1).^2 + 1.5*x(1:n-2) - a(2:n-1)).*cos(x(3:n)) - x(1:n-2);

  end

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % Return constraint Jacobian strcture
  function A = eval_jac_g_struct

    % tri-diagonal structure
    Diags = ones(n, 3);
    A = spdiags(Diags, 0:2, n-2, n);

  end

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % Evaluate constraint Jacobian
  function A = eval_jac_g(x)

    Diags = -1*ones(n-2,3);
    Diags(:,2) = (2*x(2:n-1)+1.5).*cos(x(3:n));
    Diags(:,3) = -(x(2:n-1).^2 + 1.5*x(2:n-1) - a(2:n-1)).*sin(x(3:n));
    A = spdiags(Diags, 0:2, n-2, n);

  end

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % Return Hessian of Lagrangian function structure
  function H = eval_hess_struct

    Diags = ones(n, 2);
    H = spdiags(Diags, -1:0, n, n);

  end

  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  % Evaluate Hessian of Lagrangian function
  function H = eval_hess(x, sigma, lambda)

    Diags = zeros(n,2);

    % part from the objective function
    Diags(:,2) = sigma * 2;

    % (x_i , x_i) part
    Diags(2:n-1,2) = Diags(2:n-1,2) + lambda.*cos(x(3:n));

    % (x_{i+1}, x_{i+1}) part
    Diags(3:n,2) = Diags(3:n,2) ...
      - lambda.*(x(2:n-1).^2 + 1.5*x(1:n-2) - a(2:n-1)).*cos(x(3:n));

    % (x_i, x_{i+1}) part
    Diags(2:n-1,1) = -lambda.*sin(x(3:n)).*(2*x(2:n-1)+1.5);

    H = spdiags(Diags, -1:0, n, n);

  end

end


%% Copyright (C) 2016 Utkarsh Gautam
%%
%% This file is part of OctSymPy.
%%
%% OctSymPy is free software; you can redistribute it and/or modify
%% it under the terms of the GNU General Public License as published
%% by the Free Software Foundation; either version 3 of the License,
%% or (at your option) any later version.
%%
%% This software is distributed in the hope that it will be useful,
%% but WITHOUT ANY WARRANTY; without even the implied warranty
%% of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See
%% the GNU General Public License for more details.
%%
%% You should have received a copy of the GNU General Public
%% License along with this software; see the file COPYING.
%% If not, see <http://www.gnu.org/licenses/>.

%% -*- texinfo -*-
%% @documentencoding UTF-8
%% @defmethod @@sym besseljn (@var{alpha}, @var{x})
%% Symbolic Spherical Bessel function of the first kind.
%%
%% Example:
%% @example
%% @group
%% syms n x
%% A = besseljn(n, x)
%%   @result{} A = (sym) jn(n, x)
%% diff(A)
%%   @result{} ans = (sym)
%%
%%                      (n + 1)⋅jn(n, x)
%%       jn(n - 1, x) - ────────────────
%%                           x
%% @end group
%% @end example
%%
%% @seealso{@@sym/besselyn, @@sym/besselj}
%% @end defmethod

function Y = besseljn(n, x)
  if (nargin ~= 2)
    print_usage ();
  end
  Y = elementwise_op ('jn', sym(n), sym(x));
end

%!test
%! % roundtrip
%! syms x
%! A = double(besseljn(sym(2), sym(9)));
%! q = besseljn(sym(2), x);
%! h = function_handle(q);
%! B = h(9);
%! assert (abs (A - B) <= eps)

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


function pass = test_spherefun( pref )

% Grab some preferences
if ( nargin == 0 )
    pref = chebfunpref();
end
tol = 1e2*pref.techPrefs.chebfuneps;

% Example 1
f = ballfun(@(x,y,z)cos(x.*y));
g = spherefun(f);
h = spherefun(@(x,y,z)cos(x.*y));
pass(1) = norm(g-h) < tol;

% Example 2
f = ballfun(@(x,y,z)sin(z));
g = spherefun(f, 0.5);
h = spherefun(@(x,y,z)sin(0.5*z));
pass(2) = norm(g-h) < tol;

% Example 3
f = ballfun(@(x,y,z)x);
g = f(10,:,:,'spherical');
h = spherefun(@(x,y,z)10*x);
pass(3) = norm(g-h) < tol;

if (nargout > 0)
    pass = all(pass(:));
end
end


function [ i, j ] = i4mat_min_index ( m, n, a )

%*****************************************************************************80
%
%% I4MAT_MIN_INDEX returns the location of the minimum of an I4AMT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 November 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the number of rows in A.
%
%    Input, integer N, the number of columns in A.
%
%    Input, integer A(M,N), the M by N matrix.
%
%    Output, integer I, J, the indices of the minimum entry of A.
%
  i = -1;
  j = -1;

  for jj = 1 : n
    for ii = 1 : m
      if ( ii == 1 && jj == 1 )
        i = ii;
        j = jj;
      elseif ( a(ii,jj) < a(i,j) )
        i = ii;
        j = jj;
      end
    end
  end

  return
end


function [dxdB dxdD dxdS dxdalpha dxdgParam] = gpsimXGradient(model, i, j)

% GPSIMXGRADIENT ...
%
% COPYRIGHT : Pei Gao, 2008

% SHEFFIELDML

% i: ti
% j: jth gene

dxdB = 1./model.D(j);

endPoint = model.times_index(i);

dxdD = 0;
dxdS = 0;
dxdalpha = [];
dxdgParam = [];

if model.ngParam > 0
  ngParamk = model.ngParam/model.numGenes;
  dxdgParam = zeros(1, ngParamk);
  gInd = j;
else
  gInd = 1;
end

for m = 1:endPoint
    arg = model.t(i)-model.mapt(m);
    if arg >= 0
        dxdD = dxdD+model.g(m,gInd)*arg*exp(-model.D(j)*arg+log(model.step)+log(model.S(j)));
        dxdS = dxdS+exp(-model.D(j)*arg+log(model.step))*model.g(m,gInd); % g=Sf

        if model.ngParam > 0
          for gParamInd = 1:ngParamk
            dxdgParam(gParamInd) = dxdgParam(gParamInd) + exp(- ...
            model.D(j)*arg+log(model.step)+log(model.S(j)))*model.dg(m,gInd);
          end
        end
    end
end

dxdD = -model.B(j)/(model.D(j)*model.D(j))-dxdD;

% check if g(f) is different for each gene.
if isfield(model, 'isGroupNonlinearity') && strcmp(model.nonLinearity{j}, ...
                                                   'repression')
      dxdalpha = exp(-model.D(j)*model.t(i));
      dxdD = -model.t(i)*model.alpha(j)*exp(-model.D(j)*model.t(i))+dxdD;
end


%FINDHOMOGRAPHY  Finds a perspective transformation between two planes
%
%     H = cv.findHomography(srcPoints, dstPoints)
%     [H, mask] = cv.findHomography(...)
%     [...] = cv.findHomography(..., 'OptionName', optionValue, ...)
%
% ## Input
% * __srcPoints__ Coordinates of the points in the original plane, a numeric
%   array of size Nx2/1xNx2/Nx1x2 or cell array of 2-elements vectors
%   `{[x,y], ...}` (single floating-point precision).
% * __dstPoints__ Coordinates of the points in the target plane, of same size
%   and type as `srcPoints`.
%
% ## Output
% * __H__ 3x3 Homography matrix.
% * __mask__ Nx1 mask array of same length as input points, indicates inliers
%   (which points were actually used in the best computation of `H`).
%
% ## Options
% * __Method__ Method used to compute a homography matrix. The following
%   methods are possible:
%   * __0__ a regular method using all the points, i.e. the least squares
%     method (default)
%   * __Ransac__ RANSAC-based robust method.
%   * __LMedS__ Least-Median of squares robust method.
%   * __Rho__ PROSAC-based robust method, introduced in [Bazargani15].
%     (weighted RANSAC modification, faster in the case of many outliers).
% * __RansacReprojThreshold__ Maximum allowed reprojection error to treat a
%   point pair as an inlier (used in the RANSAC and RHO methods only). That
%   is, if
%   `|| dstPoints_i - convertPointsToHomogeneous(H*srcPoints_i) ||_2 > RansacReprojThreshold`
%   then the point `i` is considered as an outlier. If `srcPoints` and
%   `dstPoints` are measured in pixels, it usually makes sense to set this
%   parameter somewhere in the range of 1 to 10. default 3.0.
% * __MaxIters__ The maximum number of RANSAC iterations. default 2000
% * __Confidence__ Confidence level, between 0 and 1. default 0.995
%
% The function finds and returns the perspective transformation `H` between
% the source and the destination planes:
%
%     s_i * [x_i'; y_i'; 1] ~ H * [x_i; y_i; 1]
%
% so that the back-projection error:
%
%     sum_{i} (x_i' - (h11*x_i + h12*y_i + h13)/(h31*x_i + h32*y_i + h33))^2 +
%             (y_i' - (h21*x_i + h22*y_i + h23)/(h31*x_i + h32*y_i + h33))^2
%
% is minimized. If the parameter method is set to the default value 0, the
% function uses all the point pairs to compute an initial homography estimate
% with a simple least-squares scheme.
%
% However, if not all of the point pairs `(srcPoints_i, dstPoints_i)` fit the
% rigid perspective transformation (that is, there are some outliers), this
% initial estimate will be poor. In this case, you can use one of the three
% robust methods. The methods RANSAC, LMedS and RHO try many different
% random subsets of the corresponding point pairs (of four pairs each,
% collinear pairs are discarded), estimate the homography matrix using this
% subset and a simple least-squares algorithm, and then compute the
% quality/goodness of the computed homography (which is the number of inliers
% for RANSAC or the least median re-projection error for LMedS). The best
% subset is then used to produce the initial estimate of the homography matrix
% and the mask of inliers/outliers.
%
% Regardless of the method, robust or not, the computed homography matrix
% is refined further (using inliers only in case of a robust method) with
% the Levenberg-Marquardt method to reduce the re-projection error even
% more.
%
% The methods RANSAC and RHO handle practically any ratio of outliers but
% need a threshold to distinguish inliers from outliers. The method LMedS
% does not need any threshold but it works correctly only when there are
% more than 50% of inliers. Finally, if there are no outliers and the noise
% is rather small, use the default method (`Method=0`).
%
% The function is used to find initial intrinsic and extrinsic matrices.
% Homography matrix is determined up to a scale. Thus, it is normalized so
% that `h33 = 1`. Note that whenever an `H` matrix cannot be estimated, an
% empty one will be returned.
%
% ## References
% [Fischler81]:
% > Fischler, M. A., and R. C. Bolles. "Random sample consensus: A paradigm
% > for model fitting with applications to image analysis and automated
% > cartography", Communications of the Association for Computing Machinery
% > 24 (1981): 381-395.
%
% [Rousseeuw84]:
% > Rousseeuw, P. J. "Least median of squares regression", Journal of the
% > American Statistical Association, 79 (1984): 871-880.
%
% [Inui03]:
% > Inui, K., S. Kaneko, and S. Igarashi. "Robust Line Fitting using LMedS
% > Clustering", Systems and Computers in Japan 34 (2003): 92-100.
%
% [Bazargani15]:
% > Hamid Bazargani, Olexa Bilaniuk, and Robert Laganiere. "A fast and robust
% > homography scheme for real-time planar target detection". Journal of
% > Real-Time Image Processing (2015): 1-20.
%
% See also: cv.getAffineTransform, cv.estimateAffine2D,
% cv.estimateAffinePartial2D, cv.getPerspectiveTransform,
% cv.estimateRigidTransform, cv.warpPerspective, cv.perspectiveTransform
%


function S = PMSAFinit(w0,mu,N,L,H,F,alpha,delta)

% PMSAFinit        Initialize Parameter Structure for the PMSAF Algorithm
%
%                  Psuedo-QMF CMFB is Used by default, H and F are used otherwise
% Arguments:
% w0               Coefficients of FIR filter at start
% mu               Step size
% N                Number of subbands
% L                length of analysis filter
% M                length of adaptive weight vector
% H                Analysis filter bank (optional), each column represents a filter
% F                Synthesis filter bank (optional), each column represents a filter
% alpha            Adjust scaling of tap weights
% delta            Small constant

if nargin > 4
    if (size(H,2)~=N)||(size(F,2)~=N)
        error('Columns of H (%d) or F (%d) not match with N = %d',size(H,2),size(F,2),N);
    end
else
% Defualt filter bank: Pseudo-QMF CMFB

    [hopt,passedge] = opt_filter(L-1,N); % Generate prototype lowpass filter
    [H,F] = make_bank(hopt,N);           % Generate filter banks using cosine modulation
    H = sqrt(N)*H';                      % Analysis section
    F = sqrt(N)*F';                      % Synthesis section
end

if nargin < 7 || isempty(alpha),         % Use default
    alpha = 0;                           % Try -0.5
end

if nargin < 8 || isempty(delta),
    delta = 1e-4;                        % Use default
end

% Assign structure fields

S.coeffs        = w0(:);                 % Convert to column vector of length M
S.step          = mu;                    % Step size
S.analysis      = H;                     % Analysis filter
S.synthesis     = F;                     % Synthesis filter
S.iter          = 0;                     % Iteration count
S.alpha         = alpha;                 % Tap weight scaling
S.delta         = ones(1,N)*delta;       % Small positive constant
S.AdaptStart    = L + length(w0);        % Running effect of analysis and adaptive filter,
                                         %   minimum L+M

github-repos/MATLAB/CharlesThaCat-acoustic-interference-cancellation/acoustic-interference-cancellation-edb394499ea6f9c96445a3e9613bd64a854c289e/Subband processing/Book/After reading Chapter 5 -- Critically Sampled and Oversampled Subband Structures/PMSAFinit.m


classdef CEC2013_F2 < PROBLEM
% <single> <real> <large>
% Shifted Rastrigin's function

%------------------------------- Reference --------------------------------
% X. Li, K. Tang, M. N. Omidvar, Z. Yang, and K. Qin, Benchmark functions
% for the CEC'2013 special session and competition on large-scale global
% optimization, RMIT University, Australia, 2013.
%------------------------------- 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
        Xopt;	% Optimal decision vector
    end
    methods
        %% Default settings of the problem
        function Setting(obj)
            CallStack = dbstack('-completenames');
            load(fullfile(fileparts(CallStack(1).file),'CEC2013.mat'),'Data');
            obj.Xopt = Data{2};
            obj.M    = 1;
            obj.D    = 1000;
            obj.lower    = zeros(1,obj.D) - 5;
            obj.upper    = zeros(1,obj.D) + 5;
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,PopDec)
            PopObj = Rastrigin(Tdiag(Tasy(Tosz(PopDec-repmat(obj.Xopt,size(PopDec,1),1)),0.2),10));
        end
    end
end

function F = Rastrigin(X)
    F = sum(X.^2-10*cos(2*pi*X)+10,2);
end

function Z = Tosz(X)
    X1 = zeros(size(X));
    X1(X~=0) = log(abs(X(X~=0)));
    C1 = zeros(size(X)) + 5.5;
    C1(X>0) = 10;
    C2 = zeros(size(X)) + 3.1;
    C2(X>0) = 7.9;
    Z = sign(X).*exp(X1+0.049*(sin(C1.*X1)+sin(C2.*X1)));
end

function Z = Tasy(X,beta)
    Z = X.^(1+repmat(beta*linspace(0,1,size(X,2)),size(X,1),1).*sqrt(X));
    Z(X<=0) = X(X<=0);
end

function Z = Tdiag(X,alpha)
    Z = X.*repmat(sqrt(alpha).^linspace(0,1,size(X,2)),size(X,1),1);
end


function [data,units] = compute_min_wing_angle(trx,n)

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

  % signed minimum: left wing will be negative, right wing positive
  data{i} = min(-trx(fly).wing_anglel,trx(fly).wing_angler);

end
units = parseunits('rad');


% Test whether it is necessary to rotate VB PCA
function nc2010_vbpca_mlexperiment(flatW, rotate, ncomps)

if nargin < 3
  ncomps = 100;
end

randn('state', 1);
rand('state', 1);

% Movie lens data with 100.000 ratings
U = load('/share/bayes/data/jaakko/movielens/u.data');
%Y = sparse(U(:,1), U(:,2), U(:,3));

% Divide into training (90%) and test sets (10%)
m = max(U(:,1));
n = max(U(:,2));
testset = (rand(rows(U),1) < 0.1);
Ytrain = full(sparse(U(~testset,1), U(~testset,2), U(~testset,3),m,n));
Ytest = full(sparse(U(testset,1), U(testset,2), U(testset,3),m,n));
Ytrain(Ytrain==0) = nan;
Ytest(Ytest==0) = nan;

%size(Ytrain)

if flatW
  disp('Using non-hierarchical flat prior for W');
  stringW = 'flatW';
else
  disp('Using hierarchical prior for W');
  stringW = 'hierW';
end

filename = sprintf(['/home/jluttine/matlab/neurocomputing2010/' ...
                    'nc2010_vbpca_mlexperiment_%s_ncomps=%d_rotate=%d'], ...
                   stringW, ncomps, rotate);

results = vbpcamv(Ytrain, ncomps, 'testset', Ytest, 'maxiters', 1e7, ...
                  'rotate', rotate, 'startupdatehyper', 1, ...
                  'startrotate', 1, 'autosavetime', 1, ...
                  'autosavefile', filename, 'fixw', flatW);