%SE2 Representation of 2D rigid-body motion
%
% This subclasss of RTBPose is an object that represents rigid-body motion in 2D.
% Internally this is a 3x3 homogeneous transformation matrix (3x3) belonging to
% the group SE(2).
%
% Constructor methods::
%  SE2          general constructor
%  SE2.exp      exponentiate an se(2) matrix
%  SE2.rand     random transformation
%  new          new SE2 object
%
% Display and print methods::
%  animate          ^graphically animate coordinate frame for pose
%  display          ^print the pose in human readable matrix form
%  plot             ^graphically display coordinate frame for pose
%  print            ^print the pose in single line format
%
% Group operations::
%  *            ^mtimes: multiplication (group operator, transform point)
%  /            ^mrdivide: multiply by inverse
%  ^            ^mpower: exponentiate (integer only):
%  inv          inverse
%  prod         ^product of elements
%
% Methods::
%  det          determinant of matrix component
%  eig          eigenvalues of matrix component
%  log          logarithm of rotation matrix
%  inv          inverse
%  simplify*    apply symbolic simplication to all elements
%  interp       interpolate between poses
%  theta        rotation angle
%
% Information and test methods::
%  dim          ^returns 2
%  isSE         ^returns true
%  issym        ^test if rotation matrix has symbolic elements
%  SE2.isa      test if matrix is SE(2)
%
% Conversion methods::
%  char*         convert to human readable matrix as a string
%  SE2.convert   convert SE2 object or SE(2) matrix to SE2 object
%  double        convert to rotation matrix
%  R             convert to rotation matrix
%  SE3           convert to SE3 object with zero translation
%  SO2           convert rotational part to SO2 object
%  T             convert to homogeneous transformation matrix
%  Twist         convert to Twist object
%  t             get.t: convert to translation column vector
%
% Compatibility methods::
%  isrot2       ^returns false
%  ishomog2     ^returns true
%  tr2rt        ^convert to rotation matrix and translation vector
%  t2r          ^convert to rotation matrix
%  transl2      ^translation as a row vector
%  trprint2     ^print single line representation
%  trplot2      ^plot coordinate frame
%  tranimate2   ^animate coordinate frame
%
% ^ inherited from RTBPose class.
%
% See also SO2, SE3, RTBPose.

% Copyright (C) 1993-2019 Peter I. Corke
%
% This file is part of The Spatial Math Toolbox for MATLAB (SMTB).
%
% Permission is hereby granted, free of charge, to any person obtaining a copy
% of this software and associated documentation files (the "Software"), to deal
% in the Software without restriction, including without limitation the rights
% to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies
% of the Software, and to permit persons to whom the Software is furnished to do
% so, subject to the following conditions:
%
% The above copyright notice and this permission notice shall be included in all
% copies or substantial portions of the Software.
%
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
% IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
% FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
% COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
% IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
% CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
%
% https://github.com/petercorke/spatial-math

% Copyright (C) 1993-2017, 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

classdef SE2 < SO2

    properties (Dependent = true)
        t
    end

    methods

        function obj = SE2(varargin)
            %SE2.SE2 Construct an SE(2) object
            %
            % Constructs an SE(2) pose object that contains a 3x3 homogeneous transformation
            % matrix.
            %
            % T = SE2() is the identity element, a null motion.
            %
            % T = SE2(X, Y) is an object representing pure translation defined by X and Y.
            %
            % T = SE2(XY) is an object representing pure translation defined by XY
            % (2x1). If XY (Nx2) returns an array of SE2 objects, corresponding to
            % the rows of XY.
            %
            % T = SE2(X, Y, THETA) is an object representing translation, X and Y, and
            % rotation, angle THETA.
            %
            % T = SE2(XY, THETA) is an object representing translation, XY (2x1), and
            % rotation, angle THETA.
            %
            % T = SE2(XYT) is an object representing translation, XYT(1) and XYT(2),
            % and rotation angle XYT(3). If XYT (Nx3) returns an array of SE2 objects, corresponding to
            % the rows of XYT.
            %
            % T = SE2(T) is an object representing translation and rotation defined by
            % the SE(2) homogeneous transformation matrix T (3x3).  If T (3x3xN) returns an
            % array (1xN) of SE2 objects, corresponding to the third index of T.
            %
            % T = SE2(R) is an object representing pure rotation defined by the
            % SO(2) rotation matrix R (2x2)
            %
            % T = SE2(R, XY) is an object representing rotation defined by the
            % orthonormal rotation matrix R (2x2) and position given by XY (2x1)
            %
            % T = SE2(T) is a copy of the SE2 object T. If T (Nx1) returns an array of SE2 objects,
            % corresponding to the index of T.
            %
            % Options::
            % 'deg'         Angle is specified in degrees
            %
            % Notes::
            % - Arguments can be symbolic
            % - The form SE2(XY) is ambiguous with SE2(R) if XY has 2 rows, the second form is assumed.
            % - The form SE2(XYT) is ambiguous with SE2(T) if XYT has 3 rows, the second form is assumed.
            % - R and T are checked to be valid SO(2) or SE(2) matrices.

            opt.deg = false;

            [opt,args] = tb_optparse(opt, varargin);

            if opt.deg
                scale = pi/180.0;
            else
                scale = 1;
            end

            % if any of the arguments is symbolic the result will be symbolic
            if any( cellfun(@(x) isa(x, 'sym'), args) )
                obj.data = sym(obj.data);
            end

            obj.data = eye(3,3);

            switch length(args)
                case 0
                    % null motion
                    return
                case 1
                    % 1 argument
                    a = args{1};

                    if isvec(a, 2)
                        % (t)
                        obj.data = [ 1 0 a(1); 0 1 a(2); 0 0 1];

                    elseif isvec(a, 3)
                        % ([x y th])
                        a = a(:);
                        obj.data(1:2,1:2) = rot2(a(3)*scale);
                        obj.t = a(1:2);

                    elseif SO2.isa(a)
                        % (R)
                        obj.data(1:2,1:2) = a;

                    elseif SE2.isa(a)
                        % (T)
                        for i=1:size(a, 3)
                            obj(i).data = a(:,:,i);
                        end
                    elseif isa(a, 'SE2')
                        % (SE2)
                        for i=1:length(a)
                            obj(i).data = a(i).data;
                        end

                    elseif any( numcols(a) == [2 3] )
                        for i=1:numrows(a)
                            obj(i) = SE2(a(i,:));
                        end
                        return
                    else
                        error('SMTB:SE2:badarg', 'unknown arguments');
                    end

                case 2
                    % 2 arguments
                    a = args{1}; b = args{2};
                    if isscalar(a) && isscalar(b)
                        % (x,y)
                        obj.data = [ 1 0 a; 0 1 b; 0 0 1];
                    elseif isvec(a,2) && isscalar(b)
                        % ([x y], th)
                        obj.data = [ rot2(b*scale) a(:); 0 0 1];
                    elseif SO2.isa(a) && isvec(b,2)
                        % (R, t)
                        obj.data = [a b(:); 0 0 1];
                    else
                        error('SMTB:SE3:badarg', 'unknown arguments');
                    end

                case 3
                    % 3 arguments
                    a = args{1}; b = args{2}; c = args{3};
                    if isscalar(a) && isscalar(b) && isscalar(c)
                        % (x, y, th)
                        obj.data = [ rot2(c*scale) [a b]'; 0 0 1];
                    else
                        error('SMTB:SE3:badarg', 'unknown arguments');
                    end
                otherwise
                    error('SMTB:SE3:badarg', 'unknown arguments');

            end

            % add the last row if required
%             if numrows(obj.data) == 2
%                 obj.data = [obj.data; 0 0 1];
%             end
            assert(all(size(obj(1).data) == [3 3]), 'SMTB:SE2:SE2', 'created wrong size data element');
            %% HACK
        end

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %  GET AND SET
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        function t = get.t(obj)
            %SE2.t  Get translational component
            %
            % P.t is a column vector (2x1) representing the translational component of
            % the rigid-body motion described by the SE2 object P.
            %
            % Notes::
            % - If P is a vector the result is a MATLAB comma separated list, in this
            %   case use P.transl().
            %
            % See also SE2.transl.
            t = obj.data(1:2,3);
        end

        function o = set.t(obj, t)
            %SE2.t  Set translational component
            %
            % P.t = TV sets the translational component of the rigid-body motion
            % described by the SE2 object P to TV (2x1).
            %
            % Notes::
            % - TV can be a row or column vector.
            % - If TV contains a symbolic value then the entire matrix becomes
            %   symbolic.

            if isa(t, 'sym') && ~isa(obj.data, 'sym')
                obj.data = sym(obj.data);
            end
            obj.data(1:2,3) = t;
            o = obj;
        end

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        %%%  conversion methods
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        function v = xyt(obj)
            %SE2.xyt  Extract configuration
            %
            % XYT = P.xyt() is a column vector (3x1) comprising the minimum three
            % configuration parameters of this rigid-body motion: translation (x,y)
            % and rotation theta.

            % TODO VECTORISE
            v = obj.t;
            v(3) = atan2(obj.data(2,1), obj.data(1,1));
        end

        function [tx,ty] = transl(obj)
            %SE2.t  Get translational component
            %
            % TV = P.transl() is a row vector (1x2) representing the translational component of
            % the rigid-body motion described by the SE2 object P.  If P is a vector of
            % objects (1xN) then TV (Nx2) will have one row per object element.

            if nargout == 1 || nargout == 0
                tx = [obj.t]';
            else
                t = obj.t;
                tx = t(1);
                ty = t(2);
            end
        end

        function T = T(obj)
            %SE2.T  Get homogeneous transformation matrix
            %
            % T = P.T() is the homogeneous transformation matrix (3x3) associated with the
            % SE2 object P, and has zero translational component.  If P is a vector
            % (1xN) then T (3x3xN) is a stack of homogeneous transformation matrices, with the third
            % dimension corresponding to the index of P.
            %
            % See also SO2.T.
            for i=1:length(obj)
                T(:,:,i) = obj(i).data;
            end
        end

        function t = SE3(obj)
            %SE2.SE3 Lift to 3D
            %
            % Q = P.SE3() is an SE3 object formed by lifting the rigid-body motion
            % described by the SE2 object P from 2D to 3D.  The rotation is about the
            % z-axis, and the translation is within the xy-plane.
            %
            % See also SE3.
            t = SE3();
            t.data(1:2,1:2) = obj.data(1:2,1:2);
            t.data(1:2,4) = obj.data(1:2,3);
        end

        function out = SO2(obj)
            %SE2.SO2  Extract SO(2) rotation
            %
            % Q = SO2(P) is an SO2 object that represents the rotational component of
            % the SE2 rigid-body motion.
            %
            % See also SE2.R.

            out = SO2( obj.R );
        end

        function tw = Twist(obj)
            %SE2.Twist  Convert to Twist object
            %
            % TW = P.Twist() is the equivalent Twist object.  The elements of the twist are the unique
            % elements of the Lie algebra of the SE2 object P.
            %
            % See also SE2.log, Twist.
            tw = Twist( obj.log );
        end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%%  SE(2) OPERATIONS
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

        function it = inv(obj)
            %SE2.inv  Inverse of SE2 object
            %
            % Q = inv(P) is the inverse of the SE2 object P.
            %
            % Notes::
            %  - This is formed explicitly, no matrix inverse required.
            %  - This is a group operator: input and output in the SE(2) group.
            %  - P*Q will be the identity group element (zero motion, identity matrix).

            it = SE2( obj.R', -obj.R'*obj.t);
        end

        function S = log(obj)
            %SE2.log  Lie algebra
            %
            % se2 = P.log() is the Lie algebra corresponding to the SE2 object P. It is
            % an augmented skew-symmetric matrix (3x3).
            %
            % See also SE2.Twist, logm, skewa, vexa.
            S = logm(obj.data);
        end

        function Ti = interp(obj1, varargin)
            %SE2.interp Interpolate between SO2 objects
            %
            % P1.interp(P2, s) is an SE2 object which is an interpolation
            % between poses represented by SE2 objects P1 and P2.  s varies from 0
            % (P1) to 1 (P2). If s is a vector (1xN) then the result will be a vector
            % of SE2 objects.
            %
            % Notes::
            % - It is an error if S is outside the interval 0 to 1.
            %
            % See also SO2.angle.

            if isa(varargin{1}, 'SE2')
                % interp(SE2, SE2, s)  interpolate between given values
                obj2 = varargin{1};
                varargin = varargin(2:end);
                try
                    Ti = SE2( trinterp2(obj1.T, obj2.T, varargin{:}) );
                catch me
                    switch me.identifier
                        case 'SMTB:trinterp2:badarg'
                            throw( MException('SMTB:SE2:interp:badarg', 'value of S outside interval [0,1]') );
                        otherwise
                            rethrow(me);
                    end
                end
            else
                % interp(SE2, s)  interpolate between null and given value
                try
                    Ti = SE2( trinterp2( obj1.T, varargin{:}) );
                catch me
                    switch me.identifier
                        case 'SMTB:trinterp2:badarg'
                            throw( MException('SMTB:SE2:interp:badarg', 'value of S outside interval [0,1]') );
                        otherwise
                            rethrow(me);
                    end
                end
            end

        end

        function print(obj, varargin)
            for T=obj
                theta = atan2(T.data(2,1), T.data(1,1)) * 180/pi;
                fprintf('t = (%.4g, %.4g), theta = %.4g deg\n', T.t, theta);
            end
        end

        function n = new(obj, varargin)
            %SE2.new  Construct a new object of the same type
            %
            % P2 = P.new(X) creates a new object of the same type as P, by invoking the SE2 constructor on the matrix
            % X (3x3).
            %
            % P2 = P.new() as above but defines a null motion.
            %
            % Notes::
            %  - Serves as a dynamic constructor.
            %  - This method is polymorphic across all RTBPose derived classes, and
            %    allows easy creation of a new object of the same class as an existing
            %    one without needing to explicitly determine its type.
            %
            % See also SE3.new, SO3.new, SO2.new.

            n = SE2(varargin{:});
        end

    end

    methods (Static)
        % Static factory methods for constructors from exotic representations

        function obj = exp(s)
            %SE2.exp  Construct SE2 from Lie algebra
            %
            % SE2.exp(SIGMA) is the SE2 rigid-body motion corresponding to the se(2)
            % Lie algebra element SIGMA (3x3).
            %
            % SE3.exp(TW) as above but the Lie algebra is represented
            % as a twist vector TW (1x1).
            %
            % Notes::
            %  - TW is the non-zero elements of X.
            %
            % Reference::
            % - Robotics, Vision & Control: Second Edition, P. Corke, Springer 2016; p25-31.
            %
            %
            % See also trexp2, skewa.

            obj = SE2( trexp2(s) );
        end

        function T = convert(tr)
            %SE2.check  Convert to SE2
            %
            % Q = SE2.convert(X) is an SE2 object equivalent to X where X is either
            % an SE2 object, or an SE(2) homogeneous transformation matrix (3x3).
            if isa(tr, 'SE2')
                T = tr;
            elseif SE2.isa(tr)
                T = SE2(tr);
            else
                error('expecting an SE2 or 3x3 matrix');
            end
        end

        function h = isa(tr, rtest)
            %SE2.ISA Test if matrix is SE(2)
            %
            % SE2.isa(T) is true (1) if the argument T is of dimension 3x3 or 3x3xN, else
            % false (0).
            %
            % SE2.isa(T, true) as above, but also checks the validity of the rotation
            % sub-matrix.
            %
            % Notes::
            %  - This is a class method.
            %  - The first form is a fast, but incomplete, test for a transform in SE(3).
            %  - There is ambiguity in the dimensions of SE2 and SO3 in matrix form.
            %
            % See also SO3.ISA, SE2.ISA, SO2.ISA, ishomog2.

            d = size(tr);
            if ndims(tr) >= 2
                h =  all(d(1:2) == [3 3]);

                if h && nargin > 1 && ~isa(tr, 'sym')
                    h = SO3.isa( tr(1:2,1:2) );
                    h = h && all(tr(4,:) == [0 0 0 1]);  % test the bottom row
                end
            else
                h = false;
            end
        end

        function T = rand()
            %SE2.rand Construct a random SE(2) object
            %
            % SE2.rand() is an SE2 object with a uniform random translation and a
            % uniform random orientation.  Random numbers are in the interval [-1 1]
            % and rotations in the interval [-pi pi].
            %
            % See also RAND.
            T = SE2(rand(1,3)*diag([2 2 2*pi]) + [-1 -1 -pi]);
        end
    end
end


function [tp, fp, tn, fn, rawcounts] = segmentationMetrics(prob, gt, threshPR, threshIU)
	% prob in HxWxnumClass

	numClass = size(prob, 3);

	% Calculate category wise precision and recall
	if(length(threshPR) == 0)
		tp = zeros(0, numClass);
		fp = tp; tn = tp; fn = tp;
	else
		for i = 1:numClass,
			gtI = double(gt == i);
			gtI(gt == 0) = NaN;
			probI = prob(:,:,i);
			[tp(:,i), fp(:,i), tn(:,i), fn(:,i)] = getPR(gtI(:), probI(:), threshPR);
		end
	end

	rawcounts = getRawCounts(gt, prob, threshIU);
end

function rawcounts = getRawCounts(gt, prob, thresh)
	% Calculate Intersection over Union for different thresholds of projection...
	num = size(prob, 3) + 1;
	rawcounts = zeros([num, num, length(thresh)]);
	[conf, ind] = max(prob, [], 3);
	locs = gt >= 0;
	for i = 1:length(thresh),
		resim = ind .* (conf >= thresh(i));
		sumim = 1+gt+resim*num;
		hs = histc(sumim(locs), 1:num*num);
    	rawcounts(:,:,i) = rawcounts(:,:,i) + reshape(hs(:), size(rawcounts(:,:,i)));
	end
end

function a = reverse(a)
	a = a(end:-1:1);
end

function [tp fp tn fn] = getPR(gt, out, thresh)
	assert(issorted(thresh));
	out = out+1;
	thresh = thresh+1;
	tp = histc(out(:).*gt(:), thresh);
	tp = reverse(cumsum(reverse(tp)));
	fp = histc(out(:).*(1-gt(:)), thresh);
	fp = reverse(cumsum(reverse(fp)));
	fn = nnz(gt(:) == 1) - tp;
	tn = nnz(gt(:) == 0) - fp;
end


function demoMultipleModelFiltering()
%%DEMOMULTIPLEMODELFILTERING This demonstrates how multiple model filters
%           can be used. The use of multiple dynamic models can improve the
%           accuracy of tracking targets than can maneuver. The scenario
%           considered is the simple 2D tracking of an aircraft. The
%           separate problems of data association, scheduling, track
%           initiation and termination are not addressed here.
%
%The example scenario is the air traffic control (ATC) scenario discussed
%in Chapter 11.7.4 of [1]. However, rather than using a fixed mode
%transition probability matrix, the use of the function
%getMarkovPTransProbMat to get the mode transition probability matrix given
%mean sojorn times and the time between samples is demonstrated. This
%allows the algorithms to be used with variable sampling rates, though in
%this instance, the sampling rate is held constant.
%
%Six scenarios are considered. One is a baseline Kalman filter with a
%polynomial dynamic model and a large process noise. The second is a Kalman
%filter with a first-order Gauss-Markov model (the integrated Ornstein-
%Uhlenbeck model), which, unlike the standard polynomial model, includes a
%correlation constant to try to better cover turns. The third is the
%reduced state filter. The fourth is the separated covariance filter. The
%other two scenarios utilize the interacting multiple model (IMM) filter.
%The first IMM scenario is as in [1], where two nearly constant velocity
%dynamic models with differing process noises are used in a standard Kalman
%filter (with the IMM). The second IMM scenario is where a nearly constant
%velocity model in a Kalman filter is used as well as a coordinated turn
%model in an extended Kalman filter (EKF). The functions multipleModelPred
%and multipleModelUpdate are used to handle the prediction and mixing in
%the IMM, though a change of the algorithm selection would allow one to
%just as easily use a generalized pseudo-Bayesian 2 filter. The IMM
%estimators can be more difficult to use as there are more parameters to
%tune.
%
%The measurements are generated in Cartesian space as measurement
%conversion and tracking using non-Cartesian measurements is not the focus
%of this example.
%
%REFERENCES:
%[1] Y. Bar-Shalom, X. R. Li, and T. Kiruabarajan, Estimation with
%    Applications to Tracking and Navigation. New York: Wiley Interscience,
%    2001.
%
%June 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

%This can be changed to try other multiple model algorithms. If GPB1 is
%selected, the multiple model filter using the coordinate dturn model is
%not used, because the GPB1 requires that all models have the same
%dimensionality.
AlgSel='GPB2';

%Interval between samples in seconds.
T=5;

%%STEP 1: Generate the simulation scenario.
disp('Generating the "true" track for the simulation scenario.')
TTotal=125+90+125+30+125;
%The extra sample is the very first step in the trajectory.
numSamples=TTotal/T+1;
xTrue=zeros(4,numSamples);

%An indicator of the maneuvering mode 1 for not maneuvering, and 2 for
%maneuvering.
mode=zeros(numSamples,1);

%The plane first flies westward at 120 m/s for 125 seconds.
xInit=[25e3;10e3; -120; 0];
xTrue(:,1)=xInit;
mode(1)=1;
numStepSeg=125/T;%The number of steps in this segment.

%The state transition matrix for constant velocity motion.
F=FPolyKal(T,4,1);

%Propagate the constant velocity model for 125 seconds.
baseStep=1;
for curStep=(baseStep+1):(baseStep+numStepSeg)
    xTrue(:,curStep)=F*xTrue(:,curStep-1);
    mode(curStep)=1;
end
baseStep=baseStep+numStepSeg;

%Next, the plane performs a 1 degree per second coordinated turn for 90
%seconds.
turnRate=1*(pi/180);%Convert from degrees per second to radians per second.

numStepSeg=90/T;
for curStep=(baseStep+1):(baseStep+numStepSeg)
    %Get the discrete-time propagation matrix for the state. The
    %FCoordTurn2D function assumes that the turn rate is part of the state.
    %In this instance, we just add it to the state, and then remove the
    %extra elements from the returned matrix.
    F=FCoordTurn2D(T,[xTrue(:,curStep-1);turnRate]);
    F=F(1:4,1:4);

    xTrue(:,curStep)=F*xTrue(:,curStep-1);
    mode(curStep)=2;
end
baseStep=baseStep+numStepSeg;

%Next, go straight (South after the previous turn) for another 125 seconds.
numStepSeg=125/T;%The number of steps in this segment.

%The state transition matrix for constant velocity motion.
F=FPolyKal(T,4,1);

for curStep=(baseStep+1):(baseStep+numStepSeg)
    xTrue(:,curStep)=F*xTrue(:,curStep-1);
    mode(curStep)=1;
end
baseStep=baseStep+numStepSeg;

%Make another turn. This time, it is at a rate of -three degrees per second
%for 30 seconds.
turnRate=-3*(pi/180);%Degrees per second to radians per second.

numStepSeg=30/T;%The number of steps in this segment.
for curStep=(baseStep+1):(baseStep+numStepSeg)
    F=FCoordTurn2D(T,[xTrue(:,curStep-1);turnRate]);
    F=F(1:4,1:4);

    xTrue(:,curStep)=F*xTrue(:,curStep-1);
    mode(curStep)=2;
end
baseStep=baseStep+numStepSeg;

%Finally, go straight again for another 125 seconds.
numStepSeg=125/T;%The number of steps in this segment.

%The state transition matrix for constant velocity motion.
F=FPolyKal(T,4,1);

for curStep=(baseStep+1):(baseStep+numStepSeg)
    xTrue(:,curStep)=F*xTrue(:,curStep-1);
    mode(curStep)=1;
end

%%STEP 2: Generate measurements.
disp('Generating the measurements for the simulation; A simple Cartesian model is used.')
%The measurements are just Cartesian with 100 meters of noise added per
%dimension, with no correlation between them.
H=[1,0,0,0;
   0,1,0,0];%Measurement matrix

SR=[100, 0;
    0,  100];%Square root covariance matrix.
R=SR*SR';%The covariance matrix.

z=zeros(2,numSamples);
for curSamp=1:numSamples
    z(:,curSamp)=H*xTrue(:,curSamp)+SR*randn(2,1);
end

%Plot the trajectory and the measurements
disp('Plotting the trajectory the measurements, and the true maneuver mode.')
figure(1)
clf
hold on
plot(xTrue(1,:),xTrue(2,:),'-b','linewidth',2)
scatter(z(1,:),z(2,:),'.k')

h1=xlabel('Meters West->East');
h2=ylabel('Meters South->North');
h3=title('The True Trajectory and the Detections');
set(gca,'FontSize',14,'FontWeight','bold','FontName','Times')
set(h1,'FontSize',14,'FontWeight','bold','FontName','Times')
set(h2,'FontSize',14,'FontWeight','bold','FontName','Times')
set(h3,'FontSize',14,'FontWeight','bold','FontName','Times')

%Plot when it is in Mode 2, 1 for being in it and 2 for not.
figure(2)
clf
hold on
plot(mode-1,'-k')
h1=xlabel('Discrete Step');
h2=ylabel('Maneuver mode');
h3=title('Indicating when the target is performing maneuvers.');
set(gca,'FontSize',14,'FontWeight','bold','FontName','Times')
set(h1,'FontSize',14,'FontWeight','bold','FontName','Times')
set(h2,'FontSize',14,'FontWeight','bold','FontName','Times')
set(h3,'FontSize',14,'FontWeight','bold','FontName','Times')

%STEP 3: Get the baseline scenario: Run a Kalman filter with a standard
%first-order white-noise process model, a Kalman filter with a first-order
%Gauss-Markov model, a reduced state estimator, and the
%separated covariance filter.
disp('Computing the baseline scenarios with just a Kalman filter and with the')
disp('reduced state estimator.')

%A direct-discrete model with a 1m/2s^2 process noise standard deviation is
%used (normally, a discretized model is preferred, because it is
%more consistent when using a variable sampling rate, and the covariance
%matrix is not singular).
Q=QPolyKalDirectDisc(T,4,1,1^2);%1m/s^2 process noise

%The state transition matrix for constant velocity motion.
F=FPolyKal(T,4,1);

%Parameters for the Gauss-Markov dynamic model. We are using a first-order
%Gauss-Markov model.
tau=20;%20 seconds; the assumed maneuver decorrelation time.
maxAccel=9.8*3;%Assume max 3G turn
%Rule of thumb- process noise suggestion.
q=processNoiseSuggest('PolyKal-ROT',maxAccel,T);
QGM=QGaussMarkov(T,4,q,tau,1);%Process noise matrix.
FGM=FGaussMarkov(T,4,tau,1);%State Transition matrix

%Allocate space for the polynomial Kalman filter estimates
xKalman=zeros(4,numSamples);%The states
PKalman=zeros(4,4,numSamples);%The covariance matrices

%Allocate space for the Gauss-Markov Kalman filter estimates
xKalmanGM=zeros(4,numSamples);%The states
PKalmanGM=zeros(4,4,numSamples);%The covariance matrices

%Allocate space for the reduced state estimator.
xRedState=zeros(4,numSamples);%The states

%Allocate space for the separated covariance filter.
xSCFState=zeros(4,numSamples);%The states

%We are using the version of the reduced state estimator that needs a
%maximum linear acceleration (directed along the direction of motion of the
%target) for the target along with a maximum turn rate acceleration.
%A 0.5m/s^2 maximum linear acceleration with 1 deg/s^2 maximum turn rate
%acceleration is used.
ARedState=0.5;
OmegaRedState=1*(pi/180);

%The separated covariance filter needs a matrix introducing the effects of
%the maximum acceleration onto the state (for covariance computation
%results. For a state consisting of position and velocity in 2D, this can
%be
aMax=9.8/2;
Ba=[T^2/2,   0;
    0,       T^2/2;
	T,       0;
	0,       T]*aMax;
%where we chose the maximum acceleration in each dimension to be 0.5G
%(1G=9.8m/s^2).

%Track initiation is by one-point differencing using just assumed maximum
%values as the standard deviations for the velocity covariance matrix. The
%assumed maximum value is 300m/s.
[xKalman(:,1),PKalman(:,:,1)]=onePointCartInit(z(:,1),SR,300);
xKalmanGM(:,1)=xKalman(:,1);
PKalmanGM(:,1)=PKalman(:,1);

%The reduced state estimator keeps the covariance matrix broken into parts
%M and D, which will be propagated and updated.
xRedState(1:2)=z(:,1);
M=PKalman(:,:,1);
D=zeros(4,2);

%The separated covariance filter. It has been chosen to start the filter
%with zero lag and all uncertainty in the covariance matrix.
xSCFState(1:2)=z(:,1);
PSCF=PKalman(:,:,1);
LSCF=zeros(4,2);

absErrKalman=zeros(numSamples,1);%Allocate space
absErrKalman(1)=norm(xKalman(1:2,1)-xTrue(1:2,1));

absErrKalmanGM=zeros(numSamples,1);%Allocate space
absErrKalmanGM(1)=norm(xKalman(1:2,1)-xTrue(1:2,1));

absErrRedState=zeros(numSamples,1);%Allocate space
absErrRedState(1)=norm(xRedState(1:2,1)-xTrue(1:2,1));

absErrSCF=zeros(numSamples,1);%Allocate space
absErrSCF(1)=norm(xSCFState(1:2,1)-xTrue(1:2,1));

%Run the filter
for curSamp=2:numSamples
    %The Kalman filter with the polynomial model
    %Predict the state forward
    [xPred, PPred]=discKalPred(xKalman(:,curSamp-1),PKalman(:,:,curSamp-1),F,Q);
    %Update the state with a measurement
    [xKalman(:,curSamp),PKalman(:,:,curSamp)]=KalmanUpdate(xPred,PPred,z(:,curSamp),R,H);

    absErrKalman(curSamp)=norm(xKalman(1:2,curSamp)-xTrue(1:2,curSamp));

    %The Kalman filter with the Gauss-Markov model
    [xPred, PPred]=discKalPred(xKalmanGM(:,curSamp-1),PKalmanGM(:,:,curSamp-1),FGM,QGM);
    %Update the state with a measurement
    [xKalmanGM(:,curSamp),PKalmanGM(:,:,curSamp)]=KalmanUpdate(xPred,PPred,z(:,curSamp),R,H);

    absErrKalmanGM(curSamp)=norm(xKalmanGM(1:2,curSamp)-xTrue(1:2,curSamp));

    %The reduced state estimator.
    %Predict the state forward
    modParams=[];
    modParams.T=T;
    modParams.A=ARedState;
    modParams.Omega=OmegaRedState;
    [xPred,M,D,PPred]=reducedStateDiscPred(xRedState(:,curSamp-1),M,D,[],[],'GenTurn',modParams);
    %Update the state with a measurement
    [xRedState(:,curSamp),M,D]=reducedStateUpdate(xPred,PPred,M,D,z(:,curSamp),R);

    absErrRedState(curSamp)=norm(xRedState(1:2,curSamp)-xTrue(1:2,curSamp));

    %The separated covariance filter.
    %Predict the state forward
    [xPred,LSCF,TSCF]=separatedCovDiscPred(xSCFState(:,curSamp-1),PSCF,LSCF,F,Ba);
    %Update the state with a measurement
    [xSCFState(:,curSamp),LSCF,PSCF]=separatedCovUpdate(xPred,LSCF,TSCF,z(:,curSamp),R,H);
    absErrSCF(curSamp)=norm(xSCFState(1:2,curSamp)-xTrue(1:2,curSamp));
end

figure(1)
hold on
plot(xKalman(1,:),xKalman(2,:),'-g','linewidth',2)
plot(xKalmanGM(1,:),xKalmanGM(2,:),'-c','linewidth',2)
plot(xRedState(1,:),xRedState(2,:),'--b','linewidth',2)
plot(xSCFState(1,:),xSCFState(2,:),'-.m','linewidth',2)

figure(3)
clf
hold on
plot(absErrKalman,'-g','linewidth',2)
plot(absErrKalmanGM,'-c','linewidth',2)
plot(absErrRedState,'--b','linewidth',2)
plot(absErrSCF,'-.m','linewidth',2)

h1=xlabel('Discrete Step');
h2=ylabel('Absolute distance error');
set(gca,'FontSize',14,'FontWeight','bold','FontName','Times')
set(h1,'FontSize',14,'FontWeight','bold','FontName','Times')
set(h2,'FontSize',14,'FontWeight','bold','FontName','Times')
set(h3,'FontSize',14,'FontWeight','bold','FontName','Times')

%%STEP 4: Run the algorithms with the two linear models.
%The linear model pair consists of trackers using direct discrete linear
%dynamic models.
disp('Computing the IMM scenario with two linear Kalman filters with different process noises.')

%The covariance matrix for the low-noise model.
QLow=QPolyKalDirectDisc(T,4,1,0.1^2);%0.1m/s^2 process noise
%The covariance matrix for the high-noice model.
QHigh=QPolyKalDirectDisc(T,4,1,2^2);%2m/s^2 process noise.

%The propagation routines for each model
transFuns{1}=@(xPrev,PPrev)discKalPred(xPrev,PPrev,F,QLow);
transFuns{2}=@(xPrev,PPrev)discKalPred(xPrev,PPrev,F,QHigh);

%The measurement update function is the same for both models.
measUpdateFuns=@(x,P,z,R)KalmanUpdate(x,P,z,R,H);

%Allocate space for the IMM model states. This is the size of the
%largest state by the number of models, except when using the GPB1
%estimator, in which case it is just the size of the state (all states have
%to be the same size).
if(~strcmp(AlgSel,'GPB1'))
    xIMML=zeros(4,2);
    PIMML=zeros(4,4,2);

    %Initialize the states using one-point differencing. The covariance matrix
    %is set using the assumed velocity for each of the models, which is
    %300m/s^2.
    %The first model.
    [xIMML(:,1),PIMML(:,:,1)]=onePointCartInit(z(:,1),SR,300);
    %The second model has the same initialization.
    xIMML(:,2)=xIMML(:,1);
    PIMML(:,:,2)=PIMML(:,:,1);
else
    [xIMML,PIMML]=onePointCartInit(z(:,1),SR,300);
end

%Allocate space for the states to display. These will be the merged values
%from the IMM model states.
xIMMLDisp=zeros(4,numSamples);
PIMMLDisp=zeros(4,4,numSamples);

%Initially, both models have the same initialization, which will thus be
%the one to display at the first step.
xIMMLDisp(:,1)=xIMML(:,1);
PIMMLDisp(:,:,1)=PIMML(:,:,1);

%Allocate space for the mode probabilities. These are saved over all scans
%so that they can be plotted.
muMode=zeros(2,numSamples);%There is one for each model at each step.
%Initially, the mode probabilities are uniform: we do not know which mode
%it is in.
muMode(:,1)=1/2;

%The mode transition probability matrix for the two linear models must be
%determined. This is based on a matrix relating the mean sojourn times of
%the models and the time between samples.
%The mean sojourn time matrix is a design parameter. The negative inverse
%of the diagonal terms is the mean time spent in that state. Each row must
%sum to one. If the matrix were larger, the relation between the
%non-diagonal values in the rows would affect to which state it
%transitions. This is a design parameter.
A=[-0.01/2, 0.01/2;
    0.02*2, -0.02*2];

%Compute the mode transition probability matrix for the given time between
%samples. Because the sample interval is constant, this does not change
%over time.
LambdaL=getMarkovPTransProbMat(A,T);

absErrIMML=zeros(numSamples,1);%Allocate space
absErrIMML(1)=norm(xIMMLDisp(1:2,1)-xTrue(1:2,1));
%Run the filter
for curSamp=2:numSamples
   %Predict the states forward
   [xIMML,PIMML]=multipleModelPred(AlgSel,xIMML,PIMML,transFuns);

   %Update the state with a measurement
   [xIMML,PIMML,muMode(:,curSamp),xIMMLDisp(:,curSamp),PIMMLDisp(:,:,curSamp)]=multipleModelUpdate(AlgSel,xIMML,PIMML,z(:,curSamp),R,measUpdateFuns,muMode(:,curSamp-1),LambdaL);
   absErrIMML(curSamp)=norm(xIMMLDisp(1:2,curSamp)-xTrue(1:2,curSamp));
end

figure(1)
plot(xIMMLDisp(1,:),xIMMLDisp(2,:),'-r','linewidth',2)

figure(2)
plot(muMode(2,:),'-r','linewidth',2)

figure(3)
plot(absErrIMML,'-r','linewidth',2)

if(strcmp(AlgSel,'GPB1'))
    return
end

%STEP 5: Run the algorithms with the linear and the coordinated turn model.
disp('Computing the IMM scenario with a linear Kalman filter and an EKF.')

%The covariance matrix for the linear model.
QLinear=QPolyKalDirectDisc(T,4,1,0.01^2);%0.01m/s^2 process noise.

%0.025m/s^2 linear acceleration (in 3D, not just directed along the
%direction of motion of the target) with 0.5 deg/s^2 turning acceleration
%for the coordinated turn model as the standard deviations going into the
%covariance matrix. Note that these assumptions differ from those used in
%the reduced state filter. The filters should be tuned separately for a
%fair comparison. Often, improving the IMM's ability to recognize a turn
%worsens its overall track performance. Thus, a tradeoff must be present.
QCT=QCoordTurn(T,zeros(5,1),0.025^2,(0.5*(pi/180))^2);

%The propagation routines for each model
transFuns=[];
transFuns{1}=@(xPrev,PPrev)discKalPred(xPrev,PPrev,F,QLinear);
f=@(x)(FCoordTurn2D(T,x)*x);
transFuns{2}=@(xPrev,PPrev)discEKFPred(xPrev,PPrev,f,@(x)JacobCoordTurn2D(T,x),QCT);

%The measurement update function differs for the different models, because
%H is different for the coordinated turn model, due to the extra element
%in the state.
HCT=[1,0,0,0,0;
     0,1,0,0,0];
measUpdateFuns=[];
measUpdateFuns{1}=@(x,P,z,R)KalmanUpdate(x,P,z,R,H);
measUpdateFuns{2}=@(x,P,z,R)KalmanUpdate(x,P,z,R,HCT);

%The number of elements in each state and the number of dimensions of each
%state that play a role in mixing the states.
numStateDims=[4;5];
numMixDims=[4;4];

%Allocate space for the IMM model states. The number of rows is the size of
%the longest state.
xLCT=zeros(5,2);
PLCT=zeros(5,2);

%Initialize the states using one-point differencing. The covariance matrix
%is set using the assumed velocity for each of the models, which is
%300m/s^2 and the maximum turn rate, which is assumed to be 20 degrees per
%second.
%The first model.
xLCT(1:2,1)=z(:,1);
PLCT(1:2,1:2,1)=R;
PLCT(3,3,1)=300^2;
PLCT(4,4,1)=300^2;
%The second model has the same initialization.
xLCT(:,2)=xLCT(:,1);
PLCT(:,:,2)=PLCT(:,:,1);
PLCT(5,5,2)=(20*(pi/180))^2;

%Allocate space for the states to display. These will be the merged values
%from the IMM model states. The size is the size of the state with the
%least number of mixing components.
xLCTDisp=zeros(4,numSamples);
PLCTDisp=zeros(4,4,numSamples);

%Initially, both models have the same initialization, which will thus be
%the one to display at the first step.
xLCTDisp(:,1)=xLCT(1:4,1);
PLCTDisp(:,:,1)=PLCT(1:4,1:4,1);

%Allocate space for the mode probabilities. These are saved over all scans
%so that they can be plotted.
muModeLCT=zeros(2,numSamples);%There is one for each model at each step.
%Initially, the mode probabilities are uniform: we do not know which mode
%it is in.
muModeLCT(:,1)=1/2;

%Compute the mode transition probability matrix.
A=[-0.01/2, 0.01/2;
    0.02*2, -0.02*2];
LambdaCT=getMarkovPTransProbMat(A,T);

absErrLCT=zeros(numSamples,1);%Allocate space
absErrLCT(1)=norm(xLCTDisp(1:2,1)-xTrue(1:2,1));
%Run the filter
for curSamp=2:numSamples
   %Predict the states forward.
   [xLCT,PLCT]=multipleModelPred(AlgSel,xLCT,PLCT,transFuns,numStateDims,numMixDims);

   %Update the state with a measurement.
   [xLCT,PLCT,muModeLCT(:,curSamp),xLCTDisp(:,curSamp),PLCTDisp(:,:,curSamp)]=multipleModelUpdate(AlgSel,xLCT,PLCT,z(:,curSamp),R,measUpdateFuns,muModeLCT(:,curSamp-1),LambdaCT,numStateDims,numMixDims);
   absErrLCT(curSamp)=norm(xLCTDisp(1:2,curSamp)-xTrue(1:2,curSamp));
end

figure(1)
plot(xLCTDisp(1,:),xLCTDisp(2,:),'--c','linewidth',2)
legend('True Trajectory','Raw Detections','Basic Kalman Filter', 'Gauss-Markov Kalman Filter','Reduced State Filter', 'Separated Covariance Filter', 'Two Linear Models','Linear and Maneuvering Models','Location','SouthEast')

figure(2)
plot(muModeLCT(2,:),'--c','linewidth',2)
legend('True Mode', 'Two Linear Models','Linear and Maneuvering Models','Location','NorthWest')

figure(3)
plot(absErrLCT,'--c','linewidth',2)
legend('Polynomial Kalman Filter','Gauss-Markov Kalman Filter','Reduced State Filter', 'Separated Covariance Filter', 'Two Linear Models','Linear and Maneuvering Models','Location','NorthWest')
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 [flatArray, index] = flattenCellMatrix(nestedArray)
% flatten nested (TRAFO) cell array
%
% (c) Thomas Kuestner
% ---------------------------------------------------------------------

    if(nnz(size(nestedArray) ~= 1) > 1 || any(any(any(any(cellfun(@iscell,nestedArray))))))
        [flatArray,maxdim] = flattenCell(nestedArray);

        index = flatArray(2,:);
        index = cellfun(@(x) x(:,1:maxdim), index, 'UniformOutput', false);
        flatArray = flatArray(1,:);
    else
        index = [];
        flatArray = nestedArray(:).';
    end

end

function [flatArray,maxdim] = flattenCell(nestedArray,cnt)

% maximal 4D cell array as input

narginchk(1,2);
if(~exist('cnt','var'))
    cnt = 1;
end
if ~iscell(nestedArray),
    error('Must be a cell array.');
end
flatArray{2,1} = [];
for i=1:numel(nestedArray)
    if iscell(nestedArray{i})
        [y, maxdim] = flattenCell(nestedArray{i},cnt+1);
        [flatArray{:,end+1:end+size(y,2)}] = deal(y{:});
        for j=size(flatArray,2):-1:size(flatArray,2)-size(y,2)+1
            [row, col, slice, page] = ind2sub(size(nestedArray),i);
            flatArray{2,j}(cnt,:) = [row, col, slice, page];
            maxdim = max([maxdim, ndims(nestedArray)]);
        end
    else
        flatArray{1,end+1} = nestedArray{i};
        flatArray{2,end} = zeros(cnt,4);
        maxdim = ndims(nestedArray);
        [row, col, slice, page] = ind2sub(size(nestedArray),i);
        flatArray{2,end}(cnt,:) = [row, col, slice, page];
    end
end
flatArray(:,1) = [];

end


function [varargout] = likUni(hyp, y, mu, s2, inf, i)

% likUni - Uniform likelihood function for classification. The expression for
% the likelihood is
%   likUni(t) = 1/2.
%
% There are no hyperparameters:
%
% hyp = [ ]
%
% Several modes are provided, for computing likelihoods, derivatives and moments
% respectively, see likFunctions.m for the details. In general, care is taken
% to avoid numerical issues when the arguments are extreme.
%
% Copyright (c) by Hannes Nickisch, 2013-09-02.
%
% See also LIKFUNCTIONS.M.

if nargin<3, varargout = {'0'}; return; end   % report number of hyperparameters

if nargin<5                              % prediction mode if inf is not present
  lp = -log(2)*ones(size(mu));
  ymu = {}; ys2 = {};
  if nargout>1
    p = exp(lp);
    ymu = 2*p-1;                                                % first y moment
    if nargout>2
      ys2 = 4*p.*(1-p);                                        % second y moment
    end
  end
  varargout = {lp,ymu,ys2};
else
  switch inf
  case 'infLaplace'
    if nargin<6                                             % no derivative mode
      lp = -log(2)*ones(size(mu)); dlp = {}; d2lp = {}; d3lp = {};
      if nargout>1
        dlp = zeros(size(mu));               % dlp, derivative of log likelihood
        if nargout>2                    % d2lp, 2nd derivative of log likelihood
          d2lp = zeros(size(mu));
          if nargout>3                  % d3lp, 3rd derivative of log likelihood
            d3lp = zeros(size(mu));
          end
        end
      end
      varargout = {lp,dlp,d2lp,d3lp};
    else                                                       % derivative mode
      varargout = {[],[],[]};                         % derivative w.r.t. hypers
    end

  case 'infEP'
    if nargin<6                                             % no derivative mode
      lZ = -log(2)*ones(size(mu));                           % log part function
      dlZ = {}; d2lZ = {};
      if nargout>1
        dlZ  = zeros(size(mu));                     % 1st derivative w.r.t. mean
        if nargout>2
          d2lZ = zeros(size(mu));                   % 2nd derivative w.r.t. mean
        end
      end
      varargout = {lZ,dlZ,d2lZ};
    else                                                       % derivative mode
      varargout = {[]};                                     % deriv. wrt hyp.lik
    end

  case 'infVB'
    % variational lower site bound
    % t(s) = 1/2
    % the bound has the form: (b+z/ga)*f - f.^2/(2*ga) - h(ga)/2
    n = numel(s2); b = zeros(n,1); y = y.*ones(n,1); z = y;
    varargout = {b,z};
  end
end


 function dot = ir_dot_double(a, b)
%function dot = ir_dot_double(a, b)
%| compute dot product between vectors a and b in double precision
%| dot = sum(a(:) .* b(:), 'double'); % double accumulate
%| user must apply conj() to a or b before calling for complex case

if nargin < 2, ir_usage, end

dot = sum(a(:) .* b(:), 'double'); % double accumulate


function x=usasi(n,fs)
%USASI generates N samples of USASI noise at sample frequency FS X=(N,FS)

% This routine is based on the USASI noise defined in [1] which was later
% reissued as [2]. USASI noise is intended to simulate the long-term average
% of typical audio program material. The routine does not currently implement
% the pulsation at 2.5Hz 12.5% duty cycle that is recommended by the standard.
% Also it should probably be scaled to a well-defined power.
%
%  [1] NRSC AM Reemphasis, Deemphasize, and Broadcast Audio Transmission Bandwidth Specifications,
%      EIA-549 Standard, Electronics Industries Association , July 1988.
%  [2] NRSC AM Reemphasis, Deemphasize, and Broadcast Audio Transmission Bandwidth Specifications,
%      NRSC-1-A Standard, Sept 2007, Online: http://www.nrscstandards.org/SG/NRSC-1-A.pdf

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

if nargin<2 fs=8000; end
b=[1 0 -1];
a=poly(exp(-[100 320]*2*pi/fs));

x=randfilt(b,a,n);


function [subIndOut,L_valid]=snapSubInd(subIndIn,siz)

%Initialize outputs
L_valid=false(size(subIndIn));
subIndOut=subIndIn;

%Fixing out of range indices
for q=1:1:size(subIndIn,2)
    subInd=subIndIn(:,q); %The current subscript index set
    L_valid(:,q)=(subInd>0) & (subInd<=siz(q)); %Logic for valid indices not out of range

    subIndToFix=subInd(~L_valid(:,q)); %Indices to fix

    %Snapping out of range indices to boundary
    subIndToFix=(subIndToFix.*(subIndToFix>1))+(subIndToFix<1); %Fix smaller than 1
    subIndToFix=(subIndToFix.*(subIndToFix<=siz(q)))+siz(q).*(subIndToFix>siz(q)); %Fix larger than siz(q)

    %Storing fixed indices in output
    subIndOut_current=subInd;
    subIndOut_current(~L_valid(:,q))=subIndToFix;
    subIndOut(:,q)=subIndOut_current;
end

%%
% _*GIBBON footer text*_
%
% License: <https://github.com/gibbonCode/GIBBON/blob/master/LICENSE>
%
% GIBBON: The Geometry and Image-based Bioengineering add-On. A toolbox for
% image segmentation, image-based modeling, meshing, and finite element
% analysis.
%
% Copyright (C) 2006-2022 Kevin Mattheus Moerman and the GIBBON contributors
%
% This program is free software: you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.


function show_curve(b)
% function show_curve(b)

if ~strcmpi(class(b),'bezier')
  error('Expecting a bezier object');
end

t=0:0.01:1;
t2=t.^2;
t3=t2.*t;
x=b.ax*t3+b.bx*t2+b.cx*t+b.x0;
y=b.ay*t3+b.by*t2+b.cy*t+b.y0;

plot(x,y,'b',[b.x0],[b.y0],'r*',...
     [b.x1],[b.y1],'ro',...
     [b.x0,b.x1],[b.y0,b.y1],'r',...
     [b.x2],[b.y2],'ro',...
     [b.x3],[b.y3],'r*',...
     [b.x2,b.x3],[b.y2,b.y3],'r');

text([b.x0],[b.y0],'(x_0,y_0)');
text([b.x1],[b.y1],'(x_1,y_1)');
text([b.x2],[b.y2],'(x_2,y_2)');
text([b.x3],[b.y3],'(x_3,y_3)');


function PlotFieldonMesh(coordinates,nodes,component)
%--------------------------------------------------------------------------
% Code written by : Siva Srinivas Kolukula                                |
%                   Senior Research Fellow                                |
%                   Structural Mechanics Laboratory                       |
%                   Indira Gandhi Center for Atomic Research              |
%                   India                                                 |
% E-mail : allwayzitzme@gmail.com                                         |
%          http://sites.google.com/site/kolukulasivasrinivas/             |
%--------------------------------------------------------------------------
%--------------------------------------------------------------------------
% Purpose:
%         To plot the profile of a component on mesh
% Synopsis :
%           ProfileonMesh(coordinates,nodes,component)
% Variable Description:
%           coordinates - The nodal coordinates of the mesh
%           -----> coordinates = [X Y ]
%           nodes - The nodal connectivity of the elements
%           -----> nodes = [node1 node2......]
%           component - The components whose profile to be plotted
%           -----> components = a column vector in the order of node
%                               numbers
%--------------------------------------------------------------------------

nel = length(nodes) ;                  % number of elements
nnode = length(coordinates) ;          % total number of nodes in system
nnel = size(nodes,2);                % number of nodes per element
%
% Initialization of the required matrices
X = zeros(nnel,nel) ;
Y = zeros(nnel,nel) ;
Z = zeros(nnel,nel) ;
profile = zeros(nnel,nel) ;
%
for iel=1:nel
     for i=1:nnel
     nd(i)=nodes(iel,i);         % extract connected node for (iel)-th element
     X(i,iel)=coordinates(nd(i),1);    % extract x value of the node
     Y(i,iel)=coordinates(nd(i),2);    % extract y value of the node
     end
     profile(:,iel) = component(nd') ;         % extract component value of the node
end

% Plotting the FEM mesh and profile of the given component
     fh = figure ;
     set(fh,'name','Postprocessing','numbertitle','off') ;
     fill(X,Y,profile)
     axis off ;
     % Colorbar Setting
     SetColorbar
 end


function n = norm(T)
%NORM Frobenius norm of a tensor.
%
%   NORM(X) returns the Frobenius norm of a tensor.
%
%   See also TENSOR.
%
%MATLAB Tensor Toolbox.
%Copyright 2012, Sandia Corporation.

% This is the MATLAB Tensor Toolbox by T. Kolda, B. Bader, and others.
% http://www.sandia.gov/~tgkolda/TensorToolbox.
% Copyright (2012) Sandia Corporation. Under the terms of Contract
% DE-AC04-94AL85000, there is a non-exclusive license for use of this
% work by or on behalf of the U.S. Government. Export of this data may
% require a license from the United States Government.
% The full license terms can be found in the file LICENSE.txt

v = reshape(T.data, numel(T.data), 1);
n = norm(v);


% DEMROBOTWIRELESSFGPLVM1 Wireless Robot data from University of Washington, without dynamics and without back constraints.

% FGPLVM

% Fix seeds
randn('seed', 1e5);
rand('seed', 1e5);

dataSetName = 'robotWireless';
experimentNo = 1;

% load data
[Y, lbls] = lvmLoadData(dataSetName);

% Set up model
options = fgplvmOptions('ftc');
latentDim = 2;
d = size(Y, 2);

model = fgplvmCreate(latentDim, d, Y, options);

% Optimise the model.
iters = 1000;
display = 1;

model = fgplvmOptimise(model, display, iters);

% Save the results.
modelWriteResult(model, dataSetName, experimentNo);

if exist('printDiagram') & printDiagram
  lvmPrintPlot(model, lbls, dataSetName, experimentNo);
end

% Load the results and display dynamically.
lvmResultsDynamic(model.type, dataSetName, experimentNo, 'vector')


function biharmF = biharmonic(f)
%BIHARMONIC   Biharmonic operator of a DISKFUN.
%   B = BIHARMONIC(F) returns a DISKFUN representing the biharmonic
%   operator applied to F.
%
% See also DISKFUN/BIHARM.

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

% Use the implementation in biharm.
biharmF = biharm(f);

end


% Copyright (C) 2007 Peter Carbonetto. All Rights Reserved.
% This code is published under the Eclipse Public License.
%
% Author: Peter Carbonetto
%         Dept. of Computer Science
%         University of British Columbia
%         May 19, 2007

function qR = bopt (K, C, f, Rv, Rf, Sv, Sf, NS, verbose)
  maxiter   = 100;         % The maximum number of iterations.
  tolerance = 1e-6;        % The convergence criterion.
  nr        = length(Rv);  % The number of large regions.
  ns        = length(Sv);  % The number of small regions.

  % Convert the input K to a row vector.
  K = K(:)';

  % Get the degrees of the separators.
  d = degrees(NS);

  % Initialize the marginals on the large and small regions to uniform
  % probability tables.
  qR = initmarginals(K,Rv);
  qS = initmarginals(K,Sv);

  % Compute the total number consistency constaints. We have one
  % constraint for every pair (R,S), then again for every possible
  % configuration xS.
  nc = numconsistencyconstraints(qS,d);

  % Run the IPOPT solver.
  qR             = vectorize(qR);
  qS             = vectorize(qS);
  nqr            = length(qR);
  nqs            = length(qS);
  [status qR qS] = ipopt({qR qS},{ repmat(eps,nqr,1) repmat(eps,nqs,1) },...
                  { repmat(inf,nqr,1) repmat(inf,nqs,1) },...
                  [ ones(1,nr) ones(1,ns) zeros(1,nc) ],...
                  [ ones(1,nr) ones(1,ns) zeros(1,nc) ],...
                  @computeJGObjective,@computeJGGradient,...
                  @computeJGConstraints,@computeJGJacobian,...
                  @computeJGHessian,{ K C f Rv Rf Sv Sf NS d },'',...
                  [],'mu_strategy','adaptive','max_iter',maxiter,...
                  'tol',tolerance,'jac_c_constant','yes',...
		  'jac_d_constant','yes','print_level',verbose*5);

  % Reshape the solution.
  qR = reshapemarginals(qR,Rv,K);
  qS = reshapemarginals(qS,Sv,K);

% ------------------------------------------------------------------
function d = degrees (NS)
  ns = length(NS);  % The number of separators.
  d  = zeros(1,ns);

  % Repeat for each separator.
  for s = 1:ns
    d(s) = length(NS{s});
  end

% ----------------------------------------------------------------
function qR = initmarginals (K, Rv)
  nr = length(Rv);  % The number of regions.
  qR = cell(1,nr);  % The return value.

  % Repeat for each region.
  for r = 1:nr
    is    = Rv{r};  % The variables nodes in the large region.
    table = ones([K(is) 1]);
    qR{r} = table / sum(table(:));
  end

% ----------------------------------------------------------------
function nc = numconsistencyconstraints (qS, d)
  ns = length(qS);  % The number of small regions.
  nc = 0;           % The return value.

  % Repeat for each small region.
  for s = 1:ns
    tablesize = numel(qS{s});
    nc        = nc + d(s) * tablesize;
  end


function [i,j] = argmax2(x)
%ARGMAX2  Index of maximum element of matrix.
% [i,j] = ARGMAX2(x) returns indices (i,j) such that x(i,j) == max(x(:)).
%
% See also ARGMAX.

[colmax,i] = max(x);
[ignore,j] = max(colmax);
i = i(j);


%This Matlab script can be used to reproduce Figure 7.8 in the monograph:
%
%Emil Bjornson, Jakob Hoydis and Luca Sanguinetti (2017),
%"Massive MIMO Networks: Spectral, Energy, and Hardware Efficiency",
%Foundations and Trends in Signal Processing: Vol. 11, No. 3-4,
%pp. 154-655. DOI: 10.1561/2000000093.
%
%For further information, visit: https://www.massivemimobook.com
%
%This is version 1.0 (Last edited: 2017-11-04)
%
%License: This code is licensed under the GPLv2 license. If you in any way
%use this code for research that results in publications, please cite our
%monograph as described above.

%Empty workspace and close figures
close all;
clear;

%Maximum number of pilots
pilotMax = 40;

%Define range of average number of UEs
Kaverages = [1 10 20 40];

%Define range of effective number of UEs
Krange = 0:pilotMax;

%Prepare to store simulation results
Kdistributions = zeros(length(Krange),length(Kaverages));

%% Go through all average numbers of UEs
for n = 1:length(Kaverages)

    %Compute the probabilities of having different numbers of active UEs,
    %based on a Poission distribution
    Kdistributions(:,n) = poisspdf(Krange,Kaverages(n));

    %Enforce scheduling to make sure that there cannot be more than
    %pilotMax UEs active per cell
    Kdistributions(end,n) = 1 - sum(Kdistributions(1:end-1,n));

end

%% Plot simulation results
figure;
hold on; box on;

plot(Krange,Kdistributions(:,1),'r*-','LineWidth',1);
plot(Krange,Kdistributions(:,2),'ko-','LineWidth',1);
plot(Krange,Kdistributions(:,3),'bs-','LineWidth',1);
plot(Krange,Kdistributions(:,4),'rd--','LineWidth',1);

xlabel('Number of active UEs');
ylabel('Probability');
legend('K=1  ','K=10  ','K=20  ','K=40  ','Location','NorthWest');
ylim([0 0.6]);


function avLL = GammaavLL(hmm,Gamma,Xi,T)
% average loglikelihood for state time course

% if isfield(hmm.train,'grouping') % DEPRECATED
%     Q = length(unique(hmm.train.grouping));
% else
%     Q = 1;
% end
Q = 1;
N = length(T); K = hmm.K;
order = (sum(T) - size(Gamma,1))/N;
avLL = 0;
do_clustering = isfield(hmm.train,'cluster') && hmm.train.cluster;

% avLL initial state % DEPRECATED
% if Q>1
%     for i = 1:Q
%         PsiDir_alphasum = psi(sum(hmm.Dir_alpha(:,i)));
%         ii = hmm.train.grouping==i;
%         for l = 1:K
%             if ~hmm.train.Pistructure(l), continue; end
%             avLL = avLL + sum(Gamma(jj(ii),l)) * (psi(hmm.Dir_alpha(l,i)) - PsiDir_alphasum);
%         end
%     end
% else
%     PsiDir_alphasum = psi(sum(hmm.Dir_alpha));
%     for l = 1:K
%         if ~hmm.train.Pistructure(l), continue; end
%         avLL = avLL + sum(Gamma(jj,l)) * (psi(hmm.Dir_alpha(l)) - PsiDir_alphasum);
%     end
% end

if ~isempty(Xi) && ~do_clustering % a proper HMM
    jj = zeros(N,1); % reference to first time point of the segments
    for in = 1:N
        jj(in) = sum(T(1:in-1)) - order*(in-1) + 1;
    end
    PsiDir_alphasum = psi(sum(hmm.Dir_alpha));
    % first time point
    for l = 1:K
        if ~hmm.train.Pistructure(l), continue; end
        avLL = avLL + sum(Gamma(jj,l)) * (psi(hmm.Dir_alpha(l)) - PsiDir_alphasum);
    end
    % avLL remaining time points
    for i = 1:Q
        if Q > 1
            ii = find(hmm.train.grouping==i)';
        else
            ii = 1:length(T);
        end
        PsiDir2d_alphasum = zeros(K,1);
        for l = 1:K, PsiDir2d_alphasum(l) = psi(sum(hmm.Dir2d_alpha(l,:,i))); end
        %PsiDir2d_alphasum = psi(sum(sum(hmm.Dir2d_alpha(:,:,i))));
        for k = 1:K
            for l = 1:K
                if ~hmm.train.Pstructure(l,k), continue; end
                if Q==1
                    avLL = avLL + sum(Xi(:,l,k)) * (psi(hmm.Dir2d_alpha(l,k))-PsiDir2d_alphasum(l));
                    %avLL = avLL + sum(Xi(:,l,k)) * (psi(hmm.Dir2d_alpha(l,k))-PsiDir2d_alphasum);
                    if isnan(avLL)
                        error(['Error computing log likelihood of the state time courses  - ' ...
                            'Out of precision?'])
                    end
                else
                    for n = ii
                        t = (1:T(n)-1-order) + sum(T(1:n-1)) - (order+1)*(n-1) ;
                        avLL = avLL + sum(Xi(t,l,k)) * (psi(hmm.Dir2d_alpha(l,k,i))-PsiDir2d_alphasum(l));
                    end
                end
            end
        end
    end
else % Simple mixture of distributions
    PsiDir_alphasum = psi(sum(hmm.Dir_alpha));
    for k = 1:K
        avLL = avLL + sum(Gamma(:,k)) * (psi(hmm.Dir_alpha(k)) - PsiDir_alphasum);
    end
end

end


function varargout = corr_filter(x, y, der_w, varargin)

opts.lambda = nan;
opts = vl_argparse(opts, varargin);

% x is [m1, m2, p, b]
% y is [m1, m2]
% der_w is same size as x

sz = size_min_ndims(x, 4);
n = prod(sz(1:2));

y_f = fft2(y);
x_f = fft2(x);
% k = 1/n sum_i x_i corr x_i + lambda delta
assert(~isnan(opts.lambda), 'lambda must be specified');
k_f = 1/n*sum(conj(x_f).*x_f, 3) + opts.lambda;
% a must satisfy n (k conv a) = y
% The signal a contains a weight per example (shift)
a_f = 1/n*bsxfun(@times, y_f, 1./k_f);

if isempty(der_w)
    % Use same weight a for all channels i.
    % w[i] = a corr x[i]
    w_f = bsxfun(@times, conj(a_f), x_f);
    % w = ifft2(w_f, 'symmetric');
    w = real(ifft2(w_f));
    varargout = {w};
else
    der_w_f = fft2(der_w);
    % a, x -> w
    % w[i] = a corr x[i]
    % dw[i] = da corr x[i] + a corr dx[i]
    % F dw[i] = conj(F da) .* F x[i] + conj(F a) .* F dx[i]
    % <der_w, dw> = sum_i <der_w[i], dw[i]> = sum_i <F der_w[i], F dw[i]>
    %   = sum_i <F der_w[i], conj(F da) .* F x[i] + conj(F a) .* F dx[i]>
    %   = <F da, sum_i conj(F der_w[i]) .* F x[i]> + sum_i <F der_w[i] .* F a, F dx[i]>
    der_a_f = sum(x_f .* conj(der_w_f), 3);
    der_x_f = bsxfun(@times, a_f, der_w_f);
    % k, y -> a
    % k conv a = 1/n y
    % dk conv a + k conv da = 1/n dy
    % dk_f .* a_f + k_f .* da_f = 1/n dy_f
    % <der_a, da> = <der_a_f, da_f>
    %   = <der_a_f, k_f^-1 .* (1/n dy_f - dk_f .* a_f)>
    %   = <1/n der_a_f .* conj(k_f^-1), dy_f> + <-der_a_f .* conj(k_f^-1 .* a_f), dk_f>
    %   = <der_y_f, dy_f> + <der_k_f, dk_f>
    der_y_f = 1/n*sum(der_a_f .* conj(1 ./ k_f), 4); % accumulate gradients over batch
    der_y = real(ifft2(der_y_f));
    der_k_f = -der_a_f .* conj(a_f ./ k_f);
    % x -> k
    % k = 1/n sum_i x_i corr x_i + lambda delta
    % dk = 1/n sum_i {dx[i] corr x[i] + x[i] corr dx[i]}
    % F dk = 1/n sum_i {conj(F dx[i]) .* F x[i] + conj(F x[i]) .* F dx[i]}
    % <der_k, dk> = <der_k, 1/n sum_i {dx[i] corr x[i] + x[i] corr dx[i]}>
    %   = sum_i <F der_k, 1/n conj(F dx[i]) .* F x[i] + conj(F x[i]) .* F dx[i]>
    %   = sum_i <F dx[i], 1/n conj(F der_k) .* F x[i]> + <1/n F der_k .* F x[i], F dx[i]>
    %   = sum_i <F dx[i], 1/n [F der_k + conj(F der_k)] .* F x[i]>
    %   = sum_i <F dx[i], 2/n real(F der_k) .* F x[i]>
    %   = sum_i <F der_x[i], F dx[i]>
    der_x_f = der_x_f + 2/n*bsxfun(@times, real(der_k_f), x_f);
    % der_x = ifft2(der_x_f, 'symmetric');
    der_x = real(ifft2(der_x_f));
    varargout = {der_x, der_y};
end

end