 function y = tomo_filter(omega, ob)
%function y = tomo_filter(omega, ob)
% build the sinogram-spectrum-sized matrix
% that is .* multiplied after 2D FT, before iFFT or NUiFFT
% Copyright 2001-1, Jeff Fessler, The University of Michigan
% Extend to fan-beam geometry, 2003-11, Yingying Zhang

%
% effect of image-domain shift
%
y = exp(1i * (omega * ob.nxy_shift));

%
% effect of image basis function (extension to non-rect by S. Matej)
% basis: b(x/Dx,y/Dy) <-FT-> Dx*Dy * B(Dx*u,Dy*v)
%
y = y .* (ob.pixel_size).^2;

if isempty(ob.basis.type) | streq(ob.basis.type, 'pixel')
	% sinc_2 due to square pixels
	if ob.chat, printf('pixel basis, pixel size=%g',ob.pixel_size), end
	y = y .* sinc(omega(:,1)/(2*pi)) .* sinc(omega(:,2)/(2*pi));

elseif streq(ob.basis.type, 'no')
	if ob.chat, printf('no image basis modeled'), end

elseif streq(ob.basis.type, 'KB')
	if ob.chat, printf('KB basis: J=%g, alpha=%g, m=%g, n=%g', ...
		ob.basis.diam, ob.basis.shape, ob.basis.m, ob.basis.dim); end
	norm = kaiser_bessel_ft(0, ...
		ob.basis.diam, ob.basis.shape, ob.basis.m, ob.basis.dim);
	nd = sqrt(omega(:,1)/(2*pi) .* omega(:,1)/(2*pi) + ...
		omega(:,2)/(2*pi) .* omega(:,2)/(2*pi));
	y = y .* kaiser_bessel_ft(nd, ...
		ob.basis.diam, ob.basis.shape, ob.basis.m, ob.basis.dim) / norm;

elseif streq(ob.basis.type, 'Gauss')
	error('Gaussian basis not yet implemented')

else
	error(sprintf('basis function %s not implemented', ob.basis.type))
end

% form into sinogram shape
K = ob.Krho;
y = reshape(y, K, ob.na);

%
% detector radial response resolution-loss effect in frequency domain
% extension to parallel-beam non-rect case by S. Matej
%

kk = [-K/2 : K/2-1]';
if isempty(ob.beam.type) | streq(ob.beam.type, 'rect')

	% Trick: in fan-beam, the detector blur is not shift-invariant.
	% We approximate by the blur at the center of object.
	% The effective strip_width / ray_spacing at the center is the same.

	strip_ray = ob.strip_width / ob.ray_spacing;
	blur = sinc(strip_ray * kk / K);
	if ob.chat
		printf('strip integrals, relative beam width=%g', strip_ray)
	end

elseif streq(ob.beam.type, 'line')
	blur = ones(size(kk));
	if ob.chat, printf('line integrals modeled'), end

elseif streq(ob.beam.type, 'KB')
	if ob.chat, printf('KB beam shape: J=%g, alpha=%g, m=%g', ...
		ob.beam.diam, ob.beam.shape, ob.beam.m), end
	rel_bdiam = ob.beam.diam;	% KB_diameter relative to ob.ray_spacing
	norm = kaiser_bessel_ft(0, rel_bdiam, ob.beam.shape, ob.beam.m, 1);
	blur = kaiser_bessel_ft(...
		kk/K, rel_bdiam, ob.beam.shape, ob.beam.m, 1) / norm;

elseif streq(ob.beam.type, 'Gauss')
	g_sigma = ob.beam.shape / sqrt(8*log(2));
	blur = exp(-(2*pi*kk/K).^2 .* g_sigma^2/2);

else
	error(sprintf('beam shape %s not implemented', ob.beam.type))

end

y = y .* repmat(blur, [1 ob.na]);	% include blur effect

%
% phase "shift" to effect a half-pixel shift in each row
% corresponding to "offset=0" in tomographic projection.
% build in the post-fft shift too while at it.
%
if streq(ob.geometry, 'par') % do we need this in fan-beam ???
	phase = exp(1i*2*pi*(K+ob.is.shift0)/2 * kk / K);
	y = y .* repmat(phase, [1 ob.na]);
	y = y ./ ob.ray_spacing;	% see JF tech. report
end


%Demo for the Fractional-Order Chaotic Systems (FOChS) - functions:

%1. Chen's system:
[t, y]=FOChen([35 3 28 -7], [0.9 0.9 0.9], 100, [-9 -5 14]);
figure
plot3(y(:,1), y(:,2), y(:,3),'k');
xlabel('x(t)'); ylabel('y(t)'); zlabel('z(t)'); grid;

%2. CNN - 3 cells net:
[t, y]=FO3CNN([1.24 1.1 1.0 4.4 3.21], [0.99 0.99 0.99], 100, [0.1 0.1 0.1]);
figure
plot3(y(:,1), y(:,2), y(:,3), 'k');
xlabel('x_1(t)'); ylabel('x_2(t)'); zlabel('x_3(t)'); grid;

%3. Arneodo's system:
[t,y]=FOArneodo([-5.5 3.5 0.8 -1.0], [0.97 0.97 0.96], 200, [-0.2 0.5 0.2]);
figure
plot3(y(:,1), y(:,2), y(:,3), 'k');
xlabel('x(t)'); ylabel('y(t)'); zlabel('z(t)'); grid;

%4. Genesio-Tesi's system:
[t, y]=FOGenTesi([1.1 1.1 0.45 1.0], [1 1 0.95], 200, [-0.1 0.5 0.2]);
figure
plot3(y(:,1), y(:,2), y(:,3),'k');
xlabel('x(t)'); ylabel('y(t)'); zlabel('z(t)'); grid;

%5. Lorenz's system:
[t, y]=FOLorenz([10 28 8/3],[0.993 0.993 0.993],100,[0.1 0.1 0.1]);
figure
plot3(y(:,1), y(:,2), y(:,3), 'k');
xlabel('x(t)'); ylabel('y(t)'); zlabel('z(t)'); grid;

%6. Newton-Leipnik's system:
[t, y]=FONewLeipnik([0.4 0.175], [0.95 0.95 0.95], 200, [0.19 0 -0.18]);
figure
plot3(y(:,1), y(:,2), y(:,3),'k');
xlabel('x(t)'); ylabel('y(t)'); zlabel('z(t)'); grid;

%7. Rossler's system:
[t, y]=FORossler([0.5 0.2 10], [0.9 0.85 0.95], 120, [0.5 1.5 0.1]);
figure
plot3(y(:,1), y(:,2), y(:,3),'k');
xlabel('x(t)'); ylabel('y(t)'); zlabel('z(t)'); grid;

%8. Lotka-Volterra system:
[t, y]=FOLotkaVolterra([1 1 1 1 2 3 2.7], [0.95 0.95 0.95], 200, [1 1.4 1]);
figure
plot3(y(:,1), y(:,2), y(:,3),'k');
xlabel('x(t)'); ylabel('y(t)'); zlabel('z(t)'); grid;

%9. Duffing's system:
[t, y]=FODuffing([0.15 0.3 1], [0.9 1], 200, [0.21 0.31]);
figure
plot(y(:,1), y(:,2), 'k');
xlabel('x(t)'); ylabel('y(t)'); grid;

%10. Van der Pol's oscillator:
[t, y]=FOvanDerPol(1, [1.2 0.8], 60, [0.2 -0.2]);
figure
plot(y(:,1), y(:,2), 'k');
xlabel('y_1(t)'); ylabel('y_2(t)'); grid;

%11. Volta's system:
[t, y]=FOVolta([19 11 0.73],[0.99 0.99 0.99], 20, [8 2 1]);
figure
plot3(y(:,1), y(:,2), y(:,3),'k');
xlabel('x(t)'); ylabel('y(t)'); zlabel('z(t)'); grid;

%12. Lu's system:
[t, y]=FOLu([36 3 20], [0.985 0.99 0.98], 60, [0.2 0.5 0.3]);
figure
plot3(y(:,1), y(:,2), y(:,3),'k');
xlabel('x(t)'); ylabel('y(t)'); zlabel('z(t)'); grid;

%13. Liu's system:
[t, y]=FOLiu([1 2.5 5 1 4 4], [0.95 0.95 0.95], 100, [0.2 0 0.5]);
figure
plot3(y(:,1), y(:,2), y(:,3),'k');
xlabel('x(t)'); ylabel('y(t)'); zlabel('z(t)'); grid;

%14. Chua's systems:
[t, y]=FOChuaNR([10.725 10.593 0.268 -0.7872 -1.1726], [0.93 0.99 0.92], 60, [0.6 0.1 -0.6]);
figure
plot3(y(:,1), y(:,2), y(:,3),'k');
xlabel('x(t)'); ylabel('y(t)'); zlabel('z(t)'); grid;
[t, y]=FOChuaM([10 13 0.1 1.5 0.3 0.8], [0.97 0.97 0.97 0.97], 200, [0.8 0.05 0.007 0.6]);
figure
plot3(y(:,1), y(:,2), y(:,3),'k');
xlabel('x(t)'); ylabel('y(t)'); zlabel('z(t)'); grid;
figure
plot3(y(:,4), y(:,1), y(:,2),'k');
xlabel('w(t)'); ylabel('x(t)'); zlabel('y(t)'); grid;

%15. Financial system:
[t, y]=FOFinanc([1 0.1 1],[1 0.95 0.99],200, [2 -1 1]);
figure
plot3(y(:,1), y(:,2), y(:,3),'k');
xlabel('x(t)'); ylabel('y(t)'); zlabel('z(t)'); grid;

% Author: Dr. Ivo Petras (ivo.petras@tuke.sk), 2010.


function [S] = ukfPropagation(dt,Rot,RotAnt,vAnt,omega_b,a_b,S,omega,acc,Qc,g)
q = length(S);
S_aug = blkdiag(S,Qc);
N_aug = length(S_aug);

% scaled unsented transform
W0 = 1-N_aug/3;
Wj = (1-W0)/(2*N_aug);
gamma = sqrt(N_aug/(1-W0));

ichi = Rot';
omega = omega-omega_b; % unbiased inputs
acc = acc-a_b;
X = gamma*[zeros(N_aug,1) S_aug' -S_aug'];% sigma-points
X(10:15,:) = X(10:15,:)+X(q+7:q+12,:)*dt;
for j = 2:2*N_aug+1
    xi_j = X(1:3,j);
    xi_v = X(4:6,j);
    xi_x = X(7:9,j);
    w_j = X(q+1:N_aug,j);
    omega_bj = X(10:12,j);
    a_bj = X(13:15,j);
    Rot_j = RotAnt*expSO3(xi_j)*expSO3((omega+w_j(1:3)-omega_bj)*dt);
    v_j = vAnt + xi_v + (Rot_j*(acc+w_j(4:6)-a_bj)+g)*dt;
    x_j = xi_x + v_j*dt;
    Xi_j = ichi*Rot_j;
    X(1:3,j) = logSO3(Xi_j); % propagated sigma points
    X(4:6,j) = v_j - vAnt;
    X(7:9,j) = x_j + (v_j-vAnt)*dt;
