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 [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 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)));


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


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


% 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;


% 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


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.


function M = oned_bilinear ( kernel, phi, test, w_g )

%*****************************************************************************80
%
%% ONE_BILINEAR evaluates a bilinear form.
%
%  Discussion:
%
%    This function computes integral ( kernel * phi * test }
%
%    The calculation that is carried out is equivalent to:
%
%      [ n_gauss, n_test ] = size ( test );
%      [ n_gauss, n_dof ] = size ( phi );
%
%      M = zeros ( n_test, n_dof );
%      for i = 1 : n_test
%        for j = 1 : n_dof
%          M(i,j) = ( kernel' .* test(:,i)' ) * ( phi(:,j) .* w_g );
%        end
%      end
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Parameters:
%
%    Input, real KERNEL(N_GAUSS), the kernel function in the integral,
%    evaluated at the Gauss points.
%
%    Input, real PHI(N_GAUSS,N_DOF), the element test functions evaluated
%    at the Gauss points.
%
%    Input, real TEST(N_GAUSS,N_TEST), the test functions evaluated at the
%    Gauss points.
%
%    Input, real W_G(N_GAUSS,1), a column vector of Gauss weights.
%
%    Output, real M(N_TEST,N_DOF), the bilinear form.
%
  M = test' * diag ( kernel .* w_g ) * phi;

  return
end


function yawm = mag_compass_update(m_n, dec, inc, DCMnb, roll, pitch)
% magh_update: calculates magnetic heading angle (yaw) from magnetometer data.
%
% INPUT
%   mag, 1x3 magnetic flux density (Tesla).
%   dec, magnetic declination angle (rad).
%   inc, magnetic inclination angle (rad).
%   DCMnb, 3x3 DCM nav-to-body.
%   roll, roll angle (rad).
%   pitch, pitch angle (rad).
%
% OUTPUT
%   yawm, yaw angle from magnetometer data.
%
%   Copyright (C) 2014, Rodrigo Gonzalez, all rights reserved.
%
%   This file is part of NaveGo, an open-source MATLAB toolbox for
%   simulation of integrated navigation systems.
%
%   NaveGo is free software: you can redistribute it and/or modify
%   it under the terms of the GNU Lesser General Public License (LGPL)
%   version 3 as published by the Free Software Foundation.
%
%   This program is distributed in the hope that it will be useful,
%   but WITHOUT ANY WARRANTY; without even the implied warranty of
%   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%   GNU Lesser General Public License for more details.
%
%   You should have received a copy of the GNU Lesser General Public
%   License along with this program. If not, see
%   <http://www.gnu.org/licenses/>.
%
% Reference:
%
%   Paul D. Groves. Principles of GNSS, Inertial, and
% Multisensor Integrated Navigation Systems. Second Edition.
% Eq. 6.2 to 6.6, page 219.
%
%   NOAA, Magnetic Field Calculators.
% https://www.ngdc.noaa.gov/geomag/calculators/magcalc.shtml
%
% Version: 002
% Date:    2021/03/20
% Author:  Rodrigo Gonzalez <rodralez@frm.utn.edu.ar>
% URL:     https://github.com/rodralez/navego

B = norm( m_n );                    % Magnitude of the flux density

D = [ cos(dec) * cos(inc); sin(dec) * cos(inc); sin(inc); ];

m_b = ( DCMnb * D * B ) ;

x =  -m_b(2) * cos(roll)  + m_b(3) * sin(roll) ;
y =   m_b(1) * cos(pitch) + m_b(2) * sin(roll) * sin(pitch) ...
    + m_b(3) * cos(roll) * sin(pitch) ;

yawm = correct_yaw (atan2(x , y) + dec);    % Four-quadrant arctangent function

end


