function [z,uTx,uTarRx,uTarTx]=Cart2RuvStdRefrac(zC,useHalfRange,zTx,zRx,M,Ns,includeW,ce,rE,spherCent)
%%CART2RUVSTDREFRAC Convert points in Cartesian coordinates (either the
%         global system or a local system at the receiver) into local
%         bistatic r-u-v coordinates of the receiver, accounting for how a
%         standard exponential atmospheric model warps the measurements.
%         r-u-v coordinates consist of a bistatic range and direction
%         cosines at the receiver. The "direction cosines" u and v are just
%         the x and y coordinates of a unit vector from the receiver to
%         the target in the coordinate system at the receiver (Here, to the
%         refraction-corrupted apparent direction). This basically assumes
%         that the boresight direction of the receiver is the z axis.
%         Assuming the target is in front of the receiver, the third unit
%         vector coordinate is not needed. However, with the includeW
%         option, it can be provided, resulting in r-u-v-w coordinates. This
%         function is not suitable for computing refraction between
%         satellites, grazing the Earth's atmosphere. The algorithm might
%         have an error if the ray path goes too far underground.
%
%INPUT: zC  A 3XN matrix of Cartesian points (target locations) in global
%           [x;y;z] Cartesian coordinates.
% useHalfRange A boolean value specifying whether the bistatic range value
%           should be divided by two. This normally comes up when operating
%           in monostatic mode, so that the range reported is a one-way
%           range. The default if this parameter is not provided (or an
%           empty matrix is provided) is false.
%       zTx The 3X1 [x;y;z] location vector of the transmitter in global
%           Cartesian coordinates.  If this parameter is omitted or an
%           empty matrix is passed, then the receiver is placed at the origin.
%       zRx The 3X1 [x;y;z] location vector of the receiver in global
%           Cartesian coordinates. If this parameter is omitted or an empty
%           matrix is passed, then the receiver is placed at the origin.
%         M A 3X3 rotation matrix to go from the alignment of the global
%           coordinate system to the local alignment of the receiver. The z
%           vector of the local coordinate system of the receiver is the
%           pointing direction of the receiver. If this matrix is omitted,
%           then the identity matrix is used.
%        Ns The atmospheric refractivity reduced to the reference sphere.
%           Note that the refractivity is (n-1)*1e6, where n is the index
%           of refraction. The function reduceStdRefrac2Spher can be used
%           to reduce a refractivity to the surface of a reference
%           ellipsoid. This function does not allow different
%           refractivities to be used as the transmitter and receiver. If
%           this parameter is omitted or an empty matrix is passed, a
%           default value of 313 is used.
%  includeW An optional boolean value indicating whether a third direction
%           cosine component should be included. The u and v direction
%           cosines are two parts of a 3D unit vector. Generally, one might
%           assume that the target is in front of the sensor, so the third
%           component would be positive and is not needed. However, the
%           third component can be included if ambiguity exists. The
%           default if this parameter is omitted or an empty matrix is
%           passed is false.
%        ce The optional decay constant of the exponential model. The
%           refractivity N at height h is N=Ns*exp(-ce*(h-h0)) where h0 is
%           the reference height (in this function, the height of the
%           reference ellipsoid surface is used). ce is related to the
%           change in refractivity at an elevation of 1km based on the
%           refractivity at sea level as
%           ce=log(Ns/(Ns+DeltaN))/1000;%Units of inverse meters.
%           where the change in refractivity for a change in elevation of
%           1km is DeltaN=-multConst*exp(expConst*Ns); In [1], standard
%           values for the two constants are expConst=0.005577; and
%           multConst=7.32; If ce is omitted or an empty matrix is passed,
%           the value based on the standard model is used.
% rE,spherCent The radius of the Earth to use for the spherical Earth
%           approximation used in the model and also the offset between the
%           global model and the local spherical model. It is assumed that
%           zC,zTx,and zRx are all given in the global model and will need
%           to be transformed to the local model to the used. If rE is
%           omitted or an empty matrix is passed, then the default of
%           [rE,spherCent]=osculatingSpher4LatLon(Cart2Ellipse(zRx)) is
%           used. The defaults here mean that a WGS-84 reference ellipsoid
%           is approximated by the local osculating sphere.
%
%OUTPUTS: z The 3XN (or 4XN if includeW is true) matrix of location vectors
%           of the points in bistatic [r;u;v] coordinates. If
%           useHalfRange=true, then the r component is half the bistatic
%           range (half the round-trip range for a monostatic scenario).
%       uTx A 3XN set of unit vectors pointing from the transmitter to the
%           refraction-corrupted position (of the target as seen by the
%           transmitter. This is in the global coordinate system.
%    uTarRx A 3XN set of unit vectors pointing from the target to the
%           refraction-corrupted position of the receiver as seen by the
%           target. This is in the global coordinate system.
%    uTarTx A 3XN set of unit vectors pointing from the target to the
%           refraction-corrupted position of the transmitter as seen by the
%           target. This is in the global coordinate system.
%
%This function implements the refraction algorithm for the basic
%exponential atmosphere as described in [1] for the bistatic case. If the
%target is collocated with the transmitter or the receiver, then NaNs will
%be returned for some values. The basic exponential refraction model is in
%[2].
%
%The model is parameterized in terms of a height above a sphere. The Earth
%is more of an ellipsoid than a sphere. Thus, we use local spherical
%approximations about the transmitter and the receiver. That is, for
%computing the refraction from the transmitter to the target, we use the
%distance from the center of the Earth to the surface of the reference
%ellipsoid at the transmitter as the radius of an approximately spherical
%Earth. Similarly, the distance from the center of the Earth to the
%receiver is used in the approximation for the path from the target to the
%receiver.
%
%The algorithm in [1] performs ray tracing by solving a boundary value
%problem. here, the bvp5c function in Matlab is used to solve the problem.
%
%For paths that are nearly vertical, it is approximated that there is no
%bending in angle and an explicit solution to the integral over the index
%of refraction in the vertical direction is used to obtain the range.
%
%EXAMPLE:
%Here, we have two radars and one target near Hawaii.
% latLonRx=deg2rad([20.269202;-155.852051]);
% AltRx=0;
% latLonTx=deg2rad([20.724568;-155.978394]);
% AltTx=0;
% latLonTar=deg2rad([20.835390;-155.313721]);
% AltTar=8e3;%8km target altitude.
% %Convert locations to Cartesian.
% zRx=ellips2Cart([latLonRx;AltRx]);
% zTx=ellips2Cart([latLonTx;AltTx]);
% zTar=ellips2Cart([latLonTar;AltTar]);
%
% %The receiver faces 45 degrees East of North and 15 degrees up from the
% %local ellipsoidal level.
% M=findRFTransParam([latLonRx;AltRx],deg2rad(45),deg2rad(15));
% Ns=350;%Assumed refractivity at the sea surface.
% useHalfRange=false;
% includeW=true;%Include third dimension of unit vector.
% [z,uTx,uTarRx,uTarTx]=Cart2RuvStdRefrac(zTar,useHalfRange,zTx,zRx,M,Ns,includeW);
% zNoRefrac=Cart2Ruv(zTar,useHalfRange,zTx,zRx,M,includeW);
% z(1)-zNoRefrac(1)%Bistatic range difference of 31.0813 meters
% %Direction difference of 0.0927 degrees
% rad2deg(angBetweenVecs(z(2:end),zNoRefrac(2:end)))
%
%REFERENCES:
%[1] D. F. Crouse, "Basic tracking using 3D monostatic and bistatic
%    measurements in refractive environments," IEEE Aerospace and
%    Electronic Systems Magazine, vol. 29, no. 8, Part II, pp. 54-75, Aug.
%    2014.
%[2] B. R. Bean and G. D. Thayer, CRPL Exponential Reference Atmosphere.
%    Washington, D.C.: U. S. Department of Commerce, National Bureau of
%    Standards, Oct. 1959. [Online]. Available:
%    http://digicoll.manoa.hawaii.edu/techreports/PDF/NBS4.pdf
%
%June 2016 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

numMeas=size(zC,2);

%The assumes refractivity at sea level to use if none is provided.
if(nargin<7||isempty(includeW))
    includeW=false;
end

if(nargin<6||isempty(Ns))
    Ns=313;
end

if(nargin<5||isempty(M))
    M=eye(3);
end

if(nargin<4||isempty(zRx))
    zRx=zeros(3,1);
end

if(nargin<3||isempty(zTx))
    zTx=zeros(3,1);
end

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

if(nargin<8||isempty(ce))
    expConst=0.005577;
    multConst=7.32;

    %The change in refractivity at an elevation of 1km based on the
    %refractivity on the surface of the Earth.
    DeltaN=-multConst*exp(expConst*Ns);
    ce=log(Ns/(Ns+DeltaN))/1000;%Units of inverse meters.
end

if(nargin<9||isempty(rE))
    %Use the radius of the Earth that is the radius of the osculating
    %sphere at the location of the observed. This will be the radius used
    %in the local spherical Earth approximation for computing atmospheric
    %refraction. This uses the WGS-84 reference ellipsoid.
    [rE,spherCent]=osculatingSpher4LatLon(Cart2Ellipse(zRx));
end

%Adjust all the Cartesian values based on the osculating sphere.
zC=zC-spherCent;
zTx=zTx-spherCent;
zRx=zRx-spherCent;

%Allocate space
if(includeW)
    z=zeros(4,numMeas);
else
    z=zeros(3,numMeas);
end
uTx=zeros(3,numMeas);
uTarTx=zeros(3,numMeas);
uTarRx=zeros(3,numMeas);

if(any(zRx~=zTx))%If the scenario is bistatic
    for curMeas=1:numMeas
        [r2,uArrive,uTarRx(:,curMeas)]=atmosRefracMeas(zRx,zC(:,curMeas),Ns,ce,rE);
        [r1,uTx(:,curMeas),uTarTx(:,curMeas)]=atmosRefracMeas(zTx,zC(:,curMeas),Ns,ce,rE);

        r=(r1+r2);
        u=M*uArrive;

        if(useHalfRange)
            r=r/2;
        end

        zCur=[r;u];

        if(includeW)
            z(:,curMeas)=zCur;
        else
            z(:,curMeas)=zCur(1:3);
        end
    end
else%The scenario is monostatic.
    for curMeas=1:numMeas
        [range,uArrive,uTarRx(:,curMeas)]=atmosRefracMeas(zTx,zC(:,curMeas),Ns,ce,rE);
        uTx=uArrive;
        r=2*range;%Round-trip range.
        u=M*uArrive;

        if(useHalfRange)
            r=r/2;
        end

        zCur=[r;u];

        if(includeW)
            z(:,curMeas)=zCur;
        else
            z(:,curMeas)=zCur(1:3);
        end

        uTx(:,curMeas)=uArrive;
        uTarTx(:,curMeas)=uTarRx(:,curMeas);
    end
end
end

function [range,uArrive,uDepart]=atmosRefracMeas(xObs,xObj,Ns,ce,rE)
%%ATMOSREFRACMEAS  Given the location of an observer and an object in the
%                  atmosphere of the Earth, find the delay and angle of
%                  arrival of a signal from the object to the observer,
%                  accounting for basic,standard  refraction. A
%                  low-fidelity exponential atmospheric model is used. This
%                  function is not suitable for computing refraction
%                  between satellites, grazing the Earth's atmosphere. The
%                  algorithm might have an error if the raypath
%                  goes too far underground.
%
%INPUTS: xObs The Cartesian location of the observer in ECEF coordinates in
%             meters as [x;y;z].
%        xObj The Cartesian location of the object being observed in
%             ECEF coordinates in meters as [x;y;z].
%          Ns The refractivity as reduced to at sea level.
% expConst,multConst The parameters of the refractivity model such that
%             increasing the height by 1km, the model is that the
%             refractivity changes by deltaN=-multConst*exp(expConst*N).
%             deltaN cannot be negative.
%          rE The radius of the Earth to use in the spherical Earth
%             approximation. An osculating sphere near a sensor is
%             suggested.
%
%OUTPUTS: range  The apparent one-way range (in meters) of the signal from
%                the transmitter to the target and back to the receiver.
%        uArrive A unit vector in ECEF coordinates pointing in the apparent
%                direction of the signal the observer received (as seen by
%                the observer).
%        uDepart The direction of the signal departing the object that
%                arrives at the observer. Put another way, if the observer
%                were to transmit a signal to the object, this is the
%                apparent direction of the observer as seen by the object.
%
%This function implements the refraction algorithm for the basic
%exponential atmosphere as described in [1] for the monostatic case. If the
%points are collocated, then NaNs are returned for uArrive and uDepart.
%
%The function will fail for paths that go too deep into the Earth.
%
%REFERENCES:
%[1] D. F. Crouse, "Basic tracking using 3D monostatic and bistatic
%    measurements in refractive environments," IEEE Aerospace and
%    Electronic Systems Magazine, vol. 29, no. 8, Part II, pp. 54-75, Aug.
%    2014.
%
%May 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

%We need the conversion from the 3D coordinate system of the observer and
%object into the 2D coordinate system used for raytracing. The 2D
%coordinate system has the center of the Earth as its origin and the x-y
%axes are in the plane of the vector from the observer to the target. One
%vector common to both coordinate systems in the local up vector, which
%will be the local y axis. The second vector common to both will be the
%local x vector, which will be the projection of xObj-xObs onto the local
%tangent plane. Here, the vertical is the spherical model vertical. Since
%the precision of the model is low enough that the difference between the
%spherical and gravitational verticals shouldn't matter.
uENU=getENUAxes(Cart2Ellipse(xObs,[],rE,0));
uVertGlobal=uENU(:,3);
uVertLocal=[0;1;0];

vec2TarGlobal=xObj-xObs;

%The projection of the xObj-xObs vector into the local tangent plane can be
%obtained by subtracting the component of the vector that is orthogonal to
%the plane.
uHorizGlobalOrig=vec2TarGlobal-dot(vec2TarGlobal,uENU(:,3))*uENU(:,3);

uHorizGlobal=uHorizGlobalOrig/norm(uHorizGlobalOrig);
uHorizLocal=[1;0;0];

%Find the rotation matrix from the global coordinate system into the local
%coordinate system.
ECEF2LocalRot=findTransParam([uVertLocal,uHorizLocal],[uVertGlobal,uHorizGlobal]);

%The third (z) coordinate in the local system should be zero after this
%transformation.
vec2TarLocal=ECEF2LocalRot*vec2TarGlobal;

%The location of the receiver in the local 2D coordinate system.
x0Init=0;
y0Init=norm(xObs);

%The location of the target in the local 2D coordinate system.
x1Init=vec2TarLocal(1);
y1Init=vec2TarLocal(2)+y0Init;

%If the two points are nearly vertical, then the ray tracing algorithm will
%fail. For nearly vertical points, the bending due to refraction in the
%model should be negligible, so we can perform an integral in the y
%direction to solve for the excess range instead of having to solve the
%more complicated general bistatic problem. To deal with x1Init not being
%exactly zero, we actually go to a full range of norm([x1Init;y1Init])
if(norm(uHorizGlobalOrig)<1e-3)
    %Integrating the index of refraction from y0Init to
    %norm([x1Init;y1Init]) at a constant x yields the following measured
    %range. It is not just the geometric range, because the index of
    %refraction is not a constant 1.
    yMax=norm([x1Init;y1Init]);

    range=((exp(ce*(rE-y0Init))-exp(ce*(rE-yMax)))*Ns)/(1e6*ce)+yMax-y0Init;

    uArrive=vec2TarGlobal/norm(vec2TarGlobal);
    uDepart=-uArrive;
    return;
end

%Now, set up the boundary-value problem to determine the path taken by
%light between the target and the receiver.

%The initial guess is just the linear solution. The solver requires a fixed
%number of steps. 20 is probably sufficient for things near the Earth. that
%is, up to distances of, say 400km. We can scale the number of steps as 20
%for every 400 kilometers with a minimum of, say 10.
%Things outside of the atmosphere should use the astronomical refraction
%routines.
numSteps=max(20,ceil(20*norm(vec2TarLocal)/400e3));
x=linspace(x0Init,x1Init,numSteps);
slope=(y1Init-y0Init)/(x1Init-x0Init);
b=y1Init-slope*x1Init;%The y-intercept.
%The initial estimate of the solution.
solInit=bvpinit(x,@(x)[x*slope+b;slope]);

%Now, solve the differential equation.
oldOpts=bvpset();
newOpts=bvpset(oldOpts,'RelTol',1e-8,'AbsTol',1e-8,'FJacobian',@(x,y)odefunJacob(x,y,Ns,rE,ce),'BCJacobian',@bcfunJacob);%Increase the accuracy.
sol=bvp5c(@(x,y)expDiffEq(x,y,Ns,rE,ce),@(y0,y1)bcfun(y0,y1,y0Init,y1Init),solInit,newOpts);

%Get the refraction-corrupted range measurement for a signal traveling from
%the object to the observer.
range=integral(@(x)pathFun2D(x,sol,Ns,rE,ce),x0Init,x1Init,'AbsTol',eps(1),'RelTol',1e-15);

if(nargout>1)
    %Get the angle of arrival for a signal traveling from the object to the
    %observer. The angle is determined by the slope at the initial point.
    thetaOrig=atan(sol.y(2,1));
    uLocal=[cos(thetaOrig);sin(thetaOrig);0];

    %The inverse rotation is given the transpose of the rotation
    %matrix. This is the apparent direction of the object as seen by the
    %observer.
    uArrive=ECEF2LocalRot'*uLocal;

    thetaEnd=atan(sol.y(2,end));
    uLocal=[cos(thetaEnd);sin(thetaEnd);0];
    %This is the apparent direction of the observer as seen by the object.
    uDepart=-ECEF2LocalRot'*uLocal;
end
end

function val=pathFun2D(x,sol,Ns,rE,ce)
    %This function is used to integrate the time taken
    y=deval(x,sol);
    val=(1+NRefracExp(x,y(1,:),Ns,rE,ce)).*sqrt(1+y(2,:).^2);
end

function res=bcfun(y0,y1,y0Init,y1Init)
    %The residue to define the boundary condition for the numeric
    %differential equation solver as applied to the 2D exponential
    %atmospheric model.

    res=[y0(1)-y0Init;
         y1(1)-y1Init];
end

function [dbcy0,dbcy1]=bcfunJacob(~,~)
    %The Jacobians of the boundary conditions for raytracing the 2D
    %exponential atmospheric refraction model.
    dbcy0=[1 0
           0 0];
    dbcy1=[0 0
           1 0];
end

function J=odefunJacob(x,y,Ns,rE,ce)
    %The Jacobian of the differential equation for raytracing the 2D
    %exponential atmospheric model.
    expVal=NRefracExp(x,y(1),Ns,rE,ce);

    J=zeros(2,2);
    J(1,2)=1;
    J(2,1)=ce*(1+y(2)^2)*(-expVal)*(ce*y(1)*(x*y(2)-y(1))*sqrt(x^2+y(1)^2)+x*(x+y(1)*y(2))*(expVal+1))/((x^2+y(1)^2)^(3/2)*(expVal+1)^2);
    J(2,2)=ce*(x-2*y(1)*y(2)+3*x*y(2)^2)*expVal/((expVal+1)*sqrt(x^2+y(1)^2));
end

function dxdy=expDiffEq(x,y,Ns,rE,ce)
    %Find the refractivity at location (x,y).
    expVal=NRefracExp(x,y(1),Ns,rE,ce);

    dxdy=[y(2)
          ce*(1+y(2)^2)*(x*y(2)-y(1))*expVal/((expVal+1)*sqrt(x^2+y(1)^2))];
end

function nRefrac=NRefracExp(x,y,Ns,rE,ce)
    %The refractivity. This is 10^6*(index of refraction-1)
    nRefrac=1e-6*Ns*exp(-ce*(sqrt(x.^2+y.^2)-rE));
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.


classdef MaOEAIT < ALGORITHM
% <multi/many> <real/integer> <constrained/none>
% Many-objective evolutionary algorithms based on an independent two-stage
% approach
% Evaluation1 --- 20000 --- Number of evaluations for NDWA
% Evaluation1 ---  6000 --- Number of evaluations for reference lines mapping
% epsilon     --- 0.999 --- Threshold in PCA

%------------------------------- Reference --------------------------------
% Y. Sun, B. Xue, M. Zhang, G. G. Yen, A new two-stage evolutionary
% algorithm for many-objective optimization, IEEE Transactions on
% Evolutionary Computation, 2019, 23(5): 748-761.
%------------------------------- 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
            [Evaluation1,Evaluation2,epsilon] = Algorithm.ParameterSet(20000,6000,0.999);

            %% Generate random population
            Population = Problem.Initialization();
            Archive    = [];

            %% Generate the weight vectors
            [W,W_num] = UniformPoint(Problem.N,Problem.M);
            UW        = [W;W(end:-1:1,:)];
            num_W     = size(UW,1);

            %% The objective value of each solution on single-objective in different stage
            SF = zeros(Problem.N+floor(Problem.N/2)*2,1);

            %% The objective value of each solution on single-objective in phase 1
            SF(1:Problem.N) = sum(Population.objs.*UW(1,:),2);

            %% Non-dominated dynamic weight aggregation (NDWA)
            while Algorithm.NotTerminated(Population) && Problem.FE < Evaluation1
                W_index = mod(ceil(Problem.FE/Problem.N)+1,num_W);
                if W_index == 0
                    W_index = num_W;
                end
                MatingPool          = TournamentSelection(2,Problem.N,SF(1:Problem.N));
                Offspring           = OperatorGA(Problem,Population(MatingPool),{0.9,20,1,20});
                SF(Problem.N+1:end) = sum(Offspring.objs.*UW(W_index,:),2);
                [Population,SF]     = NDWA_EnvironmentalSelection([Population,Offspring],SF,Problem.N);
                Archive             = [Archive,Population];
                Archive             = Archive(NDSort(Archive.objs,1)==1);
            end

            %% Pareto-optimal subspace learning
            learned_data = FindSubspace(Archive.decs,epsilon);
            mean_val     = mean(learned_data,1);
            u_limit      = ones(size(Problem.upper));
            l_limit      = zeros(size(Problem.lower));
            for i = 1:numel(mean_val)
                if abs(learned_data(1,i)-mean_val(i)) < 0.1
                    l_limit(i) = round(mean_val(i),1);
                    u_limit(i) = round(mean_val(i),1);
                else
                    l_limit(i) = Problem.lower(i);
                    u_limit(i) = Problem.upper(i);
                end
            end

            %% Reference lines mapping
            % Optimize Problem.M single-objective optimization problems
            rf_list       = diag(ones(1,Problem.M));
            SO_maxeval    = floor(Evaluation2/Problem.M);
            Extreme_point = repmat(Population(1),1,Problem.M);
            for i = 1 : Problem.M
                Curr_evl        = Problem.FE;
                PopDec          = rand(Problem.N, Problem.D).*repmat(u_limit-l_limit,Problem.N,1) + repmat(l_limit,Problem.N,1);
                Population      = Problem.Evaluation(PopDec);
                SF(1:Problem.N) = cos_v_func(Population.objs,rf_list(i,:));
                while Algorithm.NotTerminated(Population) && Problem.FE < (Curr_evl+SO_maxeval)
                    MatingPool          = TournamentSelection(2,Problem.N,SF(1:Problem.N));
                    Offspring           = Operator(Problem,Population(MatingPool).decs,l_limit,u_limit);
                    SF(Problem.N+1:end) = cos_v_func(Offspring.objs,rf_list(i,:));
                    [~,Rank]            = sort(SF,1);
                    Population          = [Population,Offspring];
                    Population          = Population(Rank(1:Problem.N));
                    SF(1:Problem.N)     = SF(Rank(1:Problem.N));
                end
                Extreme_point(i) = Population(1);
            end
            Ideal_point = min(Extreme_point.objs,[],1);
            Nadir_point = max(Extreme_point.objs,[],1);
            RefPoint    = W.*repmat(Nadir_point-Ideal_point,size(W,1),1) + repmat(Ideal_point,size(W,1),1);

            %% Diversity maintaining
            % Optimize W_num single-objective optimization problems
            SO_maxeval = floor((Problem.maxFE-Evaluation1-Evaluation2)/W_num);
            Result     = repmat(Population(1),1,size(RefPoint,1));
            for i = 1 : W_num
                Curr_evl        = Problem.FE;
                PopDec          = rand(Problem.N,Problem.D).*repmat(u_limit-l_limit,Problem.N, 1) + repmat(l_limit,Problem.N,1);
                Population      = Problem.Evaluation(PopDec);
                SF(1:Problem.N) = cos_v_func(Population.objs,RefPoint(i,:));
                while Algorithm.NotTerminated(Population) && Problem.FE < (Curr_evl+SO_maxeval)
                    MatingPool          = TournamentSelection(2,Problem.N,SF(1:Problem.N));
                    Offspring           = Operator(Problem,Population(MatingPool).decs,l_limit,u_limit);
                    SF(Problem.N+1:end) = cos_v_func(Offspring.objs,RefPoint(i,:));
                    [~,Rank]            = sort(SF,1);
                    Population          = [Population,Offspring];
                    Population          = Population(Rank(1:Problem.N));
                    SF(1:Problem.N)     = SF(Rank(1:Problem.N));
                    if i == W_num && Problem.FE == (Curr_evl+SO_maxeval)
                        Result(W_num) = Population(1);
                        Population    = Result;
                    end
                end
                Result(i) = Population(1);
            end
        end
    end
end

function [Population,SF] = NDWA_EnvironmentalSelection(Population,SF,N)
% Environmental selection

    [FrontNo,MaxFNo] = NDSort(Population.objs,Population.cons,N);
    Next = FrontNo < MaxFNo;
    Last = find(FrontNo==MaxFNo);
    Next(Last(1:N-sum(Next))) = true;
    Population = Population(Next);
    SF(1:N)    = SF(Next);
end

function SF = cos_v_func(PopObj,v)
% Calculate the objective value of each solution on each single-objective
% optimization problem

    r1 = sum(PopObj.*v,2);
    r2 = sqrt(sum(PopObj.^2,2));
    SF = -r1./r2;
end

function Offspring = Operator(Problem,ParentDec,lower,upper)
% Simulated binary crossover and polynomial mutation

    [proC,disC,proM,disM] = deal(1,20,1,20);
    Parent1 = ParentDec(1:floor(end/2),:);
    Parent2 = ParentDec(floor(end/2)+1:floor(end/2)*2,:);
    [N,D]   = size(Parent1);

    beta = zeros(N,D);
    mu   = rand(N,D);
    beta(mu<=0.5) = (2*mu(mu<=0.5)).^(1/(disC+1));
    beta(mu>0.5)  = (2-2*mu(mu>0.5)).^(-1/(disC+1));
    beta = beta.*(-1).^randi([0,1],N,D);
    beta(rand(N,D)<0.5) = 1;
    beta(repmat(rand(N,1)>proC,1,D)) = 1;
    Offspring = [(Parent1+Parent2)/2+beta.*(Parent1-Parent2)/2
                 (Parent1+Parent2)/2-beta.*(Parent1-Parent2)/2];

    Lower = repmat(lower,2*N,1);
    Upper = repmat(upper,2*N,1);
    Site  = rand(2*N,D) < proM/D;
    mu    = rand(2*N,D);
    temp  = Site & mu<=0.5;
    Offspring       = min(max(Offspring,Lower),Upper);
    Offspring(temp) = Offspring(temp)+(Upper(temp)-Lower(temp)).*((2.*mu(temp)+(1-2.*mu(temp)).*...
                      (1-(Offspring(temp)-Lower(temp))./(Upper(temp)-Lower(temp))).^(disM+1)).^(1/(disM+1))-1);
    temp = Site & mu>0.5;
    Offspring(temp) = Offspring(temp)+(Upper(temp)-Lower(temp)).*(1-(2.*(1-mu(temp))+2.*(mu(temp)-0.5).*...
                      (1-(Upper(temp)-Offspring(temp))./(Upper(temp)-Lower(temp))).^(disM+1)).^(1/(disM+1)));

    Offspring = Problem.Evaluation(Offspring);
end


function SCKS = spm_SCK(SCKS)
% FORMAT SCKS = spm_SCK(SCKS)
%__________________________________________________________________________
% Square-root Cubature Kalman Filters [2] & Square-root Rauch-Tang-Striebel
% Smoother (SCKF-SCKS [1]).
%==========================================================================
% This function performs joint estimation of the states, input and parameters
% of a model that is described as a stochastic continuous-discrete
% state-space in terms of nonlinear blind deconvolution. The state equations
% must have the form of ordinary differential equations, where the
% discretization is performed through local-linearization scheme [3].
% Additionally, the parameter noise covariance is estimated online via
% stochastic Robbins-Monro approximation method [4], and the measurement noise
% covariance is estimated using a combined variational Bayesian (VB)
% approach with a nonlinear filter/smoother [5].
%__________________________________________________________________________
%
% SCKS.M  - model structure (based on DEM [6] in SPM8 toolbox)
% SCKS.Y  - response variable, output or data
%__________________________________________________________________________
%
% generative model:
%--------------------------------------------------------------------------
%   M(1).f  = dx/dt = f(x,v,P)    {inline function, string or m-file}
%   M(1).g  = y(t)  = g(x,v,P)    {inline function, string or m-file}
%
%   M(1).xP = state error covariance matrix
%   M(1).uP = input error variance
%   M(1).wP = parameter error covariance matrix
%
%   M(1).pE = prior expectation of p model-parameters
%   M(1).pC = prior covariances of p model-parameters
%   M(1).ip = parameter indices
%   M(1).cb = constrain on parameters [lower, upper];
%
%   M(1).Q  = precision components on observation noise
%   M(1).V  = fixed precision (input noise)
%   M(1).W  = precision on state noise (approximated by annealing)
%
%   M(i).m  = number of inputs v(i + 1);
%   M(1).n  = number of states x(i);
%   M(1).l  = number of output v(i);
%
%   M(1).Qf      = form of measurement noise cov estimate:
%                  'auto'[default],'min','mean'
%   M(1).E.nN    = number of SCKF-SCKS algorithm iterations
%   M(1).E.Itol  = tolerance value for SCKF-SCKS convergence
%   M(1).E.nD    = number of integration step between observations
%   M(1).VB.N    = number of VB algorithm iterations
%   M(1).VB.Itol = tolerance value for VB convergence
%   M(1).VB.l    = VB scaling factor;
%
%   conditional moments of model-states - q(u)
%--------------------------------------------------------------------------
%   qU.x  = Conditional expectation of hidden states (backward estimate)
%   qU.v  = Conditional expectation of input (backward estimate)
%   qU.z  = Conditional prediction error
%   qU.S  = Conditional covariance: cov(x) (states - backward estimate)
%   qU.C  = Conditional covariance: cov(u) (input - backward estimate)
%
% conditional moments of model-parameters - q(p)
%--------------------------------------------------------------------------
%   qP.P    = Conditional expectation
%   qP.C    = Conditional covariance
%
%      F    = negative log-likelihood
%__________________________________________________________________________
% Copyright (c) Brno University of Technology (2010)...
% Martin Havlicek 05-12-2010
%
% References:
% [1] Havlicek M et al (2011)
% [2] Arasaratnam, I., Haykin, S. (2009) Cubature Kalman Filters. IEEE
%     Transactions on Automatic Control 54, 1254-1269.
% [3] Jimenez, J.C. (2002) A simple algebraic expression to evaluate the
%     local linearization schemes for stochastic differential equations*
%     1. Applied Mathematics Letters 15, 775-780.
% [4] Van der Merwe, R., 2004. Sigma-point Kalman filters for probabilistic
%     inference in dynamic state-space models. Ph.D.thesis, Oregon Graduate
%     Institute of Science and Technology.
% [5] Sarkka, S., Hartikainen, J. (2011?) Extension of VB-AKF to Estimation
%     of Full Covariance and Non-Linear Systems. In Press.
% [6] Friston, K.J., et al. (2008) DEM: a variational treatment of dynamic
%     systems. Neuroimage 41, 849-885.
%__________________________________________________________________________
% Copyright (C) - Martin Havlicek

% Martin Havlicek
% $Id: spm_SCK.m 4628 2012-01-27 20:51:41Z karl $
% check model specification
%--------------------------------------------------------------------------
M  = SCKS.M;
M  = spm_DEM_M_set(M);

% get integration step dt:
%--------------------------------------------------------------------------
nD = M(1).E.nD;
dt = 1/nD;

% INITIALISATION:
% =========================================================================

% interpolate observation according to integration step
%--------------------------------------------------------------------------
y     = SCKS.Y;            % observations
if size(y,1) > size(y,2)   % check the dimensions
    y = y';
end

% interpolate if dt < 1:
%--------------------------------------------------------------------------
y    = spm_interp(y',1/dt)';
if size(y,1) > size(y,2)   % check dimensions again
    y = y';
end
T    = size(y,2);          % number of time points

% initial condition:
%--------------------------------------------------------------------------
x     = M(1).x;            % states
u     = M(2).v;            % input
pE    = spm_vec(M(1).pE);  % all model parameter
ip    = M(1).ip;           % parameter indices to be estimated
theta = pE(ip);            % selected parameters

try cb  = M(1).cb;               catch, cb = []; end; % parameter constraints
try tE  = spm_vec(SCKS.pP.P{1}); catch, tE = []; end; % true parameters for display (if available)

% covariances (square-roots)
%--------------------------------------------------------------------------
sR      = cell(1,T);
[sR{:}] = deal(sparse(real(chol(inv(M(1).V)))*dt));   % observation noise variance
sQ      = sparse(real(chol(inv(M(1).W)))*dt);         % hidden state noise variance
if ~isempty(M(2).v)
    sV  = sparse(real(chol(inv(M(2).V)))*dt);         % input noise variance
else
    sV  = [];
end

% process error covariances (square-roots)
%--------------------------------------------------------------------------
Sx = sparse(real(chol(M(1).xP))*dt);
if ~isempty(M(2).v)
    Su = sparse(real(chol(M(1).uP))*dt);
else
    Su = [];
end
if ~isempty(ip)
    Sw = sparse(real(chol(M(1).wP(ip,ip)))*dt);
    sW = sparse(real(chol(M(1).pC(ip,ip)))*dt);  % parameter noise variance
    dv = diag(sW);
else
    Sw = [];
    sW = [];
end

% number of states, inputs and parameters:
%--------------------------------------------------------------------------
nx      = size(Sx,1);         % number of states
nu      = size(sV,1);         % number of states
nw      = size(sW,1);         % number of parameters
no      = size(sR{1},1);      % number of observations
noises  = nx + nu + nw + no;  % number of noise components

% concatenate state vector and square-root error covariance:
%--------------------------------------------------------------------------
xc      = [x(:); u(:); theta(:)];
xx      = zeros(nx+nu+nw,T);
xx(:,1) = xc;
Sc      = cell(1,T);
[Sc{:}] = deal(sparse(nx+nu+nw,nx+nu+nw));
Sc{1}   = blkdiag(Sx,Su,Sw);

% get vector indices for components of concatenated state vector
%--------------------------------------------------------------------------
xmask   = [ones(1,nx),ones(1,nu)*2,ones(1,nw)*3,ones(1,no)*4];
xind    = find(xmask==1);
uind    = find(xmask==2);
wind    = find(xmask==3);
clear xmask;

% setting for VB: observation noise estimation:
%--------------------------------------------------------------------------
if ~isempty(M(1).Q)
    try, iter0  = M(1).VB.N;   catch, iter0 = 3;          end
    try, lambda = M(1).VB.l;   catch, lambda = 1-exp(-2); end
    NU      = 6;
    V       = diag(repmat(1e-4,1,no));
    [sR{:}] = deal(sqrt(1./(NU-no-1).*V));
    B       = sqrt(lambda)*eye(no);
    k       = size(sR{1},1);
    iter    = iter0;
    MSE0    = zeros(no,1);
    RR0     = zeros(no,T);
    VBrun   = [];
else
    iter0   = 1;
    iter    = iter0;
    RR      = [];
    VBrun   = [];
end

% Pre-calculate cubature points:
%--------------------------------------------------------------------------
n          = nx + nu + nw + noises;       % total state vector dimension
nPts       = 2*n;                         % number of cubature points
CubPtArray = sqrt(n)*[eye(n) -eye(n)];    % cubature points array

% augment paramter matrix by number of cubature points:
%--------------------------------------------------------------------------
pE   = pE(:,ones(1,nPts));

% prepare matrix template for integration by Local linearization scheme:
%--------------------------------------------------------------------------
EXPm = repmat({[ones(nx),2*ones(nx,1);zeros(1,nx+1)]},1,nPts);
EXPm = sparse(blkdiag(EXPm{:}));
xt   = repmat([zeros(1,nx) 1],1,nPts)';

% =========================================================================
% Iteration scheme:
% =========================================================================
% get maximum number of iterations and tolerance:
%--------------------------------------------------------------------------
try, ItolVB = M(1).VB.Itol;  catch,  ItolVB = 1e-4;      end
try, Itol   = M(1).E.Itol;   catch,  Itol   = 1e-3;      end
try, RUN    = M(1).E.nN;     catch,  RUN    = 32;        end
try, ap     = M(1).E.RM;     catch,  ap     = [1e3 1e6]; end   % Robins-Monro approximation parameters

MLdiff0  = 1e-1;
mloglik0 = 0;
ML       = [];
VBrun    = RUN;
t0       = tic;
% =========================================================================
% Iteration loop (until convergence)
% =========================================================================

for run = 1:RUN
    t1  = tic;
    mloglik = -log(2.*pi).*(T/dt);
    % =====================================================================
    %   Forward pass:
    % =====================================================================
     for t = 2:T

        sQ = diag(diag((1/sqrt(0.9995)-1)*Sc{t-1}(xind,xind)));
        Sa = blkdiag(Sc{t-1},sQ,sV,sW,sR{t-1});
        xa = [xc;zeros(noises,1)];
        Xi =  xa(:,ones(1,nPts)) + Sa*CubPtArray;

        %------------------------------------------------------------------
        % PREDICTION STEP:
        %------------------------------------------------------------------
        xPred(uind,:) = Xi(uind,:) + Xi(uind+nx+nu+nw,:);  % add input noise
        xPred(wind,:) = Xi(wind,:) + Xi(wind+nx+nu+nw,:);  % add parameter noise

        % parameter constraint:
        %------------------------------------------------------------------
        if ~isempty(cb) && ~isempty(ip)
            xPred(wind,:) = min(cb(:,2*ones(1,nPts)),xPred(wind,:)); % upper constrain
            xPred(wind,:) = max(cb(:,1*ones(1,nPts)),xPred(wind,:)); % lower constrain
        end
        pE(ip,:) = xPred(wind,:);

        % propagation of cubature points through nonlinear function:
        %------------------------------------------------------------------
        f             = M(1).f(Xi(xind,:),xPred(uind,:),pE);

        % integration by local-linearization scheme:
        %------------------------------------------------------------------
        dfdx          = spm_diff_all(M(1).f,Xi(xind,:),xPred(uind,:),pE,1);
        dx            = expmall(dfdx,f,dt,EXPm)*xt;
        xPred(xind,:) = Xi(xind,:) + reshape(dx(~xt),nx,nPts) + Xi(xind+nx+nu+nw,:);

        % mean prediction:
        %------------------------------------------------------------------
        x1      = sum(xPred,2)/nPts;
        X       = (xPred-x1(:,ones(1,nPts)))/sqrt(nPts);
        [foo,S] = qr([X]',0);
        S       = S';

        % in the case of VB observation noise estimation:
        %------------------------------------------------------------------
        if ~isempty(M(1).Q) && iter~=1
            NU   = lambda.*(NU-k-1)+k+1;
            V    = B*V*B';
            NU   = NU + 1;
            V0   = V;
        end
        x10    = x1;
        S0     = S;
        yPred0 = M(1).g(xPred(xind,:),xPred(uind,:),pE);

        %------------------------------------------------------------------
        % UPDATE STEP:
        %------------------------------------------------------------------

        % VB estimation of sR (iteratively)
        %------------------------------------------------------------------
        for it = 1:iter

           % VB part - update of square-root measurement noise cov
           %---------------------------------------------------------------
            if ~isempty(M(1).Q) && iter ~= 1
                sR{t} = real(chol(1/(NU - no - 1)*V));
            end

            % propagate cubature points through observation function:
            %--------------------------------------------------------------
            yPred    = yPred0 + sR{t}*CubPtArray(end-no+1:end,:);
            y1       = sum(yPred,2)/nPts;

            Y        = (yPred-y1(:,ones(1,nPts)))/sqrt(nPts);
            [foo,Sy] = qr([Y]',0);
            Sy       = Sy';

            Pxy      = X*Y';                % cross covariance
            K        = (Pxy/Sy')/Sy;        % Kalman gain
            resid    =  y(:,t) - y1;        % innovations

            % state (input,parameter) estimates:
            %--------------------------------------------------------------
            xc = x10 + K*(resid);

            % check parameter constraints:
            %--------------------------------------------------------------
            if ~isempty(cb) && ~isempty(ip)
                xc(wind) = min(cb(:,2),xc(wind)); % upper constrain
                xc(wind) = max(cb(:,1),xc(wind)); % lower constrain
            end
            % estimate of process error covarinace:
            %--------------------------------------------------------------
            [foo,S] = qr([(X - K*Y)]',0);
            S       = S';

            % VB part
            %--------------------------------------------------------------
            if ~isempty(M(1).Q) && iter~=1
                Sa     = blkdiag(S,sQ,sV,sW,sR{t});
                Xi     = repmat([xc;zeros(noises,1)],1,nPts) + Sa*CubPtArray;
                yPreds = M(1).g(Xi(xind,:),Xi(uind,:),pE);  % no additive noise!
                D      = repmat(y(:,t),1,nPts)-yPreds;
                D      = D*D'/nPts;
                V      = V0 + D;
            end

        end
        Sc{t}   = S;
        xx(:,t) = xc;

        % Maximum log-Likelihood
        %------------------------------------------------------------------
         mloglik = mloglik - log(det(Sy*Sy')) - resid'/(Sy*Sy')*resid;

        % Robins-Monro stochastic approximation for of parameters noise cov
        %------------------------------------------------------------------
         if ~isempty(ip)
             subKG = K(wind,:);
             dv    = sqrt((1-1/ap(1))*(dv.^2) + 1/ap(1)*diag(subKG*(subKG*resid*resid')'));
             sW    = diag(dv);
             ap(1) = min(ap(1)+1,ap(2));
         end
         if ~isempty(M(1).Q) && iter~=1
             RR(:,t) = diag(sR{t});
         end
    end
    xxf = xx;
    Sf  = Sc;
    %----------------------------------------------------------------------
    % END of forward pass
    % ---------------------------------------------------------------------

    % =====================================================================
    %   Backward pass:
    % =====================================================================
    for t = T-1:-1:1
        % VB part:
        %------------------------------------------------------------------
        if ~isempty(M(1).Q) && iter~=1
            NU    = lambda.*(NU-k-1)+k+1;
            V     = B*V*B';
            NU    = NU + 1;
            V0    = V;
            sR{t} = real(chol(1/(NU-no-1)*V));
        end

        % Square-root Cubature Rauch-Tung-Striebel smoother
        %------------------------------------------------------------------

        % evaluate cubature points:
        %------------------------------------------------------------------
        Sa = blkdiag(Sc{t},sQ,sV,sW,sR{t});
        xa = [xx(:,t);zeros(noises,1)];
        Xi = xa(:,ones(1,nPts)) + Sa*CubPtArray;

        xPred(uind,:) = Xi(uind,:) + Xi(uind+nx+nu+nw,:);
        xPred(wind,:) = Xi(wind,:) + Xi(wind+nx+nu+nw,:);

        % check parameter constraints:
        %------------------------------------------------------------------
        if ~isempty(cb) && ~isempty(ip)
            xPred(wind,:) = min(cb(:,2*ones(1,nPts)),xPred(wind,:)); % upper constrain
            xPred(wind,:) = max(cb(:,1*ones(1,nPts)),xPred(wind,:)); % lower constrain
        end

        pE(ip,:)      = xPred(wind,:);
        % propagate cubature points through nonlinear function:
        %------------------------------------------------------------------
        f             = M(1).f(Xi(xind,:),xPred(uind,:),pE);
        dfdx          = spm_diff_all(M(1).f,Xi(xind,:),xPred(uind,:),pE,1);
        dx            = expmall(dfdx,f,dt,EXPm)*xt;
        xPred(xind,:) = Xi(xind,:) + reshape(dx(~xt),nx,nPts) + Xi(xind+nx+nu+nw,:) ;

        x1       = sum(xPred,2)/nPts;
        X        = (xPred-x1(:,ones(1,nPts)))/sqrt(nPts) + eps;
        x01      = xx(:,t);
        X01      = (Xi([xind,uind,wind],:) - repmat(x01,1,nPts))/sqrt(nPts);
        [foo,S]  = qr([X]',0);
        S        = S';

        Pxy      = X01*X';      % cross covariance
        K        = (Pxy/S')/S;  % Kalman gain

        % smoothed estimate of the states (input, parameters)
        % and process error covariance:
        %------------------------------------------------------------------
        xx(:,t)  = xx(:,t) + K*(xx(:,t+1) - x1);
        [foo,S]  = qr([X01 - K*X, K*Sc{t+1}]',0);
        S        = S';
        Sc{t}    = S;

        % check parameter constraint:
        %------------------------------------------------------------------
        if ~isempty(cb) && ~isempty(ip)
            xx(wind,t) = min(cb(:,2),xx(wind,t)); % upper constrain
            xx(wind,t) = max(cb(:,1),xx(wind,t));
        end

        % VB part (smoothing):
        %------------------------------------------------------------------
        if ~isempty(M(1).Q) && iter~=1
            Sa     = blkdiag(S,sQ,sV,sW,sR{t});
            Xi     = repmat([xx(:,t);zeros(noises,1)],1,nPts) + Sa*CubPtArray;
            yPreds = M(1).g(Xi(xind,:),Xi(uind,:),pE);  % no additive noise!
            D      = repmat(y(:,t),1,nPts)-yPreds;
            D      = D*D'/nPts;
            V      = V0 + D;
        end

        if ~isempty(M(1).Q) && iter~=1
            RR(:,t) = diag(sR{t});
        end

    end
    xxb = xx;
    Sb  = Sc;
    %----------------------------------------------------------------------
    % END of backward pass
    %----------------------------------------------------------------------

    str{1} = sprintf('SCKS: %i (1:%i)',run,iter);

    % iteration condition for measurement noise estimate:
    % iterate until stabilization of sR estimate
    %----------------------------------------------------------------------
    if ~isempty(M(1).Q) && iter0 ~= 1

        MSE     = mean((RR -(RR0)).^2,2);
        RR0     = RR;
        MSEdiff = abs(MSE - MSE0);
        MSE0    = MSE;

        if MSEdiff < ItolVB  % (till it gets stable)
            switch(lower(M(1).Qf))
                case('all')
                    % take all
                case('mean')
                    sR      = cell(1,T);
                    [sR{:}] = deal(diag(mean(RR,2)));
                case('min')
                    RRs     = sort(RR,2,'descend');
                    sR      = cell(1,T);
                    [sR{:}] = deal(diag(mean(RRs(:,round(T*0.90):end),2)));
                case('auto')
                    dlim    = min(RR,[],2);
                    ulim    = max(RR,[],2);
                    if all(ulim./dlim<4)
                        % take all
                    else
                        RRs     = sort(RR,2,'descend');
                        sR      = cell(1,T);
                        [sR{:}] = deal(diag(mean(RRs(:,round(T*0.90):end),2)));
                    end
            end
            iter0    = 1;
            iter     = iter0;
            mloglik0 = 0;
            VBrun    = run;
        end
    end

    % log-likelihood difference:
    %----------------------------------------------------------------------
    MLdiff(run) = abs(mloglik-mloglik0);
    ML(run)     = mloglik;
    if mloglik > 0
        mloglik     = mloglik - 5000;
        MLdiff(run) = abs(mloglik-mloglik0);
        ML(run)     = mloglik;
    end

    timed  = toc(t1);
    str{2} = sprintf('F:%.4e',ML(end));
    str{3} = sprintf('dF:%.4e',MLdiff(end));
    str{4} = sprintf('(%.2e sec)',timed);
    fprintf('%-16s%-16s%-16s%-16s\n',str{:})

    % plot estimates:
    %----------------------------------------------------------------------
    doplotting(M,xxf,xxb,Sf,Sb,ML,T,wind,ip,run,RR,VBrun);

    % stopping condition:
    %----------------------------------------------------------------------
    if RUN > 1 && (~isempty(ip) || ~isempty(M(1).Q))
        if run == 2
            MLdiff0 = MLdiff(run);
        elseif run > 2
            if MLdiff0 < MLdiff(run),
                MLdiff0 = MLdiff(run);
            end
        end
        if (((MLdiff(run)/MLdiff0)<Itol || run==RUN) || (isempty(ip)&&MLdiff(run)<Itol)) && iter0 == 1,

            yy       = M(1).g(xx(xind,:),xx(uind,:),pE(:,1));
            res      = y - yy;

            try SCKS = rmfield(SCKS,'qU'); end
            try SCKS = rmfield(SCKS,'qP'); end
            try SCKS = rmfield(SCKS,'qH'); end

            % save results in structure:
            %--------------------------------------------------------------
            SCKS.qU.v{1} = yy(:,1:nD:end);
            SCKS.qU.x{1} = xxb(xind,1:nD:end);
            SCKS.qU.v{2} = xxb(uind,1:nD:end);
            SCKS.qU.z{1} = res(:,1:nD:end);
            if ~isempty(ip)
                qP           = SCKS.M(1).pE;
                qP(ip)       = mean(xx(wind,:),2);
                SCKS.qP.P{1} = qP;
                SCKS.qP.P{2} = [];
            end
            SCKS.F       = ML;

            for i = 1:nD:T
                j = 1 + (i - 1)/nD;
                SCKS.qU.S{j} = Sb{i}(xind,xind);
                SCKS.qU.C{j} = Sb{i}(uind,uind);
                if ~isempty(ip)
                    SCKS.qP.p{j} = xxb(wind,:);
                    SCKS.qP.c{j} = Sb{i}(wind,wind);
                    SCKS.qP.ip   = ip;
                end
            end
            return
        end

        mloglik0 = mloglik;
        xc       = [xx([xind,uind],1); mean(xx(wind,:),2)];
        xx(:,1)  = xc;

    else

        pE(ip,1) = mean(xx(wind,:),2);
        yy       = M(1).g(xx(xind,:),xx(uind,:),pE(:,1));
        res      = y - yy;

        try SCKS = rmfield(SCKS,'qU'); end
        try SCKS = rmfield(SCKS,'qP'); end
        try SCKS = rmfield(SCKS,'qH'); end

        % save results in structure:
        %------------------------------------------------------------------
        SCKS.qU.v{1} = yy(:,1:nD:end);
        SCKS.qU.x{1} = xxb(xind,1:nD:end);
        SCKS.qU.v{2} = xxb(uind,1:nD:end);
        SCKS.qU.z{1} = res(:,1:nD:end);
        if ~isempty(ip)
            qP           = SCKS.M(1).pE;
            qP(ip)       = mean(xx(wind,:),2);
            SCKS.qP.P{1} = qP;
            SCKS.qP.P{2} = [];
        end
        SCKS.F       = ML;

        for i = 1:nD:T
            j = 1 + (i - 1)/nD;
            SCKS.qU.S{j} = Sb{i}(xind,xind);
            SCKS.qU.C{j} = Sb{i}(uind,uind);
            if ~isempty(ip)
                SCKS.qP.p{j} = xxb(wind,:);
                SCKS.qP.c{j} = Sb{i}(wind,wind);
                SCKS.qP.ip   = ip;
            end
        end
        return
    end
end

%==========================================================================
%==========================================================================

%--------------------------------------------------------------------------
% Plot estimates at each iteration:
%--------------------------------------------------------------------------
function doplotting(M,xxf,xxb,Sf,Sb,ML,T,wind,ip,run,RR,VBrun)

% Initialize display:
%--------------------------------------------------------------------------
spm_figure('GetWin','SCKF-SCKS estimates');
set(gcf,'Renderer','painter'); clf

% Hidden states
%--------------------------------------------------------------------------
for p=1:2
    subplot(3,3,[1:3]+3*(p-1)),
    hax = gca;
    si    = spm_invNcdf(1 - 0.05);
    s     = [];
    if p == 1,
        xxfig = xxf;
        Sfig  = Sf;
        tit   = 'SCKF - forward pass';
    else
        xxfig = xxb;
        Sfig  = Sb;
        tit   = 'SCKS - backward pass';
    end
    for i = 1:T
        s = [s abs(diag(Sfig{i}))];
    end

    % conditional covariances
    %----------------------------------------------------------------------
    j           = [1:size(xxfig(:,:),1)];
    ss          = si*s(j,:);
    [ill indss] = sort(full(mean(ss,2)),'descend');

    pf = plot(1:T,xxfig,'linewidth',1.5);
    set(hax,'xlim',[1,T],'nextplot','add')
    box(hax,'on')
    for ic = 1:size(xxfig,1)
        col0 = get(pf(indss(ic)),'color');
        col  = (ones(1,3)-col0)*0.65 + col0;
        fill([(1:T) fliplr(1:T)],[(xxfig(indss(ic),:) + ss(indss(ic),:)) fliplr((xxfig(indss(ic),:) - ss(indss(ic),:)))],...
            'r',...
            'FaceColor',col,...
            'EdgeColor',col);
        hold on;
        COL{ic} = col0;
    end
    for ic = 1:size(xxfig,1)
        plot(xxfig(indss(ic),:),'color',COL{ic},'linewidth',0.75);
    end
    title(tit);
    grid(hax,'on');
    axis(hax,'tight');
end

% Parameter estimates
%--------------------------------------------------------------------------
subplot(3,3,7)
h = plot([1:length(ML)],ML);
if ~isempty(M(1).Q)
    AYlim = get(gca,'Ylim');
    if VBrun>=run
        bkg = ones(1,run)*max(AYlim);
    else
        bkg = [ones(1,VBrun)*max(AYlim),ones(1,abs(VBrun-run))*min(AYlim)];
    end
    a = area(bkg,min(AYlim));
    axis([1 run+1 AYlim(1) AYlim(2)]); hold on;
    set(a(1),'FaceColor',ones(1,3)*0.6,'EdgeColor',ones(1,3)*0.6)
    h = plot([1:length(ML)],ML);
    axis([1 run+1 AYlim(1) AYlim(2)]); hold on;
    if VBrun>=run
        text(run/2+1,mean(AYlim),'VB-SCKS','HorizontalAlignment','center','VerticalAlignment','top');
    else
        text(VBrun/2+1,mean(AYlim),'VB-SCKS','HorizontalAlignment','center','VerticalAlignment','top');
        text(VBrun+(abs(VBrun-run)/2)+1,mean(AYlim),'SCKS','HorizontalAlignment','center','VerticalAlignment','top');
    end
    set(gca,'Layer','top')
    hold off
end
set(h,'color','k','Marker','o','Markersize',4,'MarkerFaceColor','k','linewidth',1);
title('Log-Likelihood');

if ~isempty(ip)
    subplot(3,3,8)
    b1 = bar(ip,mean(xxb(wind,:),2)','FaceColor','k');
    set(b1,'BarWidth',0.5); hold on;
    title('Parameters');
end

% Standard deviation of residuals
%--------------------------------------------------------------------------
if ~isempty(M(1).Q)
    subplot(3,3,9)
    if VBrun >= run
        plot(RR'); hold on;
    else
        plot(RR'); hold on;
        switch(lower(M(1).Qf))
            case('all')
                sR  = RR';
            case('mean')
                sR  = repmat(mean(RR,2),1,T)';
            case('min')
                RRs = sort(RR,2,'descend');
                sR  = repmat(mean(RRs(:,round(T*0.90):end),2),1,T)';
            case('auto')
                dlim    = min(RR,[],2);
                ulim    = max(RR,[],2);
                if all(ulim./dlim<4)
                    sR  = RR';
                else
                    RRs  = sort(RR,2,'descend');
                    sR   = repmat(mean(RRs(:,round(T*0.90):end),2),1,T)';
                end
        end
        plot([1:T],sR,'r','linewidth',2);
    end
    axis(gca,'tight');
    title('Standard deviation of residuals');
    hold off;
end
drawnow


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

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

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


function order = r8poly_order ( na, a )

%*****************************************************************************80
%
%% R8POLY_ORDER returns the order of a polynomial in power sum form.
%
%  Discussion:
%
%    The order of a constant polynomial is 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    30 September 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NA, the number of coefficients.
%
%    Input, real A(1:NA), the coefficients of the polynomials.
%
%    Output, integer ORDER, the order of the polynomial.
%
  order = na;

  while ( 1 < order )

    if ( a(order) ~= 0.0 )
      return
    end

    order = order - 1;

  end

  return
end


function blas1_c_test09 ( )

%*****************************************************************************80
%
%% TEST09 tests CSCAL.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 April 2006
%
%  Author:
%
%    John Burkardt
%
  n = 6;

  for j = 1 : n
    x(j) = 10 * j + j * i;
  end

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST09\n' );
  fprintf ( 1, '  CSCAL multiplies a complex scalar times a vector.\n' );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  X = \n' );
  fprintf ( 1, '\n' );
  for j = 1 : n
    fprintf ( 1, '  %6d  %10f  %10f\n', j, real ( x(j) ), imag ( x(j) ) );
  end

  da = 5.0 + 0.0 * i;
  x = cscal ( n, da, x, 1 );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  CSCAL ( N, ( %f  %f), X, 1 )\n', real ( da ), imag ( da ) );
  fprintf ( 1, '\n' );
  for j = 1 : n
    fprintf ( 1, '  %6d  %10f  %10f\n', j, real ( x(j) ), imag ( x(j) ) );
  end

  for j = 1 : n
    x(j) = 10 * j + j * i;
  end

  da = -2.0 + 1.0 * i;
  x = cscal ( 3, da, x, 2 );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  CSCAL ( 3, ( %f  %f), X, 2 )\n', real ( da ), imag ( da ) );
  fprintf ( 1, '\n' );
  for j = 1 : n
    fprintf ( 1, '  %6d  %10f  %10f\n', j, real ( x(j) ), imag ( x(j) ) );
  end

  return
end


function c = comp_fwt(f,h,a,J,ext)
%COMP_FWT Compute DWT using FWT
%   Usage:  c=comp_fwt(f,h,J,a,Lc,ext);
%
%   Input parameters:
%         f     : Input data - L*W array.
%         h     : Analysis Wavelet filters - cell-array of length *filtNo*.
%         J     : Number of filterbank iterations.
%         a     : Subsampling factors - array of length *filtNo*.
%         ext   : 'per','zero','even','odd' Type of the forward transform boundary handling.
%
%   Output parameters:
%         c     : Cell array of length M. Each element is Lc(m)*W array.
%

% This could be removed with some effort. The question is, are there such
% wavelet filters? If your filterbank has different subsampling factors following the first two filters, please send a feature request.
assert(a(1)==a(2),'First two elements of *a* are not equal. Such wavelet filterbank is not suported.');

% Time-reversed, complex conjugate impulse responses.
filtNo = length(h);
%hCell = cellfun(@(hEl) conj(flipud(hEl.h(:))),h,'UniformOutput',0);
hCell = cellfun(@(hEl) hEl.h(:),h,'UniformOutput',0);

if(strcmp(ext,'per'))
   % Initial shift of the filter to compensate for it's delay.
   % "Zero" delay transform is produced
   % offset = cellfun(@(hEl) 1-numel(hEl.h)-hEl.offset,h);
   offset = cellfun(@(hEl) hEl.offset,h);
elseif strcmp(ext,'valid')
   offset = -cellfun(@(hEl) numel(hEl.h)-1,h);
else
   % No compensation for the filter delay (filters are made causal with respect to the output sequences).
   % This creates relative shift between levels of coefficients.
   % Initial shift determines type of subsampling.
   % This is even subsampling. e.g. subs. [1,2,3,4,5,6] by a factor 3 becomes [3,6]
   % The number of output coefficients depends on it.
   offset = -(a-1);
   % For odd subsampling skip = 0; but it requires slight touches
   % elsewhere.
end

M = (filtNo-1)*J+1;
c = cell(M,1);
runPtr = M-filtNo+2;
ctmp = f;
for jj=1:J
    % Run filterbank
    ctmp = comp_filterbank_td(ctmp,hCell,a,offset,ext);
    % Bookkeeping
    c(runPtr:runPtr+filtNo-2) = ctmp(2:end);
    ctmp = ctmp{1};
    runPtr = runPtr - (filtNo - 1);
end
% Save final approximation coefficients
c{1} = ctmp;


function a = erfc(a)
%ERFC         Gradient complementary error function erfc(a)
%

% written  05/31/13     S.M. Rump
%

  e = 1e-30;
  if 1+e==1-e                           % fast check for rounding to nearest
    rndold = 0;
  else
    rndold = getround;
    setround(0)
  end

  N = getappdata(0,'INTLAB_GRADIENT_NUMVAR');
  % factorLB <= 2/sqrt(pi) <= factorUB
  INTLAB_STDFCTS_ERF = getappdata(0,'INTLAB_STDFCTS_ERF');
  factorLB = INTLAB_STDFCTS_ERF.TWO_SQRTPIINF;  % round to nearest
  factorUB = INTLAB_STDFCTS_ERF.TWO_SQRTPISUP;  % ~ 1.12

  % use full(a.x(:)): cures Matlab V6.0 bug
  % a=7; i=[1 1]; x=a(i), b=sparse(a); y=b(i)  yields row vector x but column vector y
  % ax is full anyway
  a.x = full(a.x);
  ax = exp(-a.x(:).^2);
  a.x = erfc(a.x);
  if issparse(a.dx)
    sizeax = size(a.dx,1);
    [ia,ja,sa] = find(a.dx);
    if isa(a.x,'intval')
      adx = times(ax(ia),sa,0);
      a.dx = intval(-factorUB,-factorLB,'infsup') * ...
          intval( sparse(ia,ja,adx.inf,sizeax,N) , sparse(ia,ja,adx.sup,sizeax,N) , 'infsup' );
    else
      a.dx = (-factorLB) * sparse(ia,ja,ax(ia).*sa,sizeax,N);
    end
  else
    if isa(a.x,'intval')
      ax = intval(-factorUB,-factorLB,'infsup') * ax;
      a.dx = a.dx .* ax(:,ones(1,N));
    else
      ax = (-factorLB) * ax;
      a.dx = a.dx .* ax(:,ones(1,N));
    end
  end

  if rndold
    setround(rndold)
  end


function f = fix(f)
%FIX   Round a CHEBFUN pointwise toward zero.
%   G = FIX(F) returns the CHEBFUN G such that G(x) = FIX(F(x)) for each x in
%   F.domain.
%
%   If F is complex, then the G = FIX(REAL(F)) + 1i*FIX(IMAG(F)).
%
% See also ROUND, CEIL, FLOOR.

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

% Deal with the trivial empty case:
if ( isempty(f) )
    return
end

% Deal with unbounded functions:
if ( ~isfinite(f) )
    error('CHEBFUN:CHEBFUN:fix:inf', ...
        'fix() is not defined for functions which diverge to infinity.');
end

% Deal with complex-valued functions:
if ( ~isreal(f) )
    if ( isreal(1i*f) )
        f = 1i*fix(imag(f));
    else
        f = fix(real(f)) + 1i*fix(imag(f));
    end
    return
end

% Find all the integer crossings for f:
mmvals = minandmax(f); % [TODO]: Only need a good bound?
minf = min(mmvals(1, :));
maxf = max(mmvals(2, :));
range = floor([minf, maxf]);
for k = (range(1)+1):range(2)
    f = addBreaksAtRoots(f - k) + k;
end

% Loop over the FUNs:
for k = 1:numel(f.funs)
    f.funs{k} = fix(f.funs{k});
end

% Fix the pointValues:
f.pointValues = fix(f.pointValues);

% Simplify the result:
f = merge(f);

end


function [xEst,PEst,logLikes]=singleScanUpdate(xHyp,PHyp,A,algSel1,algSel2,param3)
%%SINGLESCANUPDATE Perform the measurement update step in a single-scan
%                  tracking algorithm that uses Gaussian approximations
%                  to represent the target state before and after the
%                  measurement update given a matrix of likelihoods and a
%                  set of track update hypotheses. The algorithm can be a
%                  global nearest neighbor (GNN) update, a joint
%                  probabilistic data association (JPDA) update, a JPDA*
%                  update, a GNN-JPDA update, or approximate JPDA and
%                  GNN-JPDA updates as well as a naive nearest neighbor
%                  algorithm. In all instances except for the naive nearest
%                  neighbor algorithm, it is assumed that each measurement
%                  can be assigned to at most one target, and each target
%                  can be assigned to at most one measurement.
%
%INPUTS: xHyp An xDimXnumTarXnumHyp set of track states updated with each
%             of the (numHyp-1) measurements for each of the numTar
%             targets, with the last hypothesis being the update for the
%             missed detection hypothesis. The order of the measurement
%             hypotheses is the same as the ordering of the likelihoods in
%             the columns of the A matrix.
%        PHyp An xDimXxDimXnumTarXnumHyp set of covariance matrices for
%             each of the track states for each of the targets updated
%             conditioned on each of the numHyp measurements with the last
%             one being for the missed detection hypothesis.
%           A A matrix of positive likelihoods or likelihood ratios (NOT
%             log-likelihood ratios). A is a numTar X (numMeas+numTar)
%             matrix of all-positive likelihoods or likelihood ratios for
%             assigning the target specified by the row to the measurement
%             specified by the column. Columns > numMeas hold
%             missed-detection likelihoods. Thus, off-diagonal terms for
%             columns > numMeas should be set to 0 and the diagonal terms
%             set to the costs of a missed detection for each given
%             target.
%     algSel1 An optional parameter that selects the assignment algorithm
%             to use. Approximate variants of the algorithms also use
%             algSel2. If this parameter and the next parameter are both
%             omitted, then the algorithm is chosen as described below.
%             Possible values are
%             0) GNN-JPDA
%             1) JPDA
%             2) GNN
%             3) Parallel single-target PDAs
%             4) Naive nearest neighbor
%             5) JPDA*
%             6) Approximate GNN-JPDA
%             7) Approximate JPDA
%             8) Set JPDA
%             9) Naive nearest neighbor JPDA
%             10) Approximate naive nearest neighbor JPDA
%     algSel2 An optional parameter that further specifies the algorithm
%             used when algSel1=6-7. If omitted but algSel1 is specified, a
%             default value of 0 is used. If both algSel1 and algSel2
%             are omitted, the algorithm is chosen as described below.
%             algSel2 chooses the approximation for the beta terms in the
%             function calc2DAssignmentProbsApprox (the approxType input)
%             when using algSel1=6,7; the comments to that function
%             describe the values it can take. If algSel1=8 (Set JPDAF),
%             this is the number of position components in the state (the
%             posDim input in the calcSetJPDAUpdate function).
%      param3 For the case where algSel1=6,7 and algSel2=0, this is the
%             optional delta input to the function
%             calc2DAssignmentProbsApprox. If omitted or an empty matrix
%             is passed, the default value for that function is used.
%
%OUTPUTS: xEst An xDimXnumTar matrix of updated target states.
%         PEst An xDimXxDimXnumtar matrix of updated covariance matrices
%              for the targets.
%      logLike The numTarX1 set of logarithms of the likelihood (ratio)
%              function for the update for each target. If A contains
%              likelihoods, then this is the logarithm of the likelihood;
%              if A constains likelihood ratios, then this is a logarithm
%              of a likelihood ratio, For hard assignments, this is clearly
%              defined (it is just the entry in A assigned to that target).
%              For soft assignments in the JPDA, JPDA*, PDA, and
%              approximate JPDA this is the expected value of the
%              log-likelihood computed as using the beta terms. If the Set
%              JPDAF is chosen, an empty matrix is returned for logLike.
%
%If both algSel1 and algSel2 are omitted, then the algorithm chosen depends
%on the size of A. If A has only one row (one target), then the JPDA is
%used. Otherwise, the approximate GNN-JPDA with algSel2=4 is used (which is
%actually exact if the number of targets is two).
%
%The JPDA is discussed in Chapter 6.2 of [1], and the PDA is discussed in
%various other sections as the JPDA is a generalization of the PDA. The
%JPDA* is discussed in [2]. The concept of the GNN-JPDA is described (but
%not named) in [3]. The GNN-JPDA consists of using the global nearest
%neighbor estimate with a covariance matrix computed as in the JPDA. Thus
%the same approximations that can be used in the JPDA can be used in the
%GNN-JPDA. The Set JPDA is described in [4].
%
%The approximate routines simply change how the target-measurement
%association probabilities are computed. They are described in the comments
%to the function calc2DAssignmentProbsApprox.
%
%Naive nearest neighbor association consists of just choosing the most
%likely assignment of a target to a measurement or missed detection without
%regard to whether one measurmeent is assigned to multiple targets.
%
%One possible way to form the A matrix is to use likelihood ratios going
%into the dimensionless score function from [5]. This is implemented for a
%standard Cartesian/Gaussian model using makeStandardCartOnlyLRMatHyps.
%
%REFERENCES:
%[1] Y. Bar-Shalom, P. K. Willett, and X. Tian, Tracking and Data Fusion.
%    Storrs, CT: YBS Publishing, 2011.
%[2] H. A. Blom and E. A. Bloem, "Probabilistic data association avoiding
%    track coalescence," IEEE Transactions on Automatic Control, vol. 45,
%    no. 2, pp. 247-259, Feb. 2000.
%[3] O. E. Drummond, "Best hypothesis target tracking and sensor fusion,"
%    in Proceedings of SPIE: Signal and Data Processing of Small Targets
%    Conference, vol. 3809, Denver, CO, Oct. 1999, pp. 586-600.
%[4] L. Svensson, D. Svensson, M. Guerriero, and P. Willett, "Set JPDA
%    filter for multitarget tracking," IEEE Transactions on Signal
%    Processing, vol. 59, no. 10, pp. 4677-4691, Oct. 2011.
%[5] Y. Bar-Shalom, S. S. Blackman, and R. J. Fitzgerald, "Dimensionless
%    score function for multiple hypothesis tracking," IEEE Transactions on
%    Aerospace and Electronic Systems, vol. 43, no. 1, pp. 392-400, Jan.
%    2007.
%
%March 2015 David F. Crouse, generalizing the basic JPDAF code of David
%Karnick to many more algorithms, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

    numTar=size(A,1);
    numMeas=size(A,2)-numTar;
    numHyp=numMeas+1;

    if(nargin<6)
        param3=[];
    end

    if(nargin<4||isempty(algSel1))
        if(numTar==1)
            algSel1=1;
        else
            algSel1=6;
            algSel2=4;
        end
    end

    if(nargin==4||isempty(algSel2))%If only algSel2 was omitted.
        algSel2=0;
    end
    xDim=size(xHyp,1);

    xEst=zeros(xDim,numTar);
    PEst=zeros(xDim,xDim,numTar);
    switch(algSel1)
        case 0%GNN-JPDA
            %Perform 2D assignment
            ALog=log(A);
            tar2Meas=assign2D(ALog,true);
            logLikes=zeros(numTar,1);
            for curTar=1:numTar
                logLikes(curTar)=ALog(curTar,tar2Meas(curTar));
            end

            %Adjust for the index of the missed detection hypothesis.
            sel=tar2Meas>numMeas;
            tar2Meas(sel)=numMeas+1;

            %Set the target estimate to the ML estimate.
            for curTar=1:numTar
                maxIdx=tar2Meas(curTar);
                xEst(:,curTar)=xHyp(:,curTar,maxIdx);
            end

            %Compute the JPDAF probabilities.
            beta=calc2DAssignmentProbs(A,true);

            %Compute the covariance matrix in the manner of the JPDAF, but
            %about the ML estimate, so the result is a MSE matrix estimate.
            for curTar=1:numTar
                x=reshape(xHyp(:,curTar,:),[xDim,numHyp,1]);
                P=reshape(PHyp(:,:,curTar,:),[xDim,xDim,numHyp]);
                [~,PEst(:,:,curTar)]=calcMixtureMoments(x,beta(curTar,:),P,xEst(:,curTar));
            end
        case 1%JPDA
            %Compute the JPDAF probabilities.
            beta=calc2DAssignmentProbs(A,true);

            %The JPDAF is just the mixture mean and covariance matrix.
            for curTar=1:numTar
                x=reshape(xHyp(:,curTar,:),[xDim,numHyp,1]);
                P=reshape(PHyp(:,:,curTar,:),[xDim,xDim,numHyp]);
                [xEst(:,curTar),PEst(:,:,curTar)]=calcMixtureMoments(x,beta(curTar,:),P);
            end

            %If a likelihood is desired, then find the weighted likelihood.
            if(nargout>2)
                logLikes=logLikefromABeta(A,beta);
            end
        case 2%GNN
            %Perform 2D assignment
            ALog=log(A);
            tar2Meas=assign2D(ALog,true);
            logLikes=zeros(numTar,1);
            for curTar=1:numTar
                logLikes(curTar)=ALog(curTar,tar2Meas(curTar));
            end
            %Adjust for the index of the missed detection hypothesis.
            sel=tar2Meas>numMeas;
            tar2Meas(sel)=numMeas+1;

            for curTar=1:numTar
                maxIdx=tar2Meas(curTar);
                xEst(:,curTar)=xHyp(:,curTar,maxIdx);
                PEst(:,:,curTar)=PHyp(:,:,curTar,maxIdx);
            end
        case 3%Paralle single-target PDAs
            %This is just a bunch of independent PDAFs for each target.
            beta=zeros(numTar,numHyp);
            for curTar=1:numTar
                hypIdx=[1:numMeas,numMeas+curTar];
                beta(curTar,:)=calc2DAssignmentProbs(A(curTar,hypIdx),true);
                x=reshape(xHyp(:,curTar,:),[xDim,numHyp,1]);
                P=reshape(PHyp(:,:,curTar,:),[xDim,xDim,numHyp]);
                [xEst(:,curTar),PEst(:,:,curTar)]=calcMixtureMoments(x,beta(curTar,:),P);
            end

            %If a likelihood is desired, then find the weighted likelihood.
            if(nargout>2)
                logLikes=logLikefromABeta(A,beta);
            end
        case 4%Na�ve nearest neighbor
            %The assignment for each target
            logLikes=zeros(numTar,1);
            for curTar=1:numTar
                [maxVal,maxIdx]=max(A(curTar,:));
                %If the missed detection hypothesis is the most likely.
                if(maxIdx>numMeas)
                    maxIdx=numMeas+1;
                end

                xEst(:,curTar)=xHyp(:,curTar,maxIdx);
                PEst(:,:,curTar)=PHyp(:,:,curTar,maxIdx);

                logLikes(curTar)=log(maxVal);
            end
        case 5%JPDA*
            %This is the same as the JPDA, except the method for computing
            %the betas is different.
            %Compute the JPDAF* probabilities.
            beta=calcStarBetasBF(A);

            %The JPDAF* is just the mixture mean and covariance matrix.
            for curTar=1:numTar
                x=reshape(xHyp(:,curTar,:),[xDim,numHyp,1]);
                P=reshape(PHyp(:,:,curTar,:),[xDim,xDim,numHyp]);
                [xEst(:,curTar),PEst(:,:,curTar)]=calcMixtureMoments(x,beta(curTar,:),P);
            end

            %If a likelihood is desired, then find the weighted likelihood.
            if(nargout>2)
                logLikes=logLikefromABeta(A,beta);
            end
        case 6%Approximate GNN-JPDA
            %This is the same as the GNN-JPDA, except the method for
            %computing the betas is different.

            %Perform 2D assignment
            ALog=log(A);
            tar2Meas=assign2D(ALog,true);
            logLikes=zeros(numTar,1);
            for curTar=1:numTar
                logLikes(curTar)=ALog(curTar,tar2Meas(curTar));
            end

            %Adjust for the index of the missed detection hypothesis.
            sel=tar2Meas>numMeas;
            tar2Meas(sel)=numMeas+1;

            %Set the target estimate to the ML estimate
            for curTar=1:numTar
                maxIdx=tar2Meas(curTar);
                xEst(:,curTar)=xHyp(:,curTar,maxIdx);
            end

            %Compute the approximate JPDAF probabilities.
            beta=calc2DAssignmentProbsApprox(A,algSel2,true,param3);

            %Compute the covariance matrix in the manner of the JPDAF, but
            %about the ML estimate, so the result is a MSE matrix estimate.
            for curTar=1:numTar
                x=reshape(xHyp(:,curTar,:),[xDim,numHyp,1]);
                P=reshape(PHyp(:,:,curTar,:),[xDim,xDim,numHyp]);
                [~,PEst(:,:,curTar)]=calcMixtureMoments(x,beta(curTar,:),P,xEst(:,curTar));
            end
        case 7%Approximate JPDA
            %This is the same as the JPDA, except the method for computing
            %the betas is different.

            %Compute the approximate JPDAF probabilities.
            beta=calc2DAssignmentProbsApprox(A,algSel2,true,param3);

            %The JPDAF* is just the mixture mean and covariance matrix.
            for curTar=1:numTar
                x=reshape(xHyp(:,curTar,:),[xDim,numHyp,1]);
                P=reshape(PHyp(:,:,curTar,:),[xDim,xDim,numHyp]);
                [xEst(:,curTar),PEst(:,:,curTar)]=calcMixtureMoments(x,beta(curTar,:),P);
            end

            %If a likelihood is desired, then find the weighted likelihood.
            if(nargout>2)
                logLikes=logLikefromABeta(A,beta);
            end
        case 8%Set JPDAF
            %This is the same as the JPDA, except the method for computing
            %the betas is different.
            [xEst,PEst]=calcSetJPDAUpdate(xHyp,PHyp,A,AlgSel2);

            if(nargout>2)
                logLikes=[];
            end
        case 9%Na�ve nearest neighbor-JPDA
            %The assignment for each target
            logLikes=zeros(numTar,1);
            for curTar=1:numTar
                [maxVal,maxIdx]=max(A(curTar,:));

                %If the missed detection hypothesis is the most likely.
                if(maxIdx>numMeas)
                    maxIdx=numMeas+1;
                end

                xEst(:,curTar)=xHyp(:,curTar,maxIdx);
                logLikes(curTar)=log(maxVal);
            end

            %Compute the JPDAF probabilities.
            beta=calc2DAssignmentProbs(A,true);

            %Compute the covariance matrix in the manner of the JPDAF, but
            %about the ML estimate, so the result is a MSE matrix estimate.
            for curTar=1:numTar
                x=reshape(xHyp(:,curTar,:),[xDim,numHyp,1]);
                P=reshape(PHyp(:,:,curTar,:),[xDim,xDim,numHyp]);
                [~,PEst(:,:,curTar)]=calcMixtureMoments(x,beta(curTar,:),P,xEst(:,curTar));
            end
        case 10%Approxmate na�ve nearest neighbor-JPDA
            %This is the same as the na�ve nearest neighbor-JPDA, except
            %the method for computing the betas is different.

            %The assignment for each target
            logLikes=zeros(numTar,1);
            for curTar=1:numTar
                [maxVal,maxIdx]=max(A(curTar,:));

                %If the missed detection hypothesis is the most likely.
                if(maxIdx>numMeas)
                    maxIdx=numMeas+1;
                end

                xEst(:,curTar)=xHyp(:,curTar,maxIdx);
                logLikes(curTar)=log(maxVal);
            end

            %Compute the approximate JPDAF probabilities.
            beta=calc2DAssignmentProbsApprox(A,algSel2,true,param3);

            %Compute the covariance matrix in the manner of the JPDAF, but
            %about the ML estimate, so the result is a MSE matrix estimate.
            for curTar=1:numTar
                x=reshape(xHyp(:,curTar,:),[xDim,numHyp,1]);
                P=reshape(PHyp(:,:,curTar,:),[xDim,xDim,numHyp]);
                [~,PEst(:,:,curTar)]=calcMixtureMoments(x,beta(curTar,:),P,xEst(:,curTar));
            end
        otherwise
            error('Unknown algorithm selected.')
    end
end

function logLike=logLikefromABeta(A,beta)
    numTar=size(A,1);
    numHyp=size(beta,2);

    %Create a copy of A with the missed detection likelihoods
    %in the last column and  take the logarithm of it.
    ANew=zeros(numTar,numHyp);
    ANew(:,1:(numHyp-1))=A(:,1:(numHyp-1));
    ANew(:,numHyp)=diag(A(:,numHyp:(numHyp+numTar-1)));
    ANew=log(ANew);

    %Multiply each log-likelihood by the probability of it
    %being correct.
    ANew=ANew.*beta;
    %Get rid of non-finite terms, which should generally only
    %arise in things with zero likelihood.
    sel=~isfinite(ANew);
    ANew(sel)=0;
    %The likelihood is the sum of all of the weighted
    %log-likelihoods.
    logLike=sum(ANew,2);
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.


% plotfresp() - Plot a filter's impulse, step, magnitude, and phase response
%
% Usage:
%   >> plotfresp(b, a, nfft, fs, dir);
%
% Inputs:
%   b     - vector numerator coefficients
%
% Optional inputs:
%   a     - scalar or vector denominator coefficients (IIR support is
%           experimental!) {default 1}
%   nfft  - scalar number of points {default 512}
%   fs    - scalar sampling frequency {default 1}
%   dir   - string filter direction {default 'onepass'}
%
% Author: Andreas Widmann, University of Leipzig, 2005
%
% See also:
%   firws

%123456789012345678901234567890123456789012345678901234567890123456789012

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

function plotfresp(b, a, nfft, fs, dir)

if nargin < 5 || isempty(dir)
    dir = 'onepass';
end
if nargin < 4  || isempty(fs)
    fs = 1;
end
if nargin < 3 || isempty(nfft)
    nfft = 512;
end
if nargin < 2 || isempty(a)
    a = 1;
end
if nargin < 1
    ft_error('Not enough input arguments.');
end

% FIR?
if isscalar(a) && a == 1
    isFIR = true;
else
    isFIR = false;
end

% Linear phase FIR
if isFIR && all(b(:)' == fliplr(b(:)')) % TODO: antisymmetric
    isLinPhaseFir = true;
else
    isLinPhaseFir = false;
end

% Twopass/zerophase?
if strncmp('twopass', dir, 7)
    isTwopass = true;
    isZerophase = true;
elseif strcmp('onepass-zerophase', dir) || strcmp('onepass-reverse-zerophase', dir)
    if ~isLinPhaseFir
        ft_error('Onepass-zerophase filtering is only allowed for linear-phase FIR filters.')
    end
    isTwopass = false;
    isZerophase = true;
else
    isTwopass = false;
    isZerophase = false;
end

% Impulse response
if isFIR
    impresp = b(:)';
else
    if ~exist('impz', 'file')
        ft_warning('Plotting IIR filter responses requires signal processing toolbox.')
        return
    end
    impresp = impz(b, a)';
end

% Twopass
if isTwopass
    impresp = conv(impresp, fliplr(impresp));
end
n = length(impresp);

% Zerophase
if isZerophase
    groupdelay = (n - 1) / 2;
    x = -groupdelay:groupdelay;
else
    x = 0:n - 1;
end

nfft = max([2^ceil(log2(n)) nfft]); % Do not truncate impulse response
f = linspace(0, fs / 2, nfft / 2 + 1);
z = fft(impresp, nfft);
z = z(1:nfft / 2 + 1);

% Find open figure window
H = findobj('Tag', 'plotfiltresp', 'type', 'figure');
if ~isempty(H)
    figure(H);
else
    H = figure;
    set(H, 'Tag', 'plotfiltresp');
    posArray = get(H, 'Position');
    posArray(3) = posArray(4) * 1.6;
    set(H, 'Position', posArray);
end

% Formatting
titlePropArray = {'Fontweight', 'bold'};
axisPropArray = {'NextPlot', 'add', 'XGrid', 'on', 'YGrid', 'on', 'Box', 'on'};

% Impulse resonse
ax(1) = subplot(2, 3, 1, axisPropArray{:});
stem(x, impresp, 'fill')
title('Impulse response', titlePropArray{:});
ylabel('Amplitude');

% Step response
ax(4) = subplot(2, 3, 4, axisPropArray{:});
stem(x, cumsum(impresp), 'fill');
title('Step response', titlePropArray{:});
ylimArray = ylim;
if ylimArray(2) < -ylimArray(1) + 1;
    ylimArray(2) = -ylimArray(1) + 1;
    ylim(ylimArray);
end
xMin = []; xMax = [];
childrenArray = get(ax(4), 'Children');
for iChild =1:length(childrenArray)
    xData = get(childrenArray(iChild), 'XData');
    xMin = min([xMin min(xData)]);
    xMax = max([xMax max(xData)]);
end
set(ax([1 4]), 'XLim', [xMin xMax]);
ylabel('Amplitude');

% Magnitude response
ax(2) = subplot(2, 3, 2, axisPropArray{:});
plot(f, abs(z));
title('Magnitude response', titlePropArray{:});
ylabel('Magnitude (linear)');

ax(5) = subplot(2, 3, 5, axisPropArray{:});
plot(f, 20 * log10(abs(z)));
title('Magnitude response', titlePropArray{:});
ylimArray = ylim;
if ylimArray(1) < -200
    ylimArray(1) = -200;
    ylim(ylimArray);
end
ylabel('Magnitude (dB)');

% Phase response
ax(3) = subplot(2, 3, 3, axisPropArray{:});
phaseresp = unwrap(angle(z));
if isZerophase % Correct delay for zero-phase FIR filter?
    delay = -f / fs * groupdelay * 2 * pi;
    phaseresp = phaseresp - delay;
    phaseresp = mod(round(phaseresp / pi), 2) * pi; % Avoid rounding errors; linear-phase FIR only!
end
plot(f, phaseresp);
title('Phase response', titlePropArray{:});
ylabel('Phase (rad)');

% Formatting
xlabelArray = get(ax(1:5), 'XLabel');
if fs == 1
    set([xlabelArray{[2 3 5]}], 'String', 'Normalized frequency (2 \pi rad / sample)');
else
    set([xlabelArray{[2 3 5]}], 'String', 'Frequency (Hz)');
end
set([xlabelArray{[1 4]}], 'String', 'n (samples)');
set(ax([2 3 5]), 'XLim', [0 fs / 2]);
set(ax(1:5), 'ColorOrder', circshift(get(ax(1), 'ColorOrder'), -1));

end