function boolVal=isImag(x)
%%ISIMAG Returns true if x is a purely imaginary vector or matrix. That
%        is, it is complex and all real elements are exactly zero.
%
%INPUT: x A scalar or matrix. This must be of a numeric type; that is, not
%         a cell array, a string, etc.
%
%OUTPUTS: boolVal This is true is x is a complex matrix (isreal is false)
%                 and all real components are zero.
%
%December 2016 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(isreal(x))
   boolVal=false;
   return;
end

if(all(real(x(:))==0))
    boolVal=true;
else
    boolVal=false;
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 mBlock= PolarToRec(mRadialSlice,cubeSide,cPyramid)
%% Generates the XYZ coordiante of Polar Grid in Cartesian Grid
%%Input:   mRadialSlice: 3-d Radial slcie matrix
%%        cubeSide: order of block matrix in 3-d to be assembled
%%        cPyramid: cell containing one of 3 pyramid information
%%Output:
%%        mBlock: block matrix in 3-d to be assembled
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function mBlock= PolarToRec(mRadialIdx,mRadial,cubeSide,pyramid,F)
mBlock=zeros(cubeSide,cubeSide,cubeSide);
for j=mRadialIdx(3,1):mRadialIdx(3,2)
  for i=mRadialIdx(1,1):mRadialIdx(1,2)
    for k=mRadialIdx(2,1):mRadialIdx(2,2)
      mBlock(pyramid.X(i,k,j),pyramid.Y(i,k,j),pyramid.Z(i,k,j))=...
       +mRadial(i-mRadialIdx(1,1)+1, k-mRadialIdx(2,1)+1, j-mRadialIdx(3,1)+1);
    end
  end
end

% FToggle=F==0;
% F=F+FToggle;
%   mBlock=mBlock./F;


function A_sparse = build_sparse ( )

%*****************************************************************************80
%
%% BUILD_SPARSE demonstrates how a sparse matrix can be assembled in parallel.
%
%  Discussion:
%
%    Actually, what this script demonstrates is how components of the sparse
%    matrix can be evaluated in parallel.  To create the final sparse matrix
%    data structure requires that the "sparse()" command itself be called on
%    a single processor.
%
%    Thus, this script tries to take advantage of parallel processing, assuming
%    that the evaluation of matrix entries is expensive, compared to the minor
%    cost of collecting the entries from each processor to form the final matrix.
%
%    We want to construct a rectangular sparse matrix.  This matrix can be
%    thought of as a diagonal block matrix, with each diagonal block
%    being of arbitrary size and shape.
%
%    This problem was posed by Vitor Nunes, 18 August 2011.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 August 2011
%
%  Author:
%
%    Gene Cliff
%
%  Parameters:
%
%    Output, real sparse A_sparse(*,*), the sparse matrix.
%
%  Local parameters:
%
%    Local, real A_kb(*,*), used to store one block of the matrix.
%
%    Local, integer i_blk(N_blk), the global row index just before each
%    block begins.
%
%    Local, cell array II, the global row indices for the entries
%    in each block.
%
%    Local, integer j_blk(N_blk), the global column index just before
%    each block begins.
%
%    Local, cell array JJ(N_blk,1), the global column indices for the entries
%    in each block.
%
%    Local, integer N_blk, the number of block matrices.
%
%    Local, integer row_blks(N_blk), the row dimension of each block.
%
%    Local, integer col_blks(N_blk), the column dimension of each block.
%
%    Local, cell array VV, the values of the entries in each block.
%

%
%% Set data that defines the arrangement of blocks.
%
%  The row dimension of the global matrix is the sum of row_blks,
%  and the column dimension is the sum of col_blks.  In this
%  case, our matrix has dimensions 44 by 41, and 359 nonzero entries.
%
  N_blk = 5;
  row_blks = [ 4;  7;  9; 10; 14];
  col_blks = [ 8; 10;  5; 10;  8];

  fprintf ( 1, '\n' );
  fprintf ( 1, '  Global matrix dimension is M = %d, N = %d\n', ...
    sum ( row_blks ), sum ( col_blks ) );
%
%% Initialization.
%
  i_blk = cumsum ( row_blks(1:N_blk-1) );
  j_blk = cumsum ( col_blks(1:N_blk-1) );
  i_blk = [ 0; i_blk ];
  j_blk = [ 0; j_blk ];
%
%  The cell arrays II, JJ, and VV will contain the I, J and value
%  information for the entries in each block.
%
  II = cell ( N_blk, 1);
  JJ = cell ( N_blk, 1);
  VV = cell ( N_blk, 1);
%
%% Loop over the sub-blocks.
%
  parfor k_blk = 1 : N_blk
%
%  Here, the user would fill in the numeric values of the blocks
%  in a way that depended on the problem that was being set up.
%
%  For this demonstration, CREATE_BLOCK simply assigns arbitrary
%  values to the entries in the block.  In an actual problem, we
%  assume that the evaluation of these entries would be an expensive
%  process, so that parallel execution will result in a substantial
%  speedup.
%
    A_kb = create_block ( k_blk, row_blks(k_blk), col_blks(k_blk) );

    i_row = 1 : row_blks(k_blk);
%
%  I_BLK and J_BLK tell us how far to shift the local indices to get
%  the global row and column indices II and JJ.
%
    n_els = row_blks(k_blk) * col_blks(k_blk);

    II{k_blk}(:,1) = i_blk(k_blk) ...
      + reshape ( repmat ( i_row(:), 1, col_blks(k_blk) ), n_els, 1 );

    JJ{k_blk}(:, 1) = j_blk(k_blk) ...
      + reshape ( repmat ( 1:col_blks(k_blk), row_blks(k_blk), 1 ), n_els, 1 );

    VV{k_blk}(:, 1) = reshape ( A_kb, n_els, 1 );

  end
%
%% Now have the SPARSE command assemble the sparse matrix from the
%  data in the cell arrays.  This command does NOT execute in parallel.
%
  A_sparse = sparse ( cell2mat ( II ), cell2mat ( JJ ), cell2mat ( VV ), ...
    sum ( row_blks ), sum ( col_blks ) );

  return
end


%Sensor Refs=[use (0,1), val, obs noise std, ini std]
%u is assumed to be a random constant (with infinite initial variance)
function [u, ximu, Pimu, Pu, Pux]=selfcalib1(imu_data, Aimu, Qimu, Cimu, Rimu, PimuInv, M, sensor_ref)

DBG=0;  %Debug

nu=size(M,2); %# of kinematic variables
ns=size(M,1); %# of sensors
nimu=size(Aimu,1);    %total # of imu states

if (isempty(sensor_ref)) %not a compulsory argument
    sensor_ref=zeros(size(M,2),4);
end

%System model (augment with kv)
A=diagmat_v000(Aimu, eye(nu),[]);
Q=diagmat_v000(Qimu, zeros(nu), []);
C=[M Cimu];
R=Rimu;
Pinv=diagmat_v000(PimuInv, zeros(nu), []);

%External body acc/rot rate observations
next=0;
Cext=[];
Rext=[];
yext=[];
for in=1:nu
    if (sensor_ref(in,1)==1)     %there is an external obs
        next=next+1;
        Cext=[Cext;zeros(1,size(A,1))];
        Cext(end,in)=1;
        Rext=diagmat_v000(sensor_ref(in,3)^2,Rext,[]);
        yext=[yext;sensor_ref(in,2)];
        if (sensor_ref(in,4)~=0)    %external obs have random bias
            A=diagmat_v000(1, A, []);
            Q=diagmat_v000(0, Q, []);
            Pinv=diagmat_v000(1/sensor_ref(in,4)^2, Pinv, []);
            C=[C zeros(ns,1)];
            Cext=[Cext zeros(next,1)];
            Cext(end,end)=1;
        end
    end
end
nst=size(A,1);

%%%%% Start the estimation routine
%%Sensor outputs
%%For the first sensor data use the innovation form (Pinv is probably singular)
y=imu_data(:,1);
Rinv=inv(R);
P=inv(Pinv+C'*Rinv*C);
xest=P*(C'*Rinv*y);
%%External obs
if (next>0)
    K=P*Cext'*inv(Cext*P*Cext'+Rext);
    xest=xest+K*(yext-Cext*xest);
    P=(eye(nst)-K*Cext)*P*(eye(nst)-K*Cext)'+K*Rext*K';
end

if (DBG)
    ndat=size(imu_data,2);
    debmn=zeros(nu,ndat);
    debop=zeros(size(A,1),ndat);

    WLS=lsmat_v001(M,R,0);
    mean_dat=WLS*y;
    debmn(:,1)=mean_dat;
    debop(:,1)=xest;
end

%For the rest use standard KF
for in=2:size(imu_data,2)
    %%%prediction
    xest=A*xest;
    P=A*P*A'+Q;

    %%%filter
    %%Sensor data
    y=imu_data(:,in);
    K=P*C'*inv(C*P*C'+R);
    %K(1:nimu,:)=0;
    xest=xest+K*(y-C*xest);
    P=(eye(nst)-K*C)*P*(eye(nst)-K*C)'+K*R*K';
    %%External obs
    if (next>0)
        K=P*Cext'*inv(Cext*P*Cext'+Rext);
        xest=xest+K*(yext-Cext*xest);
        P=(eye(nst)-K*Cext)*P*(eye(nst)-K*Cext)'+K*Rext*K';
    end

    if (DBG)
        mean_dat=((in-1)*mean_dat+(WLS*y))/in;
        debmn(:,in)=mean_dat;
        debop(:,in)=xest;
    end
end

%Outputs (combine deterministic and stochastic results)
ximu=xest(nu+1:nu+nimu,1);
Pimu=P(nu+1:nu+nimu,nu+1:nu+nimu);
u=xest(1:nu,:);
Pu=P(1:nu,1:nu);
Pux=P(1:nu,nu+1:nu+nimu);

if (DBG)
    in=1;
    figure;
    plot(imu_data(in,:))
    hold on;
    plot(debmn(in,:),'r')
    plot(debop(in,:),'g')
end


function [csd,Hz] = spm_ccf2csd(ccf,Hz)
% Converts cross covariance function to cross spectral density
% FORMAT [csd,Hz] = spm_ccf2csd(ccf,Hz)
%
% ccf  (N,:,:)          - cross covariance functions
% Hz   (n x 1)          - vector of frequencies (Hz)
%
% csd  (n,:,:)          - cross spectral density (cf, mar.P)
%
% See also:
%  spm_ccf2csd.m, spm_ccf2mar, spm_csd2ccf.m, spm_csd2mar.m, spm_mar2csd.m,
%  spm_csd2coh.m and spm_Q
%
%__________________________________________________________________________
% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_ccf2csd.m 5895 2014-02-26 14:28:23Z karl $

% unpack cells
%--------------------------------------------------------------------------
if iscell(ccf)
    for i = 1:length(ccf)
       csd{i}    = spm_ccf2csd(ccf{i},Hz);
    end
    return
end

% unpack time bins (for time-frequency responses)
%--------------------------------------------------------------------------
if ndims(ccf) == 4
    for i = 1:size(ccf,1)
       csd(i,:,:,:) = spm_ccf2csd(squeeze(ccf(i,:,:,:)),Hz);
    end
    return
end

% Frequencies
%--------------------------------------------------------------------------
ds    = Hz(2) - Hz(1);
gi    = ceil(Hz/ds) + 1;
N     = Hz(end);

% Fourier transform cross-spectral density
%==========================================================================
for i = 1:size(ccf,2)
    if ismatrix(ccf)
        g          = fft(ccf(:,i));
        csd(:,i)   = g(gi)/N/ds;
    else
        for j = 1:size(ccf,3)
            g          = fft(ccf(:,i));
            csd(:,i,j) = g(gi)/N/ds;
        end
    end
end


function CNet(Net)

format compact
format long e
theta = linspace(0,2*pi,length(Net)+1);
xy = zeros(length(Net)+1,2);
x = cos(theta);
y = sin(theta);
xy(1:length(Net)+1,1) = x(1:length(Net)+1);
xy(1:length(Net)+1,2) = y(1:length(Net)+1);
figure, gplot(Net,xy,'.-');
set(gcf, 'Color', [1 1 1]);
axis('equal');
xlim([-1.1 1.1]);
ylim([-1.1 1.1]);
axis off;


%+========================================================================+
%|                                                                        |
%|            This script uses the GYPSILAB toolbox for Matlab            |
%|                                                                        |
%| COPYRIGHT : Matthieu Aussal (c) 2017-2018.                             |
%| PROPERTY  : Centre de Mathematiques Appliquees, Ecole polytechnique,   |
%| route de Saclay, 91128 Palaiseau, France. All rights reserved.         |
%| LICENCE   : This program is free software, distributed in the hope that|
%| it will be useful, but WITHOUT ANY WARRANTY. Natively, you can use,    |
%| redistribute and/or modify it under the terms of the GNU General Public|
%| License, as published by the Free Software Foundation (version 3 or    |
%| later,  http://www.gnu.org/licenses). For private use, dual licencing  |
%| is available, please contact us to activate a "pay for remove" option. |
%| CONTACT   : matthieu.aussal@polytechnique.edu                          |
%| WEBSITE   : www.cmap.polytechnique.fr/~aussal/gypsilab                 |
%|                                                                        |
%| Please acknowledge the gypsilab toolbox in programs or publications in |
%| which you use it.                                                      |
%|________________________________________________________________________|
%|   '&`   |                                                              |
%|    #    |   FILE       : nrtHmxLowrank.m                               |
%|    #    |   VERSION    : 0.50                                          |
%|   _#_   |   AUTHOR(S)  : Matthieu Aussal                               |
%|  ( # )  |   CREATION   : 25.11.2018                                    |
%|  / 0 \  |   LAST MODIF :                                               |
%| ( === ) |   SYNOPSIS   : Compare compressor with total pivoting        |
%|  `---'  |                                                              |
%+========================================================================+

clear all
close all
clc

% Gypsilab path
run('../../addpathGypsilab.m')

% Dimensions
Nx  = 100;
Ny  = 101;
rk  = 10;
tol = 1e-3;

% Compress zeros
M     = zeros(Nx,Ny);
[A,B] = hmxACA(M,tol);
norm(A*B,'inf')

% Compress ones
M     = ones(Nx,Ny);
[A,B] = hmxACA(M,tol);
norm(A*B-M,'inf')/norm(M,'inf')

% Compress eye
M          = eye(Nx,Ny);
[~,~,flag] = hmxACA(M,tol);
flag

% Compress circular permutation for eye
M = circshift(eye(Nx,Ny),3,2);
[~,~,flag] = hmxACA(M,tol);
flag

% Compress 1-eye
M          = 1-eye(Nx,Ny);
[~,~,flag] = hmxACA(M,tol);
flag

% Compress random
M     = rand(Nx,rk) * rand(rk,Ny);
[A,B] = hmxACA(M,tol);
norm(A*B-M,'inf')/norm(M,'inf')

% Compress random complex
M     = (-1+2*rand(Nx,rk)+1i*(-1+2*rand(Nx,rk))) * (rand(rk,Ny) + 1i*rand(rk,Ny));
[A,B] = hmxACA(M,tol);
norm(A*B-M,'inf')/norm(M,'inf')

% Compress 1x2 block with zeros
M     = rand(Nx,rk) * rand(rk,Ny);
Z     = zeros(Nx,10);
M     = [M Z];
[A,B] = hmxACA(M,tol);
norm(A*B-M,'inf')/norm(M,'inf')

% Compress 2x1 block with zeros
M     = rand(Nx,rk) * rand(rk,Ny);
Z     = zeros(10,Ny);
M     = [M ; Z];
[A,B] = hmxACA(M,tol);
norm(A*B-M,'inf')/norm(M,'inf')

% Compress 2x2 block random
M     = rand(Nx,rk) * rand(rk,Ny);
Z     = zeros(Nx,Ny);
M     = [M Z ; Z M];
[A,B] = hmxACA(M,tol);
norm(A*B-M,'inf')/norm(M,'inf')

% Compress 4x4 block random
M     = rand(Nx,rk) * rand(rk,Ny);
Z     = zeros(Nx,Ny);
M     = [M Z ; Z M];
Z     = zeros(size(M));
M     = [M Z ; Z M];
[A,B] = hmxACA(M,tol);
norm(A*B-M,'inf')/norm(M,'inf')

% Compress 2x2 inegal block random (to be hardly improved...)
M     = rand(Nx,rk) * rand(rk,Ny);
Z     = zeros(Nx,1);
M     = [M zeros(Nx,1) ; zeros(1,Ny) 1];
[A,B] = hmxACA(M,tol);
norm(A*B-M,'inf')/norm(M,'inf')

% Compress 4x4 inegal block random (to be hardly improved...)
M     = rand(Nx,rk) * rand(rk,Ny);
Z     = zeros(Nx,1);
M     = [M zeros(Nx,3) ; zeros(3,Ny) eye(3)];
[A,B] = hmxACA(M,tol);
norm(A*B-M,'inf')/norm(M,'inf')

% Build orthogonal mesh
vtx   = [0 0 0 ; eye(3)];
elt   = [1 3 2 ; 1 2 4];
col   = [1 ; 2];
mesh  = msh(vtx,elt,col);
mesh  = refine(mesh,0.2);
[~,I] = sort(mesh.col);
mesh  = mesh.sub(I);

% Translate
mesh2          = mesh;
mesh2.vtx(:,1) = mesh2.vtx(:,1) + 2;

% Graphical representation
figure
plot(mesh)
hold on
plotNrm(mesh)
plot(mesh2)
axis equal
view(-180,30)

% Build double layer matrix
Gxy{1} = @(X,Y) femGreenKernel(X,Y,'grady[exp(ikr)/r]1',1);
Gxy{2} = @(X,Y) femGreenKernel(X,Y,'grady[exp(ikr)/r]2',1);
Gxy{3} = @(X,Y) femGreenKernel(X,Y,'grady[exp(ikr)/r]3',1);
D      = integral(dom(mesh,3),dom(mesh2,3),fem(mesh,'P0'),Gxy,ntimes(fem(mesh2,'P0')));

% Graphical represenation
figure
imagesc(abs(D))
colorbar

% Compression
[A,B] = hmxACA(D,tol);
norm(A*B-D,'inf')/norm(D,'inf')

disp('~~> Michto gypsilab !')


clear all; close all; clc;
addpath('tools');

db_name = '256_ObjectCategories'; % '256_ObjectCategories' as a option
numRetrieval = 10;

% load dataset
if strcmp(db_name, 'faceDataset')
    load feat4096Norml.mat;
    path_imgDB = './faceDataset/';
    addpath(path_imgDB);
    queryID = 1900;
elseif strcmp(db_name, '256_ObjectCategories')
    %load 256feat2048Norml.mat; % 0.495471
    % load 256feat4096Norml.mat; % 0.484413
    load feat4096Norml.mat;
    path_imgDB = './database/';
    addpath(path_imgDB);
    %example
    queryID = 1; %
end

%if not normalize, then do
% featNorm = normalize1(feat);
% save('feat4096Norml.mat','featNorm', 'rgbImgList');

% [pc, ~] = eigs(double(cov(feat)), 128);
% feat = feat*pc;

%virsulazation
retrieval_virsulazation( queryID, numRetrieval, feat_norm, imgNamList);


function llh = GFM_llh( theta, tree )

% Evaluate the likelihood of data under the estimated GFM model
% This is *exactly* the same procedure for likelihood computation as in
% GFM_EM
%
% Syntax:
%   llh = GFM_llh( theta, tree )
%
%
% Input:
%   theta : Parameter cell for GFM model.
%
%   tree  : Wavelet tree from DWT2_TO_CELL
%
%
% Output:
%   llh   : Vector with log-likelihood for each direction and all trees

if size(theta, 1) ~= numel(tree)-1
    error('Wavelet transform must have the same size as theta');
end

L = length(tree) - 1;
M = numel( theta{1} );

% Naming convention:
% beta1 is beta_u
% beta2 is beta_{rho(u),u}

[no_rows, no_cols] = cellfun( @(x) size(x{1}), tree(2:end) );
[beta1, lh, cond_joint_probs] = deal( cell(L,1) );

for l = 1:L
    beta1{l} = ones(no_rows(l), no_cols(l), M);
    lh{l} = zeros(no_rows(l), no_cols(l));
    cond_joint_probs{l} = ones(no_rows(l), no_cols(l), M, M);
end

[beta2, prod_beta2, state_probs] = deal( beta1 );
[beta1_rep] = deal( cond_joint_probs );
llh = zeros(3, 1);

for d = 1:3
    for l = L:-1:1
        w{l} = repmat(tree{l+1}{d}, [1 1 M]);
        likelihood = normpdf( w{l}, 0, ...
            repmat(reshape(theta{l,3,d}, [1 1 M]), [no_rows(l) no_cols(l) 1]) );

        % Make sure numerically large coefficients doesn't evaluate to all
        % zeros
        idx = find( sum(likelihood, 3) == 0 );
        if ~isempty( idx )
            likelihood(idx) = 1/M;
        end

        state_probs{l} = repmat( reshape( theta{l,1,d}, [1 1 M] ), [no_rows(l) no_cols(l) 1] );

        % For the non-leaf nodes beta1 needs an additional factor
        if l < L
            % Compute beta2
            trans_probs{l+1} = repmat(reshape( theta{l+1,2,d}, [1 1 M M] ), [no_rows(l+1) no_cols(l+1) 1]);
            beta1_rep{l+1} = repmat( beta1{l+1}, [1 1 1 M] );
            state_probs_rep{l+1} = repmat( state_probs{l+1}, [1 1 1 M] );

            beta2_full = trans_probs{l+1} .* beta1_rep{l+1} ./ state_probs_rep{l+1};
            beta2{l+1} = squeeze( sum(beta2_full, 3) );

            % Product of beta2's with common parent
            prod_beta2{l} = ...
                beta2{l+1}(1:2:end, 1:2:end, :) .* ...
                beta2{l+1}(2:2:end, 1:2:end, :) .* ...
                beta2{l+1}(1:2:end, 2:2:end, :) .* ...
                beta2{l+1}(2:2:end, 2:2:end, :);

            % Unnormalized beta1
            tmp = prod_beta2{l} .* likelihood .* state_probs{l};
        else
            % Unnormalized beta1
            tmp = likelihood .* state_probs{l};
        end

        % Likelihood and beta1
        lh{l} = sum( tmp, 3 );
        beta1{l} = tmp ./ repmat( lh{l}, [1 1 M] );
    end

    % The total likelihood for this direction
    llh(d) = sum( cellfun(@(x) sum(log( max(x(:), eps) )), lh) );
end

end


function [rDisambig,sourceInfo]=disambiguateClust1D(rMeas,ampMeas,deltaR,rMax,clustDist,twoWayDisambig,threshold)
%%DISAMBIGUATECLUST1D This function uses a clustering algorithm to
%                   disambiguate measurements in one dimension. It can
%                   handle measurements from multiple simultaneous targets.
%                   Information on which detections went into the
%                   disambiguated measurements is also returned. The
%                   snapshots all have a different number of bins; the
%                   lengths in deltaR should be relatively prime (coprime)
%                   integers (possibly all times a scaling constant). When
%                   the "true" value of a detection is outside of the range
%                   0<=rMeas<deltaR(i) for the ith interval, it is aliased
%                   back in. This function tries to dealias the
%                   measurements and determine how many detections are
%                   present. This function only considers aliasing in a
%                   positive (+deltaR) direction and is thus best suited
%                   for dealiasing ranges.
%
%INPUTS: rMeas A numIntX1 or 1XnumInt cell matrix with the detections in
%              each interval. Each cell contains a numMeasX1 or 1XnumMeas
%              vector of the detections in that interval. Detections should
%      ampMeas A numIntX1 or 1XnumInt cell matrix with the positive real
%              amplitudes associated with the detections in each interval.
%              These are used to perform a weighted average to determine
%              the true target location.
%       deltaR The numIntX1 or 1XnumInt vector of the unambiguous range in
%              each interval. These values must be relatively prime
%              integers (times a common possibly non-integer scale factor)
%              for the algorithm to work.
%         rMax The maximum unambiguous range to consider for the
%              completely unaliased measurements (all dealiased values
%              are less than this. If this value is omitted or an empty
%              matrix is passed, then the default of prod(deltaR) is used,
%              which is the maximum from the Chinese remainder theorem.
%              However, as demonstrated below, the presence of multiple
%              targets can cause extra distant "ghost" detections to appear
%              due to the interference of the targets. Thus, it is often
%              good to design the maximum unambiguous value to be very big
%              and to limit the maximum number of bins actually considered.
%              Put another way, use more coprime intervals.
%    clustDist This is the distance to determine that two aliased-out
%              values are in the same cluster. This depends on the scaling
%              of the intervals. The default if this parameter is omitted
%              or an empty matrix is passed is 0.5, which is good if deltaR
%              are all integers.
% twoWayDisambig If this is false, then the integers over which aliasing
%              are performed are only positive. Otherwise, they can be
%              positive and negative, which can be more convenient for
%              Doppler disambiguation.
%    threshold An optional threshold for declaring a detection. At least
%              this many common intervals must cluster together. The
%              default if this parameter is omitted or an empty matrix is
%              passed is numInt.
%
%OUTPUTS: rDisambig The values of the detections, a numDetectX1 vector.
%              These are weighted averages of the values in rMeas that went
%              into the detections. If there are no detections, then this
%              will be an empty matrix.
%        sourceInfo Information allowing one to reconstruct which values in
%              rMeas went into forming each detection. This is a
%              numDetectX1 cell array. Each cell ion the array has a
%              numMeasX2 matrix providing information on the measurements
%              in that cluster. The first column gives the number of the
%              interval that produced the measurement (index of rMeas) and
%              the second column provides information on which measurement
%              in that interval led to the detection.
%
%The general idea of such clustering is described in [1] and [2]. However,
%it is done differently here. The authors in [2] described a
%disambiguiation algorithm that is unusable if one of the intervals does
%not produce detections and the basic algorithm in [1] is only focussed on
%a single target. Here, we alias out the measurement across all ambiguity
%intervals and then determine which aliased out measurements gate. Two
%values gate if they are from different intervals and are within clustDist
%of each other. A DisjointSet data structure is used to take the gating
%ifnromation and cluster the aliased out measurements. Clusters of a
%sufficient size are counted as detections. Note that this clustering is
%not globally optimal as it is still possible for two detections from a
%single PRI to end up in one cluster, which is only particularly likely
%with closely spaced targets. An optimal clustering algorithm would be a
%multiframe assignment problem.
%
%Note that because linear averaging is performed, points very near the
%edges will not average to the correct values. However, since measurement
%noises are typically small and unambiguous intervals large, this is seldom
%a problem.
%
%EXAMPLE 1:
%This is similar to the toy example given in [2], except we add some noise
%to the measurements In this instance, the algorithm will produce results
%that are close to the true unaliased bins.
% deltaR=[7;8;11];
% maxR=24;
%
% trueBins=[6;13];
%
% rMeas=cell(3,1);
% for curInt=1:3
%     rMeas{curInt}=unique(wrapRange(trueBins,0,deltaR(curInt)));
%     %Add noise.
%     rMeas{curInt}=rMeas{curInt}+0.1*randn(size(rMeas{curInt}));
% end
%
%[rDisambig,sourceInfo]=disambiguateClust1D(rMeas,[],deltaR,maxR)
%
%EXAMPLE 2:
%This example is better suited for Doppler disambiguation Here, positive
%and negaitve values are present, which could correspond to positive and
%negative range rates. The disambiguation is thus two-sided and the maximum
%number of bins used is limited so that irrelevant extra-fast targets are
%not detected.
% deltaR=[7;8;11;13];
% maxR=100;
% %-50 to 49
% trueBins=[6;-13];
%
% rMeas=cell(3,1);
% for curInt=1:4
%     rMeas{curInt}=unique(wrapRange(trueBins,0,deltaR(curInt)));
%     %Add noise.
%     rMeas{curInt}=rMeas{curInt}+0.1*randn(size(rMeas{curInt}));
% end
% disambiguateClust1D(rMeas,[],deltaR,maxR,[],true)
%
%REFERENCES:
%[1] G. Trunk and S. Brockett, "Range and velocity ambiguity resolution,"
%    in Record of the IEEE National Radar Conference, Lynnfield, MA, 20-22
%    Apr. 1993, pp. 146-149.
%[2] P. Stinco, M. Greco, F. Gini, A. Farina, and L. Timmoneri, "Analysis
%    and comparison of two disambiguity algorithms: The modified CA and
%    CRT," in Proceedings of the International Radar Conference -
%    Surveillance for a Safer World, Bordeaux, France, 12-16 Oct. 2009.
%
%December 2016 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<4||isempty(rMax))
   rMax=prod(deltaR);
end

numPRI=length(rMeas);
if(nargin<5||isempty(clustDist))
   clustDist=0.5;
end

if(nargin<6||isempty(twoWayDisambig))
   twoWayDisambig=false;
end

if(nargin<7||isempty(threshold))
   threshold=numPRI;
end

%Determine the maximum number of aliased out measurements in both
%directions.
numAliased=0;
for curPRI=1:numPRI
    aliasedMax=ceil(rMax/deltaR(curPRI));
    numAliased=numAliased+length(rMeas{curPRI})*aliasedMax;
end

%We will now alias out all of the measurements
measList=zeros(numAliased,1);
%souceData(i,1) holds the PRI that produced the aliased-out measurement.
%sourceData(i,2) holds the index of the measurement in the PRI.
sourceData=zeros(numAliased,2);
numAdded=0;
for curPRI=1:numPRI
    curMeas=rMeas{curPRI};
    numMeas=length(curMeas);

    aliasedMax=ceil(rMax/deltaR(curPRI));
    if(twoWayDisambig)
        valRange=(-ceil((aliasedMax)/2)):fix((aliasedMax)/2);
    else
        valRange=(0:(aliasedMax-1));
    end

    aliasedVals=bsxfun(@plus,curMeas(:),deltaR(curPRI)*valRange);
    origIdx=repmat(1:numMeas,1,aliasedMax+1);
    aliasedVals=aliasedVals(:);
    origIdx=origIdx(:);
    if(twoWayDisambig)
        %Get rid of values that aliased to values that are too low.
        %Get rid of values that aliased too low or too high
        sel=(aliasedVals>=-rMax/2)&(aliasedVals<=rMax/2);
        aliasedVals=aliasedVals(sel);
    else
        %Get rid of values that aliased to values that are too high.
        sel=aliasedVals<=rMax;
        aliasedVals=aliasedVals(sel);
    end

    origIdx=origIdx(sel);

    num2Add=length(aliasedVals);
    idx2Add=(numAdded+1):(numAdded+num2Add);

    measList(idx2Add)=aliasedVals;

    sourceData(idx2Add,1)=curPRI;
    sourceData(idx2Add,2)=origIdx;

    numAdded=numAdded+num2Add;
end

%Shrink to fit.
measList=measList(1:numAdded);
sourceData=sourceData(1:numAdded,:);

%Sort the values.
[measList,idx]=sort(measList,'ascend');
sourceData=sourceData(idx,:);

%Create a DisjointSet and determine which measurements gate with each other.
%The DisjointSet will cluster them. To gate, they must be sufficiently
%close and also not within the same interval. The loops go through all
%pairs of aliased-out measurements. We take advantage of having sorted the
%measurements so that once a distance comparison fails, no measurements are
%farther ranges are considered.
gateSet=DisjointSet(numAdded);
for idx1=1:(numAdded-1)
    for idx2=(idx1+1):numAdded
        %If they originate in different PRIs.
        if(sourceData(idx1,1)~=sourceData(idx2,1))
            if(abs(measList(idx1)-measList(idx2))<clustDist)
                %The two values gate together.
                gateSet.unionFromList([idx1;idx2]);
            else
                break;
            end
        end
    end
end

%Form clusters.
theClust=gateSet.createClusterSet();

%Count how many clusteres are large enough to keep.
numClusters=theClust.numClusters();

numPastThresh=sum(theClust.clustSize>=threshold);
rDisambig=zeros(numPastThresh,1);
sourceInfo=cell(numPastThresh,1);

curDetectClust=0;
for curClust=1:numClusters
    numMeasCur=theClust.clustSize(curClust);
    if(numMeasCur>=threshold)
        sourceDataCur=sourceData(theClust(curClust,:),:);

        rAmpCur=zeros(numMeasCur,1);
        if(~isempty(ampMeas))
            for curMeas=1:numMeasCur
                PRI=sourceDataCur(curMeas,1);
                idx=sourceDataCur(curMeas,2);
                idxR=theClust(curClust,curMeas);

                rAmpCur(curMeas,1)=measList(idxR);
                rAmpCur(curMeas,2)=ampMeas{PRI}(idx);
            end
        else
             for curMeas=1:numMeasCur
                idxR=theClust(curClust,curMeas);

                rAmpCur(curMeas,1)=measList(idxR);
                rAmpCur(curMeas,2)=1;
            end
        end

        curDetectClust=curDetectClust+1;

        %A simple weighted average.
        rDisambig(curDetectClust)=sum(rAmpCur(:,1).*rAmpCur(:,2))/sum(rAmpCur(:,2));
        sourceInfo{curDetectClust}=sourceDataCur;
    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 box_behnken_test02 ( )

%*****************************************************************************80
%
%% BOX_BEHNKEN_TEST02 tests R8MAT_WRITE.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 February 2012
%
%  Author:
%
%    John Burkardt
%
  dim_num = 4;

  range = [ ...
    0.0, 0.0, 0.0, 0.0; ...
    1.0, 1.0, 1.0, 1.0 ]';

  fprintf ( 1, '\n' );
  fprintf ( 1, 'BOX_BEHNKEN_TEST02\n' );
  fprintf ( 1, '  R8MAT_WRITE writes a Box-Behnken dataset\n' );
  fprintf ( 1, '  to a file.\n' );

  r8mat_transpose_print ( dim_num, 2, range, '  The ranges:' );

  x_num = box_behnken_size ( dim_num );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  For dimension DIM_NUM = %d\n', dim_num );
  fprintf ( 1, '  the Box-Behnken design is of size %d\n', x_num );

  x = box_behnken ( dim_num, x_num, range );

  file_out_name = 'box_behnken_04_33.txt';

  r8mat_write ( file_out_name, dim_num, x_num, x );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  The data was written to the file "%s".\n', file_out_name );

  return
end


%% FUNCTION nchoose
%   How many unique combinations of 1 to N elements are there. This
%   function is not intended to directly called by users.
%   See the Logistic_iMSF function.
%
%% LICENSE
%   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/>.
%
%   Copyright (C) 2011 - 2012 Lei Yuan, Jiayu Zhou, and Jieping Ye
%
%   You are suggested to first read the Manual.
%   For any problem, please contact with Jiayu Zhou via jiayu.zhou@asu.edu
%
%   Last modified on June 12, 2012.
%
%% RELATED FUNCTIONS
%   init_opts, Construct_iMSF, Logistic_iMSF, MultiSource_LogisticR

function W = nchoose(S)

N = numel(S) ;

% How many unique combinations of 1 to N elements are there
M = (2^N)-1 ;
if N > 18,
    warning('Nchoose:LargeOutput', ...
        'There are %d unique combinations. Please be patient ...',M) ;
end

S = S(:).' ;   % make the set a row vector, for uniform output

W = cell(M,1) ;    % Pre-allocation of output
p2=2.^(N-1:-1:0) ; % This part of the formula can be taken out of the loop

for i=1:M,
    % calculate the (reversed) binary representation of i
    % select the elements of the set based on this representation
    W{i} = S(bitget(i*p2,N) > 0) ;
end


function [ won bet happy tt ] = auction( w, iter, e, verbose )
% Subroutine AUCTION is a implemetation of the auction Algorithm
% presented in Turing's Invisible Hand. Although well discribed the
% pseudo-algorithm presented there contained an error which was corrected
% by line 62-65 replacing
%           if ((w(ii,j) - p(j)) >= 0)
%
% [ won bet happiness ] = auction( w, iter, e, verbose )
% Inputs:
%   w (g x b)
%     the rows is the bidders
%     the cols is the goods
%     the values are every bidder i's percieved value of good j
%   iter
%     iterations iterations to do
%   e
%     e is the epsilon - the minumum bet increase (default e = 1/(ngoods+1)
%     )
%
% Copyright (C) 2012 - Pieter V. Reyneke, South Africa
%
% Open Source licence, use whereever you like but at own risk, keep this
% complete copyright notice in the code and please send me updates and
% report errors to pieter.reyneke@gmail.com. Will give recognition, if
% requested, after any valid comment leading to an update was recieved.
%
% Author: PV Reyneke

if (nargin<1)
    w = rand(3)*10;
end

if (nargin<2)
    iter = Inf;
end

if (nargin<4)
    verbose = 1;
end

p   = zeros(1,size(w,2));     % reset all goods' current bets (top - tracks)
won = zeros(1,size(w,2));     % reset cuurent winners for each good
q   = 1:size(w,1);            % create queue for bidders (left - obs)

if (nargin<3)
    e = 1/(length(p)+1);
end

if (verbose)
    display('The percieved value of goods (top) by each bidder (left):');
    display([num2str(w, '%3.0f') ]);
end

i = find(q>0,1);              % take first non-committed bidder
ii = q(i);
tdead = 0;
tic
while (~isempty(ii) && (iter > 0))
    q(q==ii) = 0;             % out of queue
    [v, j] = max(w(ii,:) - p); % search for good w greatest value
                              % above current bid for bidder i
    if ( ((w(ii,j) - (p(j) + e)) >= 0) && ...
         ...                  % if our current bidder i values good j
         ...                  % more than previous bidder plus epsilon
         (won(j) ~= ii) )     % and the bot is not on himself already
        if (won(j)>0)
            k = find(q==0,1); % find first open bidder space
            q(k:(end-1)) = q((k+1):end);
                              % shift left and put
            q(end) = won(j);  % previous highest bidder back in the queue
        end
        won(j) = ii;          % assign current bidder i to good j
        p(j) = p(j) + e;      % and save the new bet
    end

    if (verbose)
        tsss = toc;
        display(['Bets(in epsilons)  : ' num2str(p/e, '%3.0f') ]);
        display(['Bidder             : ' num2str(won, '%3.0f') ]);
        display(['Left todo          : ' num2str(q, '%3.0f') ]);
        display(['-----------------------------------------' ]);
        tdead = toc - tsss;
    end
    i = find(q>0,1);          % take next non-committed bidder
    ii = q(i);
    iter = iter-1;
end
bet = p;
tt = toc;

% measuring each bidder's happiness (the invariant of above loop)
happy = won;
iown = find(won > 0);
for (ii = won(iown))
    iii = won(ii);
    if (~isempty(iii))
        jjj = find(iii==won,1);
        if (~isempty(jjj))
            happy(iii) = (sum(e + w(iii,jjj) - p < w(iii,:) - p) == 0);
        end
    end
end
iown = find(won > 0);
for (ii = won(iown))
    iii = won(ii);
    happy(iii) = happy(iii) | (sum(w(iii,:) - p > 0));
end

j = 1;
cost = 0;
for (i=won)
    cost = cost + w(i,j);
    j = j+1;
end

if (verbose)
    display(['Assignment Cost    : ' num2str(cost, '%5.3f') ]);
    display(['Happiness          : ' num2str(happy, '%3.0f') ]);
    display(['Calc took          : ' num2str((tt - tdead) * 1000, '%5.3f') ' ms' ]);
end


function V = hogDraw( H, w )
% Create visualization of hog descriptor.
%
% USAGE
%  V = hogDraw( H, [w] )
%
% INPUTS
%  H          - [m n oBin*4] computed hog features
%  w          - [15] width for each glyph
%
% OUTPUTS
%  V          - [m*w n*w] visualization of hog features
%
% EXAMPLE
%
% See also hog
%
% Piotr's Image&Video Toolbox      Version 2.41
% Copyright 2012 Piotr Dollar.  [pdollar-at-caltech.edu]
% Please email me if you find bugs, or have suggestions or questions!
% Licensed under the Simplified BSD License [see external/bsd.txt]

% fold 4 normalizations
nFold=4; s=size(H); s(3)=s(3)/nFold; w0=H; H=zeros(s);
for o=0:nFold-1, H=H+w0(:,:,(1:s(3))+o*s(3)); end;

% construct a "glyph" for each orientaion
if(nargin<2 || isempty(w)), w=15; end
bar=zeros(w,w); bar(:,round(.45*w):round(.55*w))=1;
bars=zeros([size(bar) s(3)]);
for o=1:s(3), bars(:,:,o)=imrotate(bar,-(o-1)*180/s(3),'crop'); end

% make pictures of positive weights by adding up weighted glyphs
H(H<0)=0; V=zeros(w*s(1:2));
for r=1:s(1), rs=(1:w)+(r-1)*w;
  for c=1:s(2), cs=(1:w)+(c-1)*w;
    for o=1:s(3), V(rs,cs)=V(rs,cs)+bars(:,:,o)*H(r,c,o); end
  end
end


function tests = LocnTest
  tests = functiontests(localfunctions);
  clc
end

function setupOnce(testCase)
    testCase.TestData.Duration = 50;
end

function Vehicle_test(tc)
    %%
    randinit
    V = diag([0.005, 0.5*pi/180].^2);

    v = Bicycle('covar', V);
    v.add_driver( RandomPath(10) );

    v.run(tc.TestData.Duration);
    v.plot_xy();
    s = v.char();

    J = v.Fx(v.x, [.1 .2]);
    J = v.Fv(v.x, [.1 .2]);
end

function DeadReckoning_test(tc)
    %%
    randinit
    V = diag([0.005, 0.5*pi/180].^2);
    P0 = diag([0.005, 0.005, 0.001].^2);

    v = Bicycle('covar', V);
    v.add_driver( RandomPath(10) );
    s = char(v);

    ekf = EKF(v, V, P0);
    ekf.run(tc.TestData.Duration);

    clf
    ekf.plot_xy
    hold on
    v.plot_xy('r')
    grid on
    xyzlabel

    ekf.plot_ellipse('g')
    ekf.plot_P()
end

function MapLocalization_test(tc)
    %%
    randinit
    W = diag([0.1, 1*pi/180].^2);
    P0 = diag([0.005, 0.005, 0.001].^2);
    V = diag([0.005, 0.5*pi/180].^2);

    map = LandmarkMap(20);
    map = LandmarkMap(20, 'verbose');
    map = LandmarkMap(20, 10, 'verbose');
    map = LandmarkMap(20, 10);
    s = char(map);

    veh = Bicycle('covar', V);
    veh.add_driver( RandomPath(10) );
    sensor = RangeBearingSensor(veh, map, 'covar', W);
    sensor.interval = 5;
    ekf = EKF(veh, W, P0, sensor, W, map);

    ekf.run(tc.TestData.Duration);

    clf
    map.plot()
    veh.plot_xy('b');
    ekf.plot_xy('r');
    ekf.plot_ellipse('k')
    grid on
    xyzlabel

    clf
    ekf.plot_P()
end

function Mapping_test(tc)
    %%
    randinit
    W = diag([0.1, 1*pi/180].^2);
    V = diag([0.005, 0.5*pi/180].^2);

    map = LandmarkMap(20, 10);

    veh = Bicycle('covar', V);
    veh.add_driver( RandomPath(10) );

    sensor = RangeBearingSensor(veh, map, 'covar', W);
    sensor.interval = 5;

    ekf = EKF(veh, [], [], sensor, W, []);
    ekf.run(tc.TestData.Duration);

    clf
    map.plot()
    veh.plot_xy('b');
    ekf.plot_map('g');
    grid on
    xyzlabel

    %%
    verifyEqual(tc, numcols(ekf.landmarks), 20);

end

function SLAM_test(tc)
    %%
    randinit
    W = diag([0.1, 1*pi/180].^2);
    P0 = diag([0.005, 0.005, 0.001].^2);
    V = diag([0.005, 0.5*pi/180].^2);

    map = LandmarkMap(20, 10);

    veh = Bicycle(V);
    veh.add_driver( RandomPath(10) );

    sensor = RangeBearingSensor(veh, map, 'covar', W);
    sensor.interval = 1;

    ekf = EKF(veh, V, P0, sensor, W, []);
    ekf
    ekf.verbose = false;
    ekf.run(tc.TestData.Duration);

    clf
    map.plot()
    veh.plot_xy('b');
    ekf.plot_xy('r');
    ekf.plot_ellipse('k')
    grid on
    xyzlabel

    clf
    ekf.plot_P()

    clf
    map.plot();
    ekf.plot_map('g');

    %%
    verifyEqual(tc, numcols(ekf.landmarks), 20);

end

function ParticleFilter_test(tc)
    %%
    randinit
    map = LandmarkMap(20);

    W = diag([0.1, 1*pi/180].^2);
    v = Bicycle('covar', W);
    v.add_driver( RandomPath(10) );
    V = diag([0.005, 0.5*pi/180].^2);
    sensor = RangeBearingSensor(v, map, 'covar', V);

    Q = diag([0.1, 0.1, 1*pi/180]).^2;
    L = diag([0.1 0.1]);
    pf = ParticleFilter(v, sensor, Q, L, 1000);
    pf
    pf.run(tc.TestData.Duration);

    plot(pf.std)
    xlabel('time step')
    ylabel('standard deviation')
    legend('x', 'y', '\theta')
    grid

    clf
    pf.plot_pdf();
    clf
    pf.plot_xy();
end

function posegraph_test(tc)
    pg = PoseGraph('pg1.g2o')
    tc.verifyClass(pg, 'PoseGraph');
    tc.verifyEqual(pg.graph.n, 4);

    clf
    pg.plot()
    pg.optimize('animate')
    close all

    pg = PoseGraph('killian-small.toro')
    tc.verifyClass(pg, 'PoseGraph');
    tc.verifyEqual(pg.graph.n, 1941);

    pg = PoseGraph('killian.g2o', 'laser')
    tc.verifyClass(pg, 'PoseGraph');
    tc.verifyEqual(pg.graph.n, 3873);

    [r,theta] = pg.scan(1);
    tc.verifyClass(r, 'double');
    tc.verifyLength(r, 180);
    tc.verifyClass(theta, 'double');
    tc.verifyLength(theta, 180);

    [x,y] = pg.scan(1);
    tc.verifyClass(x, 'double');
    tc.verifyLength(x, 180);
    tc.verifyClass(y, 'double');
    tc.verifyLength(y, 180);

    pose = pg.pose(1);
    tc.verifyClass(pose, 'double');
    tc.verifySize(pose, [3 1]);

    t = pg.time(1);
    tc.verifyClass(t, 'double');
    tc.verifySize(t, [1 1]);

    w = pg.scanmap('ngrid', 3000);
    tc.verifyClass(w, 'int32');
    tc.verifySize(w, [3000 3000]);

    tc.assumeTrue(exist('idisp', 'file'));  %REMINDER
    clf
    pg.plot_occgrid(w);
    close all

end

function makemap_test(tc)
        tc.assumeTrue(false);  %REMINDER

end

function chi2inv_test(tc)
    tc.verifyEqual( chi2inv_rtb(0,2), 0);
    tc.verifyEqual( chi2inv_rtb(1,2), Inf);
    tc.verifyEqual( chi2inv_rtb(3,2), NaN);

    tc.verifyError( @() chi2inv_rtb(1,1), 'RTB:chi2inv_rtb:badarg');

end


function A = my_spdiags(v,diagonals,m,n)
%MY_SPDIAGS Construct a banded matrix from diagonal input
%
% A = my_spdiags(v,diagonals,m,n);
%
% Inputs:
%  v  the diagonals to be inserted in the matrix - each column of v is one
%     diagonal in diagonals
%  diagonals  the index of diagonals requested
%  m,n  the dimension of the requested matrix
% Outputs:
%  A  the requested banded sparse matrix
%

A = ...

end


%PEAK2  Find peaks in a matrix
%
% ZP = PEAK2(Z, OPTIONS) are the peak values in the 2-dimensional signal Z.
%
% [ZP,IJ] = PEAK2(Z, OPTIONS) as above but also returns the indices of the
% maxima in the matrix Z.  Use SUB2IND to convert these to row and column
% coordinates
%
% Options::
% 'npeaks',N    Number of peaks to return (default all)
% 'scale',S     Only consider as peaks the largest value in the horizontal
%               and vertical range +/- S points.
% 'interp'      Interpolate peak (default no interpolation)
% 'plot'        Display the interpolation polynomial overlaid on the point data
%
% Notes::
% - A maxima is defined as an element that larger than its eight neighbours.
%   Edges elements will never be returned as maxima.
% - To find minima, use PEAK2(-V).
% - The interp options fits points in the neighbourhood about the peak with
%   a paraboloid and its peak position is returned.  In this case IJ will
%   be non-integer.
%
% See also PEAK, SUB2IND.

% Copyright (C) 1993-2014, by Peter I. Corke
%
% This file is part of The Robotics Toolbox for MATLAB (RTB).
%
% RTB is free software: you can redistribute it and/or modify
% it under the terms of the GNU Lesser General Public License as published by
% the Free Software Foundation, either version 3 of the License, or
% (at your option) any later version.
%
% RTB is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU Lesser General Public License for more details.
%
% You should have received a copy of the GNU Leser General Public License
% along with RTB.  If not, see <http://www.gnu.org/licenses/>.
%
% http://www.petercorke.com

% Copyright (c) Peter Corke 1/96

function [zp,xypout, aout] = peak2(z, varargin)

    % process input options
    opt.npeaks = 2;
    opt.scale = 1;
    opt.interp = false;

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

    % create a neighbourhood mask for non-local maxima
    % suppression
    h = opt.scale;
    w = 2*h+1;
    M = ones(w,w);
    M(h+1,h+1) = 0;

    % compute the neighbourhood maximum
    znh = iwindow(double(z), M, 'max', 'wrap');

    % find all pixels greater than their neighbourhood
    k = find(z > znh);

    % sort these local maxima into descending order
    [zpk,ks] = sort(z(k), 'descend');

    k = k(ks);

    npks = min(length(k), opt.npeaks);
    k = k(1:npks);

    [y,x] = ind2sub(size(z), k);
    xy = [x y]';

    % interpolate the peaks if required
    if opt.interp

        xyp = [];
        zp = [];
        ap = [];

        % for each previously identified peak x(i), y(i)
        for xyt=xy
            % fit a polynomial to the local neighbourhood
            try

                x = xyt(1); y = xyt(2);

                % now try to interpolate the peak over a 3x3 window

                zc = z(x,   y);
                zn = z(x,   y-1);
                zs = z(x,   y+1);
                ze = z(x+1, y);
                zw = z(x-1, y);

                dx = (ze - zw)/(2*(2*zc - ze - zw));
                dy = (zn - zs)/(2*(zn - 2*zc + zs));

                zest = zc - (ze - zw)^2/(8*(ze - 2*zc + zw)) - (zn - zs)^2/(8*(zn - 2*zc + zs));

                aest = min(abs([ze/2 - zc + zw/2, zn/2 - zc + zs/2]));

            catch
                % handle situation where neighbourhood falls off the data
                % vector
                warning('Peak at %f too close to edge of image, skipping', x(i));
                continue;
            end
            %

            % store x, y for the refined peak
            xyp = [xyp [x+dx; y+dy]];
            zp = [zp zest];
            ap = [ap aest];

        end
    else
        % no interpolation case
        xyp = xy;
        zp = z(k)';
        ap = [];

    end

    % return values
    if nargout > 1
        xypout = xyp;
    end
    if nargout > 2
        aout = ap;
    end


%computes the maximum of the Harmonic Product Spectrum
%> called by ::ComputePitch
%>
%> @param X: spectrogram (dimension FFTLength X Observations)
%> @param f_s: sample rate of audio data
%>
%> @retval f_0 HPS maximum (in Hz)
% ======================================================================
function [f_0] = PitchSpectralHps (X, f_s)

    % initialize
    iOrder = 4;
    f_min = 300;
    k_min = round(f_min/f_s * 2 * (size(X, 1)-1)) + 1;

    afHps = X;

    % compute the HPS
    for j = 2:iOrder
        afHps = afHps .* [X(1:j:end, :); zeros(size(X, 1)-size(X(1:j:end,:), 1), size(X, 2))];
    end

    % find max index and convert to Hz
    [fDummy, f_0] = max(afHps(k_min:end, :), [], 1);
    f_0 = (f_0 + k_min - 2) / (size(X, 1)-1) * f_s / 2;
    f_0(sum(afHps, 1) == 0) = 0;
end


function Sfi = GetSf(angles, offsets, T2f, SfTable)
%GetSf interpolate Sf values from precomputed table

Sfi = zeros(length(angles),1);
printed = false;

% This is a little workspace trick to limit the number of
% Sf table interpolation warnings to the command window/console.
% If the counterSfMiss variable exists in the base workspace,
% its value will nbe subjected to evaluation. If not, will be assigned
% with 0 and broadcasted to the base workspace.
% This is not a go-to *.m practice, especially if done without enough
% comments.

for ii = 1:length(angles)

[xi, yi, zi] = meshgrid(offsets(ii), angles(ii), T2f);
Sfi(ii) = interp3(SfTable.offsets, SfTable.angles, SfTable.T2f, SfTable.values, xi, yi, zi);

    if (isnan(Sfi(ii)))

        if ~evalin('base','exist(''counterSfMiss'')')
          counterSfMiss = 0;
          assignin('base','counterSfMiss',counterSfMiss);
         else
         counterSfMiss  = evalin('base','counterSfMiss');

        % Print this once for all the angles. Allow 10 global prints in total.
        % Fetch the variable from workspace. Doing this here instead of L14
        % means one less condition, which matters when there are thousands.

        if ~printed && counterSfMiss < 11
         % Get value from base kspace.
          cprintf('magenta','Cannot interpolate value from current Sf table : angle: %f; offset: %f; T2f: %f\n',angles(ii), offsets(ii), T2f);
          cprintf('blue','%s','Calculating the missing Sf value...');
          if counterSfMiss==10
            cprintf('blue','%s','Remaining warnings for missing Sf value interpolations have been silenced for this processs');
          end
          counterSfMiss = counterSfMiss + 1;
          assignin('base','counterSfMiss',counterSfMiss);
          printed = true;
        end

        end

        MTpulse = GetPulse(angles(ii),offsets(ii),SfTable.PulseTrf,SfTable.PulseShape,SfTable.PulseOpt);
        Sfi(ii) = computeSf(T2f, MTpulse);
    end
end


function edges = crackPattern2(box, points, alpha, varargin)
%CRACKPATTERN2 Create a (bounded) crack pattern tessellation
%
%   E = crackPattern2(BOX, POINTS, ALPHA)
%   create a crack propagation pattern wit following parameters :
%   - pattern is bounded by area BOX which is a polygon.
%   - each crack originates from points given in POINTS
%   - directions of each crack is given by a [NxM] array ALPHA, where M is
%   the number of rays emanating from each seed/
%   - a crack stop when it reaches another already created crack.
%   - all cracks stop when they reach the border of the frame, given by box
%   (a serie of 4 points).
%   The result is a collection of edges, in the form [x1 y1 x2 y2].
%
%   E = crackPattern2(BOX, POINTS, ALPHA, SPEED)
%   Also specify speed of propagation of each crack.
%
%
%   See the result with :
%     figure;
%     drawEdge(E);
%
%   See also drawEdge
%
%   ---------
%
%   author : David Legland
%   INRA - TPV URPOI - BIA IMASTE
%   created the 25/05/2004.
%

%   HISTORY :

if ~isempty(varargin)
    speed = varargin{1};
else
    speed = ones(size(points, 1), 1);
end

% Compute line equations for each initial crack.
% The 'Inf' at the end correspond to the position of the limit.
% If an intersection point is found with another line, but whose position
% is after this value, this means that another crack stopped it before it
% reach the intersection point.
NP = size(points, 1);
lines = zeros(0, 5);
for i=1:size(alpha, 2)
    lines = [lines; points speed.*cos(alpha(:,i)) speed.*sin(alpha(:,i)) Inf*ones(NP, 1)];
end
NL = size(lines, 1);

% initialize lines for borders, but assign a very high speed, to be sure
% borders will stop all cracks.
dx = (box([2 3 4 1],1)-box([1 2 3 4],1))*max(speed)*5;
dy = (box([2 3 4 1],2)-box([1 2 3 4],2))*max(speed)*5;

% add borders to the lines set
lines = [lines ; createLine(box, dx, dy) Inf*ones(4,1)];

edges = zeros(0, 4);

while true
    modif = 0;

    % try to update each line
	for i=1:NL

        % initialize first point of edge
        edges(i, 1:2) = lines(i, 1:2);

        % compute intersections with all other lines
        pi = intersectLines(lines(i,:), lines);

        % compute position of all intersection points on the current line
        pos = linePosition(pi, lines(i,:));

        % consider points to the right (positive position), and sort them
        indr = find(pos>1e-12 & pos~=Inf);
        [posr, indr2] = sort(pos(indr));

        % look for the closest intersection to the right
        for i2=1:length(indr2)

            % index of intersected line
            il = indr(indr2(i2));

            % position of point relative to intersected line
            pos2 = linePosition(pi(il, :), lines(il, :));

            % depending on the sign of position, tests if the line2 can
            % stop the current line, or if it was stopped before
            if pos2>0
                if pos2<abs(posr(i2)) && pos2<lines(il, 5)
                    if lines(i, 5) ~= posr(i2)
                        edges(i, 3:4) = pi(il,:);
                        lines(i, 5) = posr(i2);
                        modif = 1;
                    end
                    break;
                end
            end
        end   % end processing of right points of the line

	end % end processing of all lines

    % break the infinite loop if no more modification was made
    if ~modif
        break;
    end
end

% add edges of the surronding box.
edges = [edges ; box(1,:) box(2,:) ; box(2,:) box(3,:); ...
                 box(3,:) box(4,:) ; box(4,:) box(1,:)  ];


function X = arrange(X,foo)
%ARRANGE Arranges the rank-1 components of a ktensor.
%
%   ARRANGE(X) normalizes the columns of the factor matrices and then sorts
%   the ktensor components by magnitude, greatest to least.
%
%   ARRANGE(X,N) absorbs the weights into the Nth factor matrix instead of
%   lambda.
%
%   ARRANGE(X,P) rearranges the components of X according to the
%   permutation P. P should be a permutation of 1 to NCOMPOMENTS(X).
%
%   See also KTENSOR, NCOMPONENTS, NORMALIZE.
%
%MATLAB Tensor Toolbox.
%Copyright 2015, Sandia Corporation.

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

%% Just rearrange and return if second argument is a permutation
if exist('foo','var') && (length(foo) > 1)
    X.lambda = X.lambda(foo);
    for i = 1 : ndims(X)
        X.u{i} = X.u{i}(:,foo);
    end
    return;
end

%% Ensure that matrices are normalized
X = normalize(X);

%% Sort
[X.lambda, idx] = sort(X.lambda, 1, 'descend');
for i = 1 : ndims(X)
    X.u{i} = X.u{i}(:,idx);
end

%% Absorb the weight into one factor, if requested
if exist('foo','var')
    r = length(X.lambda);
    X.u{end} = full(X.u{end} * spdiags(X.lambda,0,r,r));
    X.lambda = ones(size(X.lambda));
end


function r8_epsilon_test ( )

%*****************************************************************************80
%
%% R8_EPSILON_TEST tests R8_EPSILON.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 September 2012
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'R8_EPSILON_TEST\n' );
  fprintf ( 1, '  R8_EPSILON produces the R8 roundoff unit.\n' );
  fprintf ( 1, '\n' );

  r = r8_epsilon ( );
  fprintf ( 1, '  R = R8_EPSILON()         = %e\n', r );

  s = ( 1.0 + r ) - 1.0;
  fprintf ( 1, '  ( 1 + R ) - 1            = %e\n', s );

  s = ( 1.0 + ( r / 2.0 ) ) - 1.0;
  fprintf ( 1, '  ( 1 + (R/2) ) - 1        = %e\n', s );

  return
end


function table = distableAll(board)

n = numel(board);
table = NaN * zeros(n);

for i = 1:n
    table(i,i) = 0;
    table(i, board(i).Neighbors) = 1;
    d = 1;
    N = -1;
    while nnz(~isnan(table(i,:))) ~= N;
        N = nnz(~isnan(table(i,:)));
        for j = find(table(i,:) == d)
            for k = board(j).Neighbors
                if i ~= k && isnan(table(i, k))
                    table(i, k) = d + 1;
                end
            end
        end
        d = d + 1;
    end
end


function preprocess_shape_collection(path_shapes,path_save,global_params, shot_params)
    d = dir([path_shapes,'*.mat']);

    if ~exist(path_save, 'dir')
       mkdir(path_save)
    end

    for i=1:numel(d)
        %Load shapes
        model = load([path_shapes,d(i).name]);
        model.X = model.VERT(:,1); model.Y = model.VERT(:,2); model.Z = model.VERT(:,3);

        %Calculate LBO eigenfunctions and SHOT descriptors
        [model_evecs,~,model_evals,model_S] = extract_eigen_functions_new(model,global_params.num_evecs);
        model_shot = calc_shot(model.VERT', model.TRIV', 1:model.n, shot_params.num_bins, shot_params.radius, 3)';

        %save as single
        model_evecs_trans = single(model_evecs'*model_S);
        model_evecs = single(model_evecs);
        model_S = single(full(diag(model_S)));
        save([path_save,d(i).name],'model_shot', 'model_evecs', 'model_evecs_trans', 'model_S', 'shot_params');

        display(i)
    end

    delete(gcp('nocreate'))
end


function [c, ceq] = ma_inclinationConstraint(stateLog, eventID, lbInc, ubInc, bodyIDApply, celBodyData, maData)
%ma_semiEccentricityConstraint Summary of this function goes here
%   Detailed explanation goes here
    normFact = pi;

    if(ischar(eventID) && strcmpi(eventID,'final'))
        eventNum = max(stateLog(:,13));
    else
%         hMAMainGUI = findall(0,'tag','ma_MainGUI');
%         maData = getappdata(hMAMainGUI,'ma_data');
        [~, eventNum] = getEventByID(eventID, maData.script);
    end

    eventLog = stateLog(stateLog(:,13)==eventNum,:);

    bodyEventLog = eventLog(eventLog(:,8)==bodyIDApply,:);
    if(isempty(bodyEventLog))
        finalEntry = eventLog(end,:);
    else
        finalEntry = bodyEventLog(end,:);
    end

    bodyID = finalEntry(8);

    if(bodyID == bodyIDApply || bodyIDApply==-1)
        bodyInfo = getBodyInfoByNumber(bodyID, celBodyData);
        gmu = bodyInfo.gm;
        rVect = finalEntry(2:4)';
        vVect = finalEntry(5:7)';

        [~, ~, inc, ~, ~, ~] = getKeplerFromState(rVect,vVect,gmu);

        if(lbInc == ubInc)
            c = [0 0];
            ceq(1) = inc - ubInc;
        else
            c(1) = inc - ubInc;
            c(2) = lbInc - inc;
            ceq = [0];
        end
        c = c/normFact;
        ceq = ceq/normFact;
    else
        c = [0 0];
        ceq = [0];
    end
end


function F = intkernel(X,vp,gp,avg_flag)
%INTKERNEL Expected GP kernel in scalar correlation

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

K = vp.K;           % Number of components
[N,D] = size(X);
mu(:,:) = vp.mu;
sigma(1,:) = vp.sigma;
lambda(:,1) = vp.lambda(:);
w(1,:) = vp.w;

Ns = numel(gp.post);            % Hyperparameter samples

F = zeros(N,Ns);

if isfield(vp,'delta') && ~isempty(vp.delta)
    delta = vp.delta;
else
    delta = 0;
end

% Integrated mean function being used?
integrated_meanfun = isfield(gp,'intmeanfun') && gp.intmeanfun > 0;

if integrated_meanfun
    % Evaluate basis functions
    Hs = gplite_intmeanfun(X,gp.intmeanfun);
end

% Loop over hyperparameter samples
for s = 1:Ns
    hyp = gp.post(s).hyp;

    % Extract GP hyperparameters from HYP
    ell = exp(hyp(1:D));
    ln_sf2 = 2*hyp(D+1);
    sum_lnell = sum(hyp(1:D));

    if integrated_meanfun
        %betabar = gp.post(s).intmean.betabar';
        %KinvHtbetabar = gp.post(s).intmean.HKinv'*betabar;
        plus_idx = gp.intmeanfun_var > 0;
        HKinv = gp.post(s).intmean.HKinv(plus_idx,:);
        Tplusinv = gp.post(s).intmean.Tplusinv;
    end

    L = gp.post(s).L;
    Lchol = gp.post(s).Lchol;

    sn2_eff = 1/gp.post(s).sW(1)^2;

    ddl = sq_dist(bsxfun(@rdivide,X',ell),bsxfun(@rdivide,gp.X',ell));
    ll = exp(ln_sf2 -0.5*ddl);

    if Lchol
        zz = (L\(L'\ll'))/sn2_eff;
    else
        zz = -L*ll';
    end

    for k = 1:K
        tau_k = sqrt(sigma(k)^2*lambda.^2 + ell.^2 + delta.^2);
        lnnf_k = ln_sf2 + sum_lnell - sum(log(tau_k));  % Covariance normalization factor
        delta_k = bsxfun(@rdivide,bsxfun(@minus, mu(:,k), gp.X'), tau_k);
        z_k = exp(lnnf_k -0.5 * sum(delta_k.^2,1));

        dd_k = bsxfun(@rdivide,bsxfun(@minus, mu(:,k), X'), tau_k);
        zz_k = exp(lnnf_k -0.5 * sum(dd_k.^2,1));

        F(:,s) = F(:,s) + w(k)*(zz_k - z_k*zz)';

        % Contribution of integrated mean function
        if integrated_meanfun
            switch gp.intmeanfun
                case 1; u_k = 1;
                case 2; u_k = [1,mu(:,k)'];
                case 3; u_k = [1,mu(:,k)',(mu(:,k).^2 + sigma(k)^2*lambda.^2)'];
                case 4; u_k = [1,mu(:,k)',(mu(:,k).^2 + sigma(k)^2*lambda.^2)',mumu_mat(k,:)];
            end

            F(:,s) = F(:,s) + w(k)*((u_k(plus_idx)*(Tplusinv*Hs)) ...
                + ((z_k*HKinv')*(Tplusinv*(HKinv*ll'))) ...
                - (u_k(plus_idx)*(Tplusinv*(HKinv*ll'))) ...
                - ((z_k*HKinv')*(Tplusinv*Hs)))';
        end

    end
end

% Average multiple hyperparameter samples
if Ns > 1 && avg_flag
    F = mean(F,2);
end

end


function plot_strike_dip(catalog,ax)
    % plot strike and dip for a catalog using DipDirection and Dip
    % specify the axes

    dipdir=catalog.DipDirection;
    dip=catalog.Dip;
    x0=catalog.Longitude;
    y0=catalog.Latitude;

    strikescale=0.5;
    dipscale=0.3;
    linecolor = [1 0 0];
    limit_to_axes = true;

    % figure out some scaling stuff
    axun = get(ax,'Units');
    set(ax,'Units','pixels')
    p=fix(get(ax,'Position'));
    set(ax,'Units',axun);

    % TODO: do something with p and the xlim to come up with an appropriately sized symbol

    delete(findobj(gcf,'Tag','diptest'))

    strike = wrapTo360(dipdir-90);
    scaling = ax.DataAspectRatio;
    labels = cellstr( num2str(dip) );
    labels="  "+labels;

    is_horiz = dip==0;
    is_vert = dip==90;

    dx = cosd(strike) .* strikescale ./scaling(2);
    dy = sind(strike) .* strikescale ./ scaling(1);
    xx = ([x0,x0,x0] + [-dx, dx, nan(size(x0))])';
    yy = ([y0,y0,y0] + [-dy, dy, nan(size(y0))])';
    xx=xx(:);
    yy=yy(:);

    hold on;
    plot(ax,xx,yy,'r','linewidth',1,'Tag','diptest','DisplayName','Strikes')

    dipx=cosd(dipdir(:))* dipscale./scaling(2);
    dipy=sind(dipdir(:))* dipscale./scaling(1);
    xdx = ([x0,x0,x0] + [zeros(size(x0)) - is_vert .* dipx, dipx, nan(size(x0))])';
    ydy = ([y0,y0,y0] + [zeros(size(y0)) - is_vert .* dipy, dipy, nan(size(y0))])';

    xdx=xdx(:);
    ydy=ydy(:);
    plot(ax,xdx,ydy,'Color',linecolor,'linewidth',1.5,'Tag','diptest','DisplayName','Dips')
    if any(is_horiz)
        plot(ax,x0(is_horiz),y0(is_horiz),'o','Color',linecolor,'linewidth',1.5,'Tag','diptest','DisplayName','HorizDips')
    end
    if limit_to_axes
        rangeidx=in_range(x0,xlim) & in_range(y0,ylim);
        text(ax,x0(rangeidx), y0(rangeidx), labels(rangeidx), 'VerticalAlignment','top',...
            'HorizontalAlignment','left',...
            'Tag','diptest',...
            'Color',linecolor .* 0.66);

    else
        text(ax,x0, y0, labels, 'VerticalAlignment','top',...
            'HorizontalAlignment','left',...
            'Tag','diptest',...
            'Color',linecolor .* 0.66);
    end
end


function [ x_wave ] = wavethresh_2D( x, waveletStages, waveletFilterName, wavecoeffS, lambdaWave, L )
%
% (c) Marc Fischer, Thomas Kuestner
% ---------------------------------------------------------------------

    x_wave_helper = wavedec2(x,waveletStages,waveletFilterName);
    x_wave_helper = softthresh_real(x_wave_helper,lambdaWave/L);
    x_wave = waverec2(x_wave_helper,wavecoeffS,waveletFilterName);

end


function [xe,Pe,lm_seq] = update_std(xe,Pe,lm_seq,z,R)

lenz = length(find(z(3,:)>0));
lenx = size(xe,1);

global gDISTBEAR

nf = 0;

% % update
for i= 1:lenz
    % data association (based on landmark id)
    % TODO: use nearest neighbor to do the job
    is_exist = ~(lm_seq - z(3,i));
    idx = find(is_exist);

    ii = 2*i+(-1:0);

    % update: already in the state vecor
    if ~isempty(idx)
        nf = nf+1;
        jj = 2*nf+(-1:0);
        if gDISTBEAR
            [zhat,Hii] = measurement_model_std(xe,idx);   %Hekf=Hii
            H(jj,:) = Hii;
            r(jj,1) = [ z(1,i)-zhat(1,1);  pi_to_pi(z(2,i)-zhat(2,1)) ];
            Rf(jj,jj) = R(ii,ii);
        else
            [zhat,Hii] = measurement_model_std(xe,idx);   %Hekf=Hii
            H(jj,:) = Hii;
            r(jj,1) = [ z(1,i)-zhat(1,1);  (z(2,i)-zhat(2,1)) ];
            Rf(jj,jj) = R(ii,ii);
        end
    end
end

if nf~=0
    S = H*Pe*H'+ Rf;
    S = (S+S')*0.5;

    if isspd(S)

        K = Pe*H'/S;
        xe = xe + K*r;
        Pe = (eye(length(Pe)) - K*H) * Pe *(eye(length(Pe)) - K*H)' + K*Rf*K';

    end
end

% % augment
for i= 1:lenz
    % data association (known)
    is_exist = ~(lm_seq - z(3,i));
    idx = find(is_exist);

    lenx= size(xe,1);
    ii = 2*i + (-1:0);

    % add the new landmark into the state vector
    if isempty(idx)
        lm_seq = [lm_seq; z(3,i)];

        if gDISTBEAR
            % augment state
            d = z(1,i);
            th = z(2,i);
            k_xL = [d*cos(th); d*sin(th)];

            x_L = xe(1:2,1) + [ d*cos(th+xe(3)); d*sin(th+xe(3)) ];
            xe = [xe; x_L];

            % jacobians
            J = [0 -1; 1 0];
            C = [cos(xe(3))  -sin(xe(3));   sin(xe(3))   cos(xe(3)) ];

            H_Lk = [ 1/norm(k_xL)*k_xL'; 1/norm(k_xL)^2*k_xL'*J' ];

            HR = - H_Lk* C'*[eye(2)  J*(x_L-xe(1:2,1))];
            HL = H_Lk* C';

        else
            % augment state
            k_xL = z(1:2,i);

            C = [cos(xe(3)) -sin(xe(3));  sin(xe(3)) cos(xe(3)) ];
            x_L = xe(1:2,1) + C*k_xL;
            xe = [xe; x_L];

            % jacobians
            J = [0 -1; 1 0];
            C = [cos(xe(3))  -sin(xe(3));   sin(xe(3))   cos(xe(3)) ];

            HR = - C'*[eye(2)  J*(x_L-xe(1:2,1))];
            HL = C';
        end

        V_std = null([HR,HL]);

        % augment covariance
        rng= lenx+1:lenx+2;
        Pe(rng,rng)= inv(HL)*HR*Pe(1:3,1:3)*HR'*inv(HL)' + inv(HL)*R(ii,ii)*inv(HL)'; % landmark cov
        Pe(rng,1:3)= -inv(HL)*HR*Pe(1:3,1:3); % landmark-robot xcorr
        Pe(1:3,rng)= Pe(rng,1:3)';
        if lenx>3
            rnm= 4:lenx;
            Pe(rng,rnm)= -inv(HL)*HR*Pe(1:3,rnm);
            Pe(rnm,rng)= Pe(rng,rnm)';
        end
        Pe = 0.5*(Pe+Pe');
    end
end


function x = ProxLP(y,p,tau)

if p==Inf
    p = 1e3;
end

if length(y(:))>1
    for i=1:length(y(:))
        x(i)=ProxLP(y(i),p,tau);
    end
    x = reshape(x,size(y));
    return
end

x = linspace(0,abs(y)*10,1000);
[~,k] = min( 1/2*(x-abs(y)).^2 + tau*abs(x).^p );
x = x(k)*sign(y);

end

github-repos/MATLAB/optimaltransport-optimaltransport.github.io/optimaltransport.github.io-2fa6db6e6a48ab9bd6676088db00bd7c5c8b4203/code/gradflow-metric/ProxLP (Copie en conflit de Gabriel Peyré 2017-11-26).m


% the basic RIM quantifier: Q(r)=r^alpha
function re=weight1(n,m) % n-length of weighting vector, m - is alpha
re=[];
for h=1:n
    re=[re ((h/n).^m-((h-1)/n).^m)];
end
end


function [Model, Info] = linear_sparse_space_sw(X,Y,Model,parm)
%  Estimate linear weight matrix for input-output mapping
%     Automatic Relevance Prior for each input dimension
%     in original input space (not for delay embedding space)
%     is imposed to get sparse weight matrix
%  Trial data with variable sample number is supported
%     by setting sample number of each trial
%  Notice !!
%     Delay embedding is done in this module,
%     then input vector should be original input data without embedding
%
%     Covariance is calculated in original input space,
%     then increse of embedding dimension make no problem in computation
%
%   [Model, Info] = linear_sparse_space_sw(X,Y,Model,parm)
% --- Input
%  Y  : Output data ( N x T x Ntrial )
%  X  : Input data  ( M x (T + (Dtau-1)*Tau)  x Ntrial)
%  N  =  # of output
%  M  =  # of input (original input space dimension)
%  T  =  # of time sample
%
%  Estimate the following model
%    Y(t) = W * [X(:, t + (Dtau-1)*Tau ); ...; X(:, t)]
%
%  Model : Structure for estimated model
%  if Model is empty, initialization is done before training
%  if Model is previous training result, re-training us done
%
%  parm  : Structure for learning parameter
%  parm.Npre_train :  # of VB-update in initial training
%  parm.Ntrain :  # of training
%  parm.Nskip  :  skip # for print
%  parm.a_min  :  Min value for pruning small variance component
%  parm.Prune  :  = 1 : Prune small variance & irrelevant input dimension
%
%  parm.Tau       = Lag time
%  parm.Dtau      = Number of embedding dimension
%    Total input dimension in embedding space is (M * parm.Dtau)
%  parm.Trial : Valid_sample number for each trial [Ntrial x 1]
%    if parm.Trial = [15 10] and T = 15,
%       last 5 samples in 2nd trial is not used for estimation
% --- Output
%  Model : Structure for estimated model
%  Model.SY  :  Noise variance         ( 1 x 1 )
%  Model.W   :  Weight matrix          ( N x M*D ) , D = parm.Dtau
%  Model.A   :  Prior weight variance  ( 1 x M*D )
%  Model.ix_act : Active index for W after pruning
%
%  Info  : Structure for learning process history
%  Info.FE  = LP + H : Free energy
%  Info.LP  = Log likelihood
%  Info.H   = - Model entropy
%
% 2009-10-18 Made by M. Sato

MINVAL = 1.0e-15;
MinCond = 1e-8;

fprintf('Linear sparse space (variable trial length) start\n')

% Dimension
[N ,T  ,Ntrial]  = size(Y); % N = # of output
[M ,Tx ,Ntrialx] = size(X); % M = # of input without embedding

% Number of samples for each trial
if isfield(parm,'Trial'),
	Trial = parm.Trial;
	if length(Trial) ~= Ntrial, error('Trial sample seting is wrong');end
	if max(Trial) > T, error('Trial sample seting is wrong');end
else
	Trial = repmat(T, [Ntrial 1]);
end
% Set valid sample flag for each trial
trial_sw = zeros(N,T,Ntrial);
for n = 1:Ntrial
	trial_sw(:,1:Trial(n),n) = 1;
end

Tall = sum(Trial);

if Ntrial~=Ntrialx, error('# of trial is different in X and Y'); end;

% # of total training iteration
Ntrain = parm.Ntrain;

Nskip  = 100;   % skip steps for display info
a_min  = 1e-10; % Minimum value for weight pruning
Fdiff  = 1e-10; % Threshold for convergence
Ncheck = 100;   % Minimum number of training iteration
Fstep  = 5;     % Free energy convergence check step
Prune  = 1;     % Prune mode

if isfield(parm,'Nskip'), Nskip  = parm.Nskip; end;
if isfield(parm,'Fdiff'), Fdiff   = parm.Fdiff; end;
if isfield(parm,'a_min'), a_min   = parm.a_min ; end;
if isfield(parm,'Prune'), Prune = parm.Prune; end;
if isfield(parm,'Ncheck'), Ncheck = parm.Ncheck; end
if isfield(parm,'Fstep'),Fstep  = parm.Fstep ; end

% # of embedding dimension
if isfield(parm,'Dtau')
	D    = parm.Dtau;
	tau  = parm.Tau;
else
	D    = 1;
	tau  = 1;
end

if Ntrain < 1, Info = []; return; end;

if isfield(parm,'Npre_train')
	Npre_train = parm.Npre_train;
else
%	Npre_train = Ntrain;
	if Tall >= 2*M*D
		Npre_train = 0;
	elseif Tall >= M*D
		Npre_train = fix(Ntrain/2);
	else
		Npre_train = Ntrain;
	end
end

if Npre_train > Ntrain, Npre_train = Ntrain; end;

fprintf('--- Output Dimension  = %d\n',N)
fprintf('--- Input  Dimension  = %d\n',M)
fprintf('--- Embedding  Dimension  = %d\n',D)
fprintf('--- Number of trials  = %d\n',Ntrial)
fprintf('--- Number of training sample = %d\n',Tall)
fprintf('--- Total update iteration    = %d (%d)\n',Ntrain,Npre_train)

% Original input dimension
Xdim  = M;
M_ALL = Xdim*D;

%
% --- Initialization
%  W , A : Initial variable to use 1st update
%

% Input/Output variance
sx = mean((X(:) - mean(X(:))).^2);
sy = mean((Y(:) - mean(Y(:))).^2);

SY0 = mean(sy);
A0  = 1./mean(sx);

if isfield(parm,'Ta0') && parm.Ta0 > 0,
	Ta0 = parm.Ta0;
	a0  = parm.a0 * A0;
else
	Ta0 = 0;
	a0  = 1;
end

% 'linear_sparse_space'
%    ARD term = alpha * W^2  : A = W^2
if isfield(Model,'ix_act')
	SY  = mean(Model.SY);  % 1 x 1

	% Recover old estimate of A in full embedding space
	ix_act = Model.ix_act;
	Aold = zeros(1,M_ALL); % 1 x M_ALL
	Wold = zeros(N,M_ALL);

	Aold(ix_act) = sum(Model.A,1);
	Wold(:,ix_act) = Model.W;

	if M_ALL ~= Xdim*D,
		fprintf('M_ALL=%d,Xdim=%d,D=%d\n',M_ALL ,Xdim ,D)
		error('M_ALL ~= Xdim*D')
	end
	% Summation over time delay component
	A  = mean(reshape(Aold,[Xdim,D]),2);	% Xdim x 1

	% Active index in input space without embedding
	IX_act = find(A > 0);
	A  = A(IX_act);
	M  = length(IX_act);

	id  = repmat( (0:(D-1))* Xdim ,[M 1]) + repmat(IX_act, [1 D]);
	W   = Wold(:,id(:)) ;  % N x (M*D)

	X  = X(IX_act,:,:); % M x T
else
	A = repmat(A0, [Xdim, 1]);
	W = zeros(N,M_ALL);
	SY  = SY0;
	% Save original input index for pruning
	IX_act = (1:Xdim)';
end

rx  = 1/(D + 1);
SX  = SY*rx;

fprintf('a_min = %g\n', a_min)
fprintf('SY0 = %g\n', SY0)
fprintf('SY  = %g\n', SY)

% Active index in input space without embedding
ix_act  = (1:M)';

% Delay embedding index for W
Wid = [(0:(D-1))'* M + 1 , (1:D)'* M];

% Input covariance (Spatial dimension)
Trial = Trial+(D-1)*tau;

XX = zeros(M,M);
for n=1:Ntrial
	XX = XX + X(:,1:Trial(n), n) * X(:,1:Trial(n), n)';
end
XX  = XX / sum(Trial);

SW = inv( Tall.* XX + diag(1./A) );

% Working variable
dY  = zeros(N,T,Ntrial);% N x T x Ntrial
dYX = zeros(N,M*D);     % N x M*D
G_A = zeros(M,1);       % M x 1
WW  = sum(reshape(sum(W.^2,1),[M,D]),2);	% M x 1

% Free energy histry
FE  = zeros(Ntrain,1);
LP  = zeros(Ntrain,1);
H   = zeros(Ntrain,1);
Err = zeros(Ntrain,1);
Mhist = zeros(Ntrain,1);

% ARD hyper param. history
if isfield(parm,'Debug') && ~isempty(parm.Debug) && parm.Debug > 0
	Debug = 1;
	A_tmp = zeros(M_ALL, ceil(Ntrain/Nskip));
else
	Debug = 0;
end
% recover all component
A_all  = zeros(M,1);

k_save  = 0;

%%%%%% Learning Loop %%%%%%
for k=1:Ntrain
	% Ainv = alpha , A = 1/alpha
	Ainv    = 1./A;

	%    dY  = Y - W * X
	%    dYX = (dY * X')/T;
	dY  = error_delay_time_sw(X, Y, W, tau, Trial); % N x T x Ntrial
	dY  = dY .* trial_sw;                           % mask invalid samples
    dYX = error_corr_delay_sw(X, dY, D, tau, Trial);% N x M*D

    dYY = sum(dY(:).^2)/(N*Tall);
    dYX = dYX/(Tall);

    % Noise variance update
    WWA = sum(WW .*Ainv);
    SX  = (rx) .* dYY + WWA/(N*Tall) ;
	%  SX  = rx .* dYY + sum(SW(:) .* XX(:)) ./rx ;

    % Prevent zero variance
    SX  = max( SX, MINVAL);
	SY  = SX/rx;

    % Log variance
    SWA     = diag(SW) .* Ainv;
    log_sw  = (N*D)*(log_det(SW) + sum(log(Ainv)) - sum(SWA) + M);
    log_sy  = N * sum( log(SY) );
    log_a   = Ta0*sum(log(Ainv) - a0.*Ainv + 1);

    % Free energy
    LP(k)  = - (0.5*Tall) * log_sy ;
    H(k)   = 0.5*( log_sw + log_a );
%    H(k)   = 0.5*( log_sw + log_a - WWA );
    FE(k)  = LP(k) + H(k);
    Err(k) = sum(dYY)/(SY0);

    % Weight variance
    % SW = inv( (T./SX) .* XX + diag(Ainv) );   ( M x M )
    SW  = XX + diag( Ainv./Tall );

   	if rcond(SW) > MinCond,
		SW  = inv( SW );
	else
		SW  = pinv( SW );
	end

    % Weight update
    % W   = (T/SX)*( W * XX + rx .* dYX ) * SW; ( N x MD )
	for j=1:D
    	W(:,Wid(j,1):Wid(j,2)) = ( W(:,Wid(j,1):Wid(j,2)) * XX ...
    	                       + rx * dYX(:,Wid(j,1):Wid(j,2)) ) * SW;
    end

	%  ARD for each input (average over output & delay)
	WW  = sum(reshape(sum(W.^2,1),[M,D]),2);		% M x 1

    % Hyper parameter for weight variance (ARD)
    SW  = SW /Tall; % = inv(Tall*XX + diag(Ainv))
%  	G_A = 1 - diag(SW).*Ainv ;
%  	    = diag( ((Tall*XX + Ainv) - Ainv) * SW )
%  	    = diag( (Tall*XX) * SW )
  	G_A = Tall*sum(XX.*SW,2);
	G_A = max((G_A), MINVAL);

	if k <= Npre_train,
		% VB update rule (Stable)
		% (N*D)*A  = WW./SX + (N*D) * diag(SW)
		% (N*D)*A*(1 - diag(SW)./A)  = WW./SX
		% Modefied VB update rule (Stable)
		A  = (WW./SX + (N*D) * diag(SW) + 2*Ta0*a0)/( N*D + 2*Ta0 );
%	    A  = sqrt(A.*(WW./SX)./(G_A*N*D));
	else
	    % Accelerated update rule
	    A  = sqrt(A.*(WW./SX)./(G_A*N*D));
%	    A  = ((WW./SX) + 2*Ta0*a0)./(G_A*N*D + 2*Ta0);
	end

    % Prune small variance
    if Prune > 0
	    % Find active input dimension
	    ix_act_old = ix_act;

	    % Recover all component
	    switch	Prune
	    case	1
		    A_all(ix_act) = WW/max(WW);    % Prune by Weight
	    case	2
		    A_all(ix_act) = A /max(A);	   % Prune by Alpha
	    case	3
		    A_all(ix_act) = A * (1/SX);    % Prune by Alpha
	    end

	    % Find active input dimension (absolute index)
	    ix_act = find( A_all > a_min ); % effective indices
	    Mnew   = length(ix_act);  		% # of effective input

	    if Mnew < M,
		    % convert to relative index
		    jx_act = trans_index(ix_act,ix_act_old,Xdim);

		    A   = A(jx_act) ;  % 1 x M
		    SW  = SW(jx_act,jx_act);  % M x M

			% Effective delay embedding index for W
			id  = repmat( (0:(D-1))* M ,[Mnew 1]) + repmat(jx_act, [1 D]);
		    W   = W(:,id(:)) ;  % N x MD

			% New delay embedding index for W
		    M   = Mnew;
			Wid = [(0:(D-1))'* M + 1 , (1:D)'* M];

			WW  = WW(jx_act);
		    X   = X(jx_act,:,:);  	 % M x T x Ntrial
		    XX  = XX(jx_act,jx_act); % M x M
		end
    else
		A = max(A,MINVAL);
    end
    % END of if Prune == 1
	Mhist(k) = M;

    if mod(k, Nskip)==0
        % Save history
		if Debug == 1
        	k_save = k_save + 1;
        	A_tmp(:,k_save) = A_all(:);
		end

        fprintf('Iter=%4d, M =%4d, err=%g, F=%g, H=%g, SY=%g, Wmin=%g\n',...
        		        k, M, Err(k), FE(k), -2*H(k)/log(Tall), ...
        		        SY, min(WW)/max(WW));
    end

    % Convergence check
	if k > Ncheck,
		Fdif = (FE(k) - FE(k-Fstep))/(abs(FE(k))+eps);
	else
		Fdif = Fdiff + 1;
	end

	if (Fdiff > abs(Fdif)),
		fprintf('Converged : Free energy change = %g\n',Fdif)
		break;
	end;

end

% convert to relative index
ix_act = IX_act(ix_act);

% Delay embedding index for W in full input space
id  = repmat( (0:(D-1))* Xdim ,[M 1]) + repmat(ix_act, [1 D]);
% Recover A in full embedding input space
A   = repmat(A(:), [1 D]);

% Active index
Model.ix_act = id(:);
Model.M_all  = M_ALL ;

% Save output variable
Model.A    = A(:)' ; % 1 x M*D
Model.W    = W ;
Model.SY   = SY;
Model.SW   = SW; % = inv(Tall*XX + diag(Ainv))

Model.method = 'linear_sparse_space';
Model.mode   = 'cov';
Model.sparse = 'sparse';

% Save history
Info.FE  = FE(1:k);
Info.LP  = LP(1:k);
Info.H   = H(1:k) ;
Info.Err = Err(1:k);
Info.M   = Mhist(1:k);

if exist('A_tmp','var')
	Info.A   = A_tmp(:,1:k_save) ;
end

%%% ---- Index transformation from old active_index to current active_index
function	jx = trans_index(ix,ix_old,M)

N = length(ix_old);
Itrans = zeros(M,1);
Itrans(ix_old) = 1:N;

jx = Itrans(ix);


function tx = updateFastBoundaryClassifierFeatures(X, s1, s2, eid)
% tx = updateFastBoundaryClassifierFeatures(X, s1, s2, eid)
%
% Computes a simple set of features based on two regions s1 and s2, each
% containing one or more segments from X and separated by edglets given by
% eid.

ng = 5; % five geometric classes

tx = zeros([1 33], 'single');

f = 0;

%% Edge features
tx(f+1) = sum(X.edge.pb(eid).*X.edge.length(eid)) / sum(X.edge.length(eid));

area1 = X.region.area(s1);
area2 = X.region.area(s2);

f = f+1;

%% Region features

% area
sumarea1 = sum(area1);
sumarea2 = sum(area2);
tx(f+(1:2)) = [min([sumarea1 sumarea2], [], 2) max([sumarea1 sumarea2], [], 2)];

% color
meanColor1 = sum(X.region.colorMean(s1, :).*[area1 area1 area1], 1) / sumarea1;
meanColor2 = sum(X.region.colorMean(s2, :).*[area2 area2 area2], 1) / sumarea2;
tx(f+3) = sqrt(sum((meanColor1-meanColor2).^2, 2));

% position
left1 = min(X.region.x(s1, 1));  right1 = max(X.region.x(s1, 3));
left2 = min(X.region.x(s2, 1));  right2 = max(X.region.x(s2, 3));
bot1 = min(X.region.y(s1, 1));  top1 = max(X.region.y(s1, 3));
bot2 = min(X.region.y(s2, 1));  top2 = max(X.region.y(s2, 3));

tx(f+4) = top1-top2;
tx(f+5) = bot1-bot2;
tx(f+6) = top1-bot2;
tx(f+7) = bot1-top2;
tx(f+8) = top1-bot1;
tx(f+9) = top2-bot2;
tx(f+10) = left1-left2;
tx(f+11) = right1-right2;
tx(f+12) = right1-left1;
tx(f+13) = right2-left2;

% x alignment
x1 = [left1 right1];
x2 = [left2 right2];
tx(f+14) = (min([x1(:, 2) x2(:, 2)], [], 2)-max([x1(:, 1) x2(:, 1)], [], 2)) ./ ...
    (max([x1(:, 2) x2(:, 2)], [], 2)-min([x1(:, 1) x2(:, 1)], [], 2));

% y overlap
y1 = [bot1 top1];
y2 = [bot2 top2];
tx(f+15) = (min([y1(:, 2) y2(:, 2)], [], 2)-max([y1(:, 1) y2(:, 1)], [], 2)) ./ ...
    (max([y1(:, 2) y2(:, 2)], [], 2)-min([y1(:, 1) y2(:, 1)], [], 2));

f = f + 15;

%% 3D Geometry features

% geometric context features
gc = X.region.geomContext;
gc1 = sum(gc(s1, :).*repmat(area1, [1 size(gc, 2)]), 1) / sumarea1;
gc2 = sum(gc(s2, :).*repmat(area2, [1 size(gc, 2)]), 1) / sumarea2;

tx(f+(1:ng)) = gc1;
tx(f+ng+(1:ng)) = gc2;
tx(f+2*ng+(1:ng)) = gc1-gc2;
tx(f+3*ng+1) = sum(abs(gc1-gc2), 2)/2;

[maxval1, maxlab1] = max([gc1(:, 1) sum(gc1(:, 2:4), 2) gc1(:, 5)], [], 2);
[maxval2, maxlab2] = max([gc2(:, 1) sum(gc2(:, 2:4), 2) gc2(:, 5)], [], 2);
tx(f+3*ng+2) = (maxlab1-1)*3+ maxlab2;

f= f + 17;


function F = constraint(X,quantifier,Y)
% Internal class for constraint list

superiorto('sdpvar');
superiorto('double');
superiorto('logical');

% Evaluate
switch quantifier
    case '>'
        Z = X - Y;
    case '>='
        Z = X - Y;
    case '<'
        Z = Y - X;
    case '<='
        Z = Y - X;
    case '=='
        Z = Y - X;
    otherwise
        error('Quantifier not supported')
end

if isa(Z,'double')

    if  size(Z,1)==size(Z,2) &&  norm(Z-Z',inf)<1e-12 && ~isequal(quantifier,'==')
        checkSDP = 1;
    else
        checkSDP = 0;
    end

    if checkSDP
        if min(eig(Z))>=0
            warning('Inequality constraint evaluated to trivial true.')
            F = [];
            return
        else
            error('Inequality constraint evaluated to trivial false (no decision variable in constraint)')
        end
    else
        Z = Z(:);
        switch quantifier
            case '=='
                if all(Z)==0
                    warning('Equality constraint evaluated to trivial true.')
                    F = [];
                    return
                else
                    error('Equality constraint evaluated to trivial false (no decision variable in constraint)')
                end
            case {'<=','>='}
                if all(Z>=0)
                    warning('Inequality constraint evaluated to trivial true.')
                    F = [];
                    return
                else
                    error('Inequality constraint evaluated to trivial false (no decision variable in constraint)')
                end
            case {'<','>'}
                if all(Z>0)
                    warning('Inequality constraint evaluated to trivial true.')
                    F = [];
                    return
                else
                    error('Inequality constraint evaluated to trivial false (no decision variable in constraint)')
                end
        end
    end
end

switch quantifier
    case {'>','<'}
        F.strict(1) = 1;
    case {'>=','<=','=='}
        F.strict(1) = 0;
    otherwise
        error('Quantifier not supported')
end
if isa(Z,'sdpvar') && ~isequal(quantifier,'==')
    if issquare(Z)
        if ~ishermitian(Z)
            warning('YALMIP:SuspectNonSymmetry','Suspect non-symmetry in square constaint (Learn more) ')
        end
    end
end
F.List={X,quantifier,Y};
F.Evaluated{1} = Z;
F.ConstraintID = yalmip('ConstraintID');
F.tag{1} = '';
F = class(F,'constraint');


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
%==============================================================================
%
% function [yc,y1,y2,y3] = grid2grid(yc,m,in,out)
%
% transfers input grid (c,s,n) to output grid (c,s,n) using inter- and extrapolation
% central operation are reduce/expand, acting on each dimension individually
%
% Input:
%   yc        input grid
%   m         number of discretization points
%   in        type of input grid
%   out       type of output grid
% Output:
%   yc        output grid
%   y1,y2,y3  components of yc
%
%==============================================================================

function [yc,y1,y2,y3] = grid2grid(yc,m,in,out)

if nargin == 0, % help and minimal example
  runMinmalExample;
  return;
end;

dim = length(m);  y{1} = []; y{3} = []; y{3} = [];
if ~exist('out','var'), out = in;  end;

switch in,
  case {'centered','cell-centered'}, n = prod(m);
    % decompose Yin
    for j=1:dim, y{j} = reshape(yc((j-1)*n+(1:n)),m); end;
    switch out,
      case {'centered','cell-centered'},
      case 'staggered', for j=1:dim, y{j} = extend(y{j},j); end;
      case 'nodal',
        for j=1:dim,
          for k=1:dim, y{j} = extend(y{j},k); end;
        end;
    end;

  case 'staggered',
    % decompose Yin, get dimensions right
    E = eye(dim) + m'*ones(1,dim); n = cumsum([0,prod(E)]);
    for j=1:dim, y{j} = reshape(yc(n(j)+1:n(j+1)),E(:,j)'); end;
    switch out,
      case {'centered','cell-centered'}, for j=1:dim, y{j} = reduce(y{j},j); end;
      case 'staggered',
      case 'nodal',
        for j=1:dim,
          for k=setdiff(1:dim,j), y{j} = extend(y{j},k); end;
        end;
    end;

  case 'nodal', n = prod(m+1);
    % decompose Yin
    for j=1:dim, y{j} = reshape(yc((j-1)*n+(1:n)),m+1); end;
    switch out,
      case {'centered','cell-centered'},
        for j=1:dim, for k=1:dim, y{j} = reduce(y{j},k); end; end;
      case 'staggered',
        for j=1:dim, for k=setdiff(1:dim,j) y{j} = reduce(y{j},k); end; end;
      case 'nodal',
    end;

  otherwise,
    error('can not interpret flags')
end;
y1 = y{1}; y2 = y{2}; y3 = y{3}; yc = [y1(:);y2(:);y3(:)];

%------------------------------------------------------------------------------

% the following operators act on x=y{k} in the j-th direction,
% permuting x makes j the first direction, averaging (extend creates two
% additional outside points based on linear BC) and permute back

function x = reduce(x,j)
m = size(x); J = [j,setdiff(1:length(size(x)),j)];
x = permute(x,J); x = (x(1:end-1,:,:)+x(2:end,:,:))/2; x = ipermute(x,J);

function x = extend(x,j)
m = size(x); J = [j,setdiff(1:length(size(x)),j)]; x = permute(x,J);
x = [1.5*x(1,:,:)-0.5*x(2,:,:);(x(1:end-1,:,:)+x(2:end,:,:))/2;...
  1.5*x(end,:,:)-0.5*x(end-1,:,:)];
x = ipermute(x,J);

%------------------------------------------------------------------------------

function runMinmalExample
help(mfilename);
omega = [0,6,0,4]; m = [4 3];

h   = (omega(2:2:end)-omega(1:2:end))./m;
eps = 0.2*min(h);

yC  = getCellCenteredGrid(omega,m); %yC = yC + eps*randn(size(yC));
yN  = reshape(getNodalGrid(omega,m),[],2);
yS  = getStaggeredGrid(omega,m);

yCN = grid2grid(yC,m,'centered','nodal');
yCS = grid2grid(yC,m,'centered','staggered');
ySN = grid2grid(yS,m,'staggered','nodal');
ySC = grid2grid(yS,m,'staggered','centered');
yNC = grid2grid(yN,m,'nodal','centered');
yNS = grid2grid(yN,m,'nodal','staggered');

yC  = reshape(yC, [],2);
yN  = reshape(yN, [],2);
yCN = reshape(yCN,[],2);
ySN = reshape(ySN,[],2);
ySC = reshape(ySC,[],2);
yNC = reshape(yNC,[],2);

FAIRfigure(1); clf;

subplot(3,2,1);
plotGrid(yN,omega,m,'color','g'); hold on;
plot(yC(:,1),yC(:,2),'bs');
plot(yCN(:,1),yCN(:,2),'m*');
title(sprintf('%s: example cell-centered to nodel',mfilename));

subplot(3,2,2);
plotGrid(yN,omega,m,'color','g'); hold on;
plot(yC(:,1),yC(:,2),'bs');
[y11,y12,y21,y22] = splittStaggeredGrid(yCS,omega,m);
plot(y11,y12,'m>',y21,y22,'m^');
title(sprintf('%s: example cell-centered to staggered',mfilename));

[y11,y12,y21,y22] = splittStaggeredGrid(yS,omega,m);
subplot(3,2,3);
plotGrid(yN,omega,m,'color','g'); hold on;
plot(y11,y12,'b>',y21,y22,'b^');
plot(ySC(:,1),ySC(:,2),'ms');
title(sprintf('%s: example staggered to cell-centered',mfilename));

subplot(3,2,4);
plotGrid(yN,omega,m,'color','g'); hold on;
plot(y11,y12,'b>',y21,y22,'b^');
plot(ySN(:,1),ySN(:,2),'m*');
title(sprintf('%s: example staggered to nodal',mfilename));

subplot(3,2,5);
plotGrid(yN,omega,m,'color','g'); hold on;
plot(yN(:,1),yN(:,2),'b*');
plot(yNC(:,1),yNC(:,2),'ms');
title(sprintf('%s: example nodal to cell-centered',mfilename));

subplot(3,2,6);
plotGrid(yN,omega,m,'color','g'); hold on;
plot(yN(:,1),yN(:,2),'b*');
[y11,y12,y21,y22] = splittStaggeredGrid(yNS,omega,m);
plot(y11,y12,'m>',y21,y22,'m^');

title(sprintf('%s: example nodal to staggered',mfilename));

%------------------------------------------------------------------------------

function [y11,y12,y21,y22] = splittStaggeredGrid(yS,omega,m);
dim = length(omega)/2;
e  = @(i) (1:dim == i); % i-th unit vector
ns = cumsum([0;prod(ones(dim,1)*m+eye(dim),2)]);
yN = reshape(grid2grid(yS,m,'staggered','nodal'),[m+1,2]);
y11 = reshape(yS(ns(1)+1:ns(2)),m+e(1));
y12 = (yN(:,1:end-1,2)+yN(:,2:end,2))/2;
y21 = (yN(1:end-1,:,1)+yN(2:end,:,1))/2;
y22 = reshape(yS(ns(2)+1:ns(3)),m+e(2));

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


% Originally Coded By: Niall Mangan
% Copyright 2017, All Rights Reserved
% Code by Niall Mangan for paper "Inferring biological networks by sparse
% identification of nonlinear dynamics"
% by N. M. Mangan S. L. Brunton, J. L. Proctor, and J. N. Kutz
% Original Code: https://github.com/niallmm/iSINDy
%%
% This file is modified to compare the performance of i-SINDy on
% insufficient data.
% Modified by:K, 2019/07/16
% Find equations for yeast glycolysis state variable 6
%%
clc;clear all;close all;

addpath('./utils');
addpath('./bioutils');
FolderName='Results2';

% define libarary parameters
laurentorder = 0;
polyorder = 6;
usesine = 0;
dyorder = 1;

% clear variables from other solutions.
clear Theta Thetastring Xi indTheta lambdavec numterms errorv indopt
clear indTheta1 Xi1 numterms1 nT

% Here we load the previously simulated data
load('TrainingData.mat')

%% Define some parameters for the implicit SINDy
% Initial lambda value, which is the value used for soft thresholding in ADM
% To make implicit SINDy work, you need to carefully tune these two
% parameters. The below values are the best one that I found.
tol = 1e-4;
lambda = 3e-4;

% counter
jj = 1;

% initialize the number of nonzero terms found for the lambda
num= 1;
errorvec= 0;

% Define maximum iteration you need
MaxIter = 1000;

% Plot option, we set it to zero
plottag=0;

% Define the for loop
percent_start=0.3;
percent_end=1;
d_percent=0.1;

% Define how many results you want to get for each percentage
N_Iter=20;

for iter=1:N_Iter
    for percent=percent_start:d_percent:percent_end
        fprintf('\n\n\t Using %i percent of the data...\n',percent*100)

        fprintf('\n\t\t Calculating for the %i time...\n',iter)
        % Get the data length
        new_length=round(percent*length(xt));

        % Shuffel the original data
        Sequence=randperm(size(xt,1));
        xt_dum=xt(Sequence,:);
        dxt_dum=dxt(Sequence,:);

        % Assign the value to the new variables
        Data=xt_dum(1:new_length,:);dData=dxt_dum(1:new_length,:);

        % Define the number of states
        n=size(Data,2);

        % pool Data  (i.e., build library of nonlinear time series)
        [Theta, Thetastring] = poolDatady(Data,n,polyorder,usesine, laurentorder, dData(:,6), dyorder);

        % for now calculate null space using null function
        nT = null(Theta);

        % Get the sparse vector
        tic
        [indTheta1, Xi1, numterms1] = ADMinitvary(nT,lambda,MaxIter,tol, plottag);
        fprintf('\t\t\t Calculation finished! Used %d seconds.\n',toc)

        % Save the calculation result of current iteration
        fprintf('\n\t\t\t\t Saving the result...\n')
        tic
        cc=clock;
        ResultName=strcat(FolderName,'/implicit_SINDY_Data_Length_',num2str(percent*100),'_','Iter',num2str(iter),'_',num2str(cc(3)),'_',num2str(cc(4)),'_',num2str(cc(5)),'_',num2str(round(cc(6))),'.mat');
        save(ResultName,'Xi1','indTheta1','percent')
        fprintf('\n\t\t\t\t Saving finished! Using %i seconds...\n',toc)

    end
end


function [ fx ] = f_BSLinGame(x,theta,u,in)
% evolution function for BSL learner engaging in dyadic games
% [ fx ] = f_BSLinGame(x,theta,u,in)
% BSL is simply tracking the log-odds of P(o_t=1|o_{t-1}), where o is the
% opponent's binary action. This variable is updated according to a
% Laplace-Kalman filter, yielding 2 sufficient statistics (m and V) per
% combination of past outcome. BSL can learn sequences of arbitrary depth
% (K). For example, if K=1, then BSL tracks 2 probabilities, namely:
% P(o_t=1|o_{t-1}=1) and P(o_t=1|o_{t-1}=0). More generally, BSL tracks 2^K
% probabilities. In this scheme, the only evolution param (theta) is BSL's
% prior volatity about the log-odds.
% Note: unsampled sequences will eventually be "forgotten", since the
% prediction step in the Laplace-Kalman update will dilute any previously
% sampled evidence.
% IN:
%   - x: sufficient statistics of log-odds of P(o=1):
%       x(1:2^K)= E[log-odds]
%       x((2^K)+1:2^(K+1))= log V[log-odds]
%   - theta: BSL's prior volatity
%   - u: sequence of past outcomes:
%       u(1)= opponent's last move
%       u(2)= learner's last move
%       u(3:K+2) = sequence of K past opponent's moves
%   - in: depth of sequence learning
% OUT:
%   - fx: updated sufficient statistics of log-odds of P(o=1)

if VBA_isWeird (u) % e.g., 1st trial
    fx = x;
    return
end

u(2) = []; % remove agent's previous move
[fx] = f_BSL(x,theta,u,in);


function [elem2dof,dofSign,edge] = dofBDM1(elem)
%% DOFBDM1 dof structure for BDM1 element
%
% [elem2dof,dofSign,edge] = dofBDM1(elem) constructs data structure for
% the BDM face element in 3-D. elem is the connectivity matrix for a 3-D
% triangulation. elem2dof is the elementwise pointer from elem to dof
% indices. dofSign records the consistency of the local and global edge
% orientation. edge is the edge matrix.
%
% Added by Ming Wang.
%
%  See also dof3BDM1, dofRT0, dof3RT0
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

[elem2edge,dofSign,edge] = dofRT0(elem);
NT = size(elem,1);  NE = size(edge,1);
totaledge = uint32([elem(:,[2 3]); elem(:,[3 1]); elem(:,[1 2])]);
[tempvar,i] = sort(totaledge,2); %#ok<*ASGLU>
[tempvar,j]= sort(i,2);
elem2dof = repmat(elem2edge(:),1,2) + uint32((j-1)*NE);
elem2dof = reshape(elem2dof(:),NT,6);
dofSign = repmat(dofSign,1,2);


% Test file for @chebfun/join.m.

function pass = test_join(pref)

if ( nargin == 0  )
    pref = chebfunpref();
end

% Generate a few random points in [-1 1] to use as test values.
seedRNG(7681);
xr = 2 * rand(1000, 1) - 1;

% Check scalar CHEBFUNs.
f_op = @sin;
f1 = chebfun(f_op, [-1 -0.5 0]);
f2 = chebfun(f_op, [0 0.5 1]);
f = join(f1, f2);
pass(1) = norm(feval(f, xr) - f_op(xr), inf) < 10*vscale(f)*eps;

% Check array-valued CHEBFUNs.
f_op = @(x) [sin(x) cos(x)];
f1 = chebfun(f_op, [-1 -0.5 0]);
f2 = chebfun(f_op, [0 0.5 1]);
f = join(f1, f2);
err = feval(f, xr) - f_op(xr);
pass(2) = norm(err(:), inf) < 10*vscale(f)*eps;

% Check for quasimatrices.
f1q = quasimatrix(f1);
f2q = quasimatrix(f2);
fq = join(f1q, f2q);
err = feval(fq, xr) - f_op(xr);
pass(3) = norm(err(:), inf) < 10*vscale(fq)*eps;

% Check row CHEBFUNs.
ft = join(f1.', f2.');
err = feval(ft, xr) - f_op(xr).';
pass(4) = ft.isTransposed && (norm(err(:), inf) < 10*vscale(ft)*eps);

% Check row quasimatrices.
ftq = join(f1q.', f2q.');
err = feval(ftq, xr) - f_op(xr).';
pass(5) = ftq(1,:).isTransposed && (norm(err(:), inf) < 10*vscale(ftq)*eps);

% Check operation when the domains don't match.
f = chebfun(@sin, [-1 -0.5 0]);
g = chebfun(@cos, [1 1.5 2]);
h = join(f, g);
pass(6) = isequal(h.domain, [-1 -0.5 0 0.5 1]) && ...
    norm(feval(h, xr) - h_exact_fun(xr), inf) < 10*vscale(h)*eps;

% Check error conditions.
try
    f = join(f1, f2.');
    pass(7) = false;
catch ME
    pass(7) = strcmp(ME.identifier, 'CHEBFUN:CHEBFUN:join:columnJoin:trans') ...
        || strcmp(ME.identifier, 'CHEBFUN:CHEBFUN:join:dim');
end

% Test for singular function:
op1 = @(x) sin(x)./(x+1);
op2 = @(x) sin(x);
f = chebfun(op1, [-1 -0.5 0], 'exps', [-1 0 0]);
g = chebfun(op2, [1 2]);
h = join(f, g);
x = sort(xr);
h_vals = feval(h, x);
ind = ( x < 0 );
indComp = ~ind;
h_exact1 = feval(op1, x(ind));
h_exact2 = feval(op2, x(indComp)+1);
h_exact = [h_exact1; h_exact2];
err = h_exact - h_vals;
pass(8) = isequal(h.domain, [-1 -0.5 0 1]) && ...
    norm(err, inf) < 1e5*vscale(h)*eps;

% Test for function defined on unbounded domain:

% Set the domain:
domf = [-Inf -3];
domg = [1 Inf];
domCheck = [-1e2 1e2];

% Generate a few random points to use as test values:
x = diff(domCheck) * rand(100, 1) + domCheck(1);

opf = @(x) x.*exp(x);
f = chebfun(opf, domf);
opg = @(x) (1-exp(-x))./x;
g = chebfun(opg, domg);
h = join(f, g);
x = sort(x);
h_vals = feval(h, x);
ind = ( x < -3 );
indComp = ~ind;
h_exactf = feval(opf, x(ind));
h_exactg = feval(opg, x(indComp) + (domg(1) - domf(2)));
h_exact = [h_exactf; h_exactg];
err = h_exact - h_vals;
pass(9) = isequal(h.domain, [-Inf -3 Inf]) && ...
    norm(err, inf) < 1e1*vscale(h)*eps;

end

function y = h_exact_fun(x)
    y = zeros(size(x));

    xl_ind = x <= 0;
    xl = x(xl_ind);
    y(xl_ind) = sin(xl);

    xr_ind = x > 0;
    xr = x(xr_ind);
    y(xr_ind) = cos(xr + 1);
end


function c = band(a,p,q)
%BAND         Extract band from matrix a, lower bandwidth p, upper bandwidth q
%   if parameter q is omitted, q:=p
%
%   c = band(a,p,q)
%

% written  10/16/98     S.M. Rump
% modified 11/30/98     S.M. Rump  improved speed
% modified 04/04/04     S.M. Rump  set round to nearest for safety
% modified 04/06/05     S.M. Rump  rounding unchanged
%

  if nargin<3
    q = p;
  end

  c = a;

  if a.complex
    c.mid = tril(triu(a.mid,-p),q);
    c.rad = tril(triu(a.rad,-p),q);
  else
    c.inf = tril(triu(a.inf,-p),q);
    c.sup = tril(triu(a.sup,-p),q);
  end