function [x, cost, info, options] = rlbfgs(problem, x0, options)
% Riemannian limited memory BFGS solver for smooth objective functions.
%
% function [x, cost, info, options] = rlbfgs(problem)
% function [x, cost, info, options] = rlbfgs(problem, x0)
% function [x, cost, info, options] = rlbfgs(problem, x0, options)
% function [x, cost, info, options] = rlbfgs(problem, [], options)
%
%
% This is a Riemannian limited memory BFGS solver (quasi-Newton method),
% which aims to minimize the cost function in the given problem structure.
% It requires access to the gradient of the cost function.
%
% Parameter options.memory can be used to specify the number of iterations
% the algorithm remembers and uses to approximate the inverse Hessian of
% the cost. Default value is 30.
% For unlimited memory, set options.memory = Inf.
%
%
% For a description of the algorithm and theorems offering convergence
% guarantees, see the references below.
%
% The initial iterate is x0 if it is provided. Otherwise, a random point on
% the manifold is picked. To specify options whilst not specifying an
% initial iterate, give x0 as [] (the empty matrix).
%
% The two outputs 'x' and 'cost' are the last reached point on the manifold
% and its cost.
%
% The output 'info' is a struct-array which contains information about the
% iterations:
%   iter (integer)
%       The iteration number. The initial guess is 0.
%   cost (double)
%       The corresponding cost value.
%   gradnorm (double)
%       The (Riemannian) norm of the gradient.
%   time (double)
%       The total elapsed time in seconds to reach the corresponding cost.
%   stepsize (double)
%       The size of the step from the previous to the new iterate.
%   accepted (Boolean)
%       true if step is accepted in the cautious update. 0 otherwise.
%   And possibly additional information logged by options.statsfun.
% For example, type [info.gradnorm] to obtain a vector of the successive
% gradient norms reached at each iteration.
%
% The options structure is used to overwrite the default values. All
% options have a default value and are hence optional. To force an option
% value, pass an options structure with a field options.optionname, where
% optionname is one of the following and the default value is indicated
% between parentheses:
%
%   tolgradnorm (1e-6)
%       The algorithm terminates if the norm of the gradient drops below
%       this. For well-scaled problems, a rule of thumb is that you can
%       expect to reduce the gradient norm by 8 orders of magnitude
%       (sqrt(eps)) compared to the gradient norm at a "typical" point (a
%       rough initial iterate for example). Further decrease is sometimes
%       possible, but inexact floating point arithmetic will eventually
%       limit the final accuracy. If tolgradnorm is set too low, the
%       algorithm may end up iterating forever (or at least until another
%       stopping criterion triggers).
%   maxiter (1000)
%       The algorithm terminates if maxiter iterations were executed.
%   maxtime (Inf)
%       The algorithm terminates if maxtime seconds elapsed.
%   minstepsize (1e-10)
%     The minimum norm of the tangent vector that points from the current
%     point to the next point. If the norm is less than minstepsize, the
%     program will terminate.
%   memory (30)
%     The number of previous iterations the program remembers. This is used
%     to approximate the inverse Hessian at the current point. Because of
%     difficulty of maintaining a representation of operators in terms of
%     coordinates, a recursive method is used. The number of steps in the
%     recursion is at most options.memory. This parameter can take any
%     integer value >= 0, or Inf, which is taken to be options.maxiter. If
%     options.maxiter has value Inf, then it will take value 10000 and a
%     warning will be displayed.
%   strict_inc_func (@(t) 1e-4*t)
%     The Cautious step needs a real function that has value 0 at t = 0,
%     and  is strictly increasing. See details in Wen Huang's paper
%     "A Riemannian BFGS Method without Differentiated Retraction for
%     Nonconvex Optimization Problems"
%   statsfun (none)
%       Function handle to a function that will be called after each
%       iteration to provide the opportunity to log additional statistics.
%       They will be returned in the info struct. See the generic Manopt
%       documentation about solvers for further information. statsfun is
%       called with the point x that was reached last.
%   stopfun (none)
%       Function handle to a function that will be called at each iteration
%       to provide the opportunity to specify additional stopping criteria.
%       See the generic Manopt documentation about solvers for further
%       information.
%   verbosity (2)
%       Integer number used to tune the amount of output the algorithm
%       generates during execution (mostly as text in the command window).
%       The higher, the more output. 0 means silent. 3 and above includes a
%       display of the options structure at the beginning of the execution.
%   debug (false)
%       Set to true to allow the algorithm to perform additional
%       computations for debugging purposes. If a debugging test fails, you
%       will be informed of it, usually via the command window. Be aware
%       that these additional computations appear in the algorithm timings
%       too, and may interfere with operations such as counting the number
%       of cost evaluations, etc. (the debug calls get storedb too).
%   storedepth (2)
%       Maximum number of different points x of the manifold for which a
%       store structure will be kept in memory in the storedb for caching.
%       If memory usage is an issue, you may try to lower this number.
%       Profiling may then help to investigate if a performance hit was
%       incurred as a result.
%   ls_initial_scale (@(gradnorm) 1/gradnorm)
%       A function handle that takes as input a real number (a gradient norm)
%       and outputs a real number for how to scale the initialization of
%       line-search.
%
%
% Please cite the Manopt paper as well as the research paper:
% @InBook{Huang2016,
%   title     = {A {R}iemannian {BFGS} Method for Nonconvex Optimization Problems},
%   author    = {Huang, W. and Absil, P.-A. and Gallivan, K.A.},
%   year      = {2016},
%   publisher = {Springer International Publishing},
%   editor    = {Karas{\"o}zen, B{\"u}lent and Manguo{\u{g}}lu, Murat and Tezer-Sezgin, M{\"u}nevver and G{\"o}ktepe, Serdar and U{\u{g}}ur, {\"O}m{\"u}r},
%   address   = {Cham},
%   booktitle = {Numerical Mathematics and Advanced Applications ENUMATH 2015},
%   pages     = {627--634},
%   doi       = {10.1007/978-3-319-39929-4_60}
% }
%
% We point out that, at the moment, this implementation of RLBFGS can be
% slower than the implementation in ROPTLIB by Wen Huang et al. referenced
% above. For the purpose of comparing to their work, please use their
% implementation.

% This file is part of Manopt: www.manopt.org.
% Original author: Changshuo Liu, July 19, 2017.
% Contributors: Nicolas Boumal
% Change log:
%
%   Nov. 27, 2017 (Wen Huang):
%       Changed the default strict_inc_func to @(t) 1e-4*t from @(t) t.
%
%   Jan. 18, 2018 (NB):
%       Corrected a bug related to the way the line search hint was defined
%       by default.
%
%   Aug. 2, 2018 (NB):
%       Using the new storedb.remove features to keep storedb lean, and
%       reduced the default value of storedepth from 30 to 2 as a result.
%
%   Aug. 2, 2022 (sfrcorne):
%       Added option ls_initial_scale, with default setting to ensure
%       the method is invariant to positive scaling of the cost function.

    % Verify that the problem description is sufficient for the solver.
    if ~canGetCost(problem)
        warning('manopt:getCost', ...
                'No cost provided. The algorithm will likely abort.');
    end
    if ~canGetGradient(problem) && ~canGetApproxGradient(problem)
        % Note: we do not give a warning if an approximate gradient is
        % explicitly given in the problem description, as in that case the user
        % seems to be aware of the issue.
        warning('manopt:getGradient:approx', ...
           ['No gradient provided. Using an FD approximation instead (slow).\n' ...
            'It may be necessary to increase options.tolgradnorm.\n' ...
            'To disable this warning: warning(''off'', ''manopt:getGradient:approx'')']);
        problem.approxgrad = approxgradientFD(problem);
    end

    % This solver uses linesearch_hint as a line search algorithm. By
    % default, try a step size of 1, so that if the BFGS approximation of
    % the Hessian (or inverse Hessian) is good, then the iteration is close
    % to a Newton step.
    if ~canGetLinesearch(problem)
        problem.linesearch = @(x, d) 1;
    end

    % Local defaults for the program
    localdefaults.minstepsize = 1e-10;
    localdefaults.maxiter = 1000;
    localdefaults.tolgradnorm = 1e-6;
    localdefaults.memory = 30;
    localdefaults.strict_inc_func = @(t) 1e-4*t;
    localdefaults.ls_max_steps = 25;
    localdefaults.ls_initial_scale = @(gradnorm) 1/gradnorm;
    localdefaults.storedepth = 2;

    % Merge global and local defaults, then merge w/ user options, if any.
    localdefaults = mergeOptions(getGlobalDefaults(), localdefaults);
    if ~exist('options', 'var') || isempty(options)
        options = struct();
    end
    options = mergeOptions(localdefaults, options);

    % To make sure memory in range [0, Inf)
    options.memory = max(options.memory, 0);
    if options.memory == Inf
        if isinf(options.maxiter)
            options.memory = 10000;
            warning('rlbfgs:memory', ['options.memory and options.maxiter' ...
              ' are both Inf; options.memory has been changed to 10000.']);
        else
            options.memory = options.maxiter;
        end
    end

    M = problem.M;

    timetic = tic();

    % Create a random starting point if no starting point is provided.
    if ~exist('x0', 'var')|| isempty(x0)
        xCur = M.rand();
    else
        xCur = x0;
    end

    % Create a store database and get a key for the current x
    storedb = StoreDB(options.storedepth);
    key = storedb.getNewKey();

    % __________Initialization of variables______________
    % Number of iterations since the last restart
    k = 0;
    % Total number of BFGS iterations
    iter = 0;

    % This cell stores step vectors which point from x_{t} to x_{t+1} for t
    % indexing the last iterations, capped at options.memory.
    % That is, it stores up to options.memory of the most recent step
    % vectors. However, the implementation below does not need step vectors
    % in their respective tangent spaces at x_{t}'s. Rather, it requires
    % them transported to the current point's tangent space by vector
    % transport. For details regarding the requirements on the the vector
    % transport, see the reference paper by Huang et al.
    % In this implementation, those step vectors are iteratively
    % transported to the current point's tangent space after every
    % iteration. Thus, at every iteration, vectors in sHistory are in the
    % current point's tangent space.
    sHistory = cell(1, options.memory);

    % This cell stores the differences for latest t's of the gradient at
    % x_{t+1} and the gradient at x_{t}, transported to x_{t+1}'s tangent
    % space. The memory is also capped at options.memory.
    yHistory = cell(1, options.memory);

    % rhoHistory{t} stores the reciprocal of the inner product between
    % sHistory{t} and yHistory{t}.
    rhoHistory = cell(1, options.memory);

    % Scaling of direction given by getDirection for acceptable step
    alpha = 1;

    % Norm of the step
    stepsize = 1;

    % Stores whether the step is accepted by the cautious update check.
    accepted = true;

    % Query the cost function and its gradient
    [xCurCost, xCurGradient] = getCostGrad(problem, xCur, storedb, key);

    xCurGradNorm = M.norm(xCur, xCurGradient);

    % Scaling of initial matrix, Barzilai-Borwein.
    scaleFactor = options.ls_initial_scale(xCurGradNorm);

    % Line-search statistics for recording in info.
    lsstats = [];

    % Flag to control restarting scheme to avoid infinite loops (see below)
    ultimatum = false;

    % Save stats in a struct array info, and preallocate.
    stats = savestats();
    info(1) = stats;
    info(min(10000, options.maxiter+1)).iter = [];

    if options.verbosity >= 2
        fprintf(' iter                   cost val            grad. norm           alpha\n');
    end

    % Main iteration
    while true

        % Display iteration information
        if options.verbosity >= 2
        fprintf('%5d    %+.16e        %.8e      %.4e\n', ...
                iter, xCurCost, xCurGradNorm, alpha);
        end

        % Start timing this iteration
        timetic = tic();

        % Run standard stopping criterion checks
        [stop, reason] = stoppingcriterion(problem, xCur, options, ...
                                           info, iter+1);

        % If none triggered, run specific stopping criterion check
        if ~stop
            if stats.stepsize < options.minstepsize
                % To avoid infinite loop and to push the search further
                % in case BFGS approximation of Hessian is off towards
                % the end, we erase the memory by setting k = 0;
                % In this way, it starts off like a steepest descent.
                % If even steepest descent does not work, then it is
                % hopeless and we will terminate.
                if ~ultimatum
                    if options.verbosity >= 2
                        fprintf(['stepsize is too small, restarting ' ...
                            'the bfgs procedure at the current point.\n']);
                    end
                    k = 0;
                    ultimatum = true;
                else
                    stop = true;
                    reason = sprintf(['Last stepsize smaller than '  ...
                        'minimum allowed; options.minstepsize = %g.'], ...
                        options.minstepsize);
                end
            else
                % We are not in trouble: lift the ultimatum if it was on.
                ultimatum = false;
            end
        end

        if stop
            if options.verbosity >= 1
                fprintf([reason '\n']);
            end
            break;
        end

        % Compute BFGS direction
        p = getDirection(M, xCur, xCurGradient, sHistory,...
                yHistory, rhoHistory, scaleFactor, min(k, options.memory));

        % Execute line-search
        [stepsize, xNext, newkey, lsstats] = ...
            linesearch_hint(problem, xCur, p, xCurCost, ...
                            M.inner(xCur, xCurGradient, p), ...
                            options, storedb, key);

        % Record the BFGS step-multiplier alpha which was effectively
        % selected. Toward convergence, we hope to see alpha = 1.
        alpha = stepsize/M.norm(xCur, p);
        step = M.lincomb(xCur, alpha, p);

        % Query cost and gradient at the candidate new point.
        [xNextCost, xNextGrad] = getCostGrad(problem, xNext, storedb, newkey);

        % Compute sk and yk
        sk = M.transp(xCur, xNext, step);
        yk = M.lincomb(xNext, 1, xNextGrad, ...
                             -1, M.transp(xCur, xNext, xCurGradient));

        % Computation of the BFGS step is invariant under scaling of sk and
        % yk by a common factor. For numerical reasons, we scale sk and yk
        % so that sk is a unit norm vector.
        norm_sk = M.norm(xNext, sk);
        sk = M.lincomb(xNext, 1/norm_sk, sk);
        yk = M.lincomb(xNext, 1/norm_sk, yk);

        inner_sk_yk = M.inner(xNext, sk, yk);
        inner_sk_sk = M.norm(xNext, sk)^2;    % ensures nonnegativity

        % If the cautious step is accepted (which is the intended
        % behavior), we record sk, yk and rhok and need to do some
        % housekeeping. If the cautious step is rejected, these are not
        % recorded. In all cases, xNext is the next iterate: the notion of
        % accept/reject here is limited to whether or not we keep track of
        % sk, yk, rhok to update the BFGS operator.
        cap = options.strict_inc_func(xCurGradNorm);
        if inner_sk_sk ~= 0 && (inner_sk_yk / inner_sk_sk) >= cap

            accepted = true;

            rhok = 1/inner_sk_yk;

            scaleFactor = inner_sk_yk / M.norm(xNext, yk)^2;

            % Time to store the vectors sk, yk and the scalar rhok.
            % Remember: we need to transport all vectors to the most
            % current tangent space.

            % If we are out of memory
            if k >= options.memory

                % sk and yk are saved from 1 to the end with the most
                % current recorded to the rightmost hand side of the cells
                % that are occupied. When memory is full, do a shift so
                % that the rightmost is earliest and replace it with the
                % most recent sk, yk.
                for  i = 2 : options.memory
                    sHistory{i} = M.transp(xCur, xNext, sHistory{i});
                    yHistory{i} = M.transp(xCur, xNext, yHistory{i});
                end
                if options.memory > 1
                    sHistory = sHistory([2:end, 1]);
                    yHistory = yHistory([2:end, 1]);
                    rhoHistory = rhoHistory([2:end 1]);
                end
                if options.memory > 0
                    sHistory{options.memory} = sk;
                    yHistory{options.memory} = yk;
                    rhoHistory{options.memory} = rhok;
                end

            % If we are not out of memory
            else

                for i = 1:k
                    sHistory{i} = M.transp(xCur, xNext, sHistory{i});
                    yHistory{i} = M.transp(xCur, xNext, yHistory{i});
                end
                sHistory{k+1} = sk;
                yHistory{k+1} = yk;
                rhoHistory{k+1} = rhok;

            end

            k = k + 1;

        % The cautious step is rejected: we do not store sk, yk, rhok but
        % we still need to transport stored vectors to the new tangent
        % space.
        else

            accepted = false;

            for  i = 1 : min(k, options.memory)
                sHistory{i} = M.transp(xCur, xNext, sHistory{i});
                yHistory{i} = M.transp(xCur, xNext, yHistory{i});
            end

        end

        % Update variables to new iterate.
        storedb.removefirstifdifferent(key, newkey);
        xCur = xNext;
        key = newkey;
        xCurGradient = xNextGrad;
        xCurGradNorm = M.norm(xNext, xNextGrad);
        xCurCost = xNextCost;

        % iter is the number of iterations we have accomplished.
        iter = iter + 1;

        % Make sure we don't use too much memory for the store database
        % (this is independent from the BFGS memory.)
        storedb.purge();

        % Log statistics for freshly executed iteration
        stats = savestats();
        info(iter+1) = stats;

    end

    % Housekeeping before we return
    info = info(1:iter+1);
    x = xCur;
    cost = xCurCost;

    if options.verbosity >= 1
        fprintf('Total time is %f [s] (excludes statsfun)\n', ...
                info(end).time);
    end

    % Routine in charge of collecting the current iteration stats
    function stats = savestats()
        stats.iter = iter;
        stats.cost = xCurCost;
        stats.gradnorm = xCurGradNorm;
        if iter == 0
            stats.stepsize = NaN;
            stats.time = toc(timetic);
            stats.accepted = NaN;
        else
            stats.stepsize = stepsize;
            stats.time = info(iter).time + toc(timetic);
            stats.accepted = accepted;
        end
        stats.linesearch = lsstats;
        stats = applyStatsfun(problem, xCur, storedb, key, options, stats);
    end

end

% BFGS step, see Wen's paper for details. This function takes in a tangent
% vector g, and applies an approximate inverse Hessian P to it to get Pg.
% Then, -Pg is returned.
%
% Theory requires the vector transport to be isometric and to satisfy the
% locking condition (see paper), but these properties do not seem to be
% crucial in practice. If your manifold provides M.isotransp, it may be
% good to do M.transp = M.isotransp; after loading M with a factory.
%
% This implementation operates in the tangent space of the most recent
% point since all vectors in sHistory and yHistory have been transported
% there.
function dir = getDirection(M, xCur, xCurGradient, sHistory, yHistory, ...
                            rhoHistory, scaleFactor, k)

    q = xCurGradient;

    inner_s_q = zeros(1, k);

    for i = k : -1 : 1
        inner_s_q(1, i) = rhoHistory{i} * M.inner(xCur, sHistory{i}, q);
        q = M.lincomb(xCur, 1, q, -inner_s_q(1, i), yHistory{i});
    end

    r = M.lincomb(xCur, scaleFactor, q);

    for i = 1 : k
         omega = rhoHistory{i} * M.inner(xCur, yHistory{i}, r);
         r = M.lincomb(xCur, 1, r, inner_s_q(1, i)-omega, sHistory{i});
    end

    dir = M.lincomb(xCur, -1, r);

end