function [gamma, loglik, marginals, marginalsT] = bk_ff_fb(prior, transmat, obslik, filter_only, hnodes, ns)
% BK_FF_FB Fully factored Boyen-Koller version of forwards-backwards
% [gamma, loglik, marginals, marginalsT] = bk_ff_hmm(prior, transmat, obslik, filter_only, hnodes, ns)

ss  = length(ns);
S = length(prior);
T = size(obslik, 2);
marginals = cell(ss,T);
marginalsT = cell(ss,T);
scale = zeros(1,T);
alpha = zeros(S, T);

transmat2 = transmat';
for t=1:T
  if t==1
    [alpha(:,t), scale(t)] = normalise(prior(:) .* obslik(:,t));
  else
    [alpha(:,t), scale(t)] = normalise((transmat2 * alpha(:,t-1)) .* obslik(:,t));
  end
  [marginals(:,t), marginalsT(:,t)] = project_joint_onto_marginals(alpha(:,t), hnodes, ns);
  alpha(:,t) = combine_marginals_into_joint(marginalsT(:,t), hnodes, ns);
  %fprintf('alpha t=%d\n', t);
  %celldisp(marginals(1:8,t))
end
loglik = sum(log(scale));

if filter_only
  gamma = alpha;
  return;
end

beta = zeros(S,T);
gamma = zeros(S,T);
t = T;
beta(:,t) = ones(S,1);
gamma(:,t) = normalise(alpha(:,t) .* beta(:,t));
[marginals(:,t), marginalsT(:,t)] = project_joint_onto_marginals(gamma(:,t), hnodes, ns);

for t=T-1:-1:1
  b = beta(:,t+1) .* obslik(:,t+1);
  beta(:,t) = normalise((transmat * b));
  [junk, tempT] = project_joint_onto_marginals(beta(:,t), hnodes, ns);
  beta(:,t) = combine_marginals_into_joint(tempT, hnodes, ns);
  %gamma(:,t) = normalise(alpha(:,t) .* beta(:,t));
  %[marginals(:,t), marginalsT(:,t)] = project_joint_onto_marginals(gamma(:,t), hnodes, ns);
end