end
X = sqrt(Wj)*X;
[~,Rs] = qr(X(1:q,2:2*N_aug+1)');
S = Rs(1:q,1:q);
end


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

%% -*- texinfo -*-
%% @documentencoding UTF-8
%% @defmethod  @@sym jacobian (@var{f})
%% @defmethodx @@sym jacobian (@var{f}, @var{x})
%% Symbolic Jacobian of symbolic expression.
%%
%% The Jacobian of a scalar expression is:
%% @example
%% @group
%% syms f(x, y, z)
%% jacobian(f)
%%   @result{} (sym 1×3 matrix)
%%       ⎡∂               ∂               ∂             ⎤
%%       ⎢──(f(x, y, z))  ──(f(x, y, z))  ──(f(x, y, z))⎥
%%       ⎣∂x              ∂y              ∂z            ⎦
%% @end group
%% @end example
%%
%% @var{x} can be a scalar, vector or cell list.  If omitted,
%% it is determined using @code{symvar}.
%%
%% Example:
%% @example
%% @group
%% f = sin(x*y);
%% jacobian(f)
%%   @result{} (sym) [y⋅cos(x⋅y)  x⋅cos(x⋅y)]  (1×2 matrix)
%%
%% jacobian(f, [x y z])
%%   @result{} (sym) [y⋅cos(x⋅y)  x⋅cos(x⋅y)  0]  (1×3 matrix)
%% @end group
%% @end example
%%
%% For vector input, the output is a matrix:
%% @example
%% @group
%% syms f(x,y,z) g(x,y,z)
%% jacobian([f; g])
%%   @result{} (sym 2×3 matrix)
%%       ⎡∂               ∂               ∂             ⎤
%%       ⎢──(f(x, y, z))  ──(f(x, y, z))  ──(f(x, y, z))⎥
%%       ⎢∂x              ∂y              ∂z            ⎥
%%       ⎢                                              ⎥
%%       ⎢∂               ∂               ∂             ⎥
%%       ⎢──(g(x, y, z))  ──(g(x, y, z))  ──(g(x, y, z))⎥
%%       ⎣∂x              ∂y              ∂z            ⎦
%% @end group
%% @end example
%%
%% Example:
%% @example
%% @group
%% jacobian([2*x + 3*z; 3*y^2 - cos(x)])
%%   @result{} (sym 2×3 matrix)
%%       ⎡  2      0   3⎤
%%       ⎢              ⎥
%%       ⎣sin(x)  6⋅y  0⎦
%% @end group
%% @end example
%% @seealso{@@sym/divergence, @@sym/gradient, @@sym/curl, @@sym/laplacian,
%%          @@sym/hessian}
%% @end defmethod

function g = jacobian(f, x)

  assert (isvector(f), 'jacobian: defined only for vectors expressions')

  if (nargin == 1)
    x = symvar(f);
    if (isempty(x))
      x = sym('x');
    end
  elseif (nargin == 2)
    % no-op
  else
    print_usage ();
  end

  if (~iscell(x) && isscalar(x))
    x = {x};
  end

  cmd = { '(f, x) = _ins'
          'if not f.is_Matrix:'
          '    f = Matrix([f])'
          'G = f.jacobian(x)'
          'return G,' };

  g = pycall_sympy__ (cmd, sym(f), x);

end

%!error jacobian (sym(1), 2, 3)
%!error <only for vectors> jacobian ([sym(1) 2; sym(3) 4])

%!shared x,y,z
%! syms x y z

%!test
%! % 1D
%! f = x^2;
%! assert (isequal (jacobian(f), diff(f,x)))
%! assert (isequal (jacobian(f,{x}), diff(f,x)))
%! assert (isequal (jacobian(f,x), diff(f,x)))

%!test
%! % const
%! f = sym(1);
%! g = sym(0);
%! assert (isequal (jacobian(f), g))
%! assert (isequal (jacobian(f,x), g))

%!test
%! % double const
%! f = 1;
%! g = sym(0);
%! assert (isequal (jacobian(f,x), g))

%!test
%! % diag
%! f = [x y^2];
%! g = [sym(1) 0; 0 2*y];
%! assert (isequal (jacobian(f), g))
%! assert (isequal (jacobian(f, [x y]), g))
%! assert (isequal (jacobian(f, {x y}), g))

%!test
%! % anti-diag
%! f = [y^2 x];
%! g = [0 2*y; sym(1) 0];
%! assert (isequal (jacobian(f), g))
%! assert (isequal (jacobian(f, {x y}), g))

%!test
%! % shape
%! f = [x y^2];
%! assert (isequal (size(jacobian(f, {x y z})), [2 3]))
%! assert (isequal (size(jacobian(f, [x y z])), [2 3]))
%! assert (isequal (size(jacobian(f, [x; y; z])), [2 3]))
%! assert (isequal (size(jacobian(f.', {x y z})), [2 3]))

%!test
%! % scalar f
%! f = x*y;
%! assert (isequal (size(jacobian(f, {x y})), [1 2]))
%! g = gradient(f, {x y});
%! assert (isequal (jacobian(f, {x y}), g.'))

%!test
%! % vect f wrt 1 var
%! f = [x x^2];
%! assert (isequal (size(jacobian(f, x)), [2 1]))
%! f = f.';  % same shape output
%! assert (isequal (size(jacobian(f, x)), [2 1]))


function A = GregK316(n);
%GREGK316     Example from Gregory/Karney
%
% Gregory/Karney test case 3.16, n>=2
%
% (  2  -1               )
% ( -1   2  -1           )
% (     -1   2  -1       )
% (        ........      )
% (         -1   2   1   )
% (             -1   2   )
%
%   A = GregK316(n);
%

% written   3/01/95     S.M. Rump
% modified 04/04/04     S.M. Rump  set round to nearest for safety
% modified 04/06/05     S.M. Rump  rounding unchanged
%

  if n<2
    error('dimension too small')
  end
  e = ones(n,1);
  A = spdiags( [-e 2*e -e], -1:1, n, n);


function [xUpdate,PUpdate,innov,Pzz,W]=EKFUpdate(xPred,PPred,z,R,h,HJacob,numIter,HHessian,innovTrans,measPredTrans,stateDiffTrans,stateTrans)
%%EKFUPDATE Perform the measurement update step in the first- or second-
%           order Extended Kalman Filter (EKF) or iterated Extended Kalman
%           filter (IEKF), which is what one gets with numIter>0.
%
%INPUTS: xPred The xDimX1 predicted target state.
%        PPred The xDimXxDim predicted state covariance matrix.
%            z The zDimX1 vector measurement.
%            R The zDimXzDim measurement covariance matrix.
%            h A function handle for the measurement function that takes
%              the state as its argument.
%       HJacob A function handle for the measurement Jacobian matrix that
%              takes the target state as a parameter or the zDimXxDim
%              measurement Jacobian matrix itself. Each column is the
%              derivative vector of h with respect to the corresponding
%              element of x. If not supplied or an empty matrix is passed,
%              then HJacob will be found using numerical differentiation
%              via the numDiff function with default parameters.
%      numIter The number of iterations to perform if an iterated EKF is
%              desired. If this parameter is omitted or an empty matrix is
%              passed, the default value of zero is used. That is, just
%              use the standard update without any additional iterations.
%     HHessian This parameter is only provided if a second-order EKF is
%              desired. This is either a function handle for the
%              measurement Hessian hypermatrix, or it is the measurement
%              Hessian hypermatrix itself. The matrix is
%              xDimXxDimXzDim. The matrix HHess=HHessian(x) is such that
%              HHess(i,j,k) is the second derivative of the kth element
%              of the vector returned by h with respect to the ith and jth
%              components of x. For each k, the Hessian matrix is
%              symmetric. If this parameter is omitted or an empty matrix
%              is passed, a first-order EKF update is used.
%   innovTrans An optional function handle that computes and optionally
%              transforms the value of the difference between the
%              observation and any predicted points. This is called as
%              innovTrans(a,b) and the default if omitted or an empty
%              matrix is passed is @(a,b)bsxfun(@minus,a,b). This must be
%              able to handle sets of values. For a zDimX1 measurement,
%              either of the inputs could be zDimXN in size while one of
%              the inputs could be zDimX1 in size.  This only needs to be
%              supplied when a measurement difference must be restricted
%              to a certain range. For example, the innovation between two
%              angles will be 2*pi if one angle is zero and the other
%              2*pi, even though they are the same direction. In such an
%              instance, a function handle to the
%              wrapRange(bsxfun(@minus,a,b),-pi,pi) function with the
%              appropriate parameters should be passed for innovTrans.
% measPredTrans An optional function handle that transforms the predicted
%              measurement into a particular range. The second order EKF
%              has a linear correction to the prediction of the
%              measurement, which might not be appropriate for all types of
%              measurements.
% stateDiffTrans An optional function handle that, like innovTrans does for
%              the measurements, a difference between states and transforms
%              it however might be necessary. For example, a state
%              containing angular components will generally need to be
%              transformed so that the difference between the angles is
%              wrapped to -pi/pi.
%   stateTrans An optional function handle that takes a state estimate and
%              transforms it. This is useful if one wishes the elements of
%              the state to be bound to a certain domain. For example, if
%              an element of the state is an angle, one should generally
%              want to bind it to the region +/-pi.
%
%OUTPUTS: xUpdate The xDim X 1 updated state vector.
%         PUpdate The updated xDim X xDim state covariance matrix.
%      innov, Pzz The zDimX1 innovation and the zDimXzDim innovation
%                 covariance matrix are returned in case one wishes to
%                 analyze the consistency of the estimator or use those
%                 values in gating or likelihood evaluation.
%               W The xDimXzDim gain used in the update. This can be
%                 useful when gating and using the function
%                 calcMissedGateCov.
%
%The first-order EKF is summarized in Figure 10.3.3-1 in Chapter 10.3.3 of
%[1]. The Joseph-form covariance update given in Chapter 5 of the same
%book is used for improved numerical stability. The expressions for the
%second-order EKF come from Chapter 10.3.2 of [1].
%
%The iteration of the measurement equation for the iterated EKF is given in
%Chapter 10.5.2 of the same book. In [2], it is noted that the iteration
%need only be done over each measurement update, not over an entire batch
%of measurements.
%
%The optional parameter innovTrans is not described in the above reference,
%but allow for possible modifications to the filter as described in [3].
%The parameters have been added to allow the filter to be used with
%angular quantities. For example, if the measurement consisted of
%range and angle, z=[r;theta], then
%innovTrans=@(a,b)[bsxfun(@minus,a(1,:),b(1,:));
%                  wrapRange(bsxfun(@minus,a(2,:),b(2,:)),-pi,pi)];
%should be used to approximately deal with the circular nature of the
%measurements.
%
%REFERENCES:
%[1] Y. Bar-Shalom, X. R. Li, and T. Kirubarajan, Estimation with
%    Applications to Tracking and Navigation. New York: John Wiley and
%    Sons, Inc, 2001.
%[2] T. H. Kerr, "Streamlining Measurement Iteration for EKF Target
%    Tracking," IEEE Transactions on Aerospace and Electronic Systems,
%    vol. 27, no. 2, pp. 408-421, Mar. 1991.
%[3] D. F. Crouse, "Cubature/ unscented/ sigma point Kalman filtering with
%    angular measurement models," in Proceedings of the 18th International
%    Conference on Information Fusion, Washington, D.C., 6-9 Jul. 2015.
%
%April 2015 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

zDim=size(z,1);
if(nargin<6||isempty(HJacob))
    HJacob=@(x)numDiff(x,h,zDim);
end

if(nargin<7||isempty(numIter))
    numIter=0;
end

if(nargin<8)
    HHessian=[];
end

if(nargin<9||isempty(innovTrans))
    %The function just returns the input.
    innovTrans=@(a,b)bsxfun(@minus,a,b);
end

if(nargin<10||isempty(measPredTrans))
    measPredTrans=@(x)x;
end

if(nargin<11||isempty(stateDiffTrans))
    stateDiffTrans=@(x)x;
end

if(nargin<12||isempty(stateTrans))
    stateTrans=@(x)x;
end

if(isa(HJacob,'function_handle'))
    H=HJacob(xPred);
else
    H=HJacob;%If the Jacobian matrix was directly given.
end

if(~isempty(HHessian))
    if(isa(HHessian,'function_handle'))
        HHess=HHessian(xPred);
    else
        HHess=HHessian;%If the Hessian matrix was directly given.
    end

    [zPredHessTerm,PzzHessTerm]=getHessianTerms(HHess,PPred,zDim);
else
    zPredHessTerm=0;
    PzzHessTerm=0;
end

zPred=measPredTrans(h(xPred)+zPredHessTerm);
innov=innovTrans(z,zPred);

Pzz=R+H*PPred*H'+PzzHessTerm;
%Ensure symmetry
Pzz=(Pzz+Pzz')/2;
W=PPred*H'/Pzz;

xUpdate=stateTrans(xPred+W*innov);

temp=W*H;
temp=eye(size(temp))-temp;
PUpdate=temp*PPred*temp'+W*R*W';
%Ensure symmetry
PUpdate=(PUpdate+PUpdate')/2;

for curIter=1:numIter
    if(isa(HJacob,'function_handle'))
        H=HJacob(xUpdate);
    else
        H=HJacob;%If the Jacobian matrix was directly given.
    end

    if(~isempty(HHessian))
        if(isa(HHessian,'function_handle'))
            HHess=HHessian(xUpdate);
        else
            HHess=HHessian;%If the hessian matrix was directly given.
        end

        [zPredHessTerm,PzzHessTerm]=getHessianTerms(HHess,PPred,zDim);
    else
        zPredHessTerm=0;
        PzzHessTerm=0;
    end

    %Update the PEstimate
    Pzz=R+H*PPred*H'+PzzHessTerm;
    %Ensure symmetry
    Pzz=(Pzz+Pzz')/2;
    W=PPred*H'/Pzz;
    temp=W*H;
    temp=eye(size(temp))-temp;
    PUpdate=temp*PPred*temp'+W*R*W';
    %Ensure symmetry
    PUpdate=(PUpdate+PUpdate')/2;
    zPred=measPredTrans(h(xUpdate)+zPredHessTerm);

    %Update the x estimate
    xUpdate=stateTrans(xUpdate+PUpdate*H'*lsqminnorm(R,innovTrans(z,zPred))-PUpdate*lsqminnorm(PPred,stateDiffTrans(xUpdate-xPred)));
end

end

function [zPredHessTerm,PzzHessTerm]=getHessianTerms(HHess,PPred,zDim)
%The function returns the terms associated with the second derivative in
%the second-order EKF.

PzzHessTerm=zeros(zDim,zDim);
zPredHessTerm=zeros(zDim,1);
for n=1:zDim
    en=zeros(zDim,1);
    en(n)=1;

    HPProdn=HHess(:,:,n)*PPred;
    zPredHessTerm=zPredHessTerm+en*trace(HPProdn);

    for m=1:zDim
        em=zeros(zDim,1);
        em(m)=1;
        PzzHessTerm=PzzHessTerm+en*em'*trace(HPProdn*HHess(:,:,m)'*PPred);
    end
end
PzzHessTerm=PzzHessTerm/2;
zPredHessTerm=zPredHessTerm/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.


close all; clear all; clc;
yf = spx.data.image.EhsanYaleFaces();
[faces, sizes, labels] = yf.all_faces();
% normalize the faces
faces_normalized = spx.norm.normalize_l2(faces);
[M, S]  = size(faces_normalized);

% only between 2 subjects
% interesting cases are (10, 11), (28, 29), (29, 30)
% (35, 36), (36, 37)
s1 = 35;
s2 = s1+1;
columns_s1 = (1:64) + (s1-1)*64;
columns_s2 = (1:64) + (s2-1)*64;
all_columns = [columns_s1 columns_s2];
fprintf('\n\n\n Statistics for the first pair of subjects:\n\n');
faces_normalized = faces_normalized(:, all_columns);
[M, S]  = size(faces_normalized);
sizes = [64 64];
angle_result = spx.cluster.subspace.nearest_same_subspace_neighbors_by_inner_product(faces_normalized, sizes);

fprintf('Within Neighbor Counts:\n %s\n', spx.stats.format_descriptive_statistics(angle_result.within_neighbor_counts));
fprintf('Nearest Within Neighbor Indices:\n %s\n', spx.stats.format_descriptive_statistics(angle_result.nearst_within_neighbor_indices));
nearst_within_neighbor_indices = angle_result.nearst_within_neighbor_indices;
% club all those cases where nearest within neighbor is far away.
nearst_within_neighbor_indices(nearst_within_neighbor_indices > 5) = -1;
tabulate(nearst_within_neighbor_indices);
fprintf('First In Out Angle Spreads:\n %s\n', spx.stats.format_descriptive_statistics(angle_result.first_in_out_angle_spreads));
fioas = angle_result.first_in_out_angle_spreads;
%fioas = round(fioas * 10) / 10;
fioas = round(fioas/4)*4;
fioas(fioas > 8) = 100;
fioas(fioas < -8) = 100;
angle_gt_zero_flags = angle_result.first_in_out_angle_spreads >= 0;
angle_gte_zero = angle_result.first_in_out_angle_spreads(angle_gt_zero_flags);
angle_lte_two_flags =  angle_gte_zero <= 2;
angle_lte_four_flags =  angle_gte_zero <= 4;
angle_lte_eight_flags =  angle_gte_zero <= 8;
angle_lt_zero_flags = angle_result.first_in_out_angle_spreads < 0;
angle_lt_zero =  angle_result.first_in_out_angle_spreads(angle_lt_zero_flags);
angle_gte_minus_two_flags =  angle_lt_zero >= -2;
angle_gte_minus_four_flags =  angle_lt_zero >= -4;
abs_angle_lte_four_flags = abs(angle_result.first_in_out_angle_spreads) <=4;
fprintf('angle greater than equal to 0: %.2f %%\n', sum(angle_gt_zero_flags) * 100 / S);
fprintf('angle less than 2: %.2f %%\n', sum(angle_lte_two_flags) * 100 / S);
fprintf('angle less than 4: %.2f %%\n', sum(angle_lte_four_flags) * 100 / S);
fprintf('angle less than 8: %.2f %%\n', sum(angle_lte_four_flags) * 100 / S);
fprintf('angle less than 0: %.2f %%\n', sum(angle_lt_zero_flags) * 100 / S);
fprintf('angle greater than -2: %.2f %%\n', sum(angle_gte_minus_two_flags) * 100 / S);
fprintf('angle greater than -4: %.2f %%\n', sum(angle_gte_minus_four_flags) * 100 / S);
fprintf('abs angle less than 4: %.2f %%\n', sum(abs_angle_lte_four_flags) * 100 / S);


function [Cov]=vbfa(y,nl,nem,fig)

% Output
% Regularized noise covariance from pre-stimulus data

% Input
% y(nk,nt) = Baseline or Prestimulus data
% b(nk,nl) = mixing matrix
% lam(nk,1) = sensor noise precision
% bet(nl,1) = hyperparamaeters
% sig(nk,nk) = data covariance matrix = b*b'+1/lam (approx)
% nk = data dimensionality
% nl  = factor dimensionality (try 5)
% nt = number of time points
% nem = number of EM iterations  (try 50)
% plot = flag for plotting

% experiment with nl,nem using the plots generated by the code:
% set nem such that the likelihood converges (top plot)
% set nl such that some hyperparameters bet approach infinity, i.e. some
% 1/bet vanish (middle plot)

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

nk=size(y,1);nt=size(y,2);

ryy=y*y';
[p d]=svd(ryy/nt);
d=diag(d);
b=p*diag(sqrt(d));
b=b(:,1:nl);
lam=1./diag(ryy/nt);

bet=ones(nl,1); %diff from VBFA

likeli=zeros(nem,1);
rbb=eye(nl)/nt;

for iem=1:nem
   dbet=diag(bet);ldbet=sum(log(bet));
   dlam=diag(lam);ldlam=sum(log(lam));

   gam=b'*dlam*b+eye(nl)+nk*rbb*0; %diff from VBFA
   igam=inv(gam);
   ubar=igam*b'*dlam*y;
   ryu=y*ubar';
   ruu=ubar*ubar'+nt*igam;

   [p d q]=svd(gam);ldgam=sum(log(diag(d)));
   temp1=-.5*ldgam*ones(1,nt)+.5*sum(ubar.*(gam*ubar),1);
   temp2=.5*ldlam*ones(1,nt)-.5*lam'*(y.^2);
   f=temp1+temp2;
   f3=.5*nl*ldlam+.5*nk*ldbet-.5*trace(b'*dlam*b*dbet); %diff
   likeli(iem)=mean(f)+f3/nt;

   betbar=ruu+dbet;
   ibetbar=inv(betbar);
   b=ryu*ibetbar;

   ilam=diag(ryy-b*ryu')/(nt+nl); % diff
   lam=1./ilam;
   dlam=diag(lam);

   bet=1./(diag(b'*dlam*b)/nk+diag(ibetbar));
    if nargin>3
       figure(fig)
       hsub=subplot(3,3,1);plot((1:iem)',likeli(1:iem));title('likelihood')
       hsub=subplot(3,3,4);plot((1:nl)',sqrt([mean(b.^2,1)' 1./bet]));title('1/bet');
       hsub=subplot(3,3,7);plot(1./lam);title('1/lam');
       drawnow;
    end

   rbb=ibetbar;
end

weight=b*igam*b'*dlam;
sig=b*b'+diag(1./lam);
yc=b*ubar;
cy=b*ruu*b'+diag(ilam*trace(ruu*ibetbar));
mlike=likeli(iem);

if nargin>3
    figure(fig)
    subplot(3,3,2);imagesc(ryy/nt);title('ryy/nt');colorbar;
    subplot(3,3,3);imagesc(cy/nt);title('cy/nt');colorbar;
    subplot(3,3,5);imagesc((ryy-cy)/nt);title('(ryy-cy)/nt');colorbar;
    subplot(3,3,6);imagesc(b*b');title('b*bT');colorbar;
    subplot(3,3,8);imagesc(sig);title('sig');colorbar;
end

Cov=cy;

return

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


function CostLayer = F_mixture_mse(input_layers, CostLayer)
predicted{1} = input_layers{1}.a;
predicted{2} = input_layers{2}.a;
ref{1} = input_layers{3}.a;
ref{2} = input_layers{4}.a;

[D,T,N] = size(ref{1});

DEBUG = 0;

ref_idx = [];
for i=1:N

    diff1 = [predicted{1}(:,:,i)-ref{1}(:,:,i) predicted{2}(:,:,i)-ref{2}(:,:,i)];
    cost1 = 0.5/T * sum(sum( diff1 .* conj(diff1) ));   % support both real and complex numbers

    diff2 = [predicted{1}(:,:,i)-ref{2}(:,:,i) predicted{2}(:,:,i)-ref{1}(:,:,i)];
    cost2 = 0.5/T * sum(sum( diff2 .* conj(diff2) ));   % support both real and complex numbers

    if cost1>cost2
        ref_idx(:,i) = [2 1];   % record the pair information by remembering the reference index
        cost(i) = cost2;
    else
        ref_idx(:,i) = [1 2];
        cost(i) = cost1;
    end

    if DEBUG
        subplot(1,2,1); imagesc( [predicted{1}(1:257,:,i); ref{ref_idx(1,i)}(1:257,:,i)]); colorbar
        subplot(1,2,2); imagesc( [predicted{2}(1:257,:,i); ref{ref_idx(2,i)}(1:257,:,i)]); colorbar
%         subplot(2,2,1); imagesc(predicted{1}(1:257,:,i)); colorbar
%         subplot(2,2,2); imagesc(predicted{2}(1:257,:,i)); colorbar
%         subplot(2,2,3); imagesc(ref{ref_idx(1,i)}(1:257,:,i)); colorbar;
%         subplot(2,2,4); imagesc(ref{ref_idx(2,i)}(1:257,:,i)); colorbar
        pause
    end
end

CostLayer.a = mean(cost);
CostLayer.ref_idx = ref_idx;

end


%====================================================
%Practise problem for the course of CAO
%Prof P.Beckers
%====================================================
%Porpose :Drawing the orthotomic surface of a Bezier surface  %
% Student: BUI QUOC TINH
% European Master Mechanices of Contructions	(EMMC)
% University the Liege. Belgium										    		     %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%====================================================

%The function calculation BernsTein polynomial derivation
function Bepoly=B_daoham(i,n,u)
% Calculation combination
if i==n|i==0
   c=1;
elseif i<n & i>=0
   c=factorial(n)/(factorial(i)*factorial(n-i));
else
   c=0;
end
%BernsTein polynomial
Bepoly=c*u^i*(1-u)^(n-i)*(i-n*u)/((1-u)*u);

%%%%%%%%%%%%%%%%%%%%%%%%


function [dx, dy] = evalshift(p, pval, L1, L2, L3)

% function [dx, dy] = evalshift(p, pval, L1, L2, L3)
% Purpose: compute two-dimensional Warp & Blend transform

% 1) compute Gauss-Lobatto-Legendre node distribution
gaussX = -JacobiGL(0,0,p);

% 2) compute blending function at each node for each edge
blend1 = L2.*L3; blend2 = L1.*L3; blend3 = L1.*L2;

% 3) amount of warp for each node, for each edge
warpfactor1 = 4*evalwarp(p, gaussX, L3-L2);
warpfactor2 = 4*evalwarp(p, gaussX, L1-L3);
warpfactor3 = 4*evalwarp(p, gaussX, L2-L1);

% 4) combine blend & warp
warp1 = blend1.*warpfactor1.*(1 + (pval*L1).^2);
warp2 = blend2.*warpfactor2.*(1 + (pval*L2).^2);
warp3 = blend3.*warpfactor3.*(1 + (pval*L3).^2);

% 5) evaluate shift in equilateral triangle
dx = 1*warp1 + cos(2*pi/3)*warp2 + cos(4*pi/3)*warp3;
dy = 0*warp1 + sin(2*pi/3)*warp2 + sin(4*pi/3)*warp3;
return;


function dtscale = dtscale2D;

% function dtscale = dtscale2D;
% Purpose : Compute inscribed circle diameter as characteristic
%           for grid to choose timestep

Globals2D;

% Find vertex nodes
vmask1   = find( abs(s+r+2) < NODETOL)';
vmask2   = find( abs(r-1)   < NODETOL)';
vmask3   = find( abs(s-1)   < NODETOL)';
vmask  = [vmask1;vmask2;vmask3]';
vx = x(vmask(:), :); vy = y(vmask(:), :);

% Compute semi-perimeter and area
len1 = sqrt((vx(1,:)-vx(2,:)).^2+(vy(1,:)-vy(2,:)).^2);
len2 = sqrt((vx(2,:)-vx(3,:)).^2+(vy(2,:)-vy(3,:)).^2);
len3 = sqrt((vx(3,:)-vx(1,:)).^2+(vy(3,:)-vy(1,:)).^2);
sper = (len1 + len2 + len3)/2.0;
Area = sqrt(sper.*(sper-len1).*(sper-len2).*(sper-len3));

% Compute scale using radius of inscribed circle
dtscale = Area./sper;
return;


function [J] = kWh2J(kWh)
% Convert energy or work from kilowatt-hours to joules.
% Chad A. Greene 2012
J = kWh*3600000;


function adapted = adapted_ranks(ranks, different, mode)
% adapted_ranks Performs rank adaptation on a set of ranks
%
% Performs different types of rank adaptation on a set of ranks and a matrix of determined actual differences of ranked entities.
%
% The function looks for sets of entities that are considered equal according to the `different` matrix and adapts their ranks
% in one of the following ways:
% - none: no adaptation, keep the original ranks.
% - mean: a mean rank of a set is assigned to all trackers in the set.
% - median: a median rank of a set is assigned to all trackers in the set.
% - best: a minimum rank in a set is assigned to all trackers in the set.
%
% Input:
% - ranks (double vector): A vector of `N` ranks.
% - different (boolean matrix): A `N x N` matrix that denotes which entities should actually be equal and which not.
% - mode (string): Type of rank adaptation.
%
% Output
% - adapted (double vector): Adapted ranks

adapted = zeros(1, length(ranks)) ;

switch mode
	case 'none'
        adapted = ranks;

	case 'mean'

		for tracker = 1:length(ranks)
			adapted(tracker) = mean(ranks(find(~different(tracker,:)))) ; %#ok<FNDSB>
		end

	case 'median'

		for tracker = 1:length(ranks)
			adapted(tracker) = median(ranks(find(~different(tracker,:)))) ; %#ok<FNDSB>
		end

	case 'best'

        [sorted_ranks, idx] = sort(ranks);
        [~, unsort_idx] = sort(idx);

        sorted_matrix = different(idx,idx);

        for tracker = 2:length(ranks)
            if sorted_matrix(tracker,tracker-1)
                sorted_ranks(tracker) = sorted_ranks(tracker-1);
            end;
        end;
        adapted = sorted_ranks(unsort_idx);

	%	for tracker = 1:length(ranks)
	%		adapted(tracker) = min(ranks(find(~different(tracker,:)))) ; %#ok<FNDSB>
	%	end

end;


function [val, pdf] = Sampling1DDistribution(distr, u)
%
%
%        distr = Sampling1DDistribution(distr, u)
%
%
%        Input:
%           -distr: 1D distribution
%           -u: a random value in [0,1]
%
%        Output:
%           -val: index in the distribution of a u-value
%           -pdf: PDF of u
%
%     Copyright (C) 2011  Francesco Banterle
%
%     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/>.
%

[p, val] = min(abs(distr.CDF - u));
pdf = distr.PDF(val);

end


function pass = test_size()
% Test SIZE

% Check two components:
F = chebfun3v(@(x,y,z) cos(x), @(x,y,z) sin(x+y+z));
pass(1) = all(size(F) == [2 inf inf inf]);

% Check three components:
F = chebfun3v(@(x,y,z) cos(x), @(x,y,z) sin(y), @(x,y,z) cos(z));
pass(2) = all(size(F) == [3 inf inf inf]);

end


% here's part from class
% load data ...
% do wavelets ..
% take svd ...
data = [5+ 2*randn(32*32, 80),  .1*randn(32*32, 80)];
[u,s,v] = svd(data,0);

% *** so replace above with the real cat and dog data  ***

% set up
xdog=v(1:80,2:4);
xcat=v(81:160,2:4);
xtrain=[xdog(1:50,:);xcat(1:50,:)];
xtest=[xdog(51:80,:);xcat(51:80,:)];
Ctrain=[ones(50,1); 2*ones(50,1)];
Ctest=[ones(30,1); 2*ones(30,1)];

% k-NN
knn = fitcknn(xtrain, Ctrain);
predKNN = predict(knn,xtest);

cMatKNN = confusionmat(Ctest, predKNN)
errorKNN = sum(abs(predKNN-Ctest))/60*100

% LDA
predLDA = classify(xtest, xtrain, Ctrain);

cMatLDA = confusionmat(Ctest, predLDA)
errorLDA = sum(abs(predLDA-Ctest))/60*100

% Naive Bayes
nb=fitNaiveBayes(xtrain,Ctrain);
predNB=nb.predict(xtest);

cMatNB=confusionmat(Ctest, predNB)
errorNB=sum(abs(predNB-Ctest))/60*100

% SVM
svm = svmtrain(xtrain,Ctrain);
predSVM = svmclassify(svm,xtest);

cMatSVM=confusionmat(Ctest,predSVM)
errorSVM=sum(abs(predSVM-Ctest))/60*100

% AdaBoost
% pick AdaBoost, 100 classifiers, weak learners are Discriminants
ab = fitensemble(xtrain,Ctrain,'AdaBoostM1',100,'Discriminant');
predAB = predict(ab,xtest);

cMatAB = confusionmat(Ctest,predAB)
errorAB = sum(abs(predAB-Ctest))/60*100

% EM / Gaussian mixture models
gm = fitgmdist(xtrain, 2); % choose number of clusters
predKM = cluster(gm,xtest); % can use clustering to classify new data

cMatKM = confusionmat(Ctest,predKM)
errorKM = sum(abs(predKM-Ctest))/60*100

% some cool ways to view these clustering results are here:
% http://www.mathworks.com/help/stats/gaussian-mixture-models.html#brajyl2

% k-means
km = kmeans(xtrain, 2); % choose number of clusters
% I don't think they have a built-in way to evaluate this on new data?


function [plag] = plagmake(nrad, nphi, nlag)
% plagmake - make matrix of lags for polar running AF
% G.A. Reina 16 Jan 2007
% Modified from the C code provided by D. L. Jones and R. G. Baraniuk
% "An Adaptive Optimal-Kernel Time-Frequency Representation"
%   by D. L. Jones and R. G. Baraniuk, IEEE Transactions on Signal
%   Processing, Vol. 43, No. 10, pp. 2361--2371, October 1995.

plag = ((sqrt(2)*(nlag-1)* (0:(nrad-1))' ) / nrad) * ...
    sin((pi * (0:(nphi-1)) / nphi));


function point = geod2cart(src, curve, normal)
%GEOD2CART Convert geodesic coordinates to cartesian coord.
%
%   PT2 = geod2cart(PT1, CURVE, NORMAL)
%   CURVE and NORMAL are both [N*2] array with the same length, and
%   represent positions of the curve, and normal to each point.
%   PT1 is the point to transform, in geodesic  coordinate (first coord is
%   distance from the curve start, and second coord is distance between
%   point and curve).
%
%   The function return the coordinate of PT1 in the same coordinate system
%   than for the curve.
%
%   TODO : add processing of points not projected on the curve.
%   -> use the closest end
%
%   See also
%   polylines2d, cart2geod, curveLength

% ------
% Author: David Legland
% E-mail: david.legland@inrae.fr
% Created: 2004-04-08
% Copyright 2004-2022 INRA - TPV URPOI - BIA IMASTE

t = parametrize(curve);
N = size(src, 1);
ind = zeros(N, 1);
for i=1:N
    indices = find(t>=src(i,1));
    ind(i) = indices(1);
end

theta = lineAngle([zeros(N,1) zeros(N,1) normal(ind,:)]);
d = src(:,2);
point = [curve(ind,1)+d.*cos(theta), curve(ind,2)+d.*sin(theta)];


function out = Csum(n,k, UBorLB, accuracy)
%sum of binomial coefficients from n choose 0 to n choose k.
%UBorLB = 'ub' or 'lb', upper or lower bound
%accuracy: 1 - more accurate, 0 - faster

%
%   Created in 2013 by Victoria Kostina (vkostina@caltech.edu)
%

if accuracy
    out = 0;
    for j = 0:k
        out = out + C(n, j, UBorLB);
    end
else
    mult = 1;
    if strcmpi(UBorLB, 'ub')
        if n <= 2*k
            mult = Inf;
        else
            mult = (n - k)/(n - 2*k);
        end
    end
    out = mult*C(n, k, UBorLB);
end


function [recall, precision, th, averagePrecision] = precisionRecall(confidence, testClass, col)
% Plot precision-recall curve
%    [recall, precision, th, averagePrecision] = precisionRecall(confidence, testClass, col)
%
% Input
%    confidence: score of the classifier
%    testclass:  1 = inlier, 0 = outlier
%    col: color (it will make a plot)
%
% Output:
%    recall
%    precision
%    th: threhold needed for each point in the precision-recall curve
%    area: area of the precision-recall curve
%
% Definition of precision-recall:
% Assuming that:
%    * RET is the set of all items the system has retrieved for a specific inquiry;
%    * REL is the set of relevant items for a specific inquiry;
%    * RETREL is the set of the retrieved relevant items, i.e. RETREL = RET REL.
% then precision and recall measures are obtained as follows:
%    precision = RETREL / RET
%    recall = RETREL / REL

confidence = double(confidence);

S = rand('state');
rand('state',0);
confidence = confidence + rand(size(confidence))*10^(-10);
rand('state',S)

% [th, j] = sort(confidence); th = th(:);
% %th = th(fix(linspace(1, length(th), 150))); % here the number of points is hardcoded to be 150.
% th = th(2:end-1);
%
% relevant = sum(testClass == 1);
% for t=1:length(th)
%     j = find(confidence > th(t));
%     retrieved(t) = length(j);
%     retrievedrelevant(t)  = sum(testClass(j) == 1);
% end
%
% precision = 100*retrievedrelevant ./ retrieved;
% recall    = 100*retrievedrelevant / relevant;

% Compute Precision-Recall
[S,j] = sort(-confidence); % retrieved elements are the ones above or equal to the threshold
C = testClass(j);
n = length(C);

REL    = sum(testClass);
if n>0
    RETREL = cumsum(C);
    RET    = 1:n;
else
    RETREL = 0;
    RET    = 1;
end

precision = 100*RETREL ./ RET;
recall    = 100*RETREL  / REL;
th = -S;

% compute average precision (from PASCAL source code)
ap=0;
T = linspace(0,100,101);
for t=T % why so few bins?
    p=max(precision(recall>=t));
    if isempty(p)
        p=0;
    end
    ap=ap+p/length(T);
end
averagePrecision = ap;

% Visualization
if nargin == 3
    plot(recall, precision, [col '-']); axis([0 100 0 100])

    grid on
    ylabel('Precision')
    xlabel('Recall')
    axis('square')
end


function [x,tspan] = w20scalar(f,L,tspan,x0,Q,gaussian)
%% W20scalar - Weak order 2.0 Ito-Taylor
%
% Syntax:
%   [x,tspan] = w20scalar(f,L,tspan,x0,Q,gaussian)
%
% In:
%   f        - Drift function and derivatives, {f, df, ddf}
%   L        - Diffusion function and derivatives, {L, dL, ddL}
%   tspan    - Time steps to simulate, [t0,...,tend]
%   x0       - Initial condition
%   Q        - Spectral density (default: standard Brownian motion)
%   gaussian - Use Gaussian increments (default: false)
%
% Out:
%   x      - Solved values
%   tspan  - Time steps
%
% Description:
%   Integrates the system of stochatic differential equations
%     dx = f(x,t) dt + L(x,t) dbeta,  for x(0) = x0
%   over the time interval defined in tspan.
%
% Copyright:
%   2018 - Simo Särkkä and Arno Solin
%
% License:
%   This software is provided under the MIT License. See the accompanying
%   LICENSE file for details.

%%

  % Check if Q given
  if nargin<5 || isempty(Q), Q = eye(size(L(x0,tspan(1)),2)); end

  % Check the sort of increments to use
  if nargin<6 || isempty(gaussian), gaussian = false; end

  % Extract derivatives
  ddf = f{3};
  df = f{2};
  f = f{1};
  ddL = L{3};
  dL = L{2};
  L = L{1};

  % Cholesky factor of Q
  cQ = chol(Q,'lower');

  % Number of steps
  steps = numel(tspan);

  % Allocate space
  x = zeros(size(x0,1),steps);

  % Initial state
  x(:,1) = x0;

  % Pre-calculate random numbers
  RR = rand(size(Q,1),steps);
  R = (RR<1/6)*sqrt(3) - (RR>5/6)*sqrt(3);

  % Iterate
  for k=2:steps

    % Time discretization
    dt = tspan(k)-tspan(k-1);

    % Increment
    if gaussian
      db = sqrt(dt)*cQ*randn;
      dz = 1/2*db*dt;
    else
      db = sqrt(dt)*cQ*R(:,k);
      dz = 1/2*dt^(3/2)*R(:,k);
    end

    % Evaluate only once
    fx = f(x(:,k-1),[]);
    dfx = df(x(:,k-1),[]);
    ddfx = ddf(x(:,k-1),[]);
    Lx = L(x(:,k-1),[]);
    dLx = dL(x(:,k-1),[]);
    ddLx = ddL(x(:,k-1),[]);

    % Step
    x(:,k) = x(:,k-1) + ...
        fx*dt + ...
        Lx*db + ...
        1/2*Lx*dLx*(db^2 - dt) + ...
        Lx*df(x(:,k-1),[])*dz + ...
        1/2*(fx*dfx + 1/2*Lx^2*ddfx)*dt^2 + ...
        (fx*dLx - 1/2*Lx^2*ddLx)*(db*dt - dz);

  end


function J=uGradient2D(xTar,lRx,M,includeV)
%%UGRADIENT2D Determine the gradient of a direction cosine measurement in
%          2D with respect to 2D position. Relativity and atmospheric
%          effects are not taken into account.
%
%INPUTS: xTar The 2XN set of target position vectors in the global
%          coordinate system with [x;y] components for which a gradient is
%          desired.
%      lRx The 2X1 position vector of the receiver. If omitted, the
%          receiver is placed at the origin.
%        M A 2X2 rotation matrix from the global Coordinate system to the
%          orientation of the coordinate system at the receiver. If
%          omitted, it is assumed to be the identity matrix.
% includeV An optional boolean value indicating whether a second direction
%          cosine component should be included. The u direction cosine is
%          one parts of a 2D unit vector. The default if this parameter is
%          omitted or an empty matrix is passed is false.
%
%OUTPUTS: J A 1X2XN set of N Jacobian matrices (or a 2X2XN set if includeV
%           is true) where the row represents u (and v) and the columns
%           take the partial derivatives of the rows with respect to [x,y]
%           in that order.
%
%A derivation of the components of the Jacobian of a 3D u-v measurement is
%given in [1]. The results here are similar.
%
%EXAMPLE:
%Here, we validate the results using numerical differentiation.
% x=[1000;4];
% lRx=[-40;2];
% M=eye(2,2);
% J=uGradient2D(x,lRx,M);
% f=@(x)getUDirection2D(x,lRx,M);
% N=3;
% epsVal=[1;0.01];
% numJAug=numDiff(x,f,2,N,epsVal);
% JNumDiff=numJAug(2,:);
% RelErr=max(abs((J-JNumDiff)./JNumDiff))
%One will see that J and JNumDiff agree to more than 7 digits, indicating
%good agreeement.
%
%REFERENCES:
%[1] D. F. Crouse, "Basic tracking using nonlinear 3D monostatic and
%    bistatic measurements," IEEE Aerospace and Electronic Systems
%    Magazine, vol. 29, no. 8, Part II, pp. 4-53, Aug. 2014.
%
%April 2017 David F.Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

N=size(xTar,2);

if(nargin<4||isempty(includeV))
    includeV=false;
end

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

if(nargin<2||isempty(lRx))
   lRx=zeros(2,1);
end

J=zeros(1+includeV,2,N);
for curPoint=1:N
    %Convert the state into the local coordinate system.
    xLocal=M*(xTar(1:2,curPoint)-lRx(1:2));

    r3=norm(xLocal)^3;
    x=xLocal(1);
    y=xLocal(2);

    %The gradient of u in local coordinates.
    du=zeros(1,2);
    du(1)=y^2/r3;
    du(2)=-x*y/r3;

    J(1,:,curPoint)=du*M;

    if(includeV)
        dv=zeros(1,2);
        dv(1)=-x*y/r3;
        dv(2)=x^2/r3;
        %Now, the gradient vectors for the angular components must be
        %rotated back into the global coordinate system.
        J(2,:,curPoint)=dv*M;
    end
end
end

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


function [wRed,xRed,PRed,exitCode]=kMeansGaussReduction(wOrig,xOrig,POrig,param4,xInit,PInit,maxIter)
%%KMEANSGAUSSREDUCTION Perform Gaussian mixture reduction by iterating a k-
%              means algorithm given an inital reduced estimator (or using
%              Runnals' algorithm for the initial estimate if none is
%              given.
%
%INPUTS: wOrig An NX1 or 1XN vector of weights of the components of the
%          original Gaussian mixture.
%   muOrig An xDimXN matrix of the means of the vector components of the
%          original Gaussian mixture.
%    POrig An xDimXxDim XN hypermatrix of the covariance matrices for the
%          components of the original Gaussian mixture.
%   param4 If not initial estimate of a reduced distribution is provided,
%          then this is k, the number of components to which the
%          distribution should be reduced (and xInit and PInit are either
%          not provided or are empty matrices) and RunnalsGaussMixRed will
%          be used to obtain an initial estimate. Otherwise, if an initial
%          reduced distribution is provided, this is wInit, the kX1 or 1Xk
%          set of weights for the initial estimate of the reduced
%          distribution.
%    xInit If an initial reduced distribution is provided, this is the
%          xDimXk set of Gaussian mean vectors. Otherwise, this parameter
%          can be omitted or an empty matrix passed.
%    PInit If an initial reduced distribution is provided, this is the
%          xDimXxDimXk set of covariance matrices. Otherwise, this
%          parameter can be omitted or an empty matrix passed.
%  maxIter The maximum number of iterations of the k means algorithm to
%          perform. The default if omitted or an empty matrix is passed is
%          50.
%
%OUTPUTS: wRed The KX1 weights of the mixture after reduction.
%        muRed The xDimXK means of the mixture after reduction.
%         PRed The xDimXxDimXK covariance matrices of the mixture after
%              reduction.
%     exitCode A parameter indicating how the algorithm terminated.
%              Possible values are:
%              0 The algorithm converged.
%              1 the maximum number of iterations elapsed.
%
%This implements the algorithm of [1] and [2], which is also described in
%[3]. TGlobal convergence is not guaranteed and the performance depends a
%lot on the quality of the initial estiamte. The Kullback-Leiblber
%divergence is used as a closeness criterion between components via the
%KLDivGauss function. Additional algorithmic optimizations precomputing
%various determinants are possible beyond what was done here.
%
%EXAMPLE:
%We reduce a 10-component mixture to five components. We first plot
%Runnals' algorithm. We then use a perturbed version of the Runnals
%solution as the initial estimate to the k-means estimator and we find that
%it converges back to the Runnals solution.
% w=[0.03,0.18,0.12,0.19,0.02,0.16,0.06,0.1,0.08,0.06];
% n=length(w);
% mu=[1.45,2.20,0.67,0.48,1.49,0.91,1.01,1.42,2.77,0.89];
% P=[0.0487,0.0305,0.1171,0.0174,0.0295,0.0102, 0.0323, 0.0380, 0.0115, 0.0679];
% P=reshape(P,[1,1,n]);
%
% k=6;%Number of reduced components.
%
% %Runnal's reduction
% [wRedR,muRedR,PRedR]=RunnalsGaussMixRed(w,mu,P,k);
%
% %Perturb Runnal's solution so that the kMeansReduction has a bad
% %initialization.
% wRedRP=wRedR;
% wRedRP(1)=wRedR(1)+0.1;
% wRedRP=wRedRP/sum(wRedRP);
% muRedRP=muRedR+0.1;
% [wRedkM,muRedkM,PRedkM]=kMeansGaussReduction(w,mu,P,wRedRP,muRedRP,PRedR);
%
% numPoints=500;
% xVals=linspace(0,3,numPoints);
% PDFVals1=GaussianMixtureD.PDF(xVals,w,mu,P);
% PDFVals2=GaussianMixtureD.PDF(xVals,wRedR,muRedR,PRedR);
% PDFVals3=GaussianMixtureD.PDF(xVals,wRedRP,muRedRP,PRedR);
% PDFVals4=GaussianMixtureD.PDF(xVals,wRedkM,muRedkM,PRedkM);
% figure(1)
% clf
% hold on
% plot(xVals,PDFVals1,'-k','linewidth',4)
% plot(xVals,PDFVals2,'-r','linewidth',4)
% plot(xVals,PDFVals3,'-.c','linewidth',2)
% plot(xVals,PDFVals4,'--g','linewidth',2)
% legend('Full Mixture','Algorithm of Runnals','Initialization of k-Means','k-Means Algorithm')
% set(gca,'FontSize',14,'FontWeight','bold','FontName','Times')
%
%REFERENCES:
%[1] D. Schieferdecker and M. F. Huber, "Gaussian mixture reduction via
%    clustering," in Proceedings of the 12th International Conference on
%    Information Fusion, Seattle, WA, 6-9 Jul. 2009, pp. 1536-1543.
%[2] A. Nikseresht and M. Gelgon, "Gossip-based computation of a Gaussian
%    mixture model for distributed multimedia indexing," IEEE Transactions
%    on Multimedia, vol. 10, no. 3, pp. 385-392, Apr. 2008.
%[3] D. F. Crouse, P. Willett, K. Pattipati, and L. Svensson, "A look at
%    Gaussian mixture reduction algorithms," in Proceedings of the 14th
%    International Conference on Information Fusion, Chicago, IL, 5-8 Jul.
%    2011.
%
%May 2019 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

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

if(nargin<5||isempty(xInit))
%If no initial estimate is given but the desired number of components is
%given, then get an initial estimate using Runnall's algorithm.
    [wInit,xInit,PInit]=RunnalsGaussMixRed(wOrig,xOrig,POrig,param4);
else
    wInit=param4;
end

%The original number of mixture parameters.
N=length(wOrig);
%The reduced number of mixture parameters.
k=length(wInit);

cAssign=zeros(N,1);
cAssignNew=zeros(N,1);

%We assign the Gaussians in the full mixture to cluster centers in the
%current reduced mixture. Then, we recompute the cluster centers. These two
%steps are looped until the assignments no longer change or a maximum
%number of iterations has passed.

wRed=wInit;
xRed=xInit;
PRed=PInit;

exitCode=1;
for curIter=1:maxIter
    %For each point, find the closest center.
    for curP=1:N
        minCost=Inf;
        for curC=1:k
            cost=KLDistGauss(xOrig(:,curP),POrig(:,:,curP),xRed(:,curC),PRed(:,:,curC));
            if(cost<minCost)
                minCost=cost;
                cAssignNew(curP)=curC;
            end
        end
    end

    %Terminate when the assignments no longer change.
    if(sum(cAssignNew~=cAssign)==0)
        exitCode=0;%Convergence attained.
        break;
    end
    cAssign=cAssignNew;

    %Now, merge all points having common centers.
    [cSorted,idx]=sort(cAssign,'ascend');
    [~,numReps]=runLenEncode(cSorted);
    startIdx=1;
    for curC=1:k
        selIdx=idx(startIdx:(startIdx+numReps(curC)-1));

        wSelNorm=wOrig(selIdx);
        wSum=sum(wSelNorm);
        wSelNorm=wSelNorm/wSum;

        [xMerged,PMerged]=calcMixtureMoments(xOrig(:,selIdx),wSelNorm,POrig(:,:,selIdx));

        wRed(curC)=wSum;
        xRed(:,curC)=xMerged;
        PRed(:,:,curC)=PMerged;

        startIdx=startIdx+numReps(curC);
    end

    wRed=wRed/sum(wRed);
end
end

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


%KLI_demo

% S. de Waele, March 2003.

clear

a = 1;
b = rc2arset([1 -.3 .4]);
varx = 1;

nobs = 200;
x = gendata(a,b,nobs);

Lmax = 50;
ASAglob_subtr_mean = 0;
arh = sig2ar(x,Lmax);
varh= mean(x.^2);

%Fit of model to data
%And Auto-KLI
%These quantities should be equal according to eq. 4.14 on page 88

ar_set = ar2arset(arh,0:Lmax);
for t = 0:Lmax
    kli_hat(t+1) = KLIndex_hat(x,ar_set{t+1},1,varh);
    kli_auto(t+1) = KLIndex(ar_set{t+1},1,varh,ar_set{t+1},1,varh,nobs);
end

%Kullback-Leibler index (KLI)
%for increasing AR model order
[dummy, set_kli] = KLIndex(arh,1,1,a,b,varx,nobs);

%Kullback-Leibler Discrepancy (KLI)
%The KLD is equal to the KLI apart from a constant
[dummy, set_kld] = KLDiscrepancy(arh,1,1,a,b,varx,nobs);

%Akaike information criterion
aic = kli_hat + 2*(0:Lmax);

plot(0:Lmax,[set_kld' set_kli' kli_hat'],0:Lmax,kli_auto,'-.',0:Lmax,aic)
xlabel('p')
ylabel('KL')
legend('KLD','KLI','KLI_{hat}','KLI_{auto}','AIC')
legend boxoff

title('Fit and error and AIC for estimated AR models.')


function varargout = mean2(varargin)
%MEAN2   Mean of a CHEBFUN2.
%   V = MEAN2(F) returns the mean of a CHEBFUN2:
%
%   V = 1/A*integral2( f )
%
% 	where the A is the area of the domain of F.
%
% See also MEAN, STD2.

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

[varargout{1:nargout}] = mean2@separableApprox(varargin{:});

end


function [nodes2, edges2] = grMergeNodesMedian(nodes, edges, mnodes)
%GRMERGENODESMEDIAN Replace several nodes by their median coordinate
%
%   [NODES2, EDGES2] = grMergeNodesMedian(NODES, EDGES, NODES2MERGE)
%   NODES ans EDGES are the graph structure, and NODES2MERGE is the list of
%   indices of nodes to be merged.
%   The median coordinate of merged nodes is computed, and all nodes are
%   merged to this new node.
%
%

%   -----
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 13/08/2003.
%

%   HISTORY
%   10/02/2004 : documentation

% coordinates of reference node
x = median(nodes(mnodes, 1));
y = median(nodes(mnodes, 2));

% index of reference node
refNode = findPoint([x y], nodes);
mnodes = sort(mnodes(mnodes ~= refNode));

for n = 1:length(mnodes)
    node = mnodes(n);

    % process each neighbor of the current node
    neighbors = grAdjacentNodes(edges, node);
    for e = 1:length(neighbors)
        edge = neighbors(e);

        if edges(edge, 1) == refNode || edges(edge, 2) == refNode
            continue;
        end

        % find if the node is referenced as 1 or 2 in the edge,
        % and replace it with the reference node.
        if edges(edge, 1) == node
            edges(edge, 1) = refNode;
        else
            edges(edge, 2) = refNode;
        end

    end
end

% remove nodes from the list, except the reference node.
for n = 1:length(mnodes)
    [nodes, edges] = grRemoveNode(nodes, edges, mnodes(n)-n+1);
end

nodes2 = nodes;
edges2 = edges;


% Descriptor for simple shapes (e.g letters).
% Computes the descriptor twice in order to describe horizontally and vertically with the same accuracy.
%
% Input: image (2-D, binary or gray)
%        depth of recursion (default 7),
%        plotFlag(if on - plot Illustration)
% Output:
%        vec - descriptor: Division locations, values around zero. Of length 2*(2^depth - 2)
%        (levels: the depth of the division locations.)
%
% Shahar Armon. 3/1/2012

function  [vec levels] = hierarchicalCentroid(im, depth, plotFlag)
    if nargin < 2
        depth = 7;
        plotFlag = 0;
    elseif nargin < 3
        plotFlag = 0;
    end

    im(im < 0) = 0;

    [vec1 levels1] = hierarchicalCentroid1(im  ,depth, plotFlag);
    [vec2 levels2] = hierarchicalCentroid1(im' ,depth, 0);  % transpose iamge

    vec = [vec1, vec2];
    levels = [levels1,-levels2];  % Minus is only to mark the transpose.
end

% The descriptor. Includes Normalization.
function  [vec levels] = hierarchicalCentroid1(im, d, plotFlag)

    p = hierarchicalCentroidRec(im, 1, d);

    if plotFlag  % Illustration
        showLines(im, p);
    end

    meany = ([1:size(im,1)]*sum(im,2))/sum(sum(im));
    indVer = (mod(p(:,2),2) == 1);
    % Normalization for location:
    p(indVer,1) = p(indVer,1) - p(1,1);
    p(~indVer,1) = p(~indVer,1) - meany;

    % Normalization for size (keeping aspect ratio):
    p(:,1) = p(:,1)/size(im,1);
    % Normalization for size:
    % p(indVer,1) = p(indVer,1)/size(im,2);
    % p(~indVer,1) = p(~indVer,1)/size(im,1);

    % Illustration of the lines after normalization without the image
    %     showLines([], p);

    vec = p(2:end,1)';
    levels = p(2:end,2)';
end

function p = hierarchicalCentroidRec(im, depth, maxDepth)
    if depth > maxDepth
        p = [];
    else
        area = sum(sum(im));
        [rows,cols] = size(im);
        % compute the centroid-x
        if cols == 1
            centroid = 0.5;
        elseif area == 0
            centroid = cols/2;
        elseif rows == 1
            centroid = (im*[1:cols]')/area;
        else
            centroid = sum(im)*[1:cols]'/area;
        end

        leftIm = im(:,1:floor(centroid));
        rightIm = im(:,ceil(centroid):end);

        pLeft  = hierarchicalCentroidRec(leftIm'  , depth+1, maxDepth);
        pRight = hierarchicalCentroidRec(rightIm' , depth+1, maxDepth);

        %  Updates the distances so they will be in relation to the complete image
        if size(pRight,1) > 1
            ind = find((1-mod(depth,2))-mod(pRight(2:end,2),2)) + 1;
            pRight(ind,1) = pRight(ind,1) + ceil(centroid) - 1;
            pRight(1,1) = pRight(1,1) - 1;
        end

        p = [centroid, depth; pLeft; pRight];
    end
end

function showLines(im, p)
    if max(p(:,1))<3
        hold on;
        axis([-1 1 -1 1])
        axis ij;
        temp = mod(p(:,2),2);
        indVer = find(temp);
        indHor = find(~temp);
        showLinesRec(min(p(indVer,1)),max(p(indVer,1)),min(p(indHor,1)),max(p(indHor,1)), p);
    else
        imshow(im);
        hold on;
        showLinesRec(0,size(im,2),0,size(im,1), p);
    end
    hold off
end

function p = showLinesRec(x1,x2,y1,y2, p)
    p1 = p(1,:);
    w = 1+max(p(:,2))-p1(1,2);
    t = p1(1);
    p = p(2:end,:);
    if mod(p1(1,2),2)
        plot([t t],[y1 y2],'-m','LineWidth',w);
    else
        plot([x1 x2],[t t],'-r','LineWidth',w);
    end
    if (p1(1,2) < max(p(:,2)))
        if mod(p1(1,2),2)
            p = showLinesRec(x1,t,y1,y2, p);
            p = showLinesRec(t,x2,y1,y2, p);
        else
            p = showLinesRec(x1,x2,y1,t, p);
            p = showLinesRec(x1,x2,t,y2, p);
        end
    end

end


function y = scale_cols(x, s)
% SCALE_COLS       Scale each column of a matrix.
% SCALE_COLS(x,s) returns matrix y, same size as x, such that
% y(:,i) = x(:,i)*s(i)
% It is more efficient than x*diag(s), but consumes a similar amount of memory.
% Warning: It consumes a lot of memory when x is sparse.

y = x.*repmat(s(:).', rows(x), 1);
%y = x.*(ones(rows(x),1)*s(:)');


%  Figure 7.39      Feedback Control of Dynamic Systems, 5e
%                        Franklin, Powell, Emami
%
% script to generate Fig. 7.39
clf;
np=1;
dp=[1 0 0];
nc=[1 0.619];
dc=[1 6.41];
num=conv(np,nc);
den=conv(dp,dc);
rlocus(num,den);
axis([-10 2 -4.5 4.5]);
grid;
title('Fig.7.39  Root locus for reduced-order controller for 1/s^2');


function uDot=uDotEllipsoid(u,x,a,f)
%%UDOTELLIPSOID Return the derivative of the basis vectors for the local
%               coordinate system of a moving observer when using an
%               ellipsoidal approximation of the curved Earth. Such an
%               evolving coordinate system has been termed "wander
%               coordinate" or "naturally evolving coordinates". Targets
%               moving locally in non-maneuvering, level flight with
%               a coordinate system evolving as per this coordinate system
%               will follow geodesic curves, not rhumb lines.
%
%INPUTS: u The 3X3 orthonormal basis vectors for the local coordinate
%          system. u(:,3) is the local vertical.
%        x An nX1 vector whose first three elements are Cartesian position
%          in the global ECEF coordinate system and whose next three
%          elements are velocity in the local (flat-Earth) coordinate
%          system. Other elements of x do not matter.
%        a The semi-major axis of the reference ellipsoid. If this argument
%          is omitted, the value in Constants.WGS84SemiMajorAxis is used.
%        f The flattening factor of the reference ellipsoid. If this
%          argument is omitted, the value in Constants.WGS84Flattening is
%          used.
%
%OUTPUTS: uDot The derivative of the basis vector u with respect to time.
%
%The solution is detailed in [1], and has been simplified for an
%ellipsoidal Earth so that numerical differentiation is not necessary.
%
%REFERENCES:
%[1] D. F. Crouse, "Simulating aerial targets in 3D accounting for the
%    Earth's curvature," Journal of Advances in Information Fusion, vol.
%    10, no. 1, Jun. 2015.
%
%September 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

    if(nargin<4||isempty(f))
        f=Constants.WGS84Flattening;
    end

    if(nargin<3||isempty(a))
        a=Constants.WGS84SemiMajorAxis;
    end

    %The first numerical eccentricity of the ellipsoid.
    e=sqrt(2*f-f^2);

    %The current position.
    r=x(1:3,1);
    %The local velocity.
    rDotLocal=x(4:6,1);

    %Get the velocity in global coordinates.
    rDot=getGlobalVectors(rDotLocal,u);

    %Convert to ellipsoidal coordinates.
    rEllipse=Cart2Ellipse(r,[],a,f);

    %The ellipsoidal latitude
    phi=rEllipse(1);

    %The height above the reference ellipsoid.
    h=rEllipse(3);

    %Obtain the local East-North-Up axes.
    ENUAxes=getENUAxes(rEllipse,false,a,f);
    EAxis=ENUAxes(:,1);
    NAxis=ENUAxes(:,2);

    %The local North and East components of the velocity vector.
    vE=dot(EAxis,rDot(1:3,1));
    vN=dot(NAxis,rDot(1:3,1));

    %The normal radius of curvature.
    Rp=a/sqrt(1-e^2*sin(phi)^2);

    %The radius of curvature of the meridian line.
    Rm=Rp*(1-e^2)/(1-e^2*sin(phi)^2);

    %The rotation rate North.
    omegaN=vE/(Rp+h);

    %The rotation rate East.
    omegaE=-vN/(Rm+h);

    %The 3D rotation vector.
    Omega=omegaN*NAxis+omegaE*EAxis;

    %Compute the derivatives of the coordinate system basis vectors with
    %respect to time.
    uDot(:,1)=cross(Omega,u(:,1));
    uDot(:,2)=cross(Omega,u(:,2));
    uDot(:,3)=cross(Omega,u(:,3));
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 pre = newfft_exact_p(st, om)
%function pre = newfft_exact_p(st, om)
%|
%| precomputed (large) matrix for exact forward NUFFT

if ~isvar('om') || isempty(om)
	om = st.om; % default
end
if isempty(om), fail 'om or st.om required', end

dd = st.dd;
Nd = st.Nd;
n_shift = st.n_shift;

if dd > 3
	fail 'only up to 3D is done'
else
	Nd = [Nd(:); ones(3-length(Nd),1)];
	n_shift = [n_shift(:); zeros(3-length(n_shift),1)];
end

for id=1:3 % fix: dd
	nn{id} = [0:(Nd(id)-1)]-n_shift(id);
end

[nn{1}, nn{2}, nn{3}] = ndgrid(nn{1}, nn{2}, nn{3});

pre = 0;
for id=1:dd
	pre = pre + om(:,id) * col(nn{id})';
end
pre = exp(-1i*pre);


function r8vec_variance_test ( )

%*****************************************************************************80
%
%% R8VEC_VARIANCE_TEST tests R8VEC_VARIANCE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 March 2015
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'R8VEC_VARIANCE_TEST\n' );
  fprintf ( 1, '  R8VEC_VARIANCE computes the variance of an R8VEC.\n' );;

  n = 10;
  r8_lo = -5.0;
  r8_hi = +5.0;
  seed = 123456789;
  [ a, seed ] = r8vec_uniform_ab ( n, r8_lo, r8_hi, seed );

  r8vec_print ( n, a, '  Input vector:' );

  variance = r8vec_variance ( n, a );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Variance:   %f\n', variance );

  return
end


function C = tolUnique(A, tol)
%TOLUNIQUE   UNIQUE with a tolerance for checking floating-point equality.
%   C = TOLUNIQUE(A, TOL) for a real vector A is the same as UNIQUE(A) except
%   that two values are deemed "equal" if they are within an absolute
%   difference TOL of one another.  If this happens, the mean of the two values
%   is placed in C.
%
%   C = TOLUNIQUE(A) uses a default tolerance of 100*EPS*MAX(NORM(A, Inf)).

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

if ( nargin < 3 )
    tol = 100*eps*max(norm(A, Inf));
end

% TODO:  This might not do the right thing if there are multiple values that
% are all close to one another, say, 2, 2 + 2*eps, 2 + 4*eps, and 2 + 6*eps,
% and tol = 3*eps.  This code will merge all of those into one value:  2 + eps
% (the average of 2 and 2 + 2*eps), and one can argue that behavior is wrong.
% We can resolve this if this ever becomes an issue in practice.
C = unique(A);
ind = find(diff(C) < tol);
C(ind) = (C(ind) + C(ind+1))/2;
C(ind+1) = [];

end


function x = cg_ge ( n, a, b, x )

%*****************************************************************************80
%
%% CG_GE uses the conjugate gradient method for a general storage matrix.
%
%  Discussion:
%
%    The linear system has the form A*x=b, where A is a positive-definite
%    symmetric matrix, stored as a full storage matrix.
%
%    The method is designed to reach the solution to the linear system
%      A * x = b
%    after N computational steps.  However, roundoff may introduce
%    unacceptably large errors for some problems.  In such a case,
%    calling the routine a second time, using the current solution estimate
%    as the new starting guess, should result in improved results.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 June 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Frank Beckman,
%    The Solution of Linear Equations by the Conjugate Gradient Method,
%    in Mathematical Methods for Digital Computers,
%    edited by John Ralston, Herbert Wilf,
%    Wiley, 1967,
%    ISBN: 0471706892,
%    LC: QA76.5.R3.
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, real A(N,N), the matrix.
%
%    Input, real B(N), the right hand side vector.
%
%    Input/output, real X(N).
%    On input, an estimate for the solution, which may be 0.
%    On output, the approximate solution vector.
%

%
%  Initialize
%    AP = A * x,
%    R  = b - A * x,
%    P  = b - A * x.
%
  ap(1:n,1) = a * x;

  r(1:n,1) = b(1:n,1) - ap(1:n,1);
  p(1:n,1) = b(1:n,1) - ap(1:n,1);
%
%  Do the N steps of the conjugate gradient method.
%
  for it = 1 : n
%
%  Compute the matrix*vector product AP = A*P.
%
    ap(1:n,1) = a * p;
%
%  Compute the dot products
%    PAP = P*AP,
%    PR  = P*R
%  Set
%    ALPHA = PR / PAP.
%
    pap = p(1:n,1)' * ap(1:n,1);
    pr =  p(1:n,1)' * r(1:n,1);

    if ( pap == 0.0 )
      return;
    end

    alpha = pr / pap;
%
%  Set
%    X = X + ALPHA * P
%    R = R - ALPHA * AP.
%
    x(1:n,1) = x(1:n,1) + alpha * p(1:n,1);
    r(1:n,1) = r(1:n,1) - alpha * ap(1:n,1);
%
%  Compute the vector dot product
%    RAP = R*AP
%  Set
%    BETA = - RAP / PAP.
%
    rap = r(1:n,1)' * ap(1:n,1);

    beta = - rap / pap;
%
%  Update the perturbation vector
%    P = R + BETA * P.
%
    p(1:n,1) = r(1:n,1) + beta * p(1:n,1);

  end

  return
end


function [ n_data, z, s, a, fx ] = lerch_values ( n_data )

%*****************************************************************************80
%
%% LERCH_VALUES returns some values of the Lerch transcendent function.
%
%  Discussion:
%
%    The Lerch function is defined as
%
%      Phi(z,s,a) = Sum ( 0 <= k < Infinity ) z^k / ( a + k )^s
%
%    omitting any terms with ( a + k ) = 0.
%
%    In Mathematica, the function can be evaluated by:
%
%      LerchPhi[z,s,a]
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Stephen Wolfram,
%    The Mathematica Book,
%    Fourth Edition,
%    Wolfram Media / Cambridge University Press, 1999.
%
%  Parameters:
%
%    Input/output, integer N_DATA.  The user sets N_DATA to 0 before the
%    first call.  On each call, the routine increments N_DATA by 1, and
%    returns the corresponding data; when there is no more data, the
%    output value of N_DATA will be 0 again.
%
%    Output, real Z, the parameters of the function.
%
%    Output, integer S, the parameters of the function.
%
%    Output, real A, the parameters of the function.
%
%    Output, real FX, the value of the function.
%
  n_max = 12;

  a_vec = [ ...
     0.0, ...
     0.0, ...
     0.0, ...
     1.0, ...
     1.0, ...
     1.0, ...
     2.0, ...
     2.0, ...
     2.0, ...
     3.0, ...
     3.0, ...
     3.0 ];

  fx_vec = [ ...
     0.1644934066848226E+01, ...
     0.1202056903159594E+01, ...
     0.1000994575127818E+01, ...
     0.1164481052930025E+01, ...
     0.1074426387216080E+01, ...
     0.1000492641212014E+01, ...
     0.2959190697935714E+00, ...
     0.1394507503935608E+00, ...
     0.9823175058446061E-03, ...
     0.1177910993911311E+00, ...
     0.3868447922298962E-01, ...
     0.1703149614186634E-04 ];

  s_vec = [ ...
     2, 3, 10, ...
     2, 3, 10, ...
     2, 3, 10, ...
     2, 3, 10 ];

  z_vec = [ ...
     0.1000000000000000E+01, ...
     0.1000000000000000E+01, ...
     0.1000000000000000E+01, ...
     0.5000000000000000E+00, ...
     0.5000000000000000E+00, ...
     0.5000000000000000E+00, ...
     0.3333333333333333E+00, ...
     0.3333333333333333E+00, ...
     0.3333333333333333E+00, ...
     0.1000000000000000E+00, ...
     0.1000000000000000E+00, ...
     0.1000000000000000E+00 ];

  if ( n_data < 0 )
    n_data = 0;
  end

  n_data = n_data + 1;

  if ( n_max < n_data )
    n_data = 0;
    z = 0.0;
    s = 0;
    a = 0.0;
    fx = 0.0;
  else
    z = z_vec(n_data);
    s = s_vec(n_data);
    a = a_vec(n_data);
    fx = fx_vec(n_data);
  end

  return
end


function A_com = extract_patch(A,dims,patch_size,padding)

% Extractx a patch of size patch_size centered around the centroid of each
% component.
% INPUTS:
% A:            2d matrix of spatial components
% dims:         dimensions of FOV
% patch_size:   dimensions of patch
% padding:      if true components remain centered and are zero padded,
%               otherwise they are shifted (default: true)

% OUTPUT:
% A_com:            Nd matrix of patches

if ~exist('padding','var'); padding = true; end

nd = length(dims);
if nd == 2; dims(3) = 1; patch_size(3) = 1; end
K = size(A,2);
A = A/spdiags(sqrt(sum(A.^2,1))'+eps,0,K,K);      % normalize to sum 1 for each compoennt
cm = com(A,dims(1),dims(2),dims(3));
xx = -ceil(patch_size(1)/2-1):floor(patch_size(1)/2);
yy = -ceil(patch_size(2)/2-1):floor(patch_size(2)/2);
zz = -ceil(patch_size(3)/2-1):floor(patch_size(3)/2);
A_com = zeros([patch_size,K]);

for i = 1:K
    int_x = round(cm(i,1)) + xx;
    pad_pre_x = 0;
    pad_pre_y = 0;
    pad_post_x = 0;
    pad_post_y = 0;
    if int_x(1)<1
        if padding
            pad_pre_x = 1 - int_x(1);
            int_x = 1:int_x(end);
        else
            int_x= int_x + 1 - int_x(1);
        end
    end
    if int_x(end)>dims(1)
        if padding
            pad_post_x = int_x(end) - dims(1);
            int_x = int_x(1):dims(1);
        else
            int_x = int_x - (int_x(end)-dims(1));
        end
    end
    int_y = round(cm(i,2)) + yy;
    if int_y(1)<1
        if padding
            pad_pre_y = 1 - int_y(1);
            int_y = 1:int_y(end);
        else
            int_y = int_y + 1 - int_y(1);
        end
    end
    if int_y(end)>dims(2)
        if padding
            pad_post_y = int_y(end) - dims(2);
            int_y = int_y(1):dims(2);
        else
            int_y = int_y - (int_y(end)-dims(2));
        end
    end
    if nd == 3
        int_z = round(cm(i,3)) + zz;
        if int_z(1)<1
            int_z = int_z + 1 - int_z(1);
        end
        if int_z(end)>dims(3)
            int_z = int_z - (int_z(end)-dims(3));
        end
    else
        int_z = 1;
    end
    A_temp = reshape(full(A(:,i)),dims);
    A_temp = A_temp(int_x,int_y,int_z);
    if padding
        A_temp = padarray(A_temp,[pad_pre_x,pad_pre_y],0,'pre');
        A_temp = padarray(A_temp,[pad_post_x,pad_post_y],0,'post');
    end
    A_com(:,:,i) = A_temp;
end


%CYLGRID Generate 3d cylindrical hexahedral grid.
%
%   [ GRID ] = CYLGRID( NS, NR, NZ, R, LZ, XP, AX ) Generates a hexahedral
%   grid for a cylindrical domain with NS+NR cells in the radial direction.
%   NS specifies the cell resolution of the inner square (default 4), and NR
%   ns number of cells in the radial direction of the outer layer (default 3).
%   NZ specity the numver of cells in the lengthwise direction and LZ the
%   corresponding length. The optional arguments R, XP, and AX specify the
%   radius, center coordinates of the cylinder (default R = 1 and XP = [0;0;0]),
%   and the axis of alignment (default 1 equals to the x-axis).
%
%   Examples:
%
%      1) A cylindrical grid with radius 1 and length 1:
%
%      grid = cylgrid();
%
%      2) Cylinder with radius 0.5 length 2 centered and extending
%         in the negative y-direction from the point [1 1 2].
%
%      grid = cylgrid( 3, 4, 10, 0.5, 2, [1;1;2], -2 );
%
%   See also BLOCKGRID, CIRCGRID, HOLEGRID, LINEGRID, RECTGRID, RINGGRID, SPHEREGRID

% Copyright 2013-2022 Precise Simulation, Ltd.


function x = r8lt_sl ( n, a, b, job )

%*****************************************************************************80
%
%% R8LT_SL solves a R8LT system.
%
%  Discussion:
%
%    The R8LT storage format is used for an M by N lower triangular matrix,
%    and sets aside storage even for the entries that must be zero.
%
%    No factorization of the lower triangular matrix is required.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    19 March 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Input, real A(N,N), the R8LT matrix.
%
%    Input, real B(N), the right hand side.
%    On output, the solution vector.
%
%    Input, integer JOB, is 0 to solve the untransposed system,
%    nonzero to solve the transposed system.
%
%    Output, real X(N), the solution vector.
%
  x(1:n) = b(1:n);

  if ( job == 0 )

    for j = 1 : n
      x(j) = x(j) / a(j,j);
      x(j+1:n) = x(j+1:n) - a(j+1:n,j)' * x(j);
    end

  else

    for j = n : -1 : 1
      x(j) = x(j) / a(j,j);
      x(1:j-1) = x(1:j-1) - a(j,1:j-1) * x(j);
    end

  end

  return
end


function [Y, X] = minandmax2( f )
%MINANDMAX2   Find global minimum and maximum of a SPHEREFUN.
%   M = minandmax2(F) returns the minimum and maximum value of a SPHEREFUN
%   over its domain. M is a vector of length 2 such that
%   M(1) = min(f(lambda,theta)) and M(2) = max(f(lambda,theta)).
%
%   [M, LOC] = minandmax2(F) also returns the position of the minimum and
%   maximum. For example,
%
%       F(LOC(1,1),LOC(1,2)) = M(1)  and  F(LOC(2,1),LOC(2,2)) = M(2)
%
% See also SPHEREFUN/MAX2, SPHEREFUN/MIN2, SPHEREFUN/NORM.

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

% check for empty SPHEREFUN.
if ( isempty( f ) )
    Y = [];
    X = [];
    return
end

[C, D, R] = cdr(f);
f = C*D*R.';
% Maximum possible sample matrix size:
maxsize = 4e3;

% Is the function the zero function?
if ( iszero( f )  )
    dom = f.domain;
    X = [ (dom(2) + dom(1))/2 (dom(4) + dom(3))/2 ];
    X = [ X ; X ];
    Y = [0 ; 0];
    return;
end

% Extract low rank representation:
frows = f.rows;
fcols = f.cols;
piv = f.pivotValues;
dom = f.domain;

% Share out scaling:
sgn = sign( piv ).';
sq = 1 ./ sqrt( abs( piv ) );
frows = frows * diag( sq.'.*sgn );
fcols = fcols * diag( sq );

if ( length(f) == 1 ) % Rank-1 is easy:
    % We can find it from taking maximum and minimum in x and y direction.

    % Find minandmax of rows and columns:
    [yr, xr] = minandmax( frows );
    [yc, xc] = minandmax( fcols );
    % All possible combinations:
    vv = [yr(1)*yc(1), yr(1)*yc(2), yr(2)*yc(1), yr(2)*yc(2)];

    [Y(2), indmx] = max( vv );
    [Y(1), indmn] = min( vv );

    % Work out the location of the maximum.
    X = zeros(2);
    X(1,1) = xr(2);
    X(1,2) = xc(2);

    if ( indmn <= 2 )
        X(1,1) = xr(1);
    end
    if ( mod(indmn,2) == 1 )
        X(1,2) = xc(1);
    end
    X(2,1) = xr(2);
    X(2,2) = xc(2);

    if ( indmx <= 2 )
        X(2,1) = xr(1);
    end

    if ( mod(indmx,2) == 1 ),
        X(2,2) = xc(1);
    end

elseif ( length(f) <= maxsize )

    % We seek a fast initial guess. So we first truncate the SPHEREFUN.
    ypts = [trigpts(length(fcols), fcols.domain);pi];
    xpts = [trigpts(length(frows), frows.domain);pi];
    cvals = feval(fcols, ypts);
    rvals = feval(frows, xpts);

    A = cvals*rvals.';
    % Maximum entry in discretisation.
    [ignored, ind] = min( A(:) ); %#ok<ASGLU>
    [row, col] = ind2sub(size(A), ind);
    X(1,1) = xpts( col );
    X(1,2) = ypts( row );
    Y(1) = feval( f, X(1,1), X(1,2) );
    % Minimum entry in discretisation.
    [ignored, ind] = max(A(:)); %#ok<ASGLU>
    [row, col] = ind2sub(size(A), ind);
    X(2,1) = xpts(col);
    X(2,2) = ypts(row);
    Y(2) = feval(f, X(2,1), X(2,2));

    % Get more digits with optimisation algorithms.
    lb = [ dom(1) ; dom(3) ];
    ub = [ dom(2) ; dom(4) ];

    try

        % If the optimization toolbox is available then use it to get a better maximum.

        warnstate = warning;
        warning('off'); %#ok<WNOFF> % Disable verbose warnings from fmincon.
        options = optimset('Display', 'none', 'TolFun', eps, 'TolX', eps, ...
            'algorithm', 'active-set');
        [mn, Y(1)] = fmincon(@(x,y) feval(f, x(1), x(2)), X(1, :), ...
            [], [], [], [], lb, ub, [], options);
        [mx, Y(2)] = fmincon(@(x) -feval(f, x(1), x(2)), X(2,:), ...
            [], [], [], [], lb, ub, [], options);
        Y(2) = -Y(2);
        X(1,:) = mn;
        X(2,:) = mx;
        warning(warnstate);

    catch

        try
            % Try converting to an unconstrained problem and using built-in solver.

            % Maps from [-1, 1] to [dom(1:2)] and [dom(3:4)], respectively.
            map1 = bndfun.createMap(dom(1:2));
            map2 = bndfun.createMap(dom(3:4));
            % Unconstrained initial guesses:
            Z(:,1) = asin(map1.Inv(X(:,1)));
            Z(:,2) = asin(map2.Inv(X(:,2)));
            % Maps from R to [dom(1), dom(2)] and [dom(3), dom(4)], respectively.
            map1 = @(x) map1.For(sin(x));
            map2 = @(x) map2.For(sin(x));
            % Set options:
            options = optimset('Display', 'off', 'TolFun', eps, 'TolX', eps);
            warnstate = warning;
            warning('off'); %#ok<WNOFF> % Disable verbose warnings from fminsearch.
            f_mapped = @(x) feval(f, map1(x(1)), map2(x(2)));
            [mn, Y(1)] = fminsearch(@(x) f_mapped(x), Z(1, :), options);
            [mx, Y(2)] = fminsearch(@(x) -f_mapped(x), Z(2, :), options);
            Y(2) = -Y(2);
            X(1:2,1) = map1([mn(1) ; mx(1)]);
            X(1:2,2) = map2([mn(2) ; mx(2)]);
            warning(warnstate);

        catch

            % Nothing is going to work so initial guesses will have to do.

        end

    end

elseif ( length(f) >= maxsize )

    error('CHEBFUN:SPHEREFUN:minandmax2:length', 'Rank is too large.');

end

% If the location of the max/min is outside of [-pi,pi]^2, which can happen
% because Spherefun is based on the double Fourier sphere method, then
% translate back to [0,pi]x[-pi,pi] using BMC symmetry:

% check minimum:
if ( X(1,1) < 0 )
    X(1,1) = X(1,1)+pi;
    X(1,2) = -X(1,2);
end

% check maximum:
if ( X(2,1) < 0 )
    X(2,1) = X(2,1)+pi;
    X(2,2) = -X(2,2);
end

end

%%%
% Use the approach below when bivariate rootfinding is fully implemented.
%%%
% % Use bivariate rootfinding to find all the local extrema:
% F = gradient( f );
% r = roots( F );
% if ( ~isempty( r ) )
%     [inMax, idInMax] = max( feval(f, r(:,1), r(:,2) ) );
%     [inMin, idInMin] = min( feval(f, r(:,2), r(:,2) ) );
% else
%     inMax = inf;   % max and min must occur on boundary.
%     inMin = inf;
% end
%
% % Search along boundary:
% dom = f.domain;
% left = feval(f, dom(1), ':');
% right = feval(f, dom(2), ':');
% down = feval(f, ':', dom(3));
% up = feval(f, ':', dom(4));
% [Yleft, Xleft] = minandmax( left );
% [Yright, Xright] = minandmax( right );
% [Yup, Xup] = minandmax( up );
% [Ydown, Xdown] = minandmax( down );
%
% % Store Min/Max location for later:
% BcMinLocations = [ Xleft(1,:) ; Xright(1,:) ; Xup(1,:) ; Xdown(1,:) ];
% BcMaxLocations = [ Xleft(2,:) ; Xright(2,:) ; Xup(2,:) ; Xdown(2,:) ];
%
% [ BcMax, idBcMax ] = max( [ Yleft(2), Yright(2), Yup(2), Ydown(2) ].' );
% [ BcMin, idBcMin ] = min( [ Yleft(1), Yright(1), Yup(1), Ydown(1) ].' );
%
% % What is the global min and max?:
% [Ymax, inOrOutMax] = max( [inMax, BcMax].' );
% [Ymin, inOrOutMin] = min( [inMin, BcMin].' );
% Y = [Ymin, Ymax];
% X = zeros(2);
%
% % Unravel to find locations:
% if ( inOrOutMin == 1 )
%     X(1,:) = [ r(idInMin,1), r(idInMin,2) ];
% else
%     X(1,:) = BcMinLocations(idBcMin, :);
% end
% if ( inOrOutMax == 1 )
%     X(2,:) = [ r(idInMax,1), r(idInMax,2) ];
% else
%     X(2,:) = BcMaxLocations(idBcMax, :);
% end
%
% end


function haar_test03 ( )

%*****************************************************************************80
%
%% HAAR_TEST03 tests HAAR_2D and HAAR_2D_INVERSE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 May 2013
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'HAAR_TEST03\n' );
  fprintf ( 1, '  HAAR_2D computes the Haar transform of an array.\n' );
  fprintf ( 1, '  HAAR_2D_INVERSE inverts the transform.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Apply this to a 128x128 matrix of 0 and 1 values\n' );
  fprintf ( 1, '  which is actually a bit map of the Sierpinski triangle.\n' );
%
%  Load data from a file.
%
  u = load ( 'sierpinski.txt' );

  [ m, n ] = size ( u );
%
%  Demonstrate successful inversion.
%
  r8mat_print_some ( m, n, u, 1, 1, 10, 10, '  Input array U:' );

  v = haar_2d ( u );

  r8mat_print_some ( m, n, v, 1, 1, 10, 10, '  Transformed array V:' );

  w = haar_2d_inverse ( v );

  r8mat_print_some ( m, n, w, 1, 1, 10, 10, '  Recovered array W:' );

  return
end


%TEST 04: Algebraic Reconstruction Techniques
clear
clc
fprintf('TEST 04: Algebraic Reconstruction Techniques\rCompare ART and SART\r');
angles = 0:6:179;
%Phantom
im = imtest('s4',64);
siz = size(im);
figure('name','Original Image: Phantom')
imshow(im)
[W, p, ~, ~] = buildWeightMatrix(im,angles);
%Iteration 100
fprintf('ART method\niteration 100:\r')
im_rec1 = tomo_recon_myart(W,p,siz,100);
im_rec1 = uint8(imscale(im_rec1));
figure('name','ART')
imshow(im_rec1);
fprintf('SART method\niteration 100:\r')
im_rec2 = tomo_recon_sart(W,p,siz,100);
im_rec2 = uint8(imscale(im_rec2));
figure('name','SART')
imshow(im_rec2);
%Iteration 1000
fprintf('ART method\niteration 1000:\r')
im_rec1 = tomo_recon_myart(W,p,siz,1000);
im_rec1 = uint8(imscale(im_rec1));
figure('name','ART')
imshow(im_rec1);
fprintf('SART method\niteration 1000:\r')
%Code efficiency is too low!
%im_rec2 = tomo_recon_mysart(W,p,siz,1000);
%figure('name','ART')
%imshow(im_rec2);


%MAIN_fsm

clc; clear;

%%%% Derive equations of motion %%%%
% EoM_flight();
% EoM_slide();
% EoM_hinge();

%%%% Interesting Parameters %%%%
MoI = 0.333;  %Dimensionless moment of inertia
    % 0   => Point mass at center of rod
    % 1/3 => Slender rod (uniform distribution)
    % 1   => Point mass at both ends of the rod
CoeffFriction = 0.2;
%%%% ~~~~~~~~~~~~~~~~~~~~~~ %%%%

P.g = 9.81;
P.L = 1;  %Distance between END OF STICK AND CENTER OF MASS
P.FullLength = 2*P.L;   %Length of the entire stick
P.m = 1;
P.u = CoeffFriction;   %Coefficient of friction
P.I = MoI*P.m*P.L^2;  %Theoretical Maximum - two point masses
setup.P = P;

setup.IC.th = 1e-2;
setup.IC.x = 0;
setup.IC.y = 0;
setup.IC.dth = topple_angularRate(setup.IC.th,P);
setup.IC.dx = 0;
setup.IC.dy = 0;

setup.Tspan = [0,10];  %only used for timeout of the simulation
setup.tol = 1e-12;   %Accuracy of the intergation method
setup.dataFreq = 750;   %How much data to return?
setup.solver = @ode45; %Integration method
setup.odeMaxStep = 1e-1;  %Biggest allowable step

D = runSimulation(setup);

plotData(D);

for i=1:length(D.phase)
    disp([D.phase{i} '  ->  ' D.code{i}])
end

timeRate = 0.5;   % slow motion < 1 < fast forward
animation(D,timeRate);


% Fig. 6.39   Feedback Control of Dynamic Systems, 5e
%             Franklin, Powell, Emami
%

clear all;
close all;

num=[1 1 1];
den=[1 10 24 0 0 0];
rlocus(num,den);
grid;
title('Fig. 6.39 Root Locus of a conditionally stable system');


function outvec = SHSetValue(invec,value,l,m,N,lmax,both)

% outvec = SHSetValue(invec,value,l,m[,N,lmax][,both])
%
% Sets the spherical harmonic coefficient vector entry corresponding
% to degree l and order m to the specified value. If optional arguments
% N and lmax are specified, invec is treated as a set of sections as
% described by lmax, and the value corresponding to sections N is set.
% If lmax is specified, it must be a vector describing the spherical
% harmonics degrees lmax for each of the sections 1 .. length(lmax).
% If 'both' is given, set both positive and negative order coefficients.

outvec = invec;

if l<0
    error('invalid usage: l has to be a non-negative integer');
end

if m>l
    error('invalid usage: l has to be greater or equal to m');
end

if nargin < 5
    N=1;
end

n=zeros(length(N),1);

if nargin == 5
    error('please provide sixth argument: array of lmax values');
elseif nargin > 5
    if find(N>length(lmax))>0 | find(N<1)>0 %#ok<OR2>
        error('unable to set the value since section N is not present');
    elseif find(lmax(N)<l)
        error('unable to set the values for l,m: degree is less than l');
    else
        for j=1:length(N)
            for i=1:N(j)-1
                n(j) = n(j) + SHl2n(lmax(i));
            end
        end
    end
end

n = n + SHlm2n(l,m);

if n>length(invec)
    error('invalid usage: index %d exceeds the length of the vector',n);
end

outvec(n)=value;

if nargin <= 6
    return;
end

n = n - SHlm2n(l,m) + SHlm2n(l,-m);

outvec(n)=value;


% this function compute the spatial covariance matrix from complex Fourier
% transform of array signals.
% Inputs:
%   X: D x N x T matrix of Fourier transform coefficients. D is the number
%   of frequency bins, N is the number of microphone channels, and T is the
%   number of frames
%   context_size: number of context frames used to compute the spatial
%   covariance at each time-frequency bin.
% Outputs:
%   R: N x N x D x T matrix of spatial covariance matrixes
%
% Author: Xiong Xiao, Nanyang Technological University, Singapore
% Last Modified: 22 Feb 2016.
%
function R = ComplexSpectrum2SpatialCov(X, context_size, shift, useGPU)
if nargin<4
    useGPU = 1;
end

half_ctx = (context_size-1)/2;
[D, N, T] = size(X);    % D is number of freq bin, N is number of channel

if context_size == 0    % get global spatial covariance matrix
    X2 = permute(X, [2 1 3]);
    XX = outProdND(X2);
    R = mean(XX,4);
else    % get windowed spatial covariance
    if 0
        nBlock = length(1:shift:T);
        R = zeros(N,N,D,nBlock);
        for d = 1:D
            for t = 1:shift:T
                frame_selector = t-half_ctx:t+half_ctx;
                frame_selector = min(T,max(1,frame_selector));
                X2 = squeeze(X(d,:,frame_selector));
                R(:,:,d,(t-1)/shift+1) = X2*X2' / context_size;
            end
        end
    else
        X2 = permute(X, [2 1 3]);
        XX = outProdND(X2);
%         X2cell = num2cell(X2, [1]);
%         XXcell = cellfun(@(x) (reshape(x*x', N^2,1)), X2cell, 'UniformOutput', 0);
%         XX = cell2mat(XXcell);
        XX2 = reshape(XX, N^2*D, T);
        if 1
            if useGPU
                XX2 = gpuArray(XX2);
            end
            idx = [ones(1,half_ctx) 1:T ones(1,half_ctx)*T];
            SCM = conv2(XX2(:,idx), ones(1,context_size, class(gather(X)))/context_size, 'valid');
%             SCM = SCM(:,half_ctx+1:end-half_ctx);
        else
            fake_layer.a = XX2;
            XX3 = F_splice(fake_layer, context_size);
            XX4 = reshape(XX3,  N^2*D, context_size, T);
            SCM = mean(XX4,2);
        end
        SCM2 = reshape(SCM, N^2, D, T);
        SCM3 = reshape(SCM2, N, N, D, T);
        R = SCM3(:,:,:,1:shift:end);
    end
end


function [kHz] = GHz2kHz(GHz)
% Convert frequency from gigahertz to kilohertz.
% Chad A. Greene 2012
kHz = GHz*1e+6;


function out = upSample(im, nLevels, filt)
% out = upSample(im, nLevels, [filt])
%
% Upsamples the image im by the integer nLevels.
% The upsampled image is blurred and edges are
% dealt with by zero-padding.
%
% The blurring is done with the filter kernel specified by filt
% (default = [.125 .5 .75 .5 .125]'), which should be a vector
% (applied separably as a 1D convolution kernel in X and Y),
% or a matrix (applied as a 2D convolution kernel)
%
% 99.08.16 RFD wrote it, based on upBlur (and helpful
%				comments from DJH)

if nLevels ~= fix(nLevels)
   error('nLevels must be an integer!!!');
end

if ~exist('filt', 'var')
	% default filter for post-upsample convolution
	filt = sqrt(2)*namedFilter('binom5');
end

% use a little recursion to deal with upsample steps > 2
if nLevels > 1
  im = upSample(im, nLevels-1);
end
if (nLevels >= 1)
   if (any(size(im)==1))
      if (size(im,1)==1)
         filt = filt';
      end
      out = upConv(im, filt, 'zero',(size(im)~=1)+1);
   else
      % First, upsample and blur down cols...
      out = upConv(im, filt, 'zero', [2 1]);
		% Then, upsample and blur across rows...
      out = upConv(out, filt', 'zero', [1 2]);
   end
else
   out = im;
end

return

%%% Debug/test


function varargout = plog(varargin)
%PLOG
%
% y = PLOG(x)
%
% Computes concave perspective log, x(1)*log(x(2)/x(1)) on x>0
%
% Implemented as evalutation based nonlinear operator. Hence, the concavity
% of this function is exploited to perform convexity analysis and rigorous
% modelling.

switch class(varargin{1})

    case 'double'

        if ~isequal(prod(size(varargin{1})),2)
            error('PLOG only defined for 2x1 arguments');
        end
        x = varargin{1};
        % Safe version with defined negative values (helps fmincon when
        % outside feasible region)

        if isequal(x(1),[0])
            varargout{1} = 0;
        else
            varargout{1} = x(1)*log(x(2)/x(1));
        end

    case 'sdpvar'

        if ~isequal(prod(size(varargin{1})),2)
            error('PLOG only defined for 2x1 arguments');
        else
            varargout{1} = yalmip('define',mfilename,varargin{1});
        end

    case 'char'

        operator = CreateBasicOperator('concave','callback');
        operator.range = [-inf inf];
        operator.domain = [0 inf];
        operator.bounds = @bounds;
        operator.convexhull = @convexhull;
        operator.derivative = @derivative;

        varargout{1} = [];
        varargout{2} = operator;
        varargout{3} = varargin{3};

    otherwise
        error([upper(mfilename) ' called with weird argument']);
end

function dp = derivative(x)
z = x(2)/x(1);
dp = [log(z)-1;1./z];

function [L,U] = bounds(xL,xU)
xU(isinf(xU)) = 1e12;
x1 = xL(1)*log(xL(2)/xL(1));
x2 = xU(1)*log(xU(2)/xU(1));
x3 = xL(1)*log(xU(2)/xL(1));
x4 = xU(1)*log(xL(2)/xU(1));
L = min([x1 x2 x3 x4]);

% Stationary in x1 when x1 = x2*exp(-1)
% Increasing in x2, so max at border
p1 = [exp(-1)*xU(2);xU(2)];
x5 = p1(1)*log(p1(2)/p1(1));
if x5(1) >= xL(1) && x5(1) <= xU(1)
    U = max([x1 x2 x3 x4 x5]);
else
    U = max([x1 x2 x3 x4]);
end

function [Ax,Ay,b,K] = convexhull(xL,xU)

x1 = [xL(1);xL(2)];
x2 = [xU(1);xL(2)];
x3 = [xL(1);xU(2)];
x4 = [xU(1);xU(2)];
x5 = (xL+xU)/2;

f1 = plog(x1);
f2 = plog(x2);
f3 = plog(x3);
f4 = plog(x4);
f5 = plog(x5);

df1 = derivative(x1);
df2 = derivative(x2);
df3 = derivative(x3);
df4 = derivative(x4);
df5 = derivative(x5);

[Ax,Ay,b,K] = convexhullConcave2D(x1,f1,df1,x2,f2,df2,x3,f3,df3,x4,f4,df4,x5,f5,df5);


function z = lambda_max( Y )

% LAMBDA_MAX    Maximum eigenvalue of a symmetric matrix.
%     For square matrix X, LAMBDA_MAX(X) is MAX(EIG(X)) if X is Hermitian
%     or symmetric and real; and +Inf otherwise.
%
%     An error results if X is not a square matrix.
%
%     Disciplined convex programming information:
%         LAMBDA_MAX is convex and nonmonotonic (at least with respect to
%         elementwise comparison), so its argument must be affine.

error( nargchk( 1, 1, nargin ) );
if ndims( Y ) > 2 || size( Y, 1 ) ~= size( Y, 2 ),
    error( 'Input must be a square matrix.' );
end
err = Y - Y';
Y   = 0.5 * ( Y + Y' );
if norm( err, 'fro' )  > 8 * eps * norm( Y, 'fro' ),
    z = Inf;
else
    z = max( eig( full( Y ) ) );
end

% Copyright 2010 Michael C. Grant and Stephen P. Boyd.
% See the file COPYING.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function approx_display ( )

%*****************************************************************************80
%
%% APPROX_DISPLAY displays a sequence of Bernstein approximants to SIN(X).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    11 July 2011
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'APPROX_DISPLAY\n' );
  fprintf ( 1, '  BPAB_APPROX evaluates the Bernstein polynomial\n' );
  fprintf ( 1, '  approximant to a function F(X).\n' );
  fprintf ( 1, '  This program displays the sequence of approximants\n' );
  fprintf ( 1, '  to y=sin(x) over the interval [1,3]\n' );

  a = 1.0;
  b = 3.0;

  for degree = 0 : 20
%
%  Generate data values.
%
    xdata = zeros ( degree + 1, 1 );
    ydata = zeros ( degree + 1, 1 );

    for i = 0 : degree

      if ( degree == 0 )
        xdata(i+1) = 0.5 * ( a + b );
      else
        xdata(i+1) = ( ( degree - i ) * a   ...
                     + (          i ) * b ) ...
                     / ( degree     );
      end

      ydata(i+1) = sin ( xdata(i+1) );

    end
%
%  Compare the true function and the approximant.
%
    nval = 501;

    xval = linspace ( a, b, nval );

    yval = bpab_approx ( degree, a, b, ydata, nval, xval );

    clf
    plot ( xval, yval, 'b-' );
    hold on
    plot ( xval, sin ( xval ), 'r-' );
    plot ( xdata, ydata, 'r.', 'Markersize', 30 );
    grid
    xlabel ( '<---X--->' );
    ylabel ( '<---Y--->' );
    title ( sprintf ( 'Bernstein Approximant Degree %d', degree ) );
    hold off

    fprintf ( 1, 'Bernstein approximant of degree %d\n', degree );

    pause

  end

  return
end