function F = diff(F, k, dim)
%DIFF   Componentwise partial derivative of a CHEBFUN3V object.
%   DIFF(F) is the derivative of components of F along the first variable.
%
%   DIFF(F, K) is the Kth derivative of each component of F along the first
%   variabel.
%
%   DIFF(F, K, DIM) is the Kth derivative of F along the dimension DIM.
%   DIM = 1 (default) is the derivative in the 1st input variable.
%   DIM = 2 is the derivative in the 2nd variable.
%   DIM = 3 is the derivative in the 3rd varialbe.
%
%   DIFF(F, [K1 K2], [DIM1 DIM2]) means K1-th derivative of F in dimension
%   DIM1 and K2-th derivative in dimension DIM2. DIM1 and DIM2 can be 1, 2
%   or 3 in any order.
%
%   DIFF(F, [K1 K2 K3]) is the K1-th partial derivative of F in the first
%   variable, K2-th partial derivative of F in the second variable and
%   K3-th partial derivative of F in the third variable.
%   For example, DIFF(F, [1 2 3]) is d^6F/(dx d^2y d^3z).

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

% Empty check:
if ( isempty(F) )
    return
end

% Defaults:
if ( ( nargin == 1 ) || isempty(k) )
    k = 1;
end
if ( nargin < 3 )
    dim = 1;
end

% Diff each component.
for j = 1:F.nComponents
    F.components{j} = diff(F.components{j}, k, dim);
end

end


function L = resymbol (L, A)						    %#ok
%RESYMBOL recomputes the symbolic Cholesky factorization of the matrix A.
%
%   Example:
%   L = resymbol (L, A)
%
%   Recompute the symbolic Cholesky factorization of the matrix A.  A must be
%   symmetric.  Only tril(A) is used.  Entries in L that are not in the Cholesky
%   factorization of A are removed from L.  L can be from an LL' or LDL'
%   factorization (lchol or ldlchol).  resymbol is useful after a series of
%   downdates via ldlupdate, since downdates do not remove any entries in L.
%   The numerical values of A are ignored; only its nonzero pattern is used.
%
% See also LCHOL, LDLUPDATE

%   Copyright 2006-2007, Timothy A. Davis
%   http://www.cise.ufl.edu/research/sparse

error ('resymbol not found') ;


%INTIMAGE Compute integral image
%
% OUT = INTIMAGE(IM) is an integral image corresponding to IM.
%
% Integral images can be used for rapid computation of summations over
% rectangular regions.
%
% Examples::
% Create integral images for sum of pixels over rectangular regions
%        i = intimage(im);
%
% Create integral images for sum of pixel squared values over rectangular
% regions
%        i = intimage(im.^2);
%
% See also IISUM.

% Copyright (C) 1993-2011, by Peter I. Corke
%
% This file is part of The Machine Vision Toolbox for Matlab (MVTB).
%
% MVTB 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.
%
% MVTB 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 MVTB.  If not, see <http://www.gnu.org/licenses/>.

function ii = intgimage(I)

    ii = cumsum( cumsum(I)' )';


function [Jul1,Jul2]=GPS2TCG(Jul1,Jul2)
%GPS2TT  Convert from the timescale used by the Global Positioning System
%           (GPS)  given as a two-part Julian date to geocentric coordinate
%           time (TCG), represented as a two-part Julian date.
%
%INPUTS:    Jul1, Jul2  Two parts of a Julian date given in GPS time. The
%                       units of the date are days. The full date is the
%                       sum of both terms. The date is broken into two
%                       parts to provide more bits of precision. It does
%                       not matter how the date is split.
%
%OUTPUTS:   Jul1, Jul2  The time as a Julian date in TCG.
%
%This function just calls GPS2TAU and TAI2TCG.
%
%Many temporal coordinate systems standards are compared in [1].
%
%REFERENCES:
%[1] D. F. Crouse, "An Overview of Major Terrestrial, Celestial, and
%    Temporal Coordinate Systems for Target Tracking," Formal Report, Naval
%    Research Laboratory, no. NRL/FR/5344--16-10,279, 10 Aug. 2016, 173
%    pages.
%
%October 2013 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

[Jul1,Jul2]=GPS2TAI(Jul1,Jul2);
[Jul1,Jul2]=TAI2TCG(Jul1,Jul2);
end

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


classdef EKF_filter_jekeli76 < handle
    properties (Hidden)
        type = 'ekf';
        tag  = 'EKF_crude';  % ID tag
        covDim=3; % the dimension of covariance matrix, Rx RY RZ in earth centered
        % n-frame, vn, RPY, acc bias, gyro bias, acc scale and gyro scale
    end
    % The following properties can be set only by class methods
    properties (SetAccess = private)
        dt; % sampling interval of IMU, unit sec
        % the translation from antenna to mems frame, i.e., the antenna's
        % position in the mems frame.
        xi;
        vi;
        bias=0;
        p_k_k; % the covariance of the entire state vector
        psdq; % process noise PSD
    end
    methods
        function filter = EKF_filter_jekeli76(initX, initV, deltat, procnoiseq)
            filter.xi=initX;
            filter.vi=initV;
            filter.dt=deltat;
            filter.psdq=procnoiseq;
            filter.p_k_k=eye(filter.covDim);
        end
        %===============================================================================================
        %-- State transition function
        % propogate state with accelerometer and gyro input at time k-1 to state at k,
        % i.e., X(k|k-1), from state at k-1. U1 contains IMU measurement
        % U1 is the measured acceleration
        function ffun_state(filter, U1)

            ahat=U1-filter.bias;
            vi1=filter.vi+ahat*filter.dt;
           filter.xi=filter.xi+(vi1+filter.vi)/2*filter.dt;
           filter.vi=vi1;
        end
        function ffun_covariance(filter)
            %propagate the covariance corresponds to states, rs in e, vs in e, q s2e,
            deltat=filter.dt;
            STM=[1, 0, deltat; deltat, 1, .5*deltat^2; 0,0, 1];
            Qd=filter.psdq*[deltat, .5*deltat^2, 0; .5*deltat^2, deltat^3/3, 0; 0, 0,0];
            filter.p_k_k=STM*filter.p_k_k*STM'+Qd;  % the covariance of the navigation states and imu error terms
        end
        %==============================================================================================
        %update both the covariance and state
        function correctstates(filter, predict,measure, H,R)
            p_km1_k=filter.p_k_k;
            inno= predict-measure;
            %Kalman
            K=p_km1_k*H'/(H*p_km1_k*H'+R);
            deltaX=K*inno;
            % update covariance
            filter.p_k_k=(eye(filter.covDim)-K*H)*p_km1_k*(eye(filter.covDim)-K*H)'+K*R*K';
            % compute updated state
            filter.xi=filter.xi-deltaX(1);
            filter.vi=filter.vi-deltaX(2);
            filter.bias = filter.bias + deltaX(3);
        end
        function SaveToFile(filter, preimutime, ffilres)
            %Write result to the files
            fprintf(ffilres,'%6.2f\t%6.3f\t%6.3f\t%6.3f\t%6.3f\t%6.3f\t%6.3f\n', ...
                [preimutime;filter.vi;filter.xi;filter.bias;sqrt(diag(filter.p_k_k))]);
        end
    end
end


function [] = gsp_plot_filter(G, filters, param)
%GSP_PLOT_FILTER  Plot a system of filters
%   Usage:  [filter_data,test_sum]=gsp_plot_filter(G,filters);
%           [filter_data,test_sum]=gsp_plot_filter(G,filters,param);
%
%   Input parameters:
%       G       : Graph object (Or lmax)
%       filters : Cell array of filters (or single filter)
%       param   : Optional variable containing additional parameters
%   Output parameters:
%       none
%
%
%   'gsp_plot_filters(G, filter, param)' plots a system of graph spectral
%   filters.
%
%   Example:::
%
%         Nf = 4;
%         G = gsp_sensor(100);
%         G = gsp_estimate_lmax(G);
%         g = gsp_design_mexican_hat(G, Nf);
%         gsp_plot_filter(G, g);
%
%
%   Additional parameters
%   ---------------------
%
%   * *param.line_width* : Width of the filter plots (default 4).
%   * *param.npoints* : Number of point where the filters are evaluated
%     (default 1000).
%   * *param.x_tic* : Distance between x-tick labels.
%   * *param.y_tic* : Distance between y-tick labels (default 0.25).
%   * *param.minor_tick* : To show minor tick marks (default 1).
%   * *param.plot_eigenvalues* : To plot black X marks at all eigenvalues
%     of the graph (You need to compute the Fourier basis to use this
%     option). By default the eigenvalues are plot if they are contained in
%     the Graph.
%   * *param.lambda_highlights* : To plot red X marks at highlight
%     eigenvalues (default 0).
%   * *param.x_width* : Width of X marks for the eigenvalues (default 3).
%   * *param.x_size* : Size of X marks for the eigenvalues (default 8).
%   * *param.show_sum* : To plot an extra line showing the sum of the
%     squared magnitudes of the filters (default 1 if there is multiple
%     filters).
%   * *param.colors_rgb* : To specify the line colors.
%   * *param.cla* : Clear axis (default 1).
%   * *param.yrange* : To specify a range for the y axis.
%   * *param.verbose* : Verbosity level (1 display the warning - 0 no log)
%     (default 1).
%
%   Demos: gsp_demo
%

% Author : David I Shuman, Nathanael Perraudin
% Testing: test_filter

% Read input parameters
if nargin < 3
   param = struct;
end

if ~isstruct(G)
   G.lmax = G;
   param.plot_eigenvalues = 0;
end

if ~isfield(G,'lmax')
    G = gsp_estimate_lmax(G);
    warning(['GSP_FILTER_ANALYSIS: The variable lmax is not ',...
        'available. The function will compute it for you. ',...
        'However, if you apply many time this function, you ',...
        'should precompute it using the function: ',...
        'gsp_estimate_lmax']);
end

if ~isfield(param,'verbose'), param.verbose = 1; end
if ~isfield(param,'line_width'), param.line_width = 4; end
if ~isfield(param,'x_width'), param.x_width = 3; end
if ~isfield(param,'x_size'), param.x_size = 8; end
if ~isfield(param,'show_sum'), param.show_sum = length(filters)>1; end
if ~isfield(param,'y_tic'), param.y_tic = 0.25; end
if ~isfield(param,'minor_tick'), param.minor_tick = 1; end
if ~isfield(param,'npoints'), param.npoints = 1000; end
if ~isfield(param,'cla'), param.cla = 1; end
if ~isfield(param,'x_tic')
    param.x_tic=max(1,ceil(G.lmax/10));
end
if ~isfield(param,'plot_eigenvalues')
    param.plot_eigenvalues = isfield(G,'e');
end

if ~isfield(G,'lmax')
    if param.verbose
        fprintf('GSP_KERNEL_MEXICAN_HAT has to compute lmax \n')
    end
    G = gsp_estimate_lmax(G);
end

lambdas = linspace(0,G.lmax,param.npoints);

if param.cla
    cla;
end
hold on;
% apply the filter
fd = gsp_filter_evaluate(filters,lambdas);

% plot the filter
plot(lambdas,fd,'LineWidth',param.line_width);

% plot the eigenvalues
if param.plot_eigenvalues
    if isfield(G,'e')
        plot(G.e,zeros(G.N,1),'xk','LineWidth',...
            param.x_width,'MarkerSize',param.x_size);
    else
        if param.verbose
            warning('GSP_PLOT_FILTER: No eigenvalues found in the graph');
        end
    end
end

% plot hightlights eigenvalues
if isfield(param,'lambda_highlights')
    plot(param.lambda_highlights, ...
        zeros(length(param.lambda_highlights),1),...
        'xr','LineWidth',param.x_width,'MarkerSize',param.x_size);
end

% plot the sum
if param.show_sum
    test_sum=sum(fd.^2,2);
    plot(lambdas,test_sum,'k','LineWidth',param.line_width);
end

box on;
% X axis
xlim(full([0 G.lmax]));
% Y axis
if isfield(param,'yrange')
    yrange=param.yrange;
    ylim(yrange);
    set(gca,'YTick',yrange(1):y_tic:yrange(2));
end

% Add marks
if param.minor_tick
    set(gca,'XMinorTick','on','YMinorTick','on');

end

% Change the color
if isfield(param,'colors_rgb');
    set(gca, 'ColorOrder', param.colors_rgb);
end
set(gca,'XTick',0:param.x_tic:G.lmax);

hold off;
end


% OP_UGRADU_JAC_V: assemble the second term of the Jacobian of the
% convective term (see solve_navier_stokes for usage)
%
%   mat = op_ugradu_jac_v (spu, spv, msh, coeff);
%   [rows, cols, values] = op_ugradu_jac (spu, spv, msh, uder);
%
% INPUT:
%
%  spu:   structure representing the space of trial functions (see sp_scalar/sp_evaluate_col)
%  spv:   structure representing the space of test functions  (see sp_scalar/sp_evaluate_col)
%  msh:   structure containing the domain partition and the quadrature rule (see msh_cartesian/msh_evaluate_col)
%  uder:  derivative of the velocity, evaluated in all the degrees of freedom
%
% OUTPUT:
%
%  mat:    assembled jacobian matrix
%  rows:   row indices of the nonzero entries
%  cols:   column indices of the nonzero entries
%  values: values of the nonzero entries
%
% Copyright (C) 2018 Luca Coradello, Luca Pegolotti
%
%    This program is free software: you can redistribute it and/or modify
%    it under the terms of the GNU General Public License as published by
%    the Free Software Foundation, either version 3 of the License, or
%    (at your option) any later version.

%    This program is distributed in the hope that it will be useful,
%    but WITHOUT ANY WARRANTY; without even the implied warranty of
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%    GNU General Public License for more details.
%
%    You should have received a copy of the GNU General Public License
%    along with this program.  If not, see <http://www.gnu.org/licenses/>.

function varargout = op_ugradu_jac_v (spu, spv, msh, uder)

  gradu_shpu = sum(bsxfun( @times, reshape(spu.shape_functions, ...
                   [1 size(spu.shape_functions)]),uder),2);

  shpu = reshape (gradu_shpu, spu.ncomp, msh.nqn, spu.nsh_max, msh.nel);
  shpv = reshape (spv.shape_functions, spv.ncomp, msh.nqn, spv.nsh_max, msh.nel);

  rows = zeros (msh.nel * spu.nsh_max * spv.nsh_max, 1);
  cols = zeros (msh.nel * spu.nsh_max * spv.nsh_max, 1);
  values = zeros (msh.nel * spu.nsh_max * spv.nsh_max, 1);

  jacdet_weights = msh.jacdet .* msh.quad_weights;

  ncounter = 0;
  for iel = 1:msh.nel
    if (all (msh.jacdet(:,iel)))
      shpu_iel = reshape (shpu(:, :, 1:spu.nsh(iel), iel), spu.ncomp, msh.nqn, 1, spu.nsh(iel));
      shpv_iel = reshape (shpv(:, :, 1:spv.nsh(iel), iel), spv.ncomp, msh.nqn, spv.nsh(iel), 1);

      jacdet_iel = reshape (jacdet_weights(:,iel), [1,msh.nqn,1,1]);

      jacdet_shpu = bsxfun (@times, jacdet_iel, shpu_iel);
      tmp1 = sum (bsxfun (@times, jacdet_shpu, shpv_iel), 1);
      values(ncounter+(1:spu.nsh(iel)*spv.nsh(iel))) = reshape (sum (tmp1, 2), spv.nsh(iel), spu.nsh(iel));

      [rows_loc, cols_loc] = ndgrid (spv.connectivity(:,iel), spu.connectivity(:,iel));
      rows(ncounter+(1:spu.nsh(iel)*spv.nsh(iel))) = rows_loc;
      cols(ncounter+(1:spu.nsh(iel)*spv.nsh(iel))) = cols_loc;
      ncounter = ncounter + spu.nsh(iel)*spv.nsh(iel);
    else
      warning ('geopdes:jacdet_zero_at_quad_node', 'op_u_v: singular map in element number %d', iel)
    end
  end

  if (nargout == 1 || nargout == 0)
    varargout{1} = sparse (rows(1:ncounter), cols(1:ncounter), ...
                           values(1:ncounter), spv.ndof, spu.ndof);
  elseif (nargout == 3)
    varargout{1} = rows(1:ncounter);
    varargout{2} = cols(1:ncounter);
    varargout{3} = values(1:ncounter);
  else
    error ('op_ugradu_jac_v: wrong number of output arguments')
  end

end


function [map, aps, pr, prs] = compute_map (ranks, gnd, kappas)
% COMPUTE_MAP  This function computes the mAP for a given set of returned results.
%
% Usage:
%   map = compute_map (ranks, gnd)
%         computes mean average precsion (map) only
%
%   [map, aps, pr, prs] = compute_map (ranks, gnd, kappas)
%         computes mean average precision (map), average precision (aps) for each query
%         computes mean precision at kappas (pr), precision at kappas (prs) for each query
%
% Notes:
% 1) ranks starts from 1, size(ranks) = db_size X #queries
% 2) The junk results (e.g., the query itself) should be declared in the gnd stuct array
% 3) If there are no positive images for some query, that query is excluded from the evaluation

  if ~exist('kappas'), kappas = 0; end

  nq = numel (gnd);   % number of queries
  % init map and pr
  map = 0;
  aps = zeros (nq, 1);
  pr = zeros(1, numel(kappas));
  prs = zeros (nq, numel(kappas));
  nempty = 0;

  for i = 1:nq
    qgnd = gnd(i).ok;

    if isempty(qgnd) % no positive at all, skip from the average
      aps (i) = nan;
      prs (i, :) = nan;
      nempty = nempty + 1;
      continue;
    end

    if isfield (gnd(i), 'junk')
      qgndj = gnd(i).junk;
    else
      qgndj = [];
    end

    % positions of positive and junk images
    [~, pos] = intersect (ranks (:,i), qgnd);
    [~, junk] = intersect (ranks (:,i), qgndj);

    pos = sort(pos);
    junk = sort(junk);

    k = 0;
    ij = 1;

    if length (junk)
        % decrease positions of positives based on the number of junk images appearing before them
        ip = 1;
        while ip <= numel (pos)

            while ( ij <= length (junk) & pos (ip) > junk (ij) )
                k = k + 1;
                ij = ij + 1;
            end

            pos (ip) = pos (ip) - k;
            ip = ip + 1;
        end
    end

    % compute ap
    ap = score_ap_from_ranks1 (pos, length (qgnd));
    map = map + ap;
    aps (i) = ap;

    % compute precision@k
    for j = 1:numel(kappas)
      kq = min(max(pos), kappas(j));
      prs(i, j) = numel(find(pos <= kq)) ./ kq;
    end
    pr = pr + prs(i, :);

  end

  map = map / (nq-nempty);
  pr = pr / (nq-nempty);

end

% This function computes the AP for a query
function ap = score_ap_from_ranks1 (ranks, nres)

% number of images ranked by the system
nimgranks = length (ranks);
ranks = ranks - 1;

% accumulate trapezoids in PR-plot
ap = 0;

recall_step = 1 / nres;

for j = 1:nimgranks
  rank = ranks(j);

  if rank == 0
    precision_0 = 1.0;
  else
    precision_0 = (j - 1) / rank;
  end

  precision_1 = j / (rank + 1);
  ap = ap + (precision_0 + precision_1) * recall_step / 2;
end

end


% Test file for @classicfun/plus.m

function pass = test_plus(pref)

% Get preferences.
if ( nargin < 1 )
    pref = chebfunpref();
end

singPref = pref;
singPref.blowup = true;

% Set a domain for BNDFUN.
data.domain = [-2 7];

% Generate a few random points to use as test values.
seedRNG(6178);
x = diff(data.domain) * rand(100, 1) + data.domain(1);

% A random number to use as an arbitrary additive constant.
alpha = -0.194758928283640 + 0.075474485412665i;

%%
% Check operation in the face of empty arguments.
f = bndfun();
g = bndfun(@(x) x, data, pref);
pass(1) = (isempty(f + f) && isempty(f + g) && isempty(g + f));

%%
% Check addition with scalars.
f_op = @(x) sin(x);
f = bndfun(f_op, data, pref);
pass(2:3) = test_add_function_to_scalar(f, f_op, alpha, x);

%%
% Check addition of two BNDFUN objects.
f_op = @(x) zeros(size(x));
f = bndfun(f_op, data, pref);
pass(4:5) = test_add_function_to_function(f, f_op, f, f_op, x);

f_op = @(x) exp(x) - 1;
f = bndfun(f_op, data, pref);

g_op = @(x) 1./(1 + x.^2);
g = bndfun(g_op, data, pref);
pass(6:7) = test_add_function_to_function(f, f_op, g, g_op, x);

g_op = @(x) cos(1e4*x);
g = bndfun(g_op, data, pref);
pass(8:9) = test_add_function_to_function(f, f_op, g, g_op, x);

g_op = @(t) sinh(t*exp(2*pi*1i/6));
g = bndfun(g_op, data, pref);
pass(10:11) = test_add_function_to_function(f, f_op, g, g_op, x);

%%
% Check operation for array-valued BNDFUN objects.
f_op = @(x) [zeros(size(x)) zeros(size(x)) zeros(size(x))];
f = bndfun(f_op, data, pref);
pass(12:13) = test_add_function_to_function(f, f_op, f, f_op, x);

f_op = @(x) [sin(x) cos(x) exp(x)];
f = bndfun(f_op, data, pref);
pass(14:15) = test_add_function_to_scalar(f, f_op, alpha, x);

g_op = @(x) [cosh(x) airy(1i*x) sinh(x)];
g = bndfun(g_op, data, pref);
pass(16:17) = test_add_function_to_function(f, f_op, g, g_op, x);

%%
% This should fail with a dimension mismatch error.
g_op = @(x) sin(x);
g = bndfun(g_op, data, pref);
try
    h = f + g; %#ok<NASGU>
    if ( verLessThan('matlab', '9.1') )
        pass(18) = false;
    else
        pass(18) = true;
    end
catch ME
    if ( verLessThan('matlab', '9.1') )
        pass(18) = strcmp(ME.message, 'Matrix dimensions must agree.');
    else
        pass(18) = false;
    end
end

%%
% Check that direct construction and PLUS give comparable results.
tol = 10*eps;
f = bndfun(@(x) x, data, pref);
g = bndfun(@(x) cos(x) - 1, data, pref);
h1 = f + g;
h2 = bndfun(@(x) x + cos(x) - 1, data, pref);
pass(19) = norm(feval(h1, x) - feval(h2, x), inf) < 2*tol;

%%
% Check that adding a BNDFUN to an unhappy BNDFUN gives an unhappy
% result.
f = bndfun(@(x) cos(x + 1), data);    % Happy
g = bndfun(@(x) sqrt(x + 1), data);   % Unhappy
h = f + g;  % Add unhappy to happy.
pass(20) = (~get(g, 'ishappy')) && (~get(h, 'ishappy'));
h = g + f;  % Add happy to unhappy.
pass(21) = (~get(g, 'ishappy')) && (~get(h, 'ishappy'));

%%
% Test on singular BNDFUN.
pow = -1;
op1 = @(x) (x - data.domain(2)).^pow.*sin(x);
op2 = @(x) (x - data.domain(2)).^pow.*cos(3*x);
singData = data;
singData.exponents = [0 pow];
f = bndfun(op1, singData, singPref);
g = bndfun(op2, singData, singPref);
h = f + g;
vals_h = feval(h, x);
op = @(x)  (x - data.domain(2)).^pow.*(sin(x)+cos(3*x));
h_exact = op(x);
pass(22) = ( norm(vals_h-h_exact, inf) < 1e3*max(eps, ...
    eps)*norm(h_exact, inf) );

%% Test for UNBNDFUN:

% Functions on [-inf inf]:

% Set the domain:
data.domain = [-Inf Inf];
domCheck = [-1e2 1e2];

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

opf = @(x) exp(-x.^2);
opg = @(x) x.^2.*exp(-x.^2);
oph = @(x) exp(-x.^2) + x.^2.*exp(-x.^2);
f = unbndfun(opf, data);
g = unbndfun(opg, data);
h = f + g;
hVals = feval(h, x);
hExact = oph(x);
err = hVals - hExact;
pass(23) = norm(err, inf) < 1e1*eps*get(h,'vscale');

end

%%
% Test the addition of a BNDFUN F, specified by F_OP, to a scalar ALPHA using
% a grid of points X in [a  b] for testing samples.
function result = test_add_function_to_scalar(f, f_op, alpha, x)
    g1 = f + alpha;
    g2 = alpha + f;
    result(1) = isequal(g1, g2);
    g_exact = @(x) f_op(x) + alpha;
    result(2) = norm(feval(g1, x) - g_exact(x), inf) < ...
        10*max(get(g1, 'vscale')*eps);
end

%%
% Test the addition of two BNDFUN objects F and G, specified by F_OP and
% G_OP, using a grid of points X in [-1  1] for testing samples.
function result = test_add_function_to_function(f, f_op, g, g_op, x)
    h1 = f + g;
    h2 = g + f;
    result(1) = isequal(h1, h2);
    h_exact = @(x) f_op(x) + g_op(x);
    result(2) = norm(feval(h1, x) - h_exact(x), inf) <= ...
        100*max(get(h1, 'vscale')*eps);

end


% OPTIMIZE_FILTER Optimize filter representation
%
% Usage
%    filter = OPTIMIZE_FILTER(filter_f, lowpass, options)
%
% Input
%    filter_f (numeric): The Fourier transform of the filter.
%    lowpass (boolean): If true, filter_f contains a lowpass filter.
%    options (struct): Various options on how to optimize the filter:
%       options.filter_format (char): Specifies the type of optimization,
%          either 'fourier', 'fourier_multires' or 'fourier_truncated'. See
%          description for more details.
%       options.truncate_threshold (numeric): If options.filter_format is
%          'fourier_truncated', this indicates the threshold to be passed on
%          to TRUNCATE_FILTER. See the documentation of this function for more
%          details.
%
% Output
%    filter (struct or numeric): The optimized filter structure.
%
% Description
%    Depending on the value of options.filter_format, OPTIMIZE_FILTER calls
%    different functions to optimize the filter representation. If 'fourier',
%    the function retains the Fourier representation of the filter. If
%    'fourier_multires', the filter is periodized and stored at all resolu-
%    tions using PERIODIZE_FILTER. Finally, if it equals 'fourier_truncated',
%    TRUNCATE_FILTER is called on filter_f.
%
% See also
%    PERIODIZE_FILTER, TRUNCATE_FILTER

function filter = optimize_filter(filter_f, lowpass, options)
	options = fill_struct(options,'truncate_threshold',1e-3);
	options = fill_struct(options,'filter_format','fourier_multires');

	if strcmp(options.filter_format,'fourier')
		filter = filter_f;
	elseif strcmp(options.filter_format,'fourier_multires')
		filter = periodize_filter(filter_f);
	elseif strcmp(options.filter_format,'fourier_truncated')
		filter = truncate_filter(filter_f,options.truncate_threshold,lowpass);
	else
		error(sprintf('Unknown filter format ''%s''',options.filter_format));
	end
end


% sdae_get_hidden
% Copyright (C) 2011 KyungHyun Cho, Tapani Raiko, Alexander Ilin
%
%This program is free software; you can redistribute it and/or
%modify it under the terms of the GNU General Public License
%as published by the Free Software Foundation; either version 2
%of the License, or (at your option) any later version.
%
%This program is distributed in the hope that it will be useful,
%but WITHOUT ANY WARRANTY; without even the implied warranty of
%MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%GNU General Public License for more details.
%
%You should have received a copy of the GNU General Public License
%along with this program; if not, write to the Free Software
%Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
%
function [h_mf] = sdae_get_hidden(x0, S, target_sparsity)

if nargin < 3
    target_sparsity = 0;
end

layers = S.structure.layers;
n_layers = length(layers);

h_mf = x0;

for l = 2:n_layers
    h_mf = bsxfun(@plus, h_mf * S.W{l-1}, S.biases{l}');

    if l < n_layers || S.bottleneck.binary
        h_mf = sigmoid(h_mf, S.hidden.use_tanh);
    end
end

if S.bottleneck.binary
    if target_sparsity > 0
        avg_acts = mean(h_mf, 1);
        diff_acts = max(avg_acts - (1 - target_sparsity), 0);
        h_mf = min(max(bsxfun(@minus, h_mf, diff_acts), 0), 1);
    end
end


function f=fst(x,a,b)

   f=((abs(x-a)-abs(x-b))/(b-a)+1)/2;


function L = laplacian( F )
%LAPLACIAN Vector Laplacian of a DISKFUNV.
%   LAPLACIAN(F) returns a DISKFUNV representing the vector Laplacian of F.
%
% See also DISKFUN/LAPLACIAN

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

% Empty check:
if ( isempty( F ) )
    L = diskfunv();
    return
end

% The vector LAPLACIAN of a DISKFUNV is equal to F_xx + F_yy:
L = diff(F, 1, 2) + diff(F, 2, 2);

end


function fhandle = plotCylinder(VORT)

fhandle = figure

vortmin = -5;
vortmax = 5;
V2 = VORT;
% normalize values... not symmetric
minval = min(V2(:));
maxval = max(V2(:));
if(abs(minval)<5 && abs(maxval)<5)
    if(abs(minval)>abs(maxval))
        vortmax = maxval;
        vortmin = -maxval;
    else
        vortmin = minval;
        vortmax = -minval;
    end
end
V2(V2>vortmax) = vortmax;
V2(V2<vortmin) = vortmin;
imagesc(V2)
colormap(jet);
set(gca,'XTick',[1 50 100 150 200 250 300 350 400 449],'XTickLabel',{'-1','0','1','2','3','4','5','6','7','8'})
set(gca,'YTick',[1 50 100 150 199],'YTickLabel',{'2','1','0','-1','-2'});
set(gcf,'Position',[100 100 600 260])
axis equal
hold on

cvals = [-4 -2 -1 -.5 -.25 -.155];

[c1,h1] = contour(V2,cvals*vortmax/5,'--k','LineWidth',1.);
contour(V2,-cvals*vortmax/5,'-k','LineWidth',1.)

% Take all the info from the contourline output argument:
i0 = 1;
i2 = 1;
while i0 <  length(c1)
    i1 = i0+[1:c1(2,i0)];
    zLevel(i2) = c1(1,i0);
    hold on
    % And plot it with dashed lines:
    ph(i2) = plot(c1(1,i1),c1(2,i1),'k--','linewidth',1.);
    i0 = i1(end)+1;
    i2 = i2+1;
end
% Scrap the contourlines:
delete(h1)

t = (1:100)/100'*2*pi;
x = 49+25*sin(t);
y = 99+25*cos(t);
fill(x,y,[.3 .3 .3])
plot(x,y,'k','LineWidth',1.2)

set(gcf,'PaperPositionMode','auto')
set(gcf, 'Renderer', 'painters')


% RES = reconSpyrLevs(PYR,INDICES,LOFILT,BFILTS,EDGES,LEVS,BANDS)
%
% Recursive function for reconstructing levels of a steerable pyramid
% representation.  This is called by reconSpyr, and is not usually
% called directly.

% Eero Simoncelli, 6/96.

function res = reconSpyrLevs(pyr,pind,lofilt,bfilts,edges,levs,bands);

nbands = size(bfilts,2);
lo_ind = nbands+1;
res_sz = pind(1,:);

% Assume square filters:
bfiltsz =  round(sqrt(size(bfilts,1)));

if any(levs > 1)

  if  (size(pind,1) > lo_ind)
    nres = reconSpyrLevs( pyr(1+sum(prod(pind(1:lo_ind-1,:)')):size(pyr,1)),  ...
	pind(lo_ind:size(pind,1),:), ...
	lofilt, bfilts, edges, levs-1, bands);
  else
    nres = pyrBand(pyr,pind,lo_ind); 	% lowpass subband
  end

  res = upConv(nres, lofilt, edges, [2 2], [1 1], res_sz);

else

  res = zeros(res_sz);

end

if any(levs == 1)
  ind = 1;
  for b = 1:nbands
    if any(bands == b)
      bfilt = reshape(bfilts(:,b), bfiltsz, bfiltsz);
      upConv(reshape(pyr(ind:ind+prod(res_sz)-1), res_sz(1), res_sz(2)), ...
	     bfilt, edges, [1 1], [1 1], res_sz, res); % Destructively modify res
    end
    ind = ind + prod(res_sz);
  end
end


function S = FXLMSinit(w0,mu,est_sec, sec_num, sec_den,leak)

% FXLMSinit     Initialize Parameter Structure for the FXLMS Algorithm
%
% Arguments:
% w0            Coefficients of FIR filter at start (@n=1)
% mu            Step size for the LMS algorithm
% est_sec       Estimated secondary path
% sec_num       Numerator of secondary path
% sec_den       Denomerator of secondary path
% leak          Leaky factor
%                 leak = 0 for the conventional FXLMS
%                 0 < leak <1/step for the leaky FXLMS algorithm
%
% by Lee, Gan, and Kuo, 2008
% Subband Adaptive Filtering: Theory and Implementation
% Publisher: John Wiley and Sons, Ltd

if nargin < 6               % Set default to conventional LMS algorithm
	leak = 0;
end

% Assign structure fields
S.coeffs  = w0(:);          % Weight (column) vector of FIR filter
S.step    = mu;             % Step size of LMS algorithm
S.leakage = leak;           % Leaky factor for the leaky LMS algorithm
S.iter    = 0;              % Iteration count
S.estsec  = est_sec;        % Estimated secondary path (FIR model)
S.sec_num = sec_num;        % Secondary path, numerator
S.sec_den = sec_den;        % Secondary path, denominator
S.AdaptStart = length(w0);  % Running effect of adaptive filter, minimum M


function [horiList, vertList, horiPos, vertPos] = createDeBruijnSeq(prjW, prjH)
%% Create the De Bruijn sequence we used in the paper.
% 1 = Red
% 2 = Yellow
% 3 = Lime
% 4 = Green
% 5 = Cyan
% 6 = Blue
% 7 = Purple
% 8 = Magenta
% See also: ImgProc.genStructuredLight

%% License
% ACADEMIC OR NON-PROFIT ORGANIZATION NONCOMMERCIAL RESEARCH USE ONLY
% Copyright (c) 2018 Bingyao Huang
% All rights reserved.

% Redistribution and use in source and binary forms, with or without
% modification, are permitted provided that the following conditions are met:

% The above copyright notice and this permission notice shall be included in all
% copies or substantial portions of the Software.

% If you publish results obtained using this software, please cite our paper.

% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
% IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
% FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
% AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
% LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
% OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
% SOFTWARE.

%%

HStripeT = 2;
VStripeT = 2;
TopBotbuffer = 10;
LeftRightbuffer = 10;
Debrujin_k = 4; % of color
Debrujin_n = 3; %size of window (seq length)
Debrujin_size = power(Debrujin_k, Debrujin_n)+(Debrujin_n-1); %length of Debruijn sequence, +n-1 for wrap

Hsr = TopBotbuffer; % start position of Horizontal strips
Vsr = LeftRightbuffer; % start position of Vertical strips
Hspace = floor((prjH - TopBotbuffer * 2 - HStripeT) / (Debrujin_size-1));  % has to match CreateDeBrujin
Vspace = floor((prjW - LeftRightbuffer * 2 - VStripeT) / (Debrujin_size-1));

% Create Debruijn sequence
sequence = zeros(Debrujin_size,1);

HL = 4;

digOne = HL - 1;
digTwo = HL;
i = 0;
for curHL = 1 : HL
    for curdigOne = 1:digOne
        for curdigTwo = 1:digTwo
            i = i+1;
            sequence(i) = (HL - (curdigOne - 1));
            i = i+1;
            sequence(i) = (HL - (curdigTwo - 1));
            i = i+1;
            sequence(i) = (curHL);
        end
    end

    if (curHL < HL)
        i = i+1;
        sequence(i) = (curHL);
    end

    digOne = digOne - 1;
    digTwo = digTwo - 1;

end

% Three additions of HL to the end
i = i+1;
sequence(i) = (HL);
i = i+1;
sequence(i) = (HL);
i = i+1;
sequence(i) = (HL);

horiList = zeros(Debrujin_size,1);
vertList = zeros(Debrujin_size,1);
horiPos = zeros(Debrujin_size,1);
vertPos = zeros(Debrujin_size,1);

% Create De Bruijn look up table(position and list)
for i = 1:Debrujin_size
    horiList(i) = 2 * sequence(i) - 1;
    horiPos(i)  = Hsr+2; % for 1280 and topbottom buffer = 20, space is 19.375 without HstripT
    Hsr = Hsr + Hspace;

    %Vertical
    vertList(i)  = 2 * sequence(i);
    vertPos(i)  = Vsr+2; % for 1280 and topbottom buffer = 20, space is 19.375 without HstripT
    Vsr = Vsr + Vspace;
end

horiList = horiList';
vertList = vertList';
horiPos = horiPos';
vertPos = vertPos';

end

%     horiList = [7,7,1,7,5,1,7,3,1,7,1,1,5,7,1,5,5,1,5,3,1,5,1,1,3,7,1,3,5,1,...
%         3,3,1,3,1,1,1,7,7,3,7,5,3,7,3,3,5,7,3,5,5,3,5,3,3,3,7,7,5,7,5,5,5,7];
%
%     vertList = [8,8,2,8,6,2,8,4,2,8,2,2,6,8,2,6,6,2,6,4,2,6,2,2,4,8,2,4,6,2,...
%         4,4,2,4,2,2,2,8,8,4,8,6,4,8,4,4,6,8,4,6,6,4,6,4,4,4,8,8,6,8,6,6,6,8];
%
%     horiPos = [10,21,32,43,54,65,76,87,98,109,120,131,142,153,164,175,186,197,...
%         208,219,230,241,252,263,274,285,296,307,318,329,340,351,362,373,384,395,...
%         406,417,428,439,450,461,472,483,494,505,516,527,538,549,560,571,582,593,604,...
%         615,626,637,648,659,670,681,692,703];
%
%     vertPos = [10,29,48,67,86,105,124,143,162,181,200,219,238,257,276,295,314,333,...
%         352,371,390,409,428,447,466,485,504,523,542,561,580,599,618,637,656,675,...
%         694,713,732,751,770,789,808,827,846,865,884,903,922,941,960,979,998,1017,...
%         1036,1055,1074,1093,1112,1131,1150,1169,1188,1207];


clear;
gcp;

% same demo as demo_script.m but using the class @CNMF
%% load file

addpath(genpath('utilities'));
addpath(genpath('deconvolution'));
CNM = CNMF;                                     % contruct CNMF object
filename = 'demoMovie.tif';                     % filename to be processed
K = 40;                                         % number of components to be found

options = CNMFSetParms(...
    'p',2,...                                   % order of AR dynamics
    'gSig',5,...                                % half size of neuron
    'merge_thr',0.80,...                        % merging threshold
    'nb',2,...                                  % number of background components
    'min_SNR',3,...                             % minimum SNR threshold
    'space_thresh',0.5,...                      % space correlation threshold
    'cnn_thr',0.2...                            % threshold for CNN classifier
    );

%%
% Below is the standard processing pipeline. This processing can be
% executed in one shot using the CNM.fit function:
% CNM.fit(filename,options,K)

%% load the dataset and create the object
CNM.readFile(filename);                         % insert path to file here
CNM.optionsSet(options);                        % setup the options structure

%% Process the dataset

CNM.preprocess;             % preprocessing (compute some quantities)
CNM.initComponents(K);      % initialization
CNM.plotCenters()           % plot center of ROIs detected during initialization
CNM.updateSpatial();        % update spatial components
CNM.updateTemporal(0);      % update temporal components (do not deconvolve at this point)

%% component classification

CNM.evaluateComponents();   % evaluate spatial components based on their correlation with the data
CNM.CNNClassifier('')       % evaluate spatial components with the CNN classifier
CNM.eventExceptionality();  % evaluate traces
CNM.keepComponents();       % keep the components that are above certain thresholds

%% merge found components
CNM.merge();
CNM.displayMerging();

%% repeat processing

CNM.updateSpatial();
CNM.updateTemporal();
CNM.extractDFF();           % extract DF/F values.

%% do some plotting
figure;
CNM.plotContours();
CNM.plotComponentsGUI();     % display all components


% PURPOSE: Gets the baseline (mean) value
%
%
% FORMAT:
%
% blv = blvalue2(datax, timex, blcorr)
%
% INPUT:
%
% datax      - input data
% timex      - time vector
% blcorr     - time window for getting the mean value
%
% OUTPUT:
%
% blv        - mean value from window "blcorr"
%
%
% Example
% Get the baseline value for a window of -200 to 0 ms, at bin 4, channel 23
%
% blv = blvalue(ERP.bindata(23,:,4), ERP.times, [-200 0])
%
%
% See also blvalue2.m geterpvalues.m
%
%
% *** This function is part of ERPLAB Toolbox ***
% Author: Javier Lopez-Calderon
% Center for Mind and Brain
% University of California, Davis,
% Davis, CA
% 2013

function blv = blvalue2(datax, timex, blcorr)

%
% Baseline assessment
%
if ischar(blcorr)
      if ~strcmpi(blcorr,'no') && ~strcmpi(blcorr,'none')

            if strcmpi(blcorr,'pre')
                  [bbxx bb] = closest(timex, 0);    % zero-time locked
                  aa = 1;
            elseif strcmpi(blcorr,'post')
                  bb = length(timex);
                  [aax aa] = closest(timex, 0);
            elseif strcmpi(blcorr,'all') || strcmpi(blcorr,'whole')
                  bb = length(timex);
                  aa = 1;
            else
                  blcnum = str2num(blcorr); % in ms

                  %
                  % Check & fix baseline range
                  %
                  if blcnum(1)<min(timex)
                        blcnum(1) = min(timex); %ms
                  end
                  if blcnum(2)>max(timex)
                        blcnum(2) = max(timex); %ms
                  end

                  [xxx, cindex] = closest(timex, blcnum); % 04/21/2011
                  aa = cindex(1); % ms to sample pos
                  bb = cindex(2); % ms to sample pos
            end
            blv = mean(datax(aa:bb));
      else
            blv = 0;
      end
else
      %
      % Check & fix baseline range
      %
      if blcorr(1)<min(timex)
            blcorr(1) = min(timex); %ms
      end
      if blcorr(2)>max(timex)
            blcorr(2) = max(timex); %ms
      end
      [xxx, cindex] = closest(timex, blcorr); % 04/21/2011
      aa = cindex(1); % ms to sample pos
      bb = cindex(2); % ms to sample pos
      blv = mean(datax(aa:bb));
end


function [dat, A]= proc_commonAverageReference(dat, refChans, rerefChans)
%PROC_COMMONAVERAGEREFERENCE - rereferencing to a common reference
%
%Synopsis:
% dat= proc_commonAverageReference(dat, <refChans, rerefChans>)
%
% rereference signals to common average reference. you should only
% used scalp electrodes as reference, not e.g. EMG channels.
%
%Arguments:
%      dat        - data structure of continuous or epoched data
%      refChans   - channels used as average reference,
%                   see util_chanind for format,
%                   default util_scalpChannels(dat)
%      rerefChans - those channels are rereferenced, default refChans
%
%Returns:
%      dat        - updated data structure
%
% SEE util_scalpChannels, util_chanind

dat = misc_history(dat);

misc_checkType(dat, 'STRUCT(x clab)');

if ~exist('refChans','var') || isempty(refChans)
  refChans= dat.clab(util_scalpChannels(dat));
end
if ~exist('rerefChans','var') || isempty(rerefChans),
  rerefChans= refChans;
end

misc_checkType(refChans, 'CELL{CHAR}|CHAR');
misc_checkType(rerefChans, 'CELL{CHAR}|CHAR');

rc= util_chanind(dat, refChans);
rrc= util_chanind(dat, rerefChans);
car= mean(dat.x(:,rc,:), 2);

nChans= size(dat.x,2);
A= eye(nChans, nChans);
A(rc,rrc)= A(rc,rrc) - 1/length(rc);
dat= proc_linearDerivation(dat, A, 'CLab','copy', 'Appendix',' car');


close all;
clear all;
clc;
rng('default');
sample_no = 101;
filename = sprintf('sample_%d.mat', sample_no);
load(filename);
Phi = spx.dict.MatrixOperator(PhiMtx);

% Solve the sparse recovery problem using OMP
matching_mode = 4;
options.norm_factor = 2;
options.VERBOSE = true;
result = ar_omp(PhiMtx, K, y, matching_mode, options);
% Solution vector
z = result.z;
stats = spx.commons.sparse.recovery_performance(Phi, K, y, x, z);
spx.commons.sparse.print_recovery_performance(stats);
fprintf('total_matched_atoms_count: %d \n',result.total_matched_atoms_count);
fprintf('Average atom index: %.2f \n',result.atom_index_average);
fprintf('\n\n\n\n');

% Solve the sparse recovery problem using OMP
matching_mode = 2;
options.VERBOSE = true;
result = ar_omp(PhiMtx, K, y, matching_mode, options);
% Solution vector
z = result.z;
stats = spx.commons.sparse.recovery_performance(Phi, K, y, x, z);
spx.commons.sparse.print_recovery_performance(stats);
fprintf('total_matched_atoms_count: %d \n',result.total_matched_atoms_count);
fprintf('Average atom index: %.2f \n',result.atom_index_average);

if 0
mf = spx.graphics.Figures();
mf.new_figure('OMP solution');
subplot(411);
stem(x, '.');
title('Sparse vector');
subplot(412);
stem(z, '.');
title('Recovered sparse vector');
subplot(413);
stem(abs(x - z), '.');
title('Recovery error');
subplot(414);
stem(y, '.');
title('Measurement vector');
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Q = INVERSEKINEMATIC_KR_1000_1300_TITAN(robot, T)
%   Solves the inverse kinematic problem for the KUKA KR 1000 1300 TITAN robot
%   where:
%   robot stores the robot parameters.
%   T is an homogeneous transform that specifies the position/orientation
%   of the end effector.
%
%   A call to Q=INVERSEKINEMATIC_KR_1000_1300_TITAN returns 8 possible solutions, thus,
%   Q is a 6x8 matrix where each column stores 6 feasible joint values.
%
%
%   Example code:
%
%   robot=load_robot('kuka', 'KR_1000_1300_TITAN');
%   q = [0 0 0 0 0 0];
%   T = directkinematic(robot, q);
%   %Call the inversekinematic for this robot
%   qinv = inversekinematic(robot, T);
%   check that all of them are feasible solutions!
%   and every Ti equals T
%   for i=1:8,
%        Ti = directkinematic(robot, qinv(:,i))
%   end
%	See also DIRECTKINEMATIC.
%   Authors:   Javier Martinez Gonzalez
%              Jose Francisco Munoz Sempere
%              Silvia Carretero Monasor
%              Marcos Gomez Parres
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Copyright (C) 2012, by Arturo Gil Aparicio
%
% This file is part of ARTE (A Robotics Toolbox for Education).
%
% ARTE 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.
%
% ARTE 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 ARTE.  If not, see <http://www.gnu.org/licenses/>.

function q = inversekinematic_KR_1000_1300_TITAN(robot, T)

%initialize q,
%eight possible solutions are generally feasible
q=zeros(6,8);

%Evaluate the parameters
theta = eval(robot.DH.theta);
d = eval(robot.DH.d);
a = eval(robot.DH.a);
alpha = eval(robot.DH.alpha);

L6=d(6); %Distancia de la mu�eca al efector final.

A1 = a(1);

%T= [ nx ox ax Px;
%     ny oy ay Py;
%     nz oz az Pz];
Px=T(1,4);
Py=T(2,4);
Pz=T(3,4);

%Compute the position of the wrist, being W the Z component of the end effector's system
%Tomamos el vector "a"
W = T(1:3,3); %ax, ay, az

% Pm: wrist position (Posici�n de la mu�eca)
Pm = [Px Py Pz]' - L6*W;

%first joint, two possible solutions admited:
% if q(1) is a solution, then q(1) + pi is also a solution
%Obtenemos "theta 1" mediante m�todos geom�tricos.
q1=atan2(-Pm(2), Pm(1));

%solve for q2
q2_1=solve_for_theta2(robot, [q1 0 0 0 0 0 0], Pm);

q2_2=solve_for_theta2(robot, [q1+pi 0 0 0 0 0 0], Pm);

%solve for q3
q3_1=solve_for_theta3(robot, [q1 0 0 0 0 0 0], Pm);

q3_2=solve_for_theta3(robot, [q1+pi 0 0 0 0 0 0], Pm);

%Arrange solutions, there are 8 possible solutions so far.
% if q1 is a solution, q1* = q1 + pi is also a solution.
% For each (q1, q1*) there are two possible solutions
% for q2 and q3 (namely, elbow up and elbow up solutions)
% So far, we have 4 possible solutions. Howefer, for each triplet (theta1, theta2, theta3),
% there exist two more possible solutions for the last three joints, generally
% called wrist up and wrist down solutions. For this reason,
%the next matrix doubles each column. For each two columns, two different
%configurations for theta4, theta5 and theta6 will be computed. These
%configurations are generally referred as wrist up and wrist down solution
q = [q1         q1         q1        q1       q1+pi   q1+pi   q1+pi   q1+pi;
     q2_1(1)    q2_1(1)    q2_1(2)   q2_1(2)  q2_2(1) q2_2(1) q2_2(2) q2_2(2);
     q3_1(1)    q3_1(1)    q3_1(2)   q3_1(2)  q3_2(1) q3_2(1) q3_2(2) q3_2(2);
     0          0          0         0         0      0       0       0;
     0          0          0         0         0      0       0       0;
     0          0          0         0         0      0       0       0];

%At this point, we want to asure, that, at least, the function returns
%4 real solutions q(1:4). If any of the solutions q(1:4) is complex, only
%the real part will be returned. If any of the solutions q(5:8) is complex,
%it will not be considered and removed
q = arrange_solutions(q);

%leave only the real part of the solutions
q=real(q);

%Note that in this robot, the joint q3 has a non-simmetrical range. In this
%case, the joint ranges from 60 deg to -219 deg, thus, the typical normalizing
%step is avoided in this angle (the next line is commented). When solving
%for the orientation, the solutions are normalized to the [-pi, pi] range
%only for the theta4, theta5 and theta6 joints.

%normalize q to [-pi, pi]
q(1,:) = normalize(q(1,:));
q(2,:) = normalize(q(2,:));

% % solve for the last three joints
% % for any of the possible combinations (theta1, theta2, theta3)
% %Please note the special orientation of the wrist of this robot. In this
% %case, we employ an 'ad hoc' function to solve for the final orientaion
for i=1:2:size(q,2),
    qtemp = solve_for_last_three_joints(robot, q(:,i), T, 1); %wrist up
    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i)=qtemp;

    qtemp = solve_for_last_three_joints(robot, q(:,i), T, -1); %wrist down
    qtemp(4:6)=normalize(qtemp(4:6));
    q(:,i+1)=qtemp;
end

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve for second joint theta2, two different
% solutions are returned, corresponding
% to elbow up and down solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q2 = solve_for_theta2(robot, q, Pm)

%Evaluate the parameters
d = eval(robot.DH.d);
a = eval(robot.DH.a);

%See geometry
%Par�metros con los que calculamos "theta 2" y "theta 3", tambi�n mediante
%m�todos geom�tricos.
%Tenemos en cuenta el desfase de 65mm entre los centros de los sistemas
%de referencia de los eslabones 2 y 3.
L2=abs(a(2));
L3=abs(d(4));
A2 = abs(a(3)); %ESTE ELEMENTO ES EL DESFASE CITADO
%given q1 is known, compute first DH transformation
T01=dh(robot, q, 1);

L4 = sqrt(A2^2 + L3^2); %LONGITUD DEL ESLAB�N TENIENDO EN CUENTA EL DESFASE
%(DISTANCIA REAL ENTRE MU�ECA Y ESLAB�N 2)
%Express Pm in the reference system 1, for convenience
p1 = inv(T01)*[Pm; 1];

r = sqrt(p1(1)^2 + p1(2)^2); %r ES LA DISTANCIA DEL SIST. 1 A LA MU�ECA

beta = atan2(-p1(2), p1(1)); %BETA=ARCTG(-Y/X)
gamma = (acos((L2^2+r^2-L4^2)/(2*r*L2))); %TEOREMA DEL COSENO

if ~isreal(gamma)
    disp('WARNING:inversekinematic_KUKA_1000_1300_TITAN: the point is not reachable for this configuration, imaginary solutions');
    %gamma = real(gamma);
end

%return two possible solutions
%elbow up and elbow down
%the order here is important and is coordinated with the function
%solve_for_theta
q2(1) = pi/2-gamma-beta; %elbow up (CODO ARRIBA)
q2(2) = pi/2+gamma-beta; %elbow down (CODO ABAJO)
%DEBEMOS A�ADIR UN �NGULO DE PI/2 DEBIDO A DESFASES EN NUESTROS SISTEMAS DE
%REFERENCIA

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve for third joint theta3, two different
% solutions are returned, corresponding
% to elbow up and down solution
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function q3 = solve_for_theta3(robot, q, Pm)

%Evaluate the parameters
d = eval(robot.DH.d);
a = eval(robot.DH.a);

%See geometry
L2=abs(a(2));
L3=abs(d(4));
A2 = abs(a(3));

L4 = sqrt(A2^2 + L3^2); %VOLVEMOS A TENER EN CUENTA EL DESFASE

phi=acos((A2^2+L4^2-L3^2)/(2*A2*L4)); %TEOMERA DEL COSENO
%PHI, ES EL �NGULO QUE FORMAN LAS L�NEA QUE DEFINEN EL DESFASE ENTRE 2 Y 3,
%Y EL PROPIO ESLAB�N 4
%given q1 is known, compute first DH transformation
T01=dh(robot, q, 1);

%Express Pm in the reference system 1, for convenience
p1 = inv(T01)*[Pm; 1];

r = sqrt(p1(1)^2 + p1(2)^2); %DE NUEVO, DISTANCIA ENTRE SIST.1 Y MU�ECA

eta = (acos((L2^2 + L4^2 - r^2)/(2*L2*L4))); %TEOREMA DEL COSENO
%ETA, ES EL �NGULO REAL ENTRE LOS ESLABONES 3 Y 4

if ~isreal(eta)
   disp('WARNING:inversekinematic_KUKA_1000_1300_TITAN: the point is not reachable for this configuration, imaginary solutions');
   %eta = real(eta);
end

%return two possible solutions
%elbow up and elbow down solutions
%the order here is important
q3(1) = pi - phi - eta; %CODO ARRIBA
q3(2) = pi - phi + eta; %CODO ABAJO
%SE A�ADEN LOS �NGULOS "PI" EN AMBAS SOLUCIONES DEBIDO A DESFASES ENTRE
%NUESTROS SISTEMAS DE REFERENCIA.

% Solve for the last three joints asuming an spherical wrist
function q = solve_for_last_three_joints(robot, q, T, wrist)

% T is the noa matrix defining the position/orientation of the end
% effector's reference system
vx6=T(1:3,1);
vz6=T(1:3,3);
vz5=T(1:3,3); % The vector a T(1:3,3) is coincident with z5

% Obtain the position and orientation of the system 3
% using the already computed joints q1, q2 and q3
T01=dh(robot, q, 1);
T12=dh(robot, q, 2);
T23=dh(robot, q, 3);
T03=T01*T12*T23;

vx3=T03(1:3,1);
vy3=T03(1:3,2);
vz3=T03(1:3,3);

% find z4 normal to the plane formed by z3 and a
vz4=cross(vz3, vz6);	% end effector's vector a: T(1:3,3)

% in case of degenerate solution,
% when vz3 and vz6 are parallel--> then z4=0 0 0, choose q(4)=0 as solution
if norm(vz4) <= 0.00001
    if wrist == 1 %wrist up
        q(4)=0;
    else
        q(4)=-pi; %wrist down
    end
else
    %this is the normal and most frequent solution
    cosq4=wrist*dot(-vy3,vz4);
    sinq4=wrist*dot(vx3,vz4);
    q(4)=atan2(sinq4, cosq4);
end
%propagate the value of q(4) to compute the system 4
T34=dh(robot, q, 4);
T04=T03*T34;
vx4=T04(1:3,1);
vy4=T04(1:3,2);

% solve for q5
cosq5=dot(vx4,vz5);
sinq5=dot(vy4,vz5);
q(5)=atan2(sinq5, cosq5);

%propagate now q(5) to compute T05
T45=dh(robot, q, 5);
T05=T04*T45;
vx5=T05(1:3,1);
vy5=T05(1:3,2);

% solve for q6
cosq6=dot(vx6,vx5);
sinq6=dot(vx6,vy5);
q(6)=atan2(sinq6, cosq6);

%remove complex solutions for q for the q1+pi solutions
function  qreal = arrange_solutions(q)
qreal=q(:,1:4);

%sum along rows if any angle is complex, for any possible solutions, then v(i) is complex
v = sum(q, 1);

for i=5:8,
    if isreal(v(i))
        qreal=[qreal q(:,i)]; %store the real solutions
    end
end

qreal = real(qreal);


function [mm] = cm2mm(cm)
% Convert length from centimeters to millimeters.
mm = cm*10;


function cvx_optval = sigma_max( x )

%SIGMA_MAX   Internal cvx version.

narginchk(1,1);
if ndims( x ) > 2, %#ok
    error( 'lambda_max is not defined for N-D arrays.' );
elseif ~cvx_isaffine( x ),
    error( 'Input must be affine.' );
end

%
% Construct problem
%

[ m, n ] = size( x );
cvx_optval = lambda_max( [ zeros( m, m ), x ; x', zeros( n, n ) ] );

% Copyright 2005-2016 CVX Research, Inc.
% See the file LICENSE.txt for full copyright information.
% The command 'cvx_where' will show where this file is located.


function test_bug1770

% MEM 2gb
% WALLTIME 00:10:00
% DEPENDENCY read_neuralynx_dma read_neuralynx_ncs

filenameA = dccnpath('/home/common/matlab/fieldtrip/data/test/bug1770/2012-07-14_15-33-09');
filenameB = dccnpath('/home/common/matlab/fieldtrip/data/test/bug1770/DigitalLynxRawDataFile.nrd');

hdrA = ft_read_header(filenameA);
hdrB = ft_read_header(filenameB);

begsample = 1;
endsample = 100000;

datA = ft_read_data(filenameA, 'begsample', begsample, 'endsample', endsample);
datB = ft_read_data(filenameB, 'begsample', begsample, 'endsample', endsample);

ch1A = double(datA( 1,:));
ch1B = double(datB(18,:));

figure
subplot(2,1,1);
plot(ft_preproc_standardize(ch1A))
subplot(2,1,2);
plot(-ft_preproc_standardize(ch1B))

ch1As =  ft_preproc_standardize(ch1A);
ch1Bs = -ft_preproc_standardize(ch1B);

figure
hold on
plot(ch1As, 'b.');
plot(ch1Bs, 'r.');

% sofar there seems no reason for concern w.r.t. the reading function
% let us look at the filter kernel and the delay

hA = fft(ch1As);
hB = fft(ch1Bs);
H = hA./hB;
figure
plot(ifft(H));


function [dlnZ_dmu, dlnZ_dvs] = cmpndNoiseGradVals(noise, mu, varsigma, y)

% CMPNDNOISEGRADVALS Gradient of CMPND noise log Z with respect to input mean and variance.
% FORMAT
% DESC computes the gradient of the compound
% noise with respect to the input mean and the input variance.
% ARG noise : noise structure for which gradients are being
% computed.
% ARG mu : mean input locations with respect to which gradients are
% being computed.
% ARG varSigma : variance input locations with respect to which
% gradients are being computed.
% ARG y : noise model output observed values associated with the given points.
% RETURN dlnZ_dmu : the gradient of log Z with respect to the input mean.
% RETURN dlnZ_dvs : the gradient of log Z with respect to the input variance.
%
% SEEALSO cmpndNoiseParamInit, cmpndNoiseGradientParam, noiseGradVals,
%
% COPYRIGHT : Neil D. Lawrence, 2004, 2005

% NOISE

startVal = 1;
endVal = 0;
dlnZ_dmu = zeros(size(mu));
dlnZ_dvs = zeros(size(varsigma));
for i = 1:length(noise.comp)
  fhandle = str2func([noise.comp{i}.type 'NoiseGradVals']);
  [dlnZ_dmu(:, i), dlnZ_dvs(:, i)]  = fhandle(noise.comp{i}, ...
                                              mu(:, i), ...
                                              varsigma(:, i), ...
                                              y(:, i));
end


%SerialLink.cinertia Cartesian inertia matrix
%
% M = R.cinertia(Q) is the NxN Cartesian (operational space) inertia matrix which relates
% Cartesian force/torque to Cartesian acceleration at the joint configuration Q, and N
% is the number of robot joints.
%
% See also SerialLink.inertia, SerialLink.rne.

% Ryan Steindl based on Robotics Toolbox for MATLAB (v6 and v9)
%
% Copyright (C) 1993-2011, 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

function Mx = cinertia(robot, q)
	J = jacob0(robot, q);
	Ji = inv(J);
	M = inertia(robot, q);
	Mx = Ji' * M * Ji;


function [z0,deltaZ]=addAstroRefrac(algorithm,plhObs,zTrue,Rh,P,T,wl)
%%ADDASTROREFRAC Add the effects of refraction to the true zenith angle of
%                an object outside the atmosphere using low-precision
%                atmospheric models for an observer near the surface of
%                the Earth viewing an object using a narrowband sensor.
%
%INPUTS: algorithm This specified the algorithm used. The possible values
%                  are:
%                  0: Use a numerical integration method from Chapter 7.2
%                     of [1] and from [2]. This algorithm uses the inputs
%                     plhObs,z0, Rh, P, T, and wl and makes use of the
%                     Sinclair atmospheric model. This algorithm is the
%                     most precise of all of the methods. If a point is too
%                     far below the horizon, z0 and deltaZ will be empty
%                     matrices.
%                  1: Use the simple formula of [3] for the refraction
%                     experienced by an observer at sea level viewing light
%                     with a wavelength of 0.574 micrometers (yellow). This
%                     algorithm uses the inputs z0, Rh, P, T. This
%                     algorithm is only valid for refraction-corrupted
%                     zenith distances below 70 degrees. Values that are
%                     too high will lead to z0 and deltaZ being empty
%                     matrices.
%                  2: Use the algorithm from the International Astronomical
%                     Union's (IAU) standards of fundamental astronomy
%                     library. This algorithm uses the inputs z0,Rh,P,T,wl
%                     for an observer at sea level. This algorithm will
%                     produce results for all positive zenith distances,
%                     though the results might not be very good for large
%                     values.
%           plhObs The WGS-84 ellipsoidal latitude, longitude and height
%                  of the observer in meters. Only algorithm 0 uses this
%                  parameter, and only the latitude and height above the
%                  reference ellipsoid are used (the latitude does not
%                  matter). The height is treated as an approximate height
%                  above mean sea level. Given the precision of the
%                  algorithm, the fact that geoid undulations are ignored
%                  probably does not matter.For algorithms 1 and 2, an
%                  empty matrix can be passed.
%            zTrue A vector or matrix of true (refraction-free) positive
%                  zenith "distances" (in radians) of the celestial object
%                  being observed. A zenith distance is the observed angle
%                  of an object down from the gravitational vertical (the
%                  zenith) at the observer's location. This value is
%                  inaccurate as one approaches pi/2 radians (the horizon).
%                  zTrue must be greater than zero. Given the precision of
%                  the algorithms that are available, an angle with respect
%                  to the vertical defined by the WGS-84 reference
%                  ellipsoid could probably be substituted for an angle
%                  with respect to the true gravitational vertical.
%               Rh The relative humidity at the observer (between 0 and 1).
%                  If this parameter is omitted or an empty matrix is
%                  passed, then Constants.standardRelHumid is used.
%                P The atmospheric pressure at the observer in Pascals
%                  (N/m^2). If this parameter is omitted or an empty matrix
%                  is passed, then Constants.standardAtmosphericPressure is
%                  used.
%                T The air temperature at the observer in degrees Kelvin.
%                  If this parameter is omitted or an empty matrix is
%                  passed, then Constants.standardTemp is used.
%               wl The wavelength at which the observation is made in units
%                  of meters. If this parameter is omitted or an empty
%                  matrix is passed, then a wavelength of 0.574 micrometers
%                  is used, which is in the visible spectrum (a rather
%                  yellow color).
%
%OUTPUTS: z0  An NX1 vector of the zTrue values in radians with atmospheric
%             refraction added.
%      deltaZ The refraction value that was applied. z0=zTrue-deltaZ.
%
%The function removeAstroRefrac computes deltaZ for the problem where z0 is
%known and zTrue is unknown. This function iterates the solution a few
%times to try to solve the inverse problem. A fixed 20 iterations are used,
%which is generally sufficient to ensure convergence to within working
%precision limits for all of the algorithms.
%
%REFERENCES:
%[1] S. E. Urban and K. P.Seidelmann, Eds.,Explanatory Supplement to the
%    Astronomical Almanac, 3rd ed. Mill Valley, CA: University Science
%    Books, 2013.
%[2] C. Y. Hohenkerk and A. T. Sinclair, "The computation of angular
%    atmospheric refraction at large zenith angles," United Kingdom
%    Hydrographic Office, HM Nautical Almanac, Tech. Rep. 63, Apr. 1985.
%    http://astro.ukho.gov.uk/data/tn/naotn63.pdf
%
%April 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

    if(nargin<7||isempty(wl))
       wl=0.574e-6;
    end

    if(nargin<6||isempty(T))
       T=Constants.standardTemp;
    end

    if(nargin<5||isempty(P))
        P=Constants.standardAtmosphericPressure;
    end

    if(nargin<4||isempty(Rh))
        Rh=Constants.standardRelHumid;
    end

    %The initial estimate of deltaZ is given by solving the inverse problem
    %at zTrue
    [~,deltaZ]=removeAstroRefrac(algorithm,plhObs,zTrue,Rh,P,T,wl);

    if(~isempty(deltaZ))
        numIter=20;
        for curIter=1:numIter
            [~,deltaZ]=removeAstroRefrac(algorithm,plhObs,zTrue-deltaZ,Rh,P,T,wl);
            if(isempty(deltaZ))
                %This can arise if the observation ends up too far
                %underground.
                break
            end
        end
    end
    if(isempty(deltaZ))
        z0=[];
    else
        z0=zTrue-deltaZ;
    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.


%  crs_linear_filter_update_cell_estim_K -- filter with update of K. gain
%
%  ::
%
%
%    [loglik,Incr,retcode,Filters]=crs_linear_filter_update_cell_estim_K(...
%     syst,y,U,z,options)
%
%  Args:
%
%     - **syst** [struct]: structure containing:
%
%           - **PAI00** [vector]: initial probability distributions of regimes
%
%           - **a** [cell]: initial conditions in each regime
%
%           - **Qfunc** [function handle]: transition matrix generator
%
%           - **ff** [function handle]: ft=ff(rt,xt,et), where rt is the
%           regime, xt is the vector of state variables and et the vector of
%           shocks
%
%           - **P** [cell]: initial covariance matrix of the states in each
%           regime
%
%           - **H** [cell]: Measurement error covariance matrices in each regime
%
%           - **SIGeta** [cell]: Covariance matrix of structural shocks.
%
%     - **y** [matrix]: ny x T x npages matrix of data
%
%     - **U** [[]|matrix]: ndx x T matrix of exogenous data
%
%     - **z** [function handle|logical|vector]: linear connection of the
%     observables to the state.
%
%     - **include_in_likelihood** [logical]: selector of increments to include
%     in the likelihood calculation
%
%     - **options** [struct]: structure with various options
%
%  Returns:
%     :
%
%     - **loglik** [scalar]: log likelihood
%
%     - **Incr** [vector]: increments of elements going into the likelihood
%
%     - **retcode** [{0}|integer]: flag for problems.
%
%     - **Filters** [struct]: Filtered, updated and smoothed variables
%
%  Note:
%
%     - The filter checks for violations of constraints and uses the
%     information in the database to reset the offending variables to their
%     values in the database. When this occurs, the Kalman gain is recomputed
%     so that the traditional updating equation holds.
%
%     - This strategy is adopted so as to permit the use of the efficient
%     smoothing algorithm of Durbin and Koopman instead of the classical
%     smoothing algorithm which requires multiple inversions of a potentially
%     singular covariance matrix.
%
%  Example:
%
%     See also:
%


function [sys,x0,str,ts]=ESO_2rd(t,x,u,flag,d,bet,b)

switch flag
    case 0
        [sys,x0,str,ts]=mdlInitializeSizes;
    case 1
        sys=mdlDerivatives(x,u,d,bet,b);
    case 3
        sys=mdlOutputs(x);
    case {2,4,9}
        sys=[];
    otherwise
        error(['Unhandled flag=',num2str(flag)]);
end
function [sys,x0,str,ts]=mdlInitializeSizes
    sizes=simsizes;
    sizes.NumContStates=3;
    sizes.NumDiscStates=0;
    sizes.NumOutputs=3;
    sizes.NumInputs=2;
    sizes.DirFeedthrough=1;
    sizes.NumSampleTimes=1;
    sys=simsizes(sizes);
    x0=[0;0;0];
    str=[];
    ts=[-1 0];
function sys=mdlDerivatives(x,u,d,bet,b)
    e=x(1)-u(2);
    sys(1,1)=x(2)-bet(1)*e;
    sys(2,1)=x(3)-bet(2)*fal(e,0.5,d)+b*u(1);
    sys(3,1)=-bet(3)*fal(e,0.25,d);
function sys=mdlOutputs(x)
    sys=x;
function f=fal(e,a,d)
    if abs(e)<d
        f=e*d^(a-1);
    else f=(abs(e))^a*sign(e);
    end


function isgn = i4vec_compare ( n, a1, a2 )

%*****************************************************************************80
%
%% I4VEC_COMPARE compares two I4VEC's.
%
%  Discussion:
%
%    The lexicographic ordering is used.
%
%  Example:
%
%    Input:
%
%      A1 = ( 2, 6, 2 )
%      A2 = ( 2, 8, 12 )
%
%    Output:
%
%      ISGN = -1
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 April 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of entries in the vectors.
%
%    Input, integer A1(N), A2(N), the vectors to be compared.
%
%    Output, integer ISGN, the results of the comparison:
%    -1, A1 < A2,
%     0, A1 = A2,
%    +1, A2 < A1.
%
  isgn = 0;

  k = 1;

  while ( k <= n )

    if ( a1(k) < a2(k) )
      isgn = -1;
      return
    elseif ( a2(k) < a1(k) )
      isgn = + 1;
      return
    end

    k = k + 1;

  end

  return
end


function [E0,V0,tau0,kaf,kas,eps,alpha] = BOLD_parameters()
% prior values for the Balloon HRF model

E0 = 0.34;
V0 = 4;%0.02;
tau0 = 2;%0.98;
kaf = 0.41;
kas = 0.65;
eps = 1;
alpha = 0.32;


if eps == 0
    DERIV_NOISE = 0;
end

%% Compute Derivative
Nt = size(x,1);
if any(strcmp(InputSignalType,{'sine2', 'chirp','prbs', 'sphs','mixed', 'noise','unforced'})==1) && DERIV_NOISE == 0%eps==0 % eps~=0
    % compute derivative using fourth order central difference
    % use TVRegDiff if more error
    dx = zeros(Nt-5,Nvar+1,Nic);
    for iIC = 1:Nic
    dx_tmp = zeros(Nt-5,3);
    for i=3:Nt-3
        for k=1:size(x,2)
            dx_tmp(i-2,k) = (1/(12*dt))*(-x(i+2,k,iIC)+8*x(i+1,k,iIC)-8*x(i-1,k,iIC)+x(i-2,k,iIC));
        end
    end
    dx(:,1:5,iIC) = dx_tmp;
    end

    % concatenate
    xnew = [x(3:end-3,:,:)];
    unew = [u(3:end-3,:,:)];

    disp(['DONE: computed derivative'])
else %excitation using Gaussian white noise

end

%% Reshape
% uensemble = [u;uv];
% uensemble = u;
X =[];
DX = [];
uensemble = [];
for iIC = 1:Nic
    X = [X; xnew(:,:,iIC)];
    DX = [DX; dx(:,:,iIC)];
    uensemble = [uensemble; unew(:,:,iIC)];
end
% xaug = [X repmat(uensemble(3:end-3,:),[Nic 1])];
xaug = [X uensemble];
DX(:,Nvar+1:size(xaug,2)) = repmat(0*DX(:,Nvar),[1 size(u,2)]);

M = size(xaug,1);

n = size(DX,2)-1;
%% Sparse regression
clear Theta Xi
Theta = poolData(xaug,n,polyorder,usesine);
Theta_norm = zeros(size(Theta,2),1);
for i = 1:size(Theta,2)
   Theta_norm(i) = norm(Theta(:,i));
   Theta(:,i) = Theta(:,i)./Theta_norm(i);
end
m = size(Theta,2);

if exist('lambda_vec') == 1
    Xi = sparsifyDynamicsIndependent(Theta,DX,lambda_vec,n);
else
    Xi = sparsifyDynamics(Theta,DX,lambda,n);
end

if n == 3
    str_vars = {'x','y','u'};
elseif n == 4
    str_vars = {'x','y','z','u'};
elseif n == 5
    str_vars = {'x1','x2','x3','x4','x5'};
elseif n == 6
    str_vars = {'x1','x2','x3','x4','x5','u'};
end

for i = 1:size(Theta,2)
   Xi(i,:) = Xi(i,:)./Theta_norm(i);
end

yout = poolDataLIST(str_vars,Xi,n,polyorder,usesine);


function [ a, rcond, info ] = cpoco ( a, lda, n )

%*****************************************************************************80
%
%% CPOCO factors a complex hermitian positive definite matrix.
%
%  Discussion:
%
%    The routine also estimates the condition of the matrix.
%
%    If RCOND is not needed, CPOFA is slightly faster.
%
%    To solve A*X = B, follow CPOCO by CPOSL.
%
%    To compute inverse(A)*C, follow CPOCO by CPOSL.
%
%    To compute determinant(A), follow CPOCO by CPODI.
%
%    To compute inverse(A), follow CPOCO by CPODI.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 May 2007
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Jack Dongarra, Jim Bunch, Cleve Moler, Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, 1979,
%    ISBN13: 978-0-898711-72-1,
%    LC: QA214.L56.
%
%  Parameters:
%
%    Input, complex A(LDA,N), the hermitian matrix to be factored.
%
%    Input, integer LDA, the leading dimension of A.
%
%    Input, integer N, the order of the matrix.
%
%    Output, complex A(LDA,N); an upper triangular matrix R so that
%      A = hermitian(R)*R
%    where hermitian(R) is the conjugate transpose.  The strict lower
%    triangle is unaltered.  If INFO ~= 0, the factorization is not complete.
%
%    Output, real RCOND, an estimate of the reciprocal condition of
%    the matrix.  For the system A*X = B, relative perturbations in A and B
%    of size EPSILON may cause relative perturbations in X of size
%    (EPSILON/RCOND).  If RCOND is so small that the logical expression
%      1.0 + RCOND == 1.0
%    is true, then A may be singular to working precision.  In particular,
%    RCOND is zero if exact singularity is detected or the estimate underflows.
%
%    Output, integer INFO.
%    0, for normal return.
%    K, signals an error condition.  The leading minor of order K is not
%    positive definite.
%
%  Local Parameters:
%
%    Local, complex Z(N), a work vector whose contents are usually
%    unimportant.  If A is close to a singular matrix, then Z is an
%    approximate null vector in the sense that
%      norm(A*Z) = RCOND * norm(A) * norm(Z).
%

%
%  Find norm of A using only upper half.
%
  for j = 1 : n

    z(j) = scasum ( j, a(1:j,j), 1 );

    for i = 1 : j - 1
      z(i) = real ( z(i) ) + cabs1 ( a(i,j) );
    end

  end

  anorm = 0.0;
  for j = 1 : n
    anorm = max ( anorm, real ( z(j) ) );
  end
%
%  Factor.
%
  [ a, info ] = cpofa ( a, lda, n );

  if ( info ~= 0 )
    return;
  end
%
%  RCOND = 1/(norm(A)*(estimate of norm(inverse(A)))).
%
%  Estimate = norm(Z)/norm(Y) where A*Z = Y and A*Y = E.
%
%  The components of E are chosen to cause maximum local
%  growth in the elements of W where hermitian(R)*W = E.
%
%  The vectors are frequently rescaled to avoid overflow.
%
%  Solve hermitian(R)*W = E.
%
  ek = 1.0;
  z(1:n) = 0.0;

  for k = 1 : n

    if ( cabs1 ( z(k) ) ~= 0.0 )
      ek = csign1 ( ek, -z(k) );
    end

    if ( real ( a(k,k) ) < cabs1 ( ek - z(k) ) )
      s = real ( a(k,k) ) / cabs1 ( ek - z(k) );
      z(1:n) = z(1:n) * s;
      ek = s * ek;
    end

    wk = ek - z(k);
    wkm = -ek - z(k);
    s = cabs1 ( wk );
    sm = cabs1 ( wkm );
    wk = wk / a(k,k);
    wkm = wkm / a(k,k);
    kp1 = k + 1;

    if ( kp1 <= n )

      for j = kp1 : n
        sm = sm + cabs1 ( z(j) + wkm * conj ( a(k,j) ) );
        z(j) = z(j) + wk * conj ( a(k,j) );
        s = s + cabs1 ( z(j) );
      end

      if ( s < sm )
        t = wkm - wk;
        wk = wkm;
        z(kp1:n) = z(kp1:n) + t * conj ( a(k,kp1:n) );
      end

    end

    z(k) = wk;

  end

  s = 1.0 / scasum ( n, z, 1 );
  z(1:n) = z(1:n) * s;
%
%  Solve R * Y = W.
%
  for k = n : -1 : 1

    if ( real ( a(k,k) ) < cabs1 ( z(k) ) )
      s = real ( a(k,k) ) / cabs1 ( z(k) );
      z(1:n) = z(1:n) * s;
    end

    z(k) = z(k) / a(k,k);
    t = -z(k);
    z(1:k-1) = z(1:k-1) + t * transpose ( a(1:k-1,k) );

  end

  s = 1.0 / scasum ( n, z, 1 );
  z(1:n) = z(1:n) * s;
  ynorm = 1.0;
%
%  Solve hermitian(R) * V = Y.
%
  for k = 1 : n

    z(k) = z(k) - z(1:k-1) * conj ( a(1:k-1,k) );

    if ( real ( a(k,k) ) < cabs1 ( z(k) ) )
      s = real ( a(k,k) ) / cabs1 ( z(k) );
      z(1:n) = z(1:n) * s;
      ynorm = s * ynorm;
    end

    z(k) = z(k) / a(k,k);

  end

  s = 1.0 / scasum ( n, z, 1 );
  z(1:n) = z(1:n) * s;
  ynorm = s * ynorm;
%
%  Solve R * Z = V.
%
  for k = n: -1 : 1

    if ( real ( a(k,k) ) < cabs1 ( z(k) ) )
      s = real ( a(k,k) ) / cabs1 ( z(k) );
      z(1:n) = z(1:n) * s;
      ynorm = s * ynorm;
    end

    z(k) = z(k) / a(k,k);
    t = -z(k);
    z(1:k-1) = z(1:k-1) + t * transpose ( a(1:k-1,k) );

  end
%
%  Make ZNORM = 1.
%
  s = 1.0 / scasum ( n, z, 1 );
  z(1:n) = z(1:n) * s;
  ynorm = s * ynorm;

  if ( anorm ~= 0.0 )
    rcond = ynorm / anorm;
  else
    rcond = 0.0;
  end

  return
end


function mae = CalMAE(smap, gtImg)
% Code Author: Wangjiang Zhu
% Email: wangjiang88119@gmail.com
% Date: 3/24/2014
[m,n,~] = size(gtImg);
smap = im2double(smap(:,:,1));
smap = imresize(smap,[m,n]);
smap = (smap-min(smap(:)))/(max(smap(:))-min(smap(:)));

if size(smap, 1) ~= size(gtImg, 1) || size(smap, 2) ~= size(gtImg, 2)
    error('Saliency map and gt Image have different sizes!\n');
end

if ~islogical(gtImg)
    gtImg = gtImg(:,:,1) > 128;
end

fgPixels = smap(gtImg);
fgErrSum = length(fgPixels) - sum(fgPixels);
bgErrSum = sum(smap(~gtImg));
mae = (fgErrSum + bgErrSum) / numel(gtImg);


function test_bug686

% MEM 2gb
% WALLTIME 00:30:00
% DEPENDENCY ft_convert_units ft_prepare_headmodel ft_prepare_leadfield ft_prepare_sourcemodel ft_headmodel_openmeeg headsurface

[pnt, tri] = mesh_sphere(162);
pnt = pnt .* 10;           % convert to cm
sel = find(pnt(:,3)>0);    % take the upper hemisphere

elec.pnt = pnt(sel,:);
for i=1:length(sel)
  elec.label{i} = sprintf('electrode%d', i);
end
elec.unit = 'cm';

grad.pnt = pnt(sel,:) .* 1.2;
grad.ori = pnt(sel,:);
grad.tra = eye(length(sel));
for i=1:length(sel)
  grad.ori(i,:) = grad.ori(i,:) ./ norm(grad.ori(i,:));
  grad.label{i} = sprintf('magnetometer%d', i);
end
grad.unit = 'cm';

% construct them for the different geometrical units
grad_m  = ft_convert_units(grad, 'm');
grad_dm = ft_convert_units(grad, 'dm');
grad_cm = ft_convert_units(grad, 'cm');
grad_mm = ft_convert_units(grad, 'mm');

elec_m  = ft_convert_units(elec, 'm');
elec_dm = ft_convert_units(elec, 'dm');
elec_cm = ft_convert_units(elec, 'cm');
elec_mm = ft_convert_units(elec, 'mm');

%% For EEG the following methods are available

% for some of them I cannot test with a sphere
%  cfg.method = 'bem_asa'
%  cfg.method = 'halfspace
%  cfg.method = 'infinite

geom1 = [];
geom1.pnt = pnt;
geom1.tri = tri;

geom3 = [];
geom3(1).tri = tri;
geom3(2).tri = tri;
geom3(3).tri = tri;
geom3(1).pnt = pnt;
geom3(2).pnt = pnt*0.9;
geom3(3).pnt = pnt*0.8;

cfg = [];
cfg.conductivity = 1;
cfg.method = 'singlesphere';
eegvol_singlesphere = ft_prepare_headmodel(cfg, geom1);
eegvol_singlesphere.o = [0 0 0]; % avoid rounding off errors

cfg.conductivity = [1 1 1];
cfg.method = 'concentricspheres';
eegvol_concentricspheres = ft_prepare_headmodel(cfg, geom3);
eegvol_concentricspheres.o = [0 0 0]; % avoid rounding off errors

cfg.method = 'bemcp'; % this is only implemented for 3 compartments
cfg.conductivity = [1 1 1];
eegvol_bemcp3 = ft_prepare_headmodel(cfg, geom3);

% some of the fwd solutions require the external toolbox, which is not available on all platforms
try
  cfg.method = 'dipoli';
  cfg.conductivity = 1;
  eegvol_dipoli1 = ft_prepare_headmodel(cfg, geom1);
  cfg.conductivity = [1 1 1];
  eegvol_dipoli3 = ft_prepare_headmodel(cfg, geom3);
catch
  eegvol_dipoli1 = [];
  eegvol_dipoli3 = [];
end

try
  cfg.method = 'openmeeg';
  cfg.conductivity = 1;
  eegvol_openmeeg1 = ft_prepare_headmodel(cfg, geom1);
  cfg.conductivity = [1 1 1];
  eegvol_openmeeg3 = ft_prepare_headmodel(cfg, geom3);
catch
  eegvol_openmeeg1 = [];
  eegvol_openmeeg3 = [];
end

% construct them for the different geometrical units
eegvol_singlesphere_m  = ft_convert_units(eegvol_singlesphere, 'm');
eegvol_singlesphere_dm = ft_convert_units(eegvol_singlesphere, 'dm');
eegvol_singlesphere_cm = ft_convert_units(eegvol_singlesphere, 'cm');
eegvol_singlesphere_mm = ft_convert_units(eegvol_singlesphere, 'mm');

eegvol_concentricspheres_m  = ft_convert_units(eegvol_concentricspheres, 'm');
eegvol_concentricspheres_dm = ft_convert_units(eegvol_concentricspheres, 'dm');
eegvol_concentricspheres_cm = ft_convert_units(eegvol_concentricspheres, 'cm');
eegvol_concentricspheres_mm = ft_convert_units(eegvol_concentricspheres, 'mm');

eegvol_bemcp3_m  = ft_convert_units(eegvol_bemcp3, 'm');
eegvol_bemcp3_dm = ft_convert_units(eegvol_bemcp3, 'dm');
eegvol_bemcp3_cm = ft_convert_units(eegvol_bemcp3, 'cm');
eegvol_bemcp3_mm = ft_convert_units(eegvol_bemcp3, 'mm');

try
  eegvol_dipoli1_m  = ft_convert_units(eegvol_dipoli1, 'm');
  eegvol_dipoli1_dm = ft_convert_units(eegvol_dipoli1, 'dm');
  eegvol_dipoli1_cm = ft_convert_units(eegvol_dipoli1, 'cm');
  eegvol_dipoli1_mm = ft_convert_units(eegvol_dipoli1, 'mm');
  eegvol_dipoli3_m  = ft_convert_units(eegvol_dipoli3, 'm');
  eegvol_dipoli3_dm = ft_convert_units(eegvol_dipoli3, 'dm');
  eegvol_dipoli3_cm = ft_convert_units(eegvol_dipoli3, 'cm');
  eegvol_dipoli3_mm = ft_convert_units(eegvol_dipoli3, 'mm');
catch
  fprintf('Please install Dipoli\n')
  % leaving them empty will be interpreted as an infinite volume conductor
  eegvol_dipoli1_m  = [];
  eegvol_dipoli1_dm = [];
  eegvol_dipoli1_cm = [];
  eegvol_dipoli1_mm = [];
  eegvol_dipoli3_m  = [];
  eegvol_dipoli3_dm = [];
  eegvol_dipoli3_cm = [];
  eegvol_dipoli3_mm = [];
end

try
  eegvol_openmeeg1_m  = ft_convert_units(eegvol_openmeeg1, 'm');
  eegvol_openmeeg1_dm = ft_convert_units(eegvol_openmeeg1, 'dm');
  eegvol_openmeeg1_cm = ft_convert_units(eegvol_openmeeg1, 'cm');
  eegvol_openmeeg1_mm = ft_convert_units(eegvol_openmeeg1, 'mm');
  eegvol_openmeeg3_m  = ft_convert_units(eegvol_openmeeg3, 'm');
  eegvol_openmeeg3_dm = ft_convert_units(eegvol_openmeeg3, 'dm');
  eegvol_openmeeg3_cm = ft_convert_units(eegvol_openmeeg3, 'cm');
  eegvol_openmeeg3_mm = ft_convert_units(eegvol_openmeeg3, 'mm');
catch
  fprintf('Please install OpenMEEG\n')
  % leaving them empty will be interpreted as an infinite volume conductor
  eegvol_openmeeg1_m  = [];
  eegvol_openmeeg1_dm = [];
  eegvol_openmeeg1_cm = [];
  eegvol_openmeeg1_mm = [];
  eegvol_openmeeg3_m  = [];
  eegvol_openmeeg3_dm = [];
  eegvol_openmeeg3_cm = [];
  eegvol_openmeeg3_mm = [];
end

%% For MEG the following methods are available

% for some of them I cannot test with a sphere
% cfg.method = 'infinite'

geom = [];
geom.pnt = pnt;

cfg = [];
cfg.conductivity = 1;

cfg.method = 'singlesphere';
megvol_singlesphere = ft_prepare_headmodel(cfg, geom);

cfg.grad = grad_cm;
cfg.method = 'localspheres';
megvol_localspheres = ft_prepare_headmodel(cfg, geom);

geom.tri = tri;
cfg.method = 'singleshell';
megvol_singleshell = ft_prepare_headmodel(cfg, geom);

% construct them for the different geometrical units
megvol_singlesphere_m  = ft_convert_units(megvol_singlesphere, 'm');
megvol_singlesphere_dm = ft_convert_units(megvol_singlesphere, 'dm');
megvol_singlesphere_cm = ft_convert_units(megvol_singlesphere, 'cm');
megvol_singlesphere_mm = ft_convert_units(megvol_singlesphere, 'mm');

megvol_localspheres_m  = ft_convert_units(megvol_localspheres, 'm');
megvol_localspheres_dm = ft_convert_units(megvol_localspheres, 'dm');
megvol_localspheres_cm = ft_convert_units(megvol_localspheres, 'cm');
megvol_localspheres_mm = ft_convert_units(megvol_localspheres, 'mm');

megvol_singleshell_m  = ft_convert_units(megvol_singleshell, 'm');
megvol_singleshell_dm = ft_convert_units(megvol_singleshell, 'dm');
megvol_singleshell_cm = ft_convert_units(megvol_singleshell, 'cm');
megvol_singleshell_mm = ft_convert_units(megvol_singleshell, 'mm');

%%  compute the leadfields for the volume conduction models and sensor arrays

eegvol = {
  'eegvol_singlesphere'
  'eegvol_concentricspheres'
  'eegvol_bemcp3'
  'eegvol_dipoli1'
  'eegvol_dipoli3'
  'eegvol_openmeeg1'
  'eegvol_openmeeg3'
  };

megvol = {
  'megvol_singlesphere'
  'megvol_localspheres'
  'megvol_singleshell'
  };

units = {
  'm'
  'dm'
  'cm'
  'mm'
  };

pos = {
  [0 0  0.07]
  [0 0  0.70]
  [0 0  7.00]
  [0 0 70.00]
  };

%%
eeg_leadfield = {};
for i=1:length(eegvol)
  for j=1:length(units)
    cfg             = [];
    cfg.headmodel   = eval(sprintf('%s_%s', eegvol{i}, units{j}));
    cfg.elec        = eval(sprintf('elec_%s', units{j}));
    cfg.sourcemodel.pos = pos{j};
    cfg.sourcemodel.unit = units{j};
    grid = ft_prepare_leadfield(cfg);
    eeg_leadfield{i,j} = grid.leadfield{1};
  end
end

%%
meg_leadfield = {};
for i=1:length(megvol)
  for j=1:length(units)
    cfg             = [];
    cfg.headmodel   = eval(sprintf('%s_%s', megvol{i}, units{j}));
    cfg.grad        = eval(sprintf('grad_%s', units{j}));
    cfg.sourcemodel.pos = pos{j};
    cfg.sourcemodel.unit = units{j};
    grid = ft_prepare_leadfield(cfg);
    meg_leadfield{i,j} = grid.leadfield{1};
  end
end

%% In the table with scaling factors the columns correspond to m, cm, mm,
% the rows correspond to the different volume conduction models

eeg_table = cellfun(@norm, eeg_leadfield);
disp('eeg_table')
disp(round(log10(eeg_table ./ eeg_table(1,1))))

disp('meg_table')
meg_table = cellfun(@norm, meg_leadfield);
disp(round(log10(meg_table ./ meg_table(1,1))))


function X = update_particles(F_update, Xstd_pos, Xstd_vec, X)

N = size(X, 2);

X = F_update * X;

X(1:2,:) = X(1:2,:) + Xstd_pos * randn(2, N);
X(3:4,:) = X(3:4,:) + Xstd_vec * randn(2, N);


%
% [fh, varfh] = gppred(X, f, Covf, Xh, kX, logtheta, ...)
%
% p(y|X) = N(y; f(X), inv(V))
function [f, varf] = spgpr(logtheta, covfunc, X, y, V, varargin)
%function [fh, varfh] = gppred(X, f, Covf, Xh, kX, logtheta, varargin)

% If you need more than just variances for PCA, you could give some sparse
% matrix to indicate which covariances should be evaluated. But again, this
% covariances should be zero for the latent function values fh a
% priori.. Things would be much easier if I just factored the principal
% components too.. :)

opts = struct('cholkx', [], 'khx', [], 'kh', []);
%opts = struct('cholkx', [], 'khx', [], 'kh', []);

[ opts, errmsg, wrnmsg ] = argschk( opts, varargin{:} );
if ~isempty(errmsg), error( errmsg ), end
if ~isempty(wrnmsg), warning( wrnmsg ), end

nh = cols(Xh);

if isempty(opts.cholkx)
  Kpp = feval(kX, X, X, logtheta);
  Kpp = Kpp + 1e-6*eye(size(Kpp));
  Lp = chol(Kpp, 'lower');
else
  Lp = opts.cholkx;
end

if isempty(opts.khx)
  Kxp = feval(kX, Xh, X, logtheta);
else
  Kxp = opts.khx;
end

if isempty(opts.kh)
  Kx = feval(kX, Xh, [], logtheta);
else
  Kx = opts.kh;
end

fh = Kxp * solve_triu(Lp', solve_tril(Lp, f));

if nargout >= 2
  varfh = zeros(nh,1);
  for i=1:nh
    r = solve_tril(Lp, Kxp(i,:)');
    s = solve_triu(Lp', r);
    varfh(i) = Kx(i) - r'*r + s'*Covf*s;
  end
end


function [y] = spm_fx_fmri_linear(x,u,P,M)
% state equation for a dynamic model of fMRI (linear version)
% responses
% FORMAT [y] = spm_fx_fmri_linear(x,u,P,M)
% x      - state vector
%   x(:,1) - excitatory neuronal activity             ue
%   x(:,2) - vascular signal                          s
%   x(:,3) - rCBF                                  ln(f)
%   x(:,4) - venous volume                         ln(v)
%   x(:,5) - deoyxHb                               ln(q)
%  [x(:,6) - inhibitory neuronal activity             ui]
%
% y      - dx/dt
%
%___________________________________________________________________________
%
% References for hemodynamic & neuronal state equations:
% 1. Buxton RB, Wong EC & Frank LR. Dynamics of blood flow and oxygenation
%    changes during brain activation: The Balloon model. MRM 39:855-864,
%    1998.
% 2. Friston KJ, Mechelli A, Turner R, Price CJ. Nonlinear responses in
%    fMRI: the Balloon model, Volterra kernels, and other hemodynamics.
%    Neuroimage 12:466-477, 2000.
% 3. Stephan KE, Kasper L, Harrison LM, Daunizeau J, den Ouden HE,
%    Breakspear M, Friston KJ. Nonlinear dynamic causal models for fMRI.
%    Neuroimage 42:649-662, 2008.
% 4. Marreiros AC, Kiebel SJ, Friston KJ. Dynamic causal modelling for
%    fMRI: a two-state model.
%    Neuroimage. 2008 Jan 1;39(1):269-78.
%__________________________________________________________________________

% Copyright (C) 2008 Wellcome Trust Centre for Neuroimaging

% Karl Friston & Klaas Enno Stephan
% $Id: spm_fx_fmri_linear.m 4052 2010-08-27 19:22:44Z karl $

% Neuronal motion
%==========================================================================
P.B   = full(P.B);                       % bi-linear parameters
P.C   = P.C/16;                          % exogenous parameters
P.D   = full(P.D);                       % nonlinear parameters

% excitatory connections
%--------------------------------------------------------------------------
for i = 1:size(P.B,3)
    P.A = P.A + u(i)*P.B(:,:,i);
end

% and nonlinear (state) terms
%--------------------------------------------------------------------------
for i = 1:size(P.D,3)
    P.A = P.A + x(i,1)*P.D(:,:,i);
end

% implement differential state equation y = dx/dt (neuronal)
%--------------------------------------------------------------------------
y    = x;
if size(x,2) == 5

    % one neuronal state per region
    %----------------------------------------------------------------------
    y(:,1) = P.A*x(:,1) + P.C*u(:);

else

    % extrinsic (two neuronal states)
    %----------------------------------------------------------------------
    A      = exp(P.A)/8;             % enforce positivity
    IE     = diag(diag(A));          % inhibitory to excitatory
    EE     = A - IE;                 % excitatory to excitatory
    EI     = 1;                      % excitatory to inhibitory
    SE     = 1;                      % self-inhibition (excitatory)
    SI     = 2;                      % self-inhibition (inhibitory)

    % motion - excitatory and inhibitory: y = dx/dt
    %----------------------------------------------------------------------
    y(:,1) = EE*x(:,1) - SE*x(:,1) - IE*x(:,6) + P.C*u(:);
    y(:,6) = EI*x(:,1) - SI*x(:,6);

end

% Hemodynamic motion
%==========================================================================

% hemodynamic parameters
%--------------------------------------------------------------------------
%   H(1) - signal decay                                   d(ds/dt)/ds)
%   H(2) - autoregulation                                 d(ds/dt)/df)
%   H(3) - transit time                                   (t0)
%   H(4) - exponent for Fout(v)                           (alpha)
%   H(5) - resting oxygen extraction                      (E0)
%   H(6) - ratio of intra- to extra-vascular components   (epsilon)
%          of the gradient echo signal
%--------------------------------------------------------------------------
H      = [0.65 0.41 2.00 0.32 0.34];
H      = [0.64 0.32 2.00 0.32 0.32];

% signal decay
%--------------------------------------------------------------------------
sd     = H(1)*exp(P.decay);

% transit time
%--------------------------------------------------------------------------
tt     = H(3)*exp(P.transit);

% Fout = f(v) - outflow
%--------------------------------------------------------------------------
fv     = 1 + x(:,4)/H(4);

% e = f(f) - oxygen extraction x rCBF
%--------------------------------------------------------------------------
ff     = (1 - log(H(5))).*x(:,3);

% implement differential state equation y = dx/dt (hemodynamic)
%--------------------------------------------------------------------------
y(:,2) = x(:,1) - sd.*x(:,2) - H(2)*x(:,3);
y(:,3) = x(:,2);
y(:,4) = (1  + x(:,3) - fv)./tt;
y(:,5) = (ff - x(:,5) + x(:,4))./tt;
y      = y(:);


function pass = test_get( pref )
% Test GET.

if ( nargin < 1 )
    pref = chebfunpref;
end
tol = 1e2 * pref.cheb2Prefs.chebfun2eps;
j = 1;

f = diskfun(@(x,y) 1 + sin(pi*x.*y) + sin(pi*x));
g = diskfun(@(x,y) cos(1-x.*y));
F = diskfunv(f,g);
Fc = F.components;
G = F.';
pass(j) = norm( Fc{1} - f ) < tol; j = j + 1;
pass(j) = norm( Fc{2} - g ) < tol; j = j+1;
pass(j) = norm( 2 - F.nComponents ) < tol; j = j + 1;
pass(j) = F.isTransposed==0; j = j + 1;
pass(j) = G.isTransposed==1;

end


%==============================================================================
% 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/
%==============================================================================
%
% Tutorial for FAIR: density estimation using a histogram
%
%==============================================================================

clear, close all, help(mfilename);

n = 1000;
x = linspace(0,2*pi,n); T = 0.5*(sin(x)+1); % discretized function
minT = 0; maxT = 1;                         % bounds for the bins
numberBins = 5;                             % number of bins
binWidth   = (maxT-minT)/numberBins;        % bin width
bins       = 0:binWidth:maxT;               % the bins
binsExt    = [-inf,bins(2:end-1),inf];      % don't miss anything

rhoHat = histc(T,binsExt); % compute the histogram and plot it
bar(bins+binWidth/2,rhoHat,0.99,'edgecolor','w','facecolor',0.8*[1,1,1]);
axis([minT,maxT,0,inf])

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


%% Recording algorithm behavior with MatlabBGL
% In this example, we will write a simple visitor that outputs an
% algorithm's behavior.  The algorithm we will examine is dijkstra_sp.  To
% examine the runtime behavior we will use a visitor which outputs a string
% every time a function is called.

%% Setup
% To begin, we load a graph.

load ../graphs/clr-25-2.mat

%%
% Next, let's check the documentation to see which functions to implement
% for the visitor

help dijkstra_sp

%%
% The help states that dijkstra_sp allows visitors functions for
% initialize_vertex, discover_vertex, examine_vertex, examine_edge,
% edge_relaxed, edge_not_relaxed, and finish_vertex.
%
% Rather than implementing 7 functions ourselves, we define two helper
% functions.  These helper functions return functions themselves.  There is
% one helper that returns a vertex visitor function and one helper than
% returns an edge visitor function.

vertex_vis_print_func = @(str) @(u) ...
    fprintf('%s called on %s\n', str, char(labels{u}));
edge_vis_print_func = @(str) @(ei,u,v) ...
    fprintf('%s called on (%s,%s)\n', str, char(labels{u}), char(labels{v}));

%%
% These anonymous functions return functions themselves.

ev_func = vertex_vis_print_func('examine_vertex');
ev_func(1)

%%
% I hope you see how these functions are useful in saving quite a bit of
% typing.

%% Calling dijkstra_sp
% We are almost done.  Now, we just have to setup the visitor structure to
% pass to the dijkstra_sp call.

vis = struct();
vis.initialize_vertex = vertex_vis_print_func('initialize_vertex');
vis.discover_vertex = vertex_vis_print_func('discover_vertex');
vis.examine_vertex = vertex_vis_print_func('examine_vertex');
vis.finish_vertex = vertex_vis_print_func('finish_vertex');
vis.examine_edge = edge_vis_print_func('examine_edge');
vis.edge_relaxed = edge_vis_print_func('edge_relaxed');
vis.edge_not_relaxed = edge_vis_print_func('edge_not_relaxed');

%%
% With the visitor setup, there is hardly any work left.

dijkstra_sp(A,1,struct('visitor', vis));

%% Understanding the output
% To understand the output, we find it helpful to have a copy of
% Introduction to Algorithms by Cormen, Leiserson, and Rivest.  The source
% for the graph is Figure 25-2 in that book and the authors use the graph
% to illustrate how Dijkstra's algorithm runs.  In particular, Figure 25-5
% shows a sample run of Dijkstra's algorithm.
%
% Perhaps the first thing to notice is that the initialize vertex visitor
% is never called.  This results from an error in the MatlabBGL and Boost
% documentation.  Once it is resolved, we will update the MatlabBGL
% documentation to match the Boost graph library.
%
% The results: discover_vertex is called before examine_vertex.  For the
% edges, examine_edge is always called before either edge_relaxed
% or edge_not_relaxed.  The edges that are relaxed are the shaded edges in
% Figure 25-5.
%
% Finally, finish vertex is called on a vertex after all of its edges have
% been examined and possibly relaxed.