gamma2 = zeros(S,T);
for t=T-1:-1:1
  b = beta(:,t+1) .* obslik(:,t+1);
  xi(:,:,t) = normalise((transmat .* (alpha(:,t) * b')));
  if t==T-1
    gamma2(:,T) = sum(xi(:,:,T-1), 1)';
  end
  gamma2(:,t) = sum(xi(:,:,t), 2);
  [marginals(:,t), marginalsT(:,t)] = project_joint_onto_marginals(gamma2(:,t), hnodes, ns);
end


function H = boxper(sequence,isplot,boxnumber)
%
% 'boxper' estimate the hurst parameter of a given sequence with modified
%     periodogram method.
%
% Inputs:
%     sequence: the input sequence for estimate
%     isplot: whether display the plot. without a plot if isplot equal to 0
% Outputs:
%     H: the estimated hurst coeffeient of the input sequence

%  Author: Chu Chen
%  Version 1.0,  03/10/2008
%  chen-chu@163.com
%

if nargin == 1
    boxnumber = 50;
    isplot = 0;
end

if nargin == 2
    boxnumber = 50;
end

if boxnumber < 30 || boxnumber > 100
     error('The input argument boxnumber must be a integer between [30,100]');
end

n = length(sequence);
Xk = fft(sequence);
P_origin = abs(Xk).^2/(2*pi*n);
P = P_origin(1:floor(n/2)+1);

cut_min = ceil(0.001*n/2);
M = floor(logspace(log10(cut_min),log10(0.1*n-cut_min),boxnumber+1));
M = unique(M);
N = length(M)-1;

x = zeros(1,N);
y = zeros(1,N);
for i = 1:N
    m1 = M(i) + cut_min;
    m2 = M(i+1) + cut_min;
    x(i) = log10((pi * (m2 - m1))/(n));
    y(i) = log10(sum(P(m1:m2))/(m2-m1+1));
end

X = x;
Y = y;
p1 = polyfit(X,Y,1);
Yfit = polyval(p1,X);
H = (1-(Yfit(end)-Yfit(1))/(X(end)-X(1)))/2;

if isplot ~= 0
    figure,clf,hold on;
    plot(x,y,'b.');
    plot(X,Yfit,'r-','LineWidth',2);
    xlabel('Log10(Frequency)'),ylabel('Log10(Periodogram)'),title('Boxed Periodogram Method');
end


function [idx, row, col] = extidx2original(idx, original_size, extended_size)
%EXTIDX2ORIGINAL Convert from extended indexes to original indexes
    [row, col] = ind2sub(extended_size,idx);

    offright = col > (extended_size(2) + original_size(2)) / 2;
    offleft = col < (extended_size(2) - original_size(2)) / 2 + 1;
    notoff = ~(offleft | offright);
    col(offright)=col(offright) - (extended_size(2) + original_size(2)) / 2;
    col(offleft) = col(offleft) + original_size(2) - (extended_size(2) - original_size(2)) / 2;
    col(notoff)=col(notoff) - (extended_size(2) - original_size(2)) / 2;

    idx = sub2ind(original_size,row,col);
end


function [q1New,q2New,dq1New,dq2New] = autoGen_heelStrike(q1,q2,dq1,dq2,m,I,d,l)
%AUTOGEN_HEELSTRIKE
%    [Q1NEW,Q2NEW,DQ1NEW,DQ2NEW] = AUTOGEN_HEELSTRIKE(Q1,Q2,DQ1,DQ2,M,I,D,L)

%    This function was generated by the Symbolic Math Toolbox version 6.2.
%    07-Sep-2015 19:06:42

q1New = q2;
if nargout > 1
    q2New = q1;
end
if nargout > 2
    t2 = d.^2;
    t3 = m.^2;
    t4 = l.^2;
    t5 = I.^2;
    t6 = t2.^2;
    t7 = q1-q2;
    t8 = cos(t7);
    t9 = t3.*t6;
    t10 = t2.*t3.*t4.*(3.0./2.0);
    t11 = I.*m.*t2.*2.0;
    t12 = I.*m.*t4.*2.0;
    t13 = q1.*2.0;
    t18 = q2.*2.0;
    t14 = t13-t18;
    t15 = cos(t14);
    t16 = t5+t9+t10+t11+t12-d.*l.*t2.*t3.*2.0-t2.*t3.*t4.*t15.*(1.0./2.0)-I.*d.*l.*m.*2.0;
    t17 = 1.0./t16;
    dq1New = t17.*(dq2.*t5+dq2.*t3.*t6+I.*dq2.*m.*t2.*2.0-I.*d.*dq2.*l.*m+I.*dq1.*m.*t4.*t8.*2.0-d.*dq2.*l.*t2.*t3+dq1.*t2.*t3.*t4.*t8-I.*d.*dq1.*l.*m.*t8-d.*dq1.*l.*t2.*t3.*t8);
end
if nargout > 3
    dq2New = t17.*(dq1.*t5+dq1.*t3.*t6+dq1.*t2.*t3.*t4.*3.0+I.*dq1.*m.*t2.*2.0+I.*dq1.*m.*t4.*2.0-I.*d.*dq1.*l.*m.*3.0-d.*dq1.*l.*t2.*t3.*3.0-d.*dq1.*l.*t3.*t4-dq2.*t2.*t3.*t4.*t8-dq1.*t2.*t3.*t4.*t15+I.*d.*dq2.*l.*m.*t8+d.*dq2.*l.*t2.*t3.*t8+d.*dq1.*l.*t3.*t4.*t15);
end


function [S, f] = calculateSegmentSpectrum(data, lineNoise)
% Multi-taper segmented spectrum for a univariate continuous process
%
% Usage:
%     [S, f] = calculateSegmentSpectrum(data, lineNoise)
%
% Parameters:
%      data      (single channel) -- required
%      lineNoise   structure with various parameters set
%
% The lineNoise structure has the following fields set:
%       fPassBand       Frequency band used
%       Fs 	            Sampling frequency
%       pad             FFT padding factor
%       tapers          Precomputed tapers from dpss
%       taperWindowSize Taper sliding window length
%
% Output:
%       S       Spectrum
%       f       Frequencies
%

%% Check input arguments for consistency
if nargin < 2
    error('calculateSegmentSpectrum:NotEnoughArguments', ...
        'Need to provide data and segment information arguments');
end

data = change_row_to_column(data);
if size(data, 2) ~= 1;
    error('calculateSegmentSpectrum:DataNot1Dim', ...
        'Data must beunivariate time series');
end
%% Extract argument values
win = getStructureParameters(lineNoise, 'taperWindowSize', 4);
Fs = getStructureParameters(lineNoise, 'Fs', 1);
pad = getStructureParameters(lineNoise, 'pad', 0);
fpass = getStructureParameters(lineNoise, 'fPassBand', [0 lineNoise.Fs/2]);

%% Create the segmented data for the calculation of the spectrum
N = size(data, 1); % length of segmented data
dt = 1/Fs; % sampling interval
T = N*dt; % length of data in seconds
E = 0:win:(T - win); % fictitious event triggers
win = [0, win]; % use window length to define left and right limits of windows around triggers
data = createdatamatc(data, E, Fs, win); % segmented data
N = size(data,1); % length of segmented data
nfft = max(2^(nextpow2(N) + pad), N);
[f, findx] = getfgrid(Fs, nfft, fpass);
tapers = lineNoise.tapers;
J = mtfftc(data, tapers, nfft, Fs); % compute tapered fourier transforms
J = J(findx, :, :); % restrict to specified frequencies
S = squeeze(mean(conj(J).*J, 2)); % spectra of non-overlapping segments (average over tapers)
S = squeeze(mean(S, 2)); % Mean of the spectrum averaged across segments


function term_mat = zpk2term(z,p,k,T)
%
% Utility Function: ZPK2TERM
%
% The purpose of this function is to convert zero/pole/gain format into
% the term matrix format used by the environments

% Author: Craig Borghesani
% Date: 9/5/94
% Copyright (c) 1999, Prentice-Hall

narg_vals = nargin;
if narg_vals==4,
 if ~length(T),
  narg_vals=3;
 end
end

ctz=length(z);
ctp=length(p);
ct=1;
term_mat(1,:) = [k,0,NaN,1];
term_mat(2,:) = [0,NaN,NaN,2];

if narg_vals==3,
 while ct<=ctz,
  if z(ct)==0,
   term_mat(2,1)=term_mat(2,1)-1;
   ct=ct+1;
  elseif imag(z(ct))==0,
   term_mat=[term_mat;-z(ct),NaN,NaN,5];
   ct=ct+1;
  else
   re=real(z(ct)); im=imag(z(ct));
   wn=sqrt(re^2+im^2); zta=-re/wn;
   term_mat=[term_mat;zta wn NaN 7];
   ct=ct+2;
  end
 end
 ct=1;
 while ct<=ctp,
  if p(ct)==0,
   term_mat(2,1)=term_mat(2,1)+1;
   ct=ct+1;
  elseif imag(p(ct))==0,
   term_mat=[term_mat;-p(ct),NaN,NaN,4];
   ct=ct+1;
  else
   re=real(p(ct)); im=imag(p(ct));
   wn=sqrt(re^2+im^2); zta=-re/wn;
   term_mat=[term_mat;zta wn NaN 6];
   ct=ct+2;
  end
 end
else
 while ct<=ctz,
  if z(ct)==0,
   term_mat(3,1)=term_mat(3,1)-1;
   ct=ct+1;
  elseif imag(z(ct))==0,
   if z(ct)==1,
    term_mat(2,2)=term_mat(2,2)+1;
   else
    term_mat=[term_mat;exp(-z(ct)*T),NaN,NaN,2];
   end
   ct=ct+1;
  else
   z(ct)=log(z(ct))/T;
   re=real(z(ct)); im=imag(z(ct));
   wn=sqrt(re^2+im^2); zta=-re/wn;
   term_mat=[term_mat;zta wn NaN 4];
   ct=ct+2;
  end
 end
 ct=1;
 while ct<=ctp,
  if p(ct)==0,
   term_mat(3,1)=term_mat(3,1)+1;
   ct=ct+1;
  elseif imag(p(ct))==0,
   if p(ct)==1,
    term_mat(2,1)=term_mat(2,1)+1;
   else
    term_mat=[term_mat;exp(-p(ct)*T),NaN,NaN,1];
   end
   ct=ct+1;
  else
   p(ct)=log(p(ct))/T;
   re=real(p(ct)); im=imag(p(ct));
   wn=sqrt(re^2+im^2); zta=-re/wn;
   term_mat=[term_mat;zta wn NaN 3];
   ct=ct+2;
  end
 end
end


classdef TP4 < PROBLEM
% <multi> <real> <large/none> <robust>
% Test problem for robust multi-objective optimization
% delta --- 0.05 --- Maximum disturbance degree
% H     ---   50 --- Number of disturbances

%------------------------------- Reference --------------------------------
% A. Gaspar-Cunha, J. Ferreira, and G. Recio, Evolutionary robustness
% analysis for multi-objective optimization: benchmark problems, Structural
% and Multidisciplinary Optimization, 2014, 49: 771-793.
%------------------------------- 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
        delta;      % Maximum disturbance degree
        H;          % Number of disturbances
    end
    methods
        %% Default settings of the problem
        function Setting(obj)
            [obj.delta,obj.H] = obj.ParameterSet(0.05,50);
            obj.M = 2;
            if isempty(obj.D); obj.D = 10; end
            obj.lower    = zeros(1,obj.D);
            obj.upper    = ones(1,obj.D);
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,PopDec)
            PopObj(:,1) = (exp(PopDec(:,1))-1)/(exp(1)-1);
            g = 1 + 10*mean(PopDec(:,2:end),2);
            h = sin(4*pi*PopDec(:,1))/15 - PopDec(:,1) + 1;
            PopObj(:,2) = h.*g;
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(~,N)
            x = linspace(0,1,N)';
            R(:,1) = (exp(x)-1)/(exp(1)-1);
            R(:,2) = sin(4*pi*x)/15 - x + 1;
        end
        %% Generate the image of Pareto front
        function R = GetPF(obj)
            R = obj.GetOptimum(100);
        end
        %% Calculate the metric value
        function score = CalMetric(obj,metName,Population)
            switch metName
                case {'Mean_IGD','Mean_HV','Worst_IGD','Worst_HV'}
                    score = feval(metName,Population,obj);
                otherwise
                    score = feval(metName,Population,obj.optimum);
            end
        end
        %% Perturb solutions multiple times
        function PopX = Perturb(obj,PopDec,N)
            if nargin < 3; N = obj.H; end
            Delta = repmat(obj.delta.*(obj.upper-obj.lower),N*size(PopDec,1),1);
            w     = UniformPoint(N,obj.D,'Latin');
            Dec   = 2*Delta.*w(reshape(repmat(1:end,size(PopDec,1),1),1,[]),:) + repmat(PopDec,N,1) - Delta;
            Dec   = obj.CalDec(Dec);
            PopX  = SOLUTION(Dec,obj.CalObj(Dec),obj.CalCon(Dec));
        end
    end
end


function [assigns, totvalue] = auction(valuematrix)

% [assigns, totvalue] = auction(valuematrix)
%
% First column is null assignment - any number of objects can be assigned
% this
%
% Author: Paul Horridge

bidincrement = 1e-6; % choose better later
[nobj, nhyps] = size(valuematrix); % nhyps includes null hypothesis
prices = zeros(1, nhyps);
assigns = ones(nobj, 1);
hyps2obj = zeros(1, nhyps);
happy = false(nobj, 1);

% Repeat while someone unhappy
while any(~happy)
    % Find unhappy person
    i = find(~happy, 1, 'first');
    % Get their net values
    netvals = valuematrix(i,:) - prices;
    [bestval, bestj] = max(netvals);
    if netvals(assigns(i)) >= bestval - bidincrement
        % Check if happy
        happy(i) = true;
    else
        % Get second best hypothesis
        nextval = max(netvals([1:bestj-1 bestj+1:end]));
        % Increase price if required
        if bestj > 1
            prices(bestj) = prices(bestj) + bestval - nextval + bidincrement;
            % De-assign original holder if required
            oldi = hyps2obj(bestj);
            if oldi > 0
                assigns(oldi) = 1;
                happy(oldi) = false;
            end
        end
        % Assign best hypothesis to i
        assigns(i) = bestj;
        hyps2obj(bestj) = i;
        happy(i) = true;
    end
end
totvalue = sum(valuematrix((assigns'-1)*nobj + (1:nobj)));


function pass = sampleTest(f, sampleOP, tol, flag)
%SAMPLETEST   Test an evaluation of input OP against a SPHEREFUN.
%
%   SAMPLETEST(F, SAMPLEOP, TOL) evaluates both the function OP and its
%   SPHEREFUN representation F at several points in its domain. The
%   difference of these values is computed, and if this is sufficiently
%   small the test passes and returns TRUE.
%   If the difference is large, it returns FALSE.
%
%   SAMPLETEST(F, SAMPLEOP, TOL, FLAG) is the same as above if FLAG = 0.
%   If FLAG = 1 then the OP is assumed to be unvectorized.

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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% [TODO]: Describe where we evaluate? (at low discrepancy points...)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% NOTE: Once complex-valued spherefuns are allowed, we can call
% sampleTest from separableApprox.

if ( nargin < 3 )
    % Assume op is vectorized:
    flag = 0;
end

domain = f.domain;

if ( ~flag )
    % Sample at lots of points if the op is vectorized.
    n = 100;
    [xeval, yeval] = halton( n, domain );

    % Evaluate the op:
    vOp = feval(sampleOP, xeval, yeval);
else
    % sample on less points if the op is unvectorized.
    n = 20;
    [xeval, yeval] = halton( n, domain );

    % Evaluate the op:
    vOp = zeros(n , 1 );
    for jj = 1:numel(xeval)
        vOp(jj) = feval(sampleOP, xeval(jj), yeval(jj));
    end
end

%for now, set to real-valued only to match what constructor does
vOp = real(vOp);

% Evaluate the SPHEREFUN:
vFun = feval(f, xeval, yeval);

% If the TECHS evaluation differs from the op evaluation, SAMPLETEST failed:
if ( any(max(abs(vOp - vFun)) > 100*tol) )
    pass = false; % :(
else
    pass = true;  % :)
end

end

function [x, y] = halton( numpts, domain )
% Halton sequences are sequences used to generate points in space, which
% are deterministic and of low discrepancy. They appear to be random for
% many purposes.
%
% Adapted from Grady Wright's code 22nd May 2014.

% generate Halton sequences on [0,1]^2:
ndims = 2;
p = [2 3 5 7 11 13];
H = zeros(numpts, ndims);
for k = 1:ndims
    N = p(k); v1 = 0; v2 = 0:N-1; lv1 = 1;
    while ( lv1 <= numpts )
        v2 = v2(1:max(2,min(N,ceil((numpts+1)/lv1))))/N;
        [x1,x2] = meshgrid(v2,v1);
        v1 = x1+x2;
        v1 = v1(:);
        lv1 = length(v1);
    end
    H(:,k) = v1(2:numpts+1);
end
% scale [0,1]^2 to the domain of the separableApprox.
x = H(:,1);
x = (domain(2) - domain(1))*x + domain(1);
y = H(:,2);
y = (domain(4) - domain(3))*y + domain(3);

end


function [ferns,hsPr] = fernsClfTrain( data, hs, varargin )
% Train random fern classifier.
%
% See "Fast Keypoint Recognition in Ten Lines of Code" by Mustafa Ozuysal,
% Pascal Fua and Vincent Lepetit, CVPR07.
%
% Dimensions:
%  M - number ferns
%  S - fern depth
%  F - number features
%  N - number input vectors
%  H - number classes
%
% USAGE
%  [ferns,hsPr] = fernsClfTrain( data, hs, [varargin] )
%
% INPUTS
%  data     - [NxF] N length F feature vectors
%  hs       - [Nx1] target output labels in [1,H]
%  varargin - additional params (struct or name/value pairs)
%   .S        - [10] fern depth (ferns are exponential in S)
%   .M        - [50] number of ferns to train
%   .thrr     - [0 1] range for randomly generated thresholds
%   .bayes    - [1] if true combine probs using bayes assumption
%   .ferns    - [] if given reuse previous ferns (recompute pFern)
%
% OUTPUTS
%  ferns    - learned fern model w the following fields
%   .fids     - [MxS] feature ids for each fern for each depth
%   .thrs     - [MxS] threshold corresponding to each fid
%   .pFern    - [2^SxHxM] learned log probs at fern leaves
%   .bayes    - if true combine probs using bayes assumption
%   .inds     - [NxM] cached indices for original training data
%   .H        - number classes
%  hsPr     - [Nx1] predicted output labels
%
% EXAMPLE
%  N=5000; H=5; d=2; [xs0,hs0,xs1,hs1]=demoGenData(N,N,H,d,1,1);
%  fernPrm=struct('S',4,'M',50,'thrr',[-1 1],'bayes',1);
%  tic, [ferns,hsPr0]=fernsClfTrain(xs0,hs0,fernPrm); toc
%  tic, hsPr1 = fernsClfApply( xs1, ferns ); toc
%  e0=mean(hsPr0~=hs0); e1=mean(hsPr1~=hs1);
%  fprintf('errors trn=%f tst=%f\n',e0,e1); figure(1);
%  subplot(2,2,1); visualizeData(xs0,2,hs0);
%  subplot(2,2,2); visualizeData(xs0,2,hsPr0);
%  subplot(2,2,3); visualizeData(xs1,2,hs1);
%  subplot(2,2,4); visualizeData(xs1,2,hsPr1);
%
% See also fernsClfApply, fernsInds
%
% Piotr's Image&Video Toolbox      Version 2.50
% Copyright 2010 Piotr Dollar.  [pdollar-at-caltech.edu]
% Please email me if you find bugs, or have suggestions or questions!
% Licensed under the Lesser GPL [see external/lgpl.txt]

% get additional parameters and check dimensions
dfs={'S',10,'M',50,'thrr',[0 1],'bayes',1,'ferns',[]};
[S,M,thrr,bayes,ferns]=getPrmDflt(varargin,dfs,1);
[N,F]=size(data); assert(length(hs)==N);
H=max(hs); assert(all(hs>0)); assert(S<=20);

if( isempty(ferns) )
  % create ferns model and compute inds (w/o field pFern, counts)
  thrs=rand(M,S)*(thrr(2)-thrr(1))+thrr(1);
  fids=uint32(floor(rand(M,S)*F+1)); inds=fernsInds(data,fids,thrs);
  ferns=struct('fids',fids,'thrs',thrs,'bayes',bayes,'H',H,'inds',inds);
else
  % re-use cached model (will need to recompute pFern)
  ferns.H=H; ferns.pFern=[]; ferns.counts = []; inds=ferns.inds; assert(size(inds,1)==N);
end

% get counts for each leaf for each class for each fern
% KB: use hist to avoid inner loop
pFern = zeros(2^S,H,M); edges = 1:2^S;
for h=1:H, inds1=inds(hs==h,:);
  for m=1:M, pFern(:,h,m)=histc(inds1(:,m),edges); end
end
pFern = pFern + bayes;

% old code
% pFern = bayes(ones(2^S,H,M));
% for n=1:N, h=hs(n);
%   for m=1:M, ind=inds(n,m);
%     pFern(ind,h,m)=pFern(ind,h,m)+1;
%   end
% end

% KB: store the unnormalized counts
ferns.counts = pFern;

% convert fern leaf class counts into probabilities
if( bayes<=0 )
  norm = 1./sum(pFern,2);
  % KB: use bsxfun instead of loop
  pFern = bsxfun(@times,pFern,norm);
  %for h=1:H, pFern(:,h,:)=pFern(:,h,:).*norm; end
else
  norm = 1./sum(pFern,1);
  % KB: use bsxfun instead of loop
  pFern = bsxfun(@times,pFern,norm);
  %for s=1:2^S, pFern(s,:,:)=pFern(s,:,:).*norm; end
  pFern=log(pFern);
end

% store pFern and compute output values
ferns.pFern=pFern; clear pFern;
if(nargout==2), hsPr=fernsClfApply([],ferns,inds); end

end


%QUATERNION Quaternion class
%
% A quaternion is a compact method of representing a 3D rotation that has
% computational advantages including speed and numerical robustness.
% A quaternion has 2 parts, a scalar s, and a vector v and is typically
% written: q = s <vx, vy, vz>.
%
% A unit-quaternion is one for which s^2+vx^2+vy^2+vz^2 = 1.  It can be
% considered as a rotation by an angle theta about a unit-vector V in space where
%
%         q = cos (theta/2) < v sin(theta/2)>
%
% Q = Quaternion(X) is a unit-quaternion equivalent to X which can be any
% of:
%   - orthonormal rotation matrix.
%   - homogeneous transformation matrix (rotation part only).
%   - rotation angle and vector
%
% Methods::
%  inv       inverse of quaterion
%  norm      norm of quaternion
%  unit      unitized quaternion
%  plot      same options as trplot()
%  interp    interpolation (slerp) between q and q2, 0<=s<=1
%  scale     interpolation (slerp) between identity and q, 0<=s<=1
%  dot       derivative of quaternion with angular velocity w
%  R         equivalent 3x3 rotation matrix
%  T         equivalent 4x4 homogeneous transform matrix
%  double    quaternion elements as 4-vector
%  inner     inner product of two quaternions
%
% Overloaded operators::
%  q1==q2    test for quaternion equality
%  q1~=q2    test for quaternion inequality
%  q+q2      elementwise sum of quaternions
%  q-q2      elementwise difference of quaternions
%  q*q2      quaternion product
%  q*v       rotate vector by quaternion, v is 3x1
%  s*q       elementwise multiplication of quaternion by scalar
%  q/q2      q*q2.inv
%  q^n       q to power n (integer only)
%
% Properties (read only)::
%  s         real part
%  v         vector part
%
% Notes::
% - Quaternion objects can be used in vectors and arrays.
%
% References::
% - Animating rotation with quaternion curves,
%   K. Shoemake,
%   in Proceedings of ACM SIGGRAPH, (San Fran cisco), pp. 245-254, 1985.
% - On homogeneous transforms, quaternions, and computational efficiency,
%   J. Funda, R. Taylor, and R. Paul,
%   IEEE Transactions on Robotics and Automation, vol. 6, pp. 382-388, June 1990.
% - Robotics, Vision & Control,
%   P. Corke, Springer 2011.
%
% See also trinterp, trplot.

% TODO
% properties s, v for the vector case

% Copyright (C) 1993-2015, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB 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 Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

% TODO
%  constructor handles R, T trajectory and returns vector
%  .r, .t on a quaternion vector??

classdef Quaternion

    properties (SetAccess = private)
        s       % scalar part
        v       % vector part
    end

    methods

        function q = Quaternion(a1, a2)
        %Quaternion.Quaternion Constructor for quaternion objects
        %
        % Construct a quaternion from various other orientation representations.
        %
        % Q = Quaternion() is the identitity unit-quaternion 1<0,0,0> representing a null rotation.
        %
        % Q = Quaternion(Q1) is a copy of the quaternion Q1
        %
        % Q = Quaternion([S V1 V2 V3]) is a quaternion formed by specifying directly its 4 elements
        %
        % Q = Quaternion(S) is a quaternion formed from the scalar S and zero vector part: S<0,0,0>
        %
        % Q = Quaternion(V) is a pure quaternion with the specified vector part: 0<V>
        %
        % Q = Quaternion(TH, V) is a unit-quaternion corresponding to rotation of TH about the
        % vector V.
        %
        % Q = Quaternion(R) is a unit-quaternion corresponding to the SO(3)
        % orthonormal rotation matrix R (3x3).  If R (3x3xN) is a sequence then Q
        % (Nx1) is a vector of Quaternions corresponding to the elements of R.
        %
        % Q = Quaternion(T) is a unit-quaternion equivalent to the rotational part
        % of the SE(3) homogeneous transform T (4x4). If T (4x4xN) is a sequence
        % then Q (Nx1) is a vector of Quaternions corresponding to the elements of
        % T.

            if nargin == 0
                q.v = [0,0,0];
                q.s = 1;
            elseif isa(a1, 'Quaternion')
            %   Q = Quaternion(q)       from another quaternion
                q = a1;
            elseif nargin == 1
                if isvec(a1, 4)
            %   Q = Quaternion([s v1 v2 v3])    from 4 elements
                    a1 = a1(:);
                    q.s = a1(1);
                    q.v = a1(2:4)';
                elseif isrot(a1) || ishomog(a1)
            %   Q = Quaternion(R)       from a 3x3 or 4x4 matrix
                    for i=1:size(a1,3)
                        q(i) = Quaternion( tr2q(a1(:,:,i)) );
                    end

                elseif length(a1) == 3
            %   Q = Quaternion(v)       from a vector

                    q.s = 0;
                    q.v = a1(:)';
                elseif length(a1) == 1
            %   Q = Quaternion(s)       from a scalar
                    q.s = a1(1);
                    q.v = [0 0 0];
                else
                    error('RTB:Quaternion:badarg', 'unknown dimension of input');
                end
            elseif nargin == 2
                if isscalar(a1) && isvector(a2)
                %   Q = Quaternion(theta, v)    from vector plus angle
                    q.s = cos(a1/2);
                    q.v = sin(a1/2)*unit(a2(:)');
                else
                    error ('RTB:Quaternion:badarg', 'bad argument to quaternion constructor');
                end

            end
        end

        function s = char(q)
        %Quaternion.char Convert to string
        %
        % S = Q.char() is a compact string representation of the quaternion's value
        % as a 4-tuple.  If Q is a vector then S has one line per element.

            if length(q) > 1
                s = '';
                for qq = q;
                    s = char(s, char(qq));
                end
                return
            end
            s = [num2str(q.s), ' < ' ...
                num2str(q.v(1)) ', ' num2str(q.v(2)) ', '   num2str(q.v(3)) ' >'];
        end

        function display(q)
        %Quaternion.display Display quaternion
        %
        % Q.display() displays a compact string representation of the quaternion's value
        % as a 4-tuple.  If Q is a vector then S has one line per element.
        %
        % Notes::
        % - This method is invoked implicitly at the command line when the result
        %   of an expression is a Quaternion object and the command has no trailing
        %   semicolon.
        %
        % See also Quaternion.char.

            loose = strcmp( get(0, 'FormatSpacing'), 'loose');
            if loose
                disp(' ');
            end
            disp([inputname(1), ' = '])
            if loose
                disp(' ');
            end
            disp(char(q))
            if loose
                disp(' ');
            end
        end

        function v = double(q)
        %Quaternion.double Convert a quaternion to a 4-element vector
        %
        % V = Q.double() is a 4-vector comprising the quaternion
        % elements [s vx vy vz].

            v = [q.s q.v];
        end

        function qi = inv(q)
        %Quaternion.inv Invert a unit-quaternion
        %
        % QI = Q.inv() is a quaternion object representing the inverse of Q.

            qi = Quaternion([q.s -q.v]);
        end

        function qu = unit(q)
        %Quaternion.unit Unitize a quaternion
        %
        % QU = Q.unit() is a unit-quaternion representing the same orientation as Q.
        %
        % See also Quaternion.norm.

            qu = q / norm(q);
        end

        function n = norm(q)
        %Quaternion.norm Quaternion magnitude
        %
        % QN = Q.norm(Q) is the scalar norm or magnitude of the quaternion Q.
        %
        % Notes::
        % - This is the Euclidean norm of the quaternion written as a 4-vector.
        % - A unit-quaternion has a norm of one.
        %
        % See also Quaternion.inner, Quaternion.unit.

            n = norm(double(q));
        end

        function n = inner(q1, q2)
        %Quaternion.inner Quaternion inner product
        %
        % V = Q1.inner(Q2) is the inner (dot) product of two vectors (1x4),
        % comprising the elements of Q1 and Q2 respectively.
        %
        % Notes::
        % - Q1.inner(Q1) is the same as Q1.norm().
        %
        % See also Quaternion.norm.

            n = double(q1)*double(q2)';
        end

        function q = interp(Q1, Q2, r)
        %Quaternion.interp Interpolate quaternions
        %
        % QI = Q1.interp(Q2, S) is a unit-quaternion that interpolates a rotation
        % between Q1 for S=0 and Q2 for S=1.
        %
        % If S is a vector QI is a vector of quaternions, each element
        % corresponding to sequential elements of S.
        %
        % Notes::
        % - This is a spherical linear interpolation (slerp) that can be interpretted
        %   as interpolation along a great circle arc on a sphere.
        % - The value of S is clipped to the interval 0 to 1.
        %
        % References::
        % - Animating rotation with quaternion curves,
        %   K. Shoemake,
        %   in Proceedings of ACM SIGGRAPH, (San Fran cisco), pp. 245-254, 1985.
        %
        % See also Quaternion.scale, ctraj.

            q1 = double(Q1);
            q2 = double(Q2);

            cosTheta = q1*q2';
            % take shortest path along the great circle, patch by Gauthier Gras
            if cosTheta < 0
                q1 = - q1;
                cosTheta = - cosTheta;
            end;

            theta = acos(cosTheta);
            count = 1;

            % clip values of r
            r(r<0) = 0;
            r(r>1) = 1;

            q(length(r)) = Quaternion();  % preallocate space for Quaternion vector

            for i=1:length(r)
                if theta == 0
                    q(i) = Q1;
                else
                    q(i) = Quaternion( (sin((1-r(i))*theta) * q1 + sin(r(i)*theta) * q2) / sin(theta) );
                end
            end
        end

        function q = scale(Q, r)
        %Quaternion.scale Interpolate rotations expressed by quaternion objects
        %
        % QI = Q.scale(S) is a unit-quaternion that interpolates between a null
        % rotation (identity quaternion) for S=0 to Q for S=1.  This is a spherical
        % linear interpolation (slerp) that can be interpretted as interpolation
        % along a great circle arc on a sphere.
        %
        % If S is a vector QI is a vector of quaternions, each element
        % corresponding to sequential elements of S.
        %
        % Notes::
        % - This is a spherical linear interpolation (slerp) that can be interpretted
        %   as interpolation along a great circle arc on a sphere.
        %
        % See also Quaternion.interp, ctraj.

            q2 = double(Q);

            if any(r<0) || (r>1)
                error('r out of range');
            end
            q1 = [1 0 0 0];         % identity quaternion
            theta = acos(q1*q2');

            if length(r) == 1
                if theta == 0
                    q = Q;
                else
                    q = Quaternion( (sin((1-r)*theta) * q1 + sin(r*theta) * q2) / sin(theta) ).unit;
                end
            else
                count = 1;
                for R=r(:)'
                    if theta == 0
                        qq = Q;
                    else
                        qq = Quaternion( (sin((1-r)*theta) * q1 + sin(r*theta) * q2) / sin(theta) ).unit;
                    end
                    q(count) = qq;
                    count = count + 1;
                end
            end
        end

        function e = eq(q1, q2)
        %EQ Test quaternion equality
        %
        % Q1==Q2 is true if the quaternions Q1 and Q2 are equal.
        %
        % Notes::
        % - Overloaded operator '=='.
        % - Note that for unit Quaternions Q and -Q are the equivalent
        %   rotation, so non-equality does not mean rotations are not
        %   equivalent.
        % - If Q1 is a vector of quaternions, each element is compared to
        %   Q2 and the result is a logical array of the same length as Q1.
        % - If Q2 is a vector of quaternions, each element is compared to
        %   Q1 and the result is a logical array of the same length as Q2.
        % - If Q1 and Q2 are vectors of the same length, then the result
        %   is a logical array of the same length.
        %
        % See also Quaternion.ne.
            if (numel(q1) == 1) && (numel(q2) == 1)
                e = all( eq(q1.double, q2.double) );
            elseif (numel(q1) >  1) && (numel(q2) == 1)
                e = zeros(1, numel(q1));
                for i=1:numel(q1)
                    e(i) = q1(i) == q2;
                end
            elseif (numel(q1) == 1) && (numel(q2) > 1)
                e = zeros(1, numel(q2));
                for i=1:numel(q2)
                    e(i) = q2(i) == q1;
                end
            elseif numel(q1) == numel(q2)
                e = zeros(1, numel(q1));
                for i=1:numel(q1)
                    e(i) = q1(i) == q2(i);
                end
            else
                error('RTB:quaternion:badargs');
            end
        end

        function e = ne(q1, q2)
        %NE Test quaternion inequality
        %
        % Q1~=Q2 is true if the quaternions Q1 and Q2 are not equal.
        %
        % Notes::
        % - Overloaded operator '~='
        % - Note that for unit Quaternions Q and -Q are the equivalent
        %   rotation, so non-equality does not mean rotations are not
        %   equivalent.
        % - If Q1 is a vector of quaternions, each element is compared to
        %   Q2 and the result is a logical array of the same length as Q1.
        % - If Q2 is a vector of quaternions, each element is compared to
        %   Q1 and the result is a logical array of the same length as Q2.
        % - If Q1 and Q2 are vectors of the same length, then the result
        %   is a logical array of the same length.
        %
        % See also Quaternion.eq.
            if (numel(q1) == 1) && (numel(q2) == 1)
                e = all( ne(q1.double, q2.double) );
            elseif (numel(q1) >  1) && (numel(q2) == 1)
                e = zeros(1, numel(q1));
                for i=1:numel(q1)
                    e(i) = q1(i) ~= q2;
                end
            elseif (numel(q1) == 1) && (numel(q2) > 1)
                e = zeros(1, numel(q2));
                for i=1:numel(q2)
                    e(i) = q2(i) ~= q1;
                end
            elseif numel(q1) == numel(q2)
                e = zeros(1, numel(q1));
                for i=1:numel(q1)
                    e(i) = q1(i) ~= q2(i);
                end
            else
                error('RTB:quaternion:badargs');
            end
        end

        function qp = plus(q1, q2)
        %PLUS Add quaternions
        %
        % Q1+Q2 is the element-wise sum of quaternion elements.
        %
        % Notes::
        % - Overloaded operator '+'
        % - The result is not guaranteed to be a unit-quaternion.
        %
        % See also Quaternion.minus, Quaternion.mtimes.

            if isa(q1, 'Quaternion') && isa(q2, 'Quaternion')
                qp = Quaternion(double(q1) + double(q2));
            end
        end

        function qp = minus(q1, q2)
        %Quaternion.minus Subtract quaternions
        %
        % Q1-Q2 is the element-wise difference of quaternion elements.
        %
        % Notes::
        % - Overloaded operator '-'
        % - The result is not guaranteed to be a unit-quaternion.
        %
        % See also Quaternion.plus, Quaternion.mtimes.

            if isa(q1, 'Quaternion') && isa(q2, 'Quaternion')

                qp = Quaternion(double(q1) - double(q2));
            end
        end

        function qp = mtimes(q1, q2)
        %Quaternion.mtimes Multiply a quaternion object
        %
        % Q1*Q2   is a quaternion formed by the Hamilton product of two quaternions.
        % Q*V     is a vector formed by rotating the vector V by the quaternion Q.
        % Q*S     is the element-wise multiplication of quaternion elements by the scalar S.
        %
        % Notes::
        % - Overloaded operator '*'
        % - If the two multiplicands are unit-quaternions, the product will be a
        %   unit quaternion.
        %
        % See also Quaternion.mrdivide, Quaternion.mpower, Quaternion.plus, Quaternion.minus.

            if isa(q1, 'Quaternion') && isa(q2, 'Quaternion')
            %QQMUL  Multiply unit-quaternion by unit-quaternion
            %
            %   QQ = qqmul(Q1, Q2)
            %
            %   Return a product of unit-quaternions.
            %
            %   See also: TR2Q

                % decompose into scalar and vector components
                s1 = q1.s;  v1 = q1.v;
                s2 = q2.s;  v2 = q2.v;

                % form the product
                qp = Quaternion([s1*s2-v1*v2' s1*v2+s2*v1+cross(v1,v2)]);

            elseif isa(q1, 'Quaternion') && isa(q2, 'double')

            %QVMUL  Multiply vector by unit-quaternion
            %
            %   VT = qvmul(Q, V)
            %
            %   Rotate the vector V by the unit-quaternion Q.
            %
            %   See also: QQMUL, QINV

                if length(q2) == 3
                    qp = q1 * Quaternion([0 q2(:)']) * inv(q1);
                    qp = qp.v(:);
                elseif length(q2) == 1
                    qp = Quaternion( double(q1)*q2);
                else
                    error('quaternion-vector product: must be a 3-vector or scalar');
                end

            elseif isa(q2, 'Quaternion') && isa(q1, 'double')
                if length(q1) == 3
                    qp = q2 * Quaternion([0 q1(:)']) * inv(q2);
                    qp = qp.v;
                elseif length(q1) == 1
                    qp = Quaternion( double(q2)*q1);
                else
                    error('quaternion-vector product: must be a 3-vector or scalar');
                end
            end
        end

        function qp = mpower(q, p)
        %Quaternion.mpower Raise quaternion to integer power
        %
        % Q^N is the quaternion Q raised to the integer power N.
        %
        % Notes::
        % - Overloaded operator '^'
        % - Computed by repeated multiplication.
        % - If the argument is a unit-quaternion, the result will be a
        %   unit quaternion.
        %
        % See also Quaternion.mrdivide, Quaternion.mpower, Quaternion.plus, Quaternion.minus.

            % check that exponent is an integer
            if (p - floor(p)) ~= 0
                error('quaternion exponent must be integer');
            end

            qp = q;

            % multiply by itself so many times
            for i = 2:abs(p)
                qp = qp * q;
            end

            % if exponent was negative, invert it
            if p<0
                qp = inv(qp);
            end
        end

        function qq = mrdivide(q1, q2)
        %Quaternion.mrdivide Quaternion quotient.
        %
        % Q1/Q2   is a quaternion formed by Hamilton product of Q1 and inv(Q2).
        % Q/S     is the element-wise division of quaternion elements by the scalar S.
        %
        % Notes::
        % - Overloaded operator '/'
        % - If the dividend and divisor are unit-quaternions, the quotient will be a
        %   unit quaternion.
        %
        % See also Quaternion.mtimes, Quaternion.mpower, Quaternion.plus, Quaternion.minus.

            if isa(q2, 'Quaternion')
                % qq = q1 / q2
                %    = q1 * qinv(q2)

                qq = q1 * inv(q2);
            elseif isa(q2, 'double')
                qq = Quaternion( double(q1) / q2 );
            end
        end

        function plot(Q, varargin)
        %Quaternion.plot Plot a quaternion object
        %
        % Q.plot(options) plots the quaternion as an oriented coordinate frame.
        %
        % Options::
        % Options are passed to trplot and include:
        %
        % 'color',C          The color to draw the axes, MATLAB colorspec C
        % 'frame',F          The frame is named {F} and the subscript on the axis labels is F.
        % 'view',V           Set plot view parameters V=[az el] angles, or 'auto'
        %                    for view toward origin of coordinate frame
        %
        % See also trplot.

            %axis([-1 1 -1 1 -1 1])

            trplot( Q.R, varargin{:});
            drawnow
        end

        function r = R(q)
        %Quaternion.R Convert to orthonormal rotation matrix
        %
        % R = Q.R() is the equivalent SO(3) orthonormal rotation matrix (3x3).  If
        % Q represents a sequence (Nx1) then R is 3x3xN.
        %

            r = zeros(3,3,numel(q));
            for i=1:numel(q)
                r(:,:,i) = t2r( q2tr(q(i)) );
            end
        end

        function t = T(q)
        %Quaternion.T Convert to homogeneous transformation matrix
        %
        % T = Q.T() is the equivalent SE(3) homogeneous transformation matrix
        % (4x4).    If Q represents a sequence (Nx1) then T is 4x4xN.
        %
        % Notes:
        % - Has a zero translational component.
            t = zeros(4,4,numel(q));
            for i=1:numel(q)
                t(:,:,i) = q2tr(q(i));
            end
        end

        function qd = dot(q, omega)
        %Quaternion.dot Quaternion derivative
        %
        % QD = Q.dot(omega) is the rate of change of a frame with attitude Q and
        % angular velocity OMEGA (1x3) expressed as a quaternion.
            E = q.s*eye(3,3) - skew(q.v);
            omega = omega(:);
            qd = Quaternion([-0.5*q.v*omega; 0.5*E*omega]);
        end
    end % methods
end % classdef

%TR2Q   Convert homogeneous transform to a unit-quaternion
%
%   Q = tr2q(T)
%
%   Return a unit-quaternion corresponding to the rotational part of the
%   homogeneous transform T.
%
%   See also: Q2TR

function q = tr2q(t)

    if ishomog(t)
        t = t2r(t);
    end
    qs = sqrt(trace(t)+1)/2.0;
    kx = t(3,2) - t(2,3);   % Oz - Ay
    ky = t(1,3) - t(3,1);   % Ax - Nz
    kz = t(2,1) - t(1,2);   % Ny - Ox

    if (t(1,1) >= t(2,2)) && (t(1,1) >= t(3,3))
        kx1 = t(1,1) - t(2,2) - t(3,3) + 1; % Nx - Oy - Az + 1
        ky1 = t(2,1) + t(1,2);          % Ny + Ox
        kz1 = t(3,1) + t(1,3);          % Nz + Ax
        add = (kx >= 0);
    elseif (t(2,2) >= t(3,3))
        kx1 = t(2,1) + t(1,2);          % Ny + Ox
        ky1 = t(2,2) - t(1,1) - t(3,3) + 1; % Oy - Nx - Az + 1
        kz1 = t(3,2) + t(2,3);          % Oz + Ay
        add = (ky >= 0);
    else
        kx1 = t(3,1) + t(1,3);          % Nz + Ax
        ky1 = t(3,2) + t(2,3);          % Oz + Ay
        kz1 = t(3,3) - t(1,1) - t(2,2) + 1; % Az - Nx - Oy + 1
        add = (kz >= 0);
    end

    if add
        kx = kx + kx1;
        ky = ky + ky1;
        kz = kz + kz1;
    else
        kx = kx - kx1;
        ky = ky - ky1;
        kz = kz - kz1;
    end
    nm = norm([kx ky kz]);
    if nm == 0
        q = Quaternion([1 0 0 0]);
    else
        s = sqrt(1 - qs^2) / nm;
        qv = s*[kx ky kz];

        q = Quaternion([qs qv]);

    end
end

%Q2TR   Convert unit-quaternion to homogeneous transform
%
%   T = q2tr(Q)
%
%   Return the rotational homogeneous transform corresponding to the unit
%   quaternion Q.
%
%   See also: TR2Q

function t = q2tr(q)

    q = double(q);
    s = q(1);
    x = q(2);
    y = q(3);
    z = q(4);

    r = [   1-2*(y^2+z^2)   2*(x*y-s*z) 2*(x*z+s*y)
        2*(x*y+s*z) 1-2*(x^2+z^2)   2*(y*z-s*x)
        2*(x*z-s*y) 2*(y*z+s*x) 1-2*(x^2+y^2)   ];
    t = eye(4,4);
    t(1:3,1:3) = r;
    t(4,4) = 1;
end


function x = mono_next_grlex ( m, x )

%*****************************************************************************80
%
%% MONO_NEXT_GRLEX: grlex next monomial.
%
%  Discussion:
%
%    Example:
%
%    M = 3
%
%    #  X(1)  X(2)  X(3)  Degree
%      +------------------------
%    1 |  0     0     0        0
%      |
%    2 |  0     0     1        1
%    3 |  0     1     0        1
%    4 |  1     0     0        1
%      |
%    5 |  0     0     2        2
%    6 |  0     1     1        2
%    7 |  0     2     0        2
%    8 |  1     0     1        2
%    9 |  1     1     0        2
%   10 |  2     0     0        2
%      |
%   11 |  0     0     3        3
%   12 |  0     1     2        3
%   13 |  0     2     1        3
%   14 |  0     3     0        3
%   15 |  1     0     2        3
%   16 |  1     1     1        3
%   17 |  1     2     0        3
%   18 |  2     0     1        3
%   19 |  2     1     0        3
%   20 |  3     0     0        3
%
%    Thanks to Stefan Klus for pointing out a discrepancy in a previous
%    version of this code, 05 February 2015.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 February 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the spatial dimension.
%
%    Input, integer X(M), the current monomial.
%    The first item is X = [ 0, 0, ..., 0, 0 ].
%
%    Output, integer X(M), the next monomial.
%

%
%  Ensure that 1 <= M.
%
  if ( m < 1 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'MONO_NEXT_GRLEX - Fatal error!' );
    fprintf ( 1, '  M < 1\n' );
    error ( 'MONO_NEXT_GRLEX - Fatal error!' );
  end
%
%  Ensure that 0 <= XC(I).
%
  for i = 1 : m
    if ( x(i) < 0 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'MONO_NEXT_GRLEX - Fatal error!' );
      fprintf ( 1, '  X(I) < 0\n' );
      error ( 'MONO_NEXT_GRLEX - Fatal error!' );
    end
  end
%
%  Find I, the index of the rightmost nonzero entry of X.
%
  i = 0;
  for j = m : -1 : 1
    if ( 0 < x(j) )
      i = j;
      break
    end
  end
%
%  set T = X(I)
%  set X(I) to zero,
%  increase X(I-1) by 1,
%  increment X(M) by T-1.
%
  if ( i == 0 )
    x(m) = 1;
    return
  elseif ( i == 1 )
    t = x(1) + 1;
    im1 = m;
  elseif ( 1 < i )
    t = x(i);
    im1 = i - 1;
  end

  x(i) = 0;
  x(im1) = x(im1) + 1;
  x(m) = x(m) + t - 1;

  return
end


function [mfcMVN, meanV, varV] = SideMVNcore(mfcRaw)

meanAcc = 0;    varAcc = 0;
for j=1:length(mfcRaw)
    nFr(j) = size(mfcRaw{j},1);
    meanAcc = meanAcc + sum(mfcRaw{j});
    varAcc = varAcc + sum(mfcRaw{j}.^2);
end
meanV = meanAcc / sum(nFr);
varV = sqrt( varAcc/sum(nFr) - meanV.^2 );
precision = 1./varV;

% Normalize the mean and variance
for j=1:length(mfcRaw)
    mfcMVN{j} = bsxfun(@minus, mfcRaw{j}, meanV);
    mfcMVN{j} = bsxfun(@times, mfcMVN{j}, precision);
end


function xhfd=hfd(x,kmax)
%function xhfd=hfd(x,kmax)
%Input:
%x: (either column or row) vector of length N
%kmax: maximum value of k
%Output:
%xhfd: Higuchi fractal dimension of x

if ~exist('kmax','var')||isempty(kmax),
    kmax=5;
end;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x=x(:)';
N=length(x);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Lmk=zeros(kmax,kmax);
for k=1:kmax,
    for m=1:k,
        Lmki=0;
        for i=1:fix((N-m)/k),
            Lmki=Lmki+abs(x(m+i*k)-x(m+(i-1)*k));
        end;
        Ng=(N-1)/(fix((N-m)/k)*k);
        Lmk(m,k)=(Lmki*Ng)/k; % Here is the problem in the code by Mr. Tikkuhirvi & Mr. Aino
    end;
end;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Lk=zeros(1,kmax);
for k=1:kmax,
    Lk(1,k)=sum(Lmk(1:k,k))/k;
end;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
lnLk=log(Lk);
lnk=log(1./[1:kmax]);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
b=polyfit(lnk,lnLk,1);
xhfd=b(1);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


function [tuples,fStar,qStar,u,exitCode]=assign3D(C,maximize,subgradMethod,subgradParams,maxIter,AbsTol,RelTol)
%%ASSIGN3D Approximate the solution to the operations research axial 3D
%         assignment problem using a dual-primal Lagrangian relaxation
%         technique. Such problems are NP-hard. The optimization problem
%         being solved is
%         minimize (or maximize)
%         \sum_{i=1}^{n1}\sum_{j=1}^{n2}\sum_{k=1}^{n3}C_{i,j,k}*\rho_{i,j,k}
%         subject to
%         \sum_{i=1}^{n1}\sum_{j=1}^{n2}\rho_{i,j,k}<=1 for all k
%         \sum_{i=1}^{n1}\sum_{k=1}^{n3}\rho_{i,j,k}<=1 for all j
%         \sum_{j=1}^{n2}\sum_{k=1}^{n3}\rho_{i,j,k} =1 for all i
%         \rho_{i,j,k} = 0 or 1
%         assuming that n1<=n2<=n3, and C is an n1Xn2Xn3 cost matrix.
%         This is equivalent to the optimization problem
%         min (or max) sum_{i} C(i,phi_2(i),phi_3(i))
%         where phi_2 and phi_3 are length n1 arrangements of n2 and n3
%         items over which the minimization is performed.
%
%INPUTS: C An n1Xn2Xn3 cost hypermatrix with n1<=n2<=n3. C cannot contain
%          any NaNs and the largest finite element minus the smallest
%          element is a finite quantity (does not overflow) when performing
%          minimization and where the smallest finite element minus the
%          largest element is finite when performing maximization.
%          Forbidden assignments can be given costs of +Inf for
%          minimization and -Inf for maximization. During minimization,
%          there should be no elements with -Inf cost and no elements with
%          +Inf cost during maximization.
% maximize If true, the minimization problem is transformed into a
%          maximization problem. The default if this parameter is omitted
%          or an empty matrix is passed is false.
% subgradMethod A parameter indicating the subgradient optimization
%          algorithm to use. Possible values are:
%          0 Polyak's Method from [2]. (The default if omitted or an empty
%            matrix is passed) In this method, for minimization, the dual
%            variables are updated as uNew=u+gamma*(fStar-q)/norm(g)^2*g,
%            where g is the subgradient, q is the current dual cost, and g
%            is the subgradient vector. gamma is a design parameter.
%          1 Bragin's Method from [3]. The dual update for minimization is
%            uNew=u+alpha*g
%            where during the first step, alpha=(fStar-q)/norm(g)^2 and for
%            subsequent steps it is
%            alpha=(1-1/(M*k^(1-1/k^r)))*alphaPrev*norm(gPrev)/norm(g)
%            where alphaPrev is alpha for the previous step, k is the step
%            number (starting at 0), gPrev is the subgradient before the
%            current step and M and r are design parameters.
%          2 Bertsekas' Heuristic Method from [4]. The update is
%            uNew=u+alpha*g
%            where alpha=((1+a/beta^b)*qMax-q)/norm(g)^2 where qMax is the
%            highest dual cost value encountered thus far (when
%            minimizing), a and b are design parameters, and beta is a
%            value that increases or decreases in the algorithm depending
%            on whether or not there was an improvement in the best dual
%            cost found after the last step.
%          3 Shor's Space Dilation Algorithm from [5].
%            The update is
%            uNew=u+alpha*H*g;
%            where alpha=(2*M/(M+1))*(fStar-q)/norm(d), d=g and H is a
%            matrix that also depends on M and d, which is a design
%            parameter. Also, after NR iterations, the recursive turms that
%            go into H reset.
%          4 The r Space Dilation Algorithm from [5]. This is the same as 3
%            except d=g-gPrev except for the first iteration and if
%            g=gPrev, in which case d=g.
% subgradParams This is a structure that takes the design parameters for
%          the selected cubgradient algorithm. Possible fields of the
%          structure as well as default values depend on the selected
%          subgradient method and are:
%          Method 0: 'gamma' with default 1.
%          Method 1: 'M' and 'r' with defaults 2.8 and 0.06.
%          Method 2: 'a and 'b' with defaults 0.3 and 1.5.
%          Method 3,4: 'M', 'NR', and 'normBound' with defaults 2, n3, and
%                     eps(). normBound is such that if norm(B.'*d) in the
%                     computation of the matrix H is <= normBound, then B
%                     is reset to the identity matrix.
%  maxIter The maximum number of iterations to perform. The default value
%          if this parameter is omitted or an empty matrix is passed is 20.
%   AbsTol The absolute duality gap to use for convergence determiniation.
%          Convergence is declared if (fStar-qStar)<=AbsTol, where if
%          minimizing, fStar is the smallest value of the primal function
%          found and qStar is the highest value of the dual cost function
%          found. The default if omitted or an empty matrix is passed is
%          1e-10.
%   RelTol The relative duality gap to use for convergence determiniation.
%          Convergence is declared if (fStar-qStar)<=RelTol*abs(qStar). The
%          default if omitted or an empty matrix is passed is 0.05.
%
%OUTPUTS: tuples A 3Xn1 matrix where tuples(:,i) is the ith assigned tuple
%                in C as described above. An empty matrix is returned if
%                the problem is infeasible or the heuristic to obtain a
%                feasible solution failed.
%          fStar The cost of the assignment in tuples. This is the sum of
%                the values of the assigned elements in C.
%          qStar The value of the best dual solution found. The absolute
%                duality gap is fStar-qStar and the relative duality gap is
%                abs(fStar-qStar)/abs(qStar)
%              u The n3X1 vector of dual variables.
%       exitCode A parameter indicating how the algorithm terminated.
%                Possible values are:
%                -3 A non-finite number arose in the dual variables, so the
%                   algorithm stopped.
%                -2 The algorithm did not converge within the alotted
%                   number of iterations. However, a valid feasible
%                   assignment was found.
%                -1 A subproblem in computing the dual or in obtaining a
%                   feasible primal solution was infeasible.The output
%                   tuples will be an empty matrix.
%                >=0 Values that are zero or positive indicate the
%                   convergence was obtained and the returned value is the
%                   number of iterations.
%
%This function implements the algorithm of [1], but modified so that it
%does not have any unconstrained indices and offering different subgradient
%methods. We are solving the "operations research" 3D assignment problem
%rather than the "data fusion" 3D assignment problem of [1].
%
%EXAMPLE:
%Here is an example using a random problem:
% n1=10;
% n2=12;
% n3=15;
% C=randn(n1,n2,n3);
% maximize=false;
% [tuples,fStar,qStar,u,exitCode]=assign3D(C,maximize)
%
%REFERENCES:
%[1] K. Pattipati, S. Deb, Y. Bar-Shalom, and R. B. Washburn Jr., "A
%    new relaxation algorithm and passive sensor data association," IEEE
%    Transactions on Automatic Control, vol. 37, no. 2, pp. 198-213, Feb.
%    1992.
%[2] B. T. Polyak, "Minimization of unsmooth functionals," USSR
%    Computational Mathematics and Mathematical Physics, vol. 9, no. 3, pp.
%    14-29, 1969.
%[3] M. A. Bragin, P. B. Luh, J. H. Yan, N. Yu, and G. A. Stern,
%    "Convergence of the surrogate Lagrangian relaxation method," Journal
%    of Optimization Theory and Applications, vol. 164, no. 1, pp. 173-201,
%    Jan. 2015.
%[4] D. P. Bertsekas, Nonlinear Programming, 3rd ed. Belmont, MA: Athena
%    Scientific, 2016, Chapter 7.5.
%[5] N. Z. Shor, "Utilization of the operation of space dilation in the
%    minimization of convex functions," Cybernetics, vol. 6, no. 1, pp. 7-
%    15, Dec. 1972.
%
%February 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<2||isempty(maximize))
   maximize=false;
end

if(nargin<3||isempty(subgradMethod))
    subgradMethod=0;
end

if(nargin<4)
    subgradParams=[];
end

if(nargin<5||isempty(maxIter))
    maxIter=20;
end

if(nargin<6||isempty(AbsTol))
    AbsTol=1e-10;
end

if(nargin<7||isempty(RelTol))
    RelTol=0.05;
end

nVals=size(C);

%Deal with the special case of C being scalar.
if(isscalar(C))
    tuples=[1;1;1];
    fStar=C(1);
    qStar=fStar;
    u=0;
    exitCode=0;
    return;
end

if(isempty(C))%The empty matrix special case.
    tuples=[];
    fStar=0;
    qStar=0;
    u=[];
    exitCode=0;
    return;
end

if(maximize)
    C=-C;
end

if(length(nVals)<3||~(nVals(1)<=nVals(2)&&nVals(2)<=nVals(3)))
    error('It is required that size(C,1)<=size(C,2)<=size(C,3)')
end

n1=nVals(1);
n2=nVals(2);
n3=nVals(3);

if((n1==n2)&&(n2==n3))
    unconstU=true;
else
    unconstU=false;
end

%Get the parameters and initial values for the chosen subgradient method.
switch(subgradMethod)
    case 0%Polyak's method
        gammaParam=1;

        if(nargin>3&&~isempty(subgradParams))
            if(isfield(subgradParams,'gamma'))
                gammaParam=subgradParams.gamma;
            end
        end
    case 1%Bragin's Method
        M=2.8;
        r=0.06;
        gNormPrev=[];
        alphaPrev=[];

        if(nargin>3&&~isempty(subgradParams))
            if(isfield(subgradParams,'M'))
                M=subgradParams.M;
            end

            if(isfield(subgradParams,'r'))
                r=subgradParams.r;
            end
        end
    case 2%Bertsekas' Heuristic Method
        a=0.3;
        b=1.5;
        beta=1;%Initialize

        if(nargin>3&&~isempty(subgradParams))
            if(isfield(subgradParams,'a'))
                a=subgradParams.a;
            end

            if(isfield(subgradParams,'b'))
                b=subgradParams.b;
            end
        end
    case 3%Shor's Space Dilation Algorithm.
        NR=n3;
        M=2;
        rho=(M-1)/(M+1);
        normBound=eps();

        B=eye(n3,n3);%Allocate and initialize
        dPrev=[];

        if(nargin>3&&~isempty(subgradParams))
            if(isfield(subgradParams,'NR'))
                NR=subgradParams.NR;
            end

            if(isfield(subgradParams,'M'))
                M=subgradParams.M;
            end

            if(isfield(subgradParams,'normBound'))
                normBound=subgradParams.normBound;
            end
        end
    case 4%The r Space Dilation Algorithm.
        NR=n3;
        M=2;
        rho=(M-1)/(M+1);
        normBound=eps();

        B=eye(n3,n3);%Allocate and initialize
        gPrev=[];
        dPrev=[];

        if(nargin>3&&~isempty(subgradParams))
            if(isfield(subgradParams,'NR'))
                NR=subgradParams.NR;
            end

            if(isfield(subgradParams,'M'))
                M=subgradParams.M;
            end

            if(isfield(subgradParams,'normBound'))
                normBound=subgradParams.normBound;
            end
        end
    otherwise
        error('Invalid subgradient method specified.')
end

u=zeros(n3,1);%Allocate and initialize the dual variables.
%Allocate space for the return tuples.
tuples=zeros(3,n1);

%qStar is the best (maximum) dual cost encountered thus far.
qStar=-Inf;

%fStar is the best (minimum) feasible primal cost encountered
%thus far.
fStar=Inf;
%If the loop exits without convergence, then exitCode is not modified.
exitCode=-2;
for k=0:(maxIter-1)
%%%%%%
%DUAL COST AND SUBGRADIENT UPDATE. SEE FIG. 3 IN [1].
%%%%%%
    %Note that d3=C;
    [d2,gamma2]=min(bsxfun(@plus,C,reshape(u,[1,1,n3])),[],3);

    %gamma1 is the columns for rows (nonzero elements in omega).
    [gamma1, ~, minVal]=assign2D(d2,false);

    %If the subproblem is infeasible.
    if(isempty(gamma1))
        tuples=[];
        fStar=[];
        qStar=[];
        u=[];
        exitCode=-1;
        return
    end

    q=minVal-sum(u);%The dual cost.

    %Keep track of the maximum q value. This is used for testing
    %convergence.
    if(q>qStar)
        qStar=q;

        %The duality gap.
        costGap=fStar-qStar;

        %The correctness of this on the first iterations requires proper
        %handling of NaNs.
        if(costGap<=AbsTol||(costGap<abs(qStar)*RelTol))
            exitCode=k+1;%The algorithm converged.
            break;
        end
    end

    %Compute the subgradient
    g=-1*ones(n3,1);
    for i1=1:n1
        i2=gamma1(i1);
        i3=gamma2(i1,i2);

        g(i3)=g(i3)+1;
    end

%%%%%%
%TEST THE DUAL VARIABLES AND FINISH THE LOOP. See Fig. 2 in [1].
%%%%%%
    gNorm2=dot(g,g);

    %If no constraints are violated, then we have a feasible solution and
    %the algorithm has converged to a local or global minimum point.
    if(gNorm2==0)
        %If the dual cost is less than the best primal solution found with
        %a heuristic, then we use the tuples for that solution. Otherwise,
        %the best primal solution to this point is returned.
        if(q<fStar)
            fStar=q;
            %Record the tuples.
            for i1=1:n1
                i2=gamma1(i1);
                i3=gamma2(i1,i2);
                tuples(:,i1)=[i1;i2;i3];
            end
        end

        exitCode=k+1;%The algorithm converged.
        break;
    end

    [tuples,fStar,retVal]=updateBestFeasSol3D(C,gamma1,tuples,fStar,qStar,AbsTol,RelTol);

    %If the primal converged.
    if(retVal==0)
        exitCode=k+1;
        break;
    end

    %If an error occurred obtaining a feasible solution.
    if(retVal<0)
        tuples=[];
        fStar=[];
        qStar=[];
        u=[];
        exitCode=retVal;
        return;
    end

    %Perform the subgradient update.
    switch(subgradMethod)
        case 0%Polyak's method
            alpha=gammaParam*((fStar-q)/gNorm2);
            u=u+alpha*g;
        case 1%Bragin's Method
            gNorm=sqrt(gNorm2);
            if(k==0)
                alpha=((fStar-q)/gNorm2);
            else
                alpha=(1-1/(M*k^(1-k^(-r))))*alphaPrev*(gNormPrev/gNorm);
            end
            u=u+alpha*g;

            alphaPrev=alpha;
            gNormPrev=gNorm;
        case 2%Bertsekas' Heuristic Method
            %Note that we do not have to keep track of qStar value from the
            %previous iteration. Though in [1], the comparison is with the
            %previous qStar, if q>qStarPrev, then qStar will be set to q,
            %so the comparison q<qStarPrev has the same result as q<qStar
            %and if q<=qStarPrev, then qStarPrev=qStar, so the comparison
            %is still correct.
            if(q<qStar)
                beta=beta+1;
            else
                beta=max(beta-1,1);
            end

            alpha=abs(((1+a)/(beta^b))*qStar-q)/gNorm2;

            u=u+alpha*g;
        case 3%Shor's Space Dilation Algorithm.
            d=g;

            %The H matrix is initialized on the first iteration and reset
            %to the identity matrix every NR iterations.
            if(mod(k,NR)==0)
                B=eye(n3,n3);
                normVal=norm(d);%=norm(B'*d)
            else
                zeta=B.'*dPrev/norm(B.'*dPrev);
                R=eye(n3,n3)+(rho-1)*(zeta*zeta.');
                B=B*R;
                normVal=norm(B.'*d);
                if(normVal<=normBound)
                    B=eye(n3,n3);
                    normVal=norm(d);
                end
            end
            H=(B*B.')/normVal;

            alpha=(2*M/(M+1))*((fStar-q)/norm(d));

            u=u+alpha*H*d;

            dPrev=d;
        case 4%The r Space Dilation Algorithm.
            if(k==0||all(g==gPrev))
                d=g;
            else
                d=g-gPrev;
            end

            %The H matrix is initialized on the first iteration and reset
            %to the identity matrix every NR iterations.
            if(mod(k,NR)==0)
                B=eye(n3,n3);
                normVal=norm(d);%=norm(B'*d)
            else
                zeta=B.'*dPrev/norm(B.'*dPrev);
                R=eye(n3,n3)+(rho-1)*(zeta*zeta.');
                B=B*R;
                normVal=norm(B.'*d);
                if(norm(B.'*d)<=normBound)
                    B=eye(n3,n3);
                    normVal=norm(d);
                end
            end
            H=(B*B.')/normVal;

            alpha=(2*M/(M+1))*((fStar-q)/norm(d));

            u=u+alpha*H*d;

            gPrev=g;
            dPrev=d;
        otherwise
            error('Invalid subgradient method specified.')
    end

    if(any(~isfinite(u)))
        %This can sometimes occur with big problems and poor stepsizes.
        exitCode=-3;
        break;
    end

    %Unless the constraints are equality constraints, the dual variables
    %have to be clipped.
    if(unconstU==false)
        u(u<0)=0;
    end
end

%Adjust the gains for the case where the initial cost matrix is transformed
%so that maximization can be performed.
if(maximize)
    fStar=-fStar;
    qStar=-qStar;
    u=-u;
end
end

function [optTuples,fStar,exitCode]=updateBestFeasSol3D(C,gamma1,optTuples,fStar,qStar,AbsTol,RelTol)
%%UPDATEBESTFEASSOL3D This function implements a subroutine to
%         heuristically obtain a feasible solution given a partial set of
%         assignments. Here, it is just for the 3D assignment problem.
%         Additionally, this function updates the best feasible solution
%         cost fStar and its associated set of tuples optTuples.
%
%INPUTS: C The original n1Xn2Xn3 3D cost matrix.
%   gamma1 gamma1(i1) gives the value of n2 for each value of i1.
% optTuples The set of tuples associated with the current best primal cost.
%    fStar The current best (lowest) primal cost function value found.
%    qStar The current best (highest) dual cost function value found.
%   AbsTol The absolute threshold for determining convergence of the
%          algorithm according to the duality gap.
%   RelTol The threshold for determining convergence of the algorithm
%          according to the relative duality gap.
%
%OUTPUTS: optTuples The updated set of tuples associated with fStar.
%             fStar The updated best cost function found thus far.
%          exitCode A value indicating the terminating state of the
%                   function. Possible values are:
%                  -1 The subproblem posted is not feasible.
%                   0 Convergence achieved.
%                   1 No errors occurred, but convegrence was not achieved.
%
%February 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.

    n1=size(C,1);
    n3=size(C,3);

    CFeas=zeros(n1,n3);
    for i1=1:n1
        CFeas(i1,:)=reshape(C(i1,gamma1(i1),:),[1,n3]);
    end

    [gammaTilde3,~,f]=assign2D(CFeas,false);

    %If the subproblem is not feasible.
    if(isempty(gammaTilde3))
        exitCode=-1;
        return;
    end

    if(f<fStar)
        fStar=f;

        for i1=1:n1
           optTuples(:,i1)=[i1;gamma1(i1);gammaTilde3(i1)];
        end

        %The duality gap
        costGap=fStar-qStar;

        if(costGap<=AbsTol||(costGap<abs(qStar)*RelTol))
            exitCode=0;%The algorithm converged.
        else
            exitCode=1;
        end
    else
        exitCode=1;
    end
end

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


function Ref = RefSelect(Population,k)
% Reference solutions selection by RSEA strategy

%------------------------------- 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".
%--------------------------------------------------------------------------

% This function is written by Cheng He

    k      = min(k,length(Population));
    PopObj = Population.objs;
	[FrontNO,MaxFNO] = NDSort(PopObj,k);
    Next = find(FrontNO<=MaxFNO);
    Pmin = min(PopObj,[],1) + 1e-6;
    Pmax = max(PopObj,[],1);
    if Pmax > Pmin
        PopObj = (PopObj-repmat(Pmin,size(PopObj,1),1))./repmat(Pmax-Pmin,size(PopObj,1),1);
    end

    %% Environmental selection
    Choose = LastSelection(PopObj(Next,:),ismember(Next,find(FrontNO<MaxFNO)),ceil(sqrt(k)),k);
    Ref    = Population(Next(Choose));
end

function Choose = LastSelection(PopObj,Choose,div,k)
% Select part of the solutions based on the radar grid

    %% Identify the extreme solutions
	[~,Extreme] = min(sqrt(sum(PopObj.^2,2)).*sqrt(1-(1-pdist2(PopObj,ones(1,size(PopObj,2)),'cosine')).^2),[],1); %Calculate the extreme points based on PBI
    Choose      = Choose | ismember(1:size(PopObj,1),Extreme);

    %% Calculate the convergence of each solution
	Con = sum(PopObj.^1,2).^1;
    Con = Con./max(Con);

    %% Calculate the radar grid of each solution
    [Site,RLoc] = RadarGrid(PopObj,div);
    RDis        = pdist2(RLoc,RLoc);
    RDis(logical(eye(length(RDis)))) = inf;
    CrowdG      = zeros(1,max(Site));
    temp        = tabulate(Site(Choose));
    CrowdG(temp(:,1)) = temp(:,2);

    %% Select k solutions
    while sum(Choose) < k
        % Delete outline solutions
        remainS  = find(~Choose);
        remainG  = unique(Site(remainS));
        bestG    = CrowdG(remainG) == min(CrowdG(remainG));
        current  = remainS(ismember(Site(remainS),remainG(bestG)));
        fitness  = 0.1.*size(PopObj,2).*Con(current) - min(RDis(current,Choose),[],2); % - 0.1.* min(Dis(current,Choose),[],2);
        [~,best] = min(fitness);
        Choose(current(best))       = true;
        CrowdG(Site(current(best))) = CrowdG(Site(current(best))) + 1;
    end
end


function sigma = sigupdate(nrad,nphi,nits,vol,mu0,maxrad,polafm2,lastsigma)
%   sigupdate: update RG kernel parameters
% G.A. Reina 16 Jan 2007
% Modified from the C code provided by D. L. Jones and R. G. Baraniuk
% "An Adaptive Optimal-Kernel Time-Frequency Representation"
%   by D. L. Jones and R. G. Baraniuk, IEEE Transactions on Signal
%   Processing, Vol. 43, No. 10, pp. 2361--2371, October 1995.

sigma = lastsigma;

for ii=0:(nits-1),
    gradsum = 0.0;
    gradsum1 = 0.0;

    for i=0:(nphi-1),
        grad(i+1) = 0.0;

        ee1 = exp( - 1.0/(sigma(i+1).^2) );	% use Kaiser's efficient method
        ee2 = 1.0;
        eec = ee1*ee1;

        for j=1:(maxrad(i+1)-1)
            ee2 = ee1*ee2;
            ee1 = eec*ee1;
            grad(i+1) = grad(i+1) + (j.^3)*ee2*polafm2(j+1, i+1);
        end
        grad(i+1) = grad(i+1)/(sigma(i+1).^3);

        gradsum = gradsum + grad(i+1).^2;
        gradsum1 = gradsum1 + sigma(i+1)*grad(i+1);

    end

    gradsum1 = 2*gradsum1;

    if ( gradsum < 0.0000001 )
        gradsum = 0.0000001;
    end

    if ( gradsum1 < 0.0000001 )
        gradsum1 = 0.0000001;
    end

    mu = ( sqrt(gradsum1.^2 + 4.0*gradsum*vol*mu0) - gradsum1 ) / ( 2.0*gradsum );

    sigma = sigma + mu*grad;
    sigma(sigma < 0.5) = 0.5;
    tvol = sum(sigma.^2);

    volfac = sqrt(vol/tvol);

    sigma = volfac*sigma;

end


function  [Truecol,x,y]=m_shadedrelief(x,y,Z,varargin)
% M_SHADEDRELIEF Shaded relief topography in an image
%  M_SHADEDRELIEF(X,Y,Z) presents a shaded relief topography, as would
%  be seen if a 3D model was artifically lit from the side. Slopes
%  facing the light are lightened, and slopes facing away from the
%  light are darkened. X and Y are horizontal and vertical coordinate
%  VECTORS, and these should be in the same units as the height Z
%  (e.g., all in meters), otherwise the slope angle calculations will be
%  in error.
%
%  Usage notes:
%
%  (1) M_SHADEDRELIEF is a replacement for a low-level call to IMAGE
%  displaying a true-colour image so it MUST be preceded by COLORMAP and
%  CAXIS calls.
%
%  (2) M_SHADEDRELIEF probably is most useful as a backdrop to maps with
%  a rectangular outline box - either a cylindrical projection, or some
%  other projection with M_PROJ(...'rectbox','on').
%
%  (3) Finally, the simplest way of not running into problems:
%     - if your elevation data is in LAT/LON coords (i.e. in a matrix where
%       each row has points with the same latitude, and each column has points
%       with the same longitude), use
%                  M_PROJ('equidistant cylindrical',...)
%     - if your elevation data is in UTM coords (meters E/N), i.e. in a matrix
%       where each row has the same UTM northing and the each column has the
%       same UTM easting, use
%                  M_PROJ('utm',....)
%
%  M_SHADEDRELIEF(...,'parameter',value) lets you set various properties.
%  These are:
%       'coords' : Coordinates of X/Y/Z:
%                     'geog' for lat/lon, Z meters,  (default)
%                     'map'  for X/Y map coordinates, Z meters
%                     'Z' if X/Y/Z are all in same units (e.g., meters)
%       'lightangle' : true direction (degrees) of light source (default
%                      -45, i.e. from the north-west)
%       'gradient': Shading effects increase with slope angle
%                   until slopes reach this value (in degrees), and are
%                   held constant for higher slopes (default 10). Reduce
%                   for smoother surfaces.
%       'clipval' : Fractional change in shading for slopes>='gradient'.
%                  0 means no change, 1 means saturation to white or black
%                  if slope is facing directly towards or away from light
%                  source, (default 0.9).
%       'nancol'  : RGB colour of NaN values (default [1 1 1]);
%       'lakecol' : RGB colour of lakes (flat sections) (default NaN)
%                   If set to NaN lakes are ignored.
%
%   IM=M_SHADEDRELIEF(...) returns a handle to the image.
%
%   [SR,X,Y]=m_SHADEDRELIEF(...) does not create an image but only returns
%   the  true-color matrix SR of the shaded relief, as well as the X/Y
%   vectors needed to display it.
%
%   Example:
%           load topo
%           subplot(2,1,1);  % Example without it
%           imagesc(topolonlim,topolatlim,topo);
%           caxis([-5000 5000]);
%           colormap([m_colmap('water',64);m_colmap('gland',64)]);
%           set(gca,'ydir','normal');
%
%           subplot(2,1,2);  % Example with it
%           caxis([-5000 5000]);
%           colormap([m_colmap('water',64);m_colmap('gland',64)]);
%           m_shadedrelief(topolonlim,topolatlim,topo,'gradient',5e2,'coord','Z');
%           axis tight
%

% Rich Pawlowicz (rich@eoas.ubc.ca) Dec/2017
%
% This software is provided "as is" without warranty of any kind. But
% it's mine, so you can't sell it.
%
% Changes:
% Jan/2018 - changed outputs for flexibility
%            and added 'map' coordinate handling
% Mar/2019 - added alphamapping for out-of-map areas, started using colormap
%            local to AXES not to FIGURE.
% Apr/2019 - some parts relied on the ones-expansion; went back to meshgrid
%            for compatibility with older matlab versions (thanks P. Grahn)

global MAP_PROJECTION MAP_VAR_LIST

lighthead=-45;
gradfac=10;
clipval=.9;
nancol=[1 1 1];
lakecol=NaN; %[.7 .9 1];
geocoords='geog';
scfac=6400000;  % Used for geo coordinates if on sphere radius 1

while ~isempty(varargin)
    switch lower(varargin{1}(1:3))
        case 'coo'
            switch lower(varargin{2}(1))
                case 'g'
                    geocoords='geog';
                case 'm'
                    geocoords='map';
                case {'z','u'}
                    geocoords='z';
                otherwise
                    error('Unknown coordinate specification');
            end
        case 'lig'
            lighthead=varargin{2};
        case 'gra'
            gradfac=varargin{2};
        case 'cli'
            clipval=varargin{2};
        case 'nan'
            nancol=varargin{2};
        case 'lak'
            lakecol=varargin{2};
        otherwise
	        error(['m_shadedrelief: Unknown option: ' varargin{1}]);
     end
     varargin(1:2)=[];
end

% All kinds of issues dealing with coords:
% First, we need VECTOR x/y as an input to gradient function.

if isvector(x) && size(x,2)==1
     x=x';
elseif ~isvector(x)       % Can't be a matrix
    error('Input X must be a VECTOR');
end
if isvector(y) && size(y,1)==1
    y=y';
elseif ~isvector(y)
    error('Input Y must be a VECTOR');
end

%  Now handle the case if we just give a starting and an ending point
if length(x)==2
    x=linspace(0,1,size(Z,2))*diff(x)+x(1);
end

if length(y)==2
    y=linspace(0,1,size(Z,1))'*diff(y)+y(1);
end

if strcmp(geocoords,'geog')                           %  If its Lat/Long points

    % Have to have initialized a map first
    if isempty(MAP_PROJECTION)
      disp('No Map Projection initialized - call M_PROJ first!');
      return;
    end

    % Convert to X/Y
    [X,Y]=m_ll2xy(x(1,:),repmat(mean(y(:,1)),1,size(x,2)));
    [X2,Y2]=m_ll2xy(repmat(mean(x(1,:)),size(y,1),1),y(:,1));
    x=X;
    y=Y2;

end

% Note - 'image' spaces points evenly, so we should just check that they
% are even otherwise the image won't line up with coastlines...

if max(abs( x - linspace(x(1),x(end),length(x)) ) )/abs(x(end)-x(1)) >.005
    warning(['********** Image will be distorted in X direction!! use M_IMAGE to re-map? *************']);
end

if max(abs( y - linspace(y(1),y(end),length(y))' ) )/abs(y(end)-y(1)) >.005
    warning(['********** Image will be distorted in Y direction!! use M_IMAGE to re-map? *************']);
end

% Convert colours to uint8s
if all(nancol<=1)
    nancol=uint8(nancol*255);
end

% Convert colours to uint8s
if all(lakecol<=1)
    lakecol=uint8(lakecol*255);
end

% Get caxis

clims=caxis;
if all(clims==[0 1])   % Not set
    clims=[min(Z(:)) max(Z(:))];
end

% Get colormap for the current axes
cc=colormap(gca);
cc2=round(cc*255);  % we need these in 0-255 range to get Truecolor
lcc=size(cc,1);

%inan=isnan(Z);

% Get slopes
% If we are using a normal ellipsoid we need to rescale
% x/y to get true slope angles
if  (strcmp(geocoords,'map') || strcmp(geocoords,'geog')) && strcmp(MAP_VAR_LIST.ellipsoid,'normal')
   scfac=6370997;
   [Fx,Fy]=gradient( Z, x*scfac, y*scfac);
else
   [Fx,Fy]=gradient( Z, x, y);
end

% Find NaN
[inan,jnan]=find(isnan(Z) | isnan(Fx) | isnan(Fy) );

% Probable lakes
[islake,jlake]=find(Fx==0 & Fy==0);

% Convert z levels into a colormap index.

% Some iteration to discover the exact formula that matlab uses for mapping to
% color indices (from 1 to lcc)
%idx=min( floor( min(max( (Z-clims(1))/(clims(2)-clims(1)),0) ,1 )*lcc )+1,lcc);
idx=max(min( floor(   (Z-clims(1))/(clims(2)-clims(1))*lcc  )+1  ,lcc),1);

% The slope angle relative to the light direction in degrees.
Fnw=atand(imag(-(Fx+i*Fy)*exp(i*lighthead*pi/180)));

%Put an upper and lower limit on the angles
%%Fnw=min(clipval,max(-clipval,Fnw/gradfac));
Fnw=clipval*tanh(Fnw/gradfac);

% Now get the colormap for each pixel and scale the RGB value 'c'.
% If the correction is -0.1 then scale  c*(1 - |-0.1|)
% If the correction is  0.1 then scale  c*(1 - |+0.1|) + 0.1*255
%depending on the slope.

%Truecol=uint8(max(0,min(255,   reshape([cc2(idx,:)],[size(idx) 3]).*repmat(1-abs(Fnw),1,1,3)+repmat(255*Fnw.*(Fnw>0),1,1,3) ) ));
Truecol=uint8( reshape([cc2(idx,:)],[size(idx) 3]).*repmat(1-abs(Fnw),1,1,3)+repmat(255*Fnw.*(Fnw>0),1,1,3) ) ;

%Truecol=uint8(max(0,min(255,   reshape([cc2(idx,:)],[size(idx) 3]).*repmat(1+Fnw/gradfac,1,1,3) ) ));

% Colour Lakes
if any(islake) && isfinite(lakecol(1))
    Truecol(sub2ind(size(Truecol),islake,jlake,  ones(size(islake))))=lakecol(1);
    Truecol(sub2ind(size(Truecol),islake,jlake,1+ones(size(islake))))=lakecol(2);
    Truecol(sub2ind(size(Truecol),islake,jlake,2+ones(size(islake))))=lakecol(3);
end

% Colour the NaNs
if any(inan)
    Truecol(sub2ind(size(Truecol),inan,jnan,  ones(size(inan))))=nancol(1);
    Truecol(sub2ind(size(Truecol),inan,jnan,1+ones(size(inan))))=nancol(2);
    Truecol(sub2ind(size(Truecol),inan,jnan,2+ones(size(inan))))=nancol(3);
end

if strcmp(geocoords,'map')   % Have to "make invisible" the points outside the map limits.

    [xm,ym]=meshgrid(x,y);
    [HLG,HLT]=m_xy2ll(xm,ym);

    % Find pixels outside the limits of the actual map (if the boundary
    % isn't a rectangle)
    if strcmp(MAP_VAR_LIST.rectbox,'off')
        [I,J]=find(HLT<MAP_VAR_LIST.lats(1) | HLT>MAP_VAR_LIST.lats(2) | HLG<MAP_VAR_LIST.longs(1) | HLG>MAP_VAR_LIST.longs(2));
    elseif strcmp(MAP_VAR_LIST.rectbox,'circle')
        R=(xm.^2 +ym.^2);
        [I,J]=find(R>MAP_VAR_LIST.rhomax.^2);
    else
        I=[];J=[];
    end

    backcolor=uint8(get(gcf,'color')*255);

   if any(I)                          % if some pixels are outside the map area
       for k=1:3
           IJ=sub2ind(size(Truecol),I,J,repmat(k,size(I)));
           Truecol(IJ)=backcolor(k);           % Set them to the background colour
       end
   end
else
    I=[];J=[];
end

if nargout<=1
  x_ok = ~isnan(x);
  y_ok = ~isnan(y);
  if any(I)  % make pixels outside the map area transparent, if needed.
     alphadata=  ones(size(Truecol,1),size(Truecol,2),'logical');
     IJ=sub2ind(size(alphadata),I,J);
     alphadata(IJ)=0;
     Truecol=image('xdata',x(x_ok),'ydata',y(y_ok),'cdata', Truecol(y_ok, x_ok,:),'alphadata',alphadata,'tag','m_shadedrelief');
  else
     Truecol=image('xdata',x(x_ok),'ydata',y(y_ok),'cdata', Truecol(y_ok, x_ok,:),'tag','m_shadedrelief');
  end
end


classdef MOSD < ALGORITHM
% <multi> <real> <large/none> <constrained/none>
% Multiobjective steepest descent
% step --- 0.1 --- Step size

%------------------------------- Reference --------------------------------
% X. Liu and A. C. Reynolds, A multiobjective steepest descent method with
% applications to optimal well control, Computational Geosciences, 2016,
% 20: 355-374.
%------------------------------- 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
            step = Algorithm.ParameterSet(0.1);

            %% Generate random population
            Population = Problem.Initialization(Problem.N);
            Archive    = UpdateArchive(Population,Problem.N);
            step0      = step;

            %% Optimization
            while Algorithm.NotTerminated(Archive)
                for i = 1 : Problem.N
                    gk  = FiniteDifference(Problem,Population(i));
                    gk1 = norm(gk(:,1));
                    gk2 = norm(gk(:,2));
                    if gk1 <= gk2
                        gs = gk(:,1);
                        gl = gk(:,2);
                    else
                        gs = gk(:,2);
                        gl = gk(:,1);
                    end
                    if gs'*(gs-gl) <= 0
                        d = -gs/norm(gs);
                    else
                        D = (((norm(gl).^2-gs'*gl)/(norm(gs).^2-gs'*gl))*(-gs))-gl;
                        d = D/norm(D);
                    end
                    Offspringdec = Population(i).dec+step*d';
                    Offspring    = Problem.Evaluation(Offspringdec);
                    Archive      = UpdateArchive([Archive,Offspring],Problem.N);
                    if ~any(Offspring.obj<Population(i).obj)
                        Population(i) = Archive(randi(end));
                        step = step/2;
                    else
                        Population(i) = Offspring;
                        step = min([2*step,step0]);
                    end
                end
            end
        end
    end
end

function df = FiniteDifference(Problem,X)
    if any(X.con>0)
        df = Problem.CalConGrad(X.dec)';
    else
        df = Problem.CalObjGrad(X.dec)';
    end
end

function P = UpdateArchive(P,N)
    P = P(NDSort(P.objs,P.cons,1)==1);
    if length(P) > N
        Choose = true(1,length(P));
        Dis    = pdist2(P.objs,P.objs);
        Dis(logical(eye(length(Dis)))) = inf;
        while sum(Choose) > N
            Remain   = find(Choose);
            Temp     = sort(Dis(Remain,Remain),2);
            [~,Rank] = sortrows(Temp);
            Choose(Remain(Rank(1))) = false;
        end
        P = P(Choose);
    end
end


function [x,mn,mx]=melbankm(p,n,fs,fl,fh,w)
if nargin < 6
    w='tz';
    if nargin < 5
        fh=0.5;
        if nargin < 4
            fl=0;
        end
    end
end
f0=700/fs;
fn2=floor(n/2);
lr=log((f0+fh)/(f0+fl))/(p+1);
bl=n*((f0+fl)*exp([0 1 p p+1]*lr)-f0);
b2=ceil(bl(2));
b3=floor(bl(3));
if any(w=='y')
    pf=log((f0+(b2:b3)/n)/(f0+fl))/lr;
    fp=floor(pf);
    r=[ones(1,b2) fp fp+1 p*ones(1,fn2-b3)];
    c=[1:b3+1 b2+1:fn2+1];
    v=2*[0.5 ones(1,b2-1) 1-pf+fp pf-fp ones(1,fn2-b3-1) 0.5];
    mn=1;
    mx=fn2+1;
else
    b1=floor(bl(1))+1;
    b4=min(fn2,ceil(bl(4)))-1;
    pf=log((f0+(b1:b4)/n)/(f0+fl))/lr;
    fp=floor(pf);
    pm=pf-fp;
    k2=b2-b1+1;
    k3=b3-b1+1;
    k4=b4-b1+1;
    r=[fp(k2:k4) 1+fp(1:k3)];
    c=[k2:k4 1:k3];
    v=2*[1-pm(k2:k4) pm(1:k3)];
    mn=b1+1;
    mx=b4+1;
end
if any(w=='n')
    v=1-cos(v*pi/2);
elseif any(w=='m')
    v=1-0.92/1.08*cos(v*pi/2);
end
if nargout > 1
    x=sparse(r,c,v);
else
    x=sparse(r,c+mn-1,v,p,1+fn2);
end


% This script compares three elliptical extended object trackig methods.
% It gives Fig.2 in
% S. Yang and M. Baum Extended Kalman Filter for Extended Object Tracking.
% Proceedings of the 2017 IEEE International Conference on Acoustics, Speech,
% and Signal Processing (ICASSP), New Orleans, USA, 2017.

close all
clc
clear
dbstop warning
set(0,'defaulttextinterpreter','latex')

%% parameters
motionmodel = {'NCV'};
% nr of measurements we get follows a possion distribution
possion_lambda = 5;
H = [1 0 0 0; 0 1 0 0]; % matrix maps kinematic state into position
H_velo = [zeros(2,2),eye(2)];
% parameters for EKF
C_h = diag([1/4, 1/4]);
C_v = 0.2*diag([100^2,20^2]);

%% generate ground truth
[gt_kin,gt_par, time_steps, delta_t] =get_ground_truth;

%% setting prior
hat_r0 = [100,100,5,-8]'; % kinematic state: position and velocity
hat_p0 = [-pi/3,200,90]'; % shape variable: orientation and semi-axes lengths
hat_x0 = [hat_r0; hat_p0];

C_r0 = blkdiag( 900*eye(2),16*eye(2));
C_p0 = blkdiag(0.02*eye(1),400*eye(2));
C_x0 = blkdiag(C_r0,C_p0);

Ar = [eye(2),delta_t*eye(2); zeros(2,2),eye(2)];
Ap = eye(3);

C_w_r = blkdiag(100*eye(2),eye(2)); % process noise covariance for kinematic state
C_w_p = blkdiag(0.04,0.5*eye(2)); % process noise covariance for shape variable

hat_r_EKF = [100,100,5,-8]'; % kinematic state: position and velocity
hat_p_EKF = [-pi/3,200,90]';
Cr_EKF = C_r0;
Cp_EKF = C_p0;

% parameters for SOEKF
sgh1 = 1/4;
sgh2 = 1/4;
C_w = blkdiag(C_w_r, C_w_p);
Ax = blkdiag(Ar,Ap);

hat_x_SOEKF = hat_x0;
Cx_SOEKF = C_x0;

[ f_g_ekf2, f_jacobian_ekf2, f_hessian_ekf2] = get_jacobian_hessian(motionmodel,C_h);

% parameters for Random Matrix
alpha = 50;
tau = 10;
T = 10;
const_z = 1/4;
hat_x_RMM = hat_r0;
hat_X_RMM = get_random_matrix_state(hat_p0);
Cx_RMM = C_r0;

figure;

hold on

for t = 1:time_steps
          N = poissrnd(possion_lambda);
        while N == 0
            N = poissrnd(possion_lambda);
        end
        disp(['time step:' num2str(t) ', ' num2str(N) ' measurements']);

        %% ------------------get measurements------------------------------------
        gt_cur_par = gt_par(:,t);
        gt_velo = H_velo*gt_kin(:,t);
        gt_rot = [cos(gt_cur_par(3)), -sin(gt_cur_par(3)); sin(gt_cur_par(3)), cos(gt_cur_par(3))];
        gt_len = gt_cur_par(4:5);
        y = zeros(2,N);
        for n = 1:N
            h_noise(n,:) = -1 + 2.*rand(1,2);
            while norm(h_noise(n,:)) > 1
                h_noise(n,:) = -1 + 2.*rand(1,2);
            end
            y(:,n) = H*gt_kin(:,t) + gt_rot*diag(gt_len)*h_noise(n,:)'+ mvnrnd([0 0], C_v, 1)';
        end
    %% update RMM
    meas_mean = mean(y,2);
    meas_spread = (N - 1) * cov(y');
    [hat_x_RMM, hat_X_RMM, C_x_RMM, alpha_update]...
        = updateRMM(hat_x_RMM, hat_X_RMM, Cx_RMM, alpha,meas_mean, ...
        meas_spread, C_v,N,H,const_z);

    [~, len_RMM,ang_RMM] = get_random_matrix_ellipse(hat_X_RMM);
    rmm_par = [H*hat_x_RMM; ang_RMM;len_RMM];
    %% update EKF and SOEKF
    for n = 1:N
        [hat_x_SOEKF, Cx_SOEKF] = updateSOEKF(hat_x_SOEKF, Cx_SOEKF, y(:,n),...
            f_g_ekf2, f_jacobian_ekf2, f_hessian_ekf2, C_v, C_h);
        [ hat_r_EKF, Cr_EKF,hat_p_EKF, Cp_EKF ] = updateEKF(hat_r_EKF, Cr_EKF, hat_p_EKF, Cp_EKF, y(:,n), C_v, C_h);

    end

    %% visulization udpated shapes
    if mod(t,3)==1
        meas_points=plot( y(1,:)/1000, y(2,:)/1000, '.k','lineWidth',0.5);
        hold on
        gt_plot = plot_extent([gt_cur_par(1:2)/1000; gt_cur_par(3);gt_cur_par(4:5)/1000], '-','k',1);
        axis equal

        est_plot_rmm = plot_extent([rmm_par(1:2)/1000;rmm_par(3);rmm_par(4:5)/1000],'-','g',1);
        est_plot_ekf2 = plot_extent([hat_x_SOEKF(1:2)/1000;hat_x_SOEKF(5);hat_x_SOEKF(6:7)/1000],'-', 'r', 1);
        est_plot_ekf = plot_extent([H*hat_r_EKF/1000;hat_p_EKF(1);hat_p_EKF(2:3)/1000],'-', 'b', 1);
    end

    %%  predict　RMM
    [hat_x_RMM, hat_X_RMM,Cx_RMM, alpha] = predictRMM(....
        hat_x_RMM, hat_X_RMM, C_x_RMM, alpha_update,Ar,C_w_r,T,tau);
    if alpha_update<=2
        error('alpha<2')
    end
    %% predict SOEKF
    [hat_x_SOEKF,Cx_SOEKF] = predictSOEKF(Ax,hat_x_SOEKF,Cx_SOEKF,C_w);
     %% predict EKF
    [hat_r_EKF,Cr_EKF, hat_p_EKF,Cp_EKF] = predictEKF(Ar,Ap, hat_r_EKF, hat_p_EKF, Cr_EKF, Cp_EKF,C_w_r, C_w_p);

end
legend([meas_points,gt_plot,est_plot_rmm, est_plot_ekf2,est_plot_ekf],...
    {'measurement','ground truth','random matrix','SOEKF','EKF'})
box on
grid on
ylim([-3200 1200]/1000)
xlim([-200 8000]/1000)


function [path, totalCost, farthestPreviousHop, farthestNextHop] = dijkstra(n, netCostMatrix, s, d, farthestPreviousHop, farthestNextHop)
% path: the list of nodes in the path from source to destination;
% totalCost: the total cost of the path;
% farthestNode: the farthest node to reach for each node after performing
% the routing;
% n: the number of nodes in the network;
% s: source node index;
% d: destination node index;

% clear;
% noOfNodes  = 50;
% rand('state', 0);
% figure(1);
% clf;
% hold on;
% L = 1000;
% R = 200; % maximum range;
% netXloc = rand(1,noOfNodes)*L;
% netYloc = rand(1,noOfNodes)*L;
% for i = 1:noOfNodes
%     plot(netXloc(i), netYloc(i), '.');
%     text(netXloc(i), netYloc(i), num2str(i));
%     for j = 1:noOfNodes
%         distance = sqrt((netXloc(i) - netXloc(j))^2 + (netYloc(i) - netYloc(j))^2);
%         if distance <= R
%             matrix(i, j) = 1;   % there is a link;
%             line([netXloc(i) netXloc(j)], [netYloc(i) netYloc(j)], 'LineStyle', ':');
%         else
%             matrix(i, j) = inf;
%         end;
%     end;
% end;
%
%
% activeNodes = [];
% for i = 1:noOfNodes,
%     % initialize the farthest node to be itself;
%     farthestPreviousHop(i) = i;     % used to compute the RTS/CTS range;
%     farthestNextHop(i) = i;
% end;
%
% [path, totalCost, farthestPreviousHop, farthestNextHop] = dijkstra(noOfNodes, matrix, 1, 15, farthestPreviousHop, farthestNextHop);
% path
% totalCost
% if length(path) ~= 0
%     for i = 1:(length(path)-1)
%         line([netXloc(path(i)) netXloc(path(i+1))], [netYloc(path(i)) netYloc(path(i+1))], 'Color','r','LineWidth', 0.50, 'LineStyle', '-.');
%     end;
% end;
% hold off;
% return;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% all the nodes are un-visited;
visited(1:n) = 0;

distance(1:n) = inf;    % it stores the shortest distance between each node and the source node;
parent(1:n) = 0;

distance(s) = 0;
for i = 1:(n-1),
    temp = [];
    for h = 1:n,
         if visited(h) == 0   % in the tree;
             temp=[temp distance(h)];
         else
             temp=[temp inf];
         end
     end;
     [t, u] = min(temp);    % it starts from node with the shortest distance to the source;
     visited(u) = 1;       % mark it as visited;
     for v = 1:n,           % for each neighbors of node u;
         if ( ( netCostMatrix(u, v) + distance(u)) < distance(v) )
             distance(v) = distance(u) + netCostMatrix(u, v);   % update the shortest distance when a shorter path is found;
             parent(v) = u;                                     % update its parent;
         end;
     end;
end;

path = [];
if parent(d) ~= 0   % if there is a path!
    t = d;
    path = [d];
    while t ~= s
        p = parent(t);
        path = [p path];

        if netCostMatrix(t, farthestPreviousHop(t)) < netCostMatrix(t, p)
            farthestPreviousHop(t) = p;
        end;
        if netCostMatrix(p, farthestNextHop(p)) < netCostMatrix(p, t)
            farthestNextHop(p) = t;
        end;

        t = p;
    end;
end;

totalCost = distance(d);

return;


function results = vl_test_imdisttf(varargin)
% VL_TEST_DISTTF
vl_test_init ;

function test_basic()
for conv = {@single, @double}
  conv = conv{1} ;

  I = conv([0 0 0 ; 0 -2 0 ; 0 0 0]) ;
  D = vl_imdisttf(I);
  assert(isequal(D, conv(- [0 1 0 ; 1 2 1 ; 0 1 0]))) ;

  I(2,2) = -3 ;
  [D,map] = vl_imdisttf(I) ;
  assert(isequal(D, conv(-1 - [0 1 0 ; 1 2 1 ; 0 1 0]))) ;
  assert(isequal(map, 5 * ones(3))) ;
end

function test_1x1()
assert(isequal(1, vl_imdisttf(1))) ;

function test_rand()
I = rand(13,31) ;
for t=1:4
  param = [rand randn rand randn] ;
  [D0,map0] = imdisttf_equiv(I,param) ;
  [D,map] = vl_imdisttf(I,param) ;
  vl_assert_almost_equal(D,D0,1e-10)
  assert(isequal(map,map0)) ;
end

function test_param()
I = zeros(3,4) ;
I(1,1) = -1 ;

[D,map] = vl_imdisttf(I,[1 0 1 0]);
assert(isequal(-[1 0 0 0 ;
                 0 0 0 0 ;
                 0 0 0 0 ;], D)) ;

D0 = -[1 .9 .6 .1 ;
       0 0 0 0 ;
       0 0 0 0 ;] ;
[D,map] = vl_imdisttf(I,[.1 0 1 0]);
vl_assert_almost_equal(D,D0,1e-10);

D0 = -[1  .9 .6 .1 ;
       .9 .8 .5  0 ;
       .6 .5 .2  0 ;]  ;
[D,map] = vl_imdisttf(I,[.1 0 .1 0]);
vl_assert_almost_equal(D,D0,1e-10);

D0 = -[.9  1  .9  .6 ;
       .8 .9  .8  .5 ;
       .5 .6  .5  .2 ; ] ;
[D,map] = vl_imdisttf(I,[.1 1 .1 0]);
vl_assert_almost_equal(D,D0,1e-10);

function test_special()
I = rand(13,31) -.5 ;
D = vl_imdisttf(I, [0 0 1e5 0]) ;
vl_assert_almost_equal(D(:,1),min(I,[],2),1e-10);
D = vl_imdisttf(I, [1e5 0 0 0]) ;
vl_assert_almost_equal(D(1,:),min(I,[],1),1e-10);

function [D,map]=imdisttf_equiv(I,param)
D = inf + zeros(size(I)) ;
map = zeros(size(I)) ;
ur = 1:size(D,2) ;
vr = 1:size(D,1) ;
[u,v] = meshgrid(ur,vr) ;
for v_=vr
  for u_=ur
    E = I(v_,u_) + ...
        param(1) * (u - u_ - param(2)).^2 + ...
        param(3) * (v - v_ - param(4)).^2 ;
    map(E < D) = sub2ind(size(I),v_,u_) ;
    D = min(D,E) ;
  end
end


% Fig. 6.82   Feedback Control of Dynamic Systems, 6e
%             Franklin, Powell, Emami
%

clf
clear all;
%close all;

numS=[1 11 10 0];
denS=[1 11 60 100];
w=logspace(-1,2,100);
sysS=tf(numS,denS);
[m,p]=bode(sysS,w);
loglog(w,squeeze(m));
axis([.1 100 .01 10])

hold on
% add line at mag = 1
w2=[.1 100];
mcl=[1 1];
loglog(w2,mcl,'r')

xlabel('\omega (rad/sec)');
ylabel('Magnitude');
title('Fig. 6.82 Sensitivity Function for Example 6.24');
bodegrid;
hold off


close all; clear;
d = 2;
k = 3;
n = 5000;
%% Generate data
[X,label] = kmeansRnd(d,k,n);
plotClass(X,label);
%% kmeans init with kmeans++ seeding (recomended)
y = kmeans(X,kseeds(X,k));
figure;
plotClass(X,y);
%% kmeans with random initialization
y = kmeans(X,k);
figure;
plotClass(X,y);
%% kmeans init with labels
y = kmeans(X,label);
figure;
plotClass(X,y);
%% kmeans init with centers
mu = rand(d,k);
y = kmeans(X,mu);
figure;
plotClass(X,y);
%% kmeans++ seeding
mu = kseeds(X,k);
[~,y] = min(dot(mu,mu,1)'/2-mu'*X,[],1); % assign sample labels
figure;
plotClass(X,y);


function imchi=sskkimbook2(omega,rechi,omega1,imchi1)
%The program inputs are the vector of the frequency
%(or energy) components, the vector of the real
% part of the susceptibility
%under examination, anchor point, the value of the
%imaginary part at the anchor point.
%The two vectors must have the same length
%and the frequency vector omega must be equispaced.
%If not, apply MATLAB functions such as interp.
%Note that the anchor point must be situated in one of the values
%of the vector omega.
%This function uses the function kkimbook2
%The output is the estimate of the
%real part as obtained by using SSKK relations.
%This software is distributed under the GNU licence agreement
%by Valerio Lucarini
%email: lucarini@alum.mit.edu
%University of Camerino
%Department of Mathematics and Computer Science
%Camerino, Italy

if size(omega,1)>size(omega,2);
omega=omega';
end; if size(rechi,1)>size(rechi,2);
rechi=rechi';
end;
%Here the program rearranges the two vectors so that,
%whichever their initial shape, they become row vectors.
g=size(omega,2);
%Size of the vectors.%
k=0;
for j=1:g;
if omega(j)==omega1;
k=j;
end;
end;
%Determination of the anchor point.
imchi=kkimbook2(omega,rechi,0);
%Application of K-K relations
imchi=imchi+omega1^(-1)*omega.^(1)*(imchi1-imchi(k));
%The subtracted relation upgrades the estimate obtained
%with K-K relations.