function y = load_signal(name, n, options)

% load_signal - load a 1D signal
%
%   y = load_signal(name, n, options);
%
% name is a string that can be :
%   'regular' (options.alpha gives regularity)
%   'step', 'rand',
%   'gaussiannoise' (options.sigma gives width of filtering in pixels),
%   [natural signals]
%   'tiger', 'bell', 'bird'
%   [WAVELAB signals]
%   'HeaviSine', 'Bumps', 'Blocks',
%   'Doppler', 'Ramp', 'Cusp', 'Sing', 'HiSine',
%   'LoSine', 'LinChirp', 'TwoChirp', 'QuadChirp',
%   'MishMash', 'WernerSorrows' (Heisenberg),
%   'Leopold' (Kronecker), 'Piece-Regular' (Piece-Wise Smooth),
%	'Riemann','HypChirps','LinChirps', 'Chirps', 'Gabor'
%	'sineoneoverx','Cusp2','SmoothCusp','Gaussian'
%	'Piece-Polynomial' (Piece-Wise 3rd degree polynomial)

if nargin<2
    n = 1024;
end
options.null = 0;
if isfield(options, 'alpha')
    alpha = options.alpha;
else
    alpha = 2;
end

options.rep = '';
switch lower(name)
    case 'regular'
        y = gen_signal(n,alpha);
    case 'step'
        y = linspace(0,1,n)>0.5;
    case 'stepregular'
        y = linspace(0,1,n)>0.5; y=y(:);
        a = gen_signal(n,2); a = a(:);
        a = rescale(a,-0.1,0.1);
        y = y+a;
    case 'gaussiannoise'
        % filtered gaussian noise
        y = randn(n,1);
        if isfield(options, 'sigma')
            sigma = options.sigma; % variance in number of pixels
        else
            sigma = 20;
        end
        m = min(n, 6*round(sigma/2)+1);
        h = compute_gaussian_filter(m,sigma/(4*n),n);
        options.bound = 'per';
        y = perform_convolution(y,h, options);
    case 'rand'
        if isfield(options, 'p1')
            p1 = options.p1;
        else
            c = 10;
            p1 = 1:c; p1 = p1/sum(p1);
        end
        p1 = p1(:); c = length(p1);
        if isfield(options, 'p2')
            p2 = options.p2;
        else
            if isfield(options, 'evol')
                evol = options.evol;
            else
                evol = 0;
            end
            p2 = p1(:) + evol*(rand(c,1)-0.5);
            p2 = max(p2,0); p2 = p2/sum(p2);
        end
        y = zeros(n,1);
        for i=1:n
            a = (i-1)/(n-1);
            p = a*p1+(1-a)*p2; p = p/sum(p);
            y(i) = rand_discr(p, 1);
        end
    case 'bird'
        [y,fs] = load_sound([name '.wav'], n, options);
    case 'tiger'
        [y,fs] = load_sound([name '.au'], n, options);
    case 'bell'
        [y,fs] = load_sound([name '.wav'], n, options);
    otherwise
        y = MakeSignal(name,n);
end

y = y(:);

function y = gen_signal(n,alpha)

%   gen_signal -  generate a 1D C^\alpha signal of length n.
%
%   y = gen_signal(n,alpha);
%
%   The signal is scaled in [0,1].
%
%   Copyright (c) 2003 Gabriel Peyr?

if nargin<2
    alpha = 2;
end

y = randn(n,1);
fy = fft(y);
fy = fftshift(fy);
% filter with |omega|^{-\alpha}
h = (-n/2+1):(n/2);
h = (abs(h)+1).^(-alpha-0.5);
fy = fy.*h';
fy = fftshift(fy);
y = real( ifft(fy) );

y = (y-min(y))/(max(y)-min(y));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function sig = MakeSignal(Name,n)
% MakeSignal -- Make artificial signal
%  Usage
%    sig = MakeSignal(Name,n)
%  Inputs
%    Name   string: 'HeaviSine', 'Bumps', 'Blocks',
%            'Doppler', 'Ramp', 'Cusp', 'Sing', 'HiSine',
%            'LoSine', 'LinChirp', 'TwoChirp', 'QuadChirp',
%            'MishMash', 'WernerSorrows' (Heisenberg),
%            'Leopold' (Kronecker), 'Piece-Regular' (Piece-Wise Smooth),
%	     'Riemann','HypChirps','LinChirps', 'Chirps', 'Gabor'
%	     'sineoneoverx','Cusp2','SmoothCusp','Gaussian'
%	     'Piece-Polynomial' (Piece-Wise 3rd degree polynomial)
%    n      desired signal length
%  Outputs
%    sig    1-d signal
%
%  References
%    Various articles of D.L. Donoho and I.M. Johnstone
%
if nargin > 1,
    t = (1:n) ./n;
end
Name = lower(Name);
if strcmp(Name,'heavisine'),
    sig = 4.*sin(4*pi.*t);
    sig = sig - sign(t - .3) - sign(.72 - t);
elseif strcmp(Name,'bumps'),
    pos = [ .1 .13 .15 .23 .25 .40 .44 .65  .76 .78 .81];
    hgt = [ 4  5   3   4  5  4.2 2.1 4.3  3.1 5.1 4.2];
    wth = [.005 .005 .006 .01 .01 .03 .01 .01  .005 .008 .005];
    sig = zeros(size(t));
    for j =1:length(pos)
        sig = sig + hgt(j)./( 1 + abs((t - pos(j))./wth(j))).^4;
    end
elseif strcmp(Name,'blocks'),
    pos = [ .1 .13 .15 .23 .25 .40 .44 .65  .76 .78 .81];
    hgt = [4 (-5) 3 (-4) 5 (-4.2) 2.1 4.3  (-3.1) 2.1 (-4.2)];
    sig = zeros(size(t));
    for j=1:length(pos)
        sig = sig + (1 + sign(t-pos(j))).*(hgt(j)/2) ;
    end
elseif strcmp(Name,'doppler'),
    sig = sqrt(t.*(1-t)).*sin((2*pi*1.05) ./(t+.05));
elseif strcmp(Name,'ramp'),
    sig = t - (t >= .37);
elseif strcmp(Name,'cusp'),
    sig = sqrt(abs(t - .37));
elseif strcmp(Name,'sing'),
    k = floor(n * .37);
    sig = 1 ./abs(t - (k+.5)/n);
elseif strcmp(Name,'hisine'),
    sig = sin( pi * (n * .6902) .* t);
elseif strcmp(Name,'losine'),
    sig = sin( pi * (n * .3333) .* t);
elseif strcmp(Name,'linchirp'),
    sig = sin(pi .* t .* ((n .* .500) .* t));
elseif strcmp(Name,'twochirp'),
    sig = sin(pi .* t .* (n .* t)) + sin((pi/3) .* t .* (n .* t));
elseif strcmp(Name,'quadchirp'),
    sig = sin( (pi/3) .* t .* (n .* t.^2));
elseif strcmp(Name,'mishmash'),  % QuadChirp + LinChirp + HiSine
    sig = sin( (pi/3) .* t .* (n .* t.^2)) ;
    sig = sig +  sin( pi * (n * .6902) .* t);
    sig = sig +  sin(pi .* t .* (n .* .125 .* t));
elseif strcmp(Name,'wernersorrows'),
    sig = sin( pi .* t .* (n/2 .* t.^2)) ;
    sig = sig +  sin( pi * (n * .6902) .* t);
    sig = sig +  sin(pi .* t .* (n .* t));
    pos = [ .1 .13 .15 .23 .25 .40 .44 .65  .76 .78 .81];
    hgt = [ 4  5   3   4  5  4.2 2.1 4.3  3.1 5.1 4.2];
    wth = [.005 .005 .006 .01 .01 .03 .01 .01  .005 .008 .005];
    for j =1:length(pos)
        sig = sig + hgt(j)./( 1 + abs((t - pos(j))./wth(j))).^4;
    end
elseif strcmp(Name,'leopold'),
    sig = (t == floor(.37 * n)/n);  % Kronecker
elseif strcmp(Name,'riemann'),
    sqn = round(sqrt(n));
    sig = t .* 0;  % Riemann's Non-differentiable Function
    sig((1:sqn).^2) = 1. ./ (1:sqn);
    sig = real(ifft(sig));
elseif strcmp(Name,'hypchirps'), % Hyperbolic Chirps of Mallat's book
    alpha	= 15*n*pi/1024;
    beta    = 5*n*pi/1024;
    t  	= (1.001:1:n+.001)./n;
    f1      = zeros(1,n);
    f2      = zeros(1,n);
    f1  	= sin(alpha./(.8-t)).*(0.1<t).*(t<0.68);
    f2  	= sin(beta./(.8-t)).*(0.1<t).*(t<0.75);
    M  	= round(0.65*n);
    P 	= floor(M/4);
    enveloppe = ones(1,M); % the rising cutoff function
    enveloppe(1:P) = (1+sin(-pi/2+((1:P)-ones(1,P))./(P-1)*pi))/2;
    enveloppe(M-P+1:M) = reverse(enveloppe(1:P));
    env 	= zeros(1,n);
    env(ceil(n/10):M+ceil(n/10)-1) = enveloppe(1:M);
    sig     = (f1+f2).*env;
elseif strcmp(Name,'linchirps'), % Linear Chirps of Mallat's book
    b 	= 100*n*pi/1024;
    a 	= 250*n*pi/1024;
    t 	= (1:n)./n;
    A1 	= sqrt((t-1/n).*(1-t));
    sig	= A1.*(cos((a*(t).^2)) + cos((b*t+a*(t).^2)));
elseif strcmp(Name,'chirps'), % Mixture of Chirps of Mallat's book
    t 	= (1:n)./n.*10.*pi;
    f1 	= cos(t.^2*n/1024);
    a 	= 30*n/1024;
    t 	= (1:n)./n.*pi;
    f2 	= cos(a.*(t.^3));
    f2 	= reverse(f2);
    ix 	= (-n:n)./n.*20;
    g 	= exp(-ix.^2*4*n/1024);
    i1 	= (n/2+1:n/2+n);
    i2 	= (n/8+1:n/8+n);
    j  	= (1:n)/n;
    f3 	= g(i1).*cos(50.*pi.*j*n/1024);
    f4 	= g(i2).*cos(350.*pi.*j*n/1024);
    sig 	= f1+f2+f3+f4;
    enveloppe = ones(1,n); % the rising cutoff function
    enveloppe(1:n/8) = (1+sin(-pi/2+((1:n/8)-ones(1,n/8))./(n/8-1)*pi))/2;
    enveloppe(7*n/8+1:n) = reverse(enveloppe(1:n/8));
    sig 	= sig.*enveloppe;
elseif strcmp(Name,'gabor'), % two modulated Gabor functions in
    % Mallat's book
    N = 512;
    t = (-N:N)*5/N;
    j = (1:N)./N;
    g = exp(-t.^2*20);
    i1 = (2*N/4+1:2*N/4+N);
    i2 = (N/4+1:N/4+N);
    sig1 = 3*g(i1).*exp(i*N/16.*pi.*j);
    sig2 = 3*g(i2).*exp(i*N/4.*pi.*j);
    sig = sig1+sig2;
elseif strcmp(Name,'sineoneoverx'), % sin(1/x) in Mallat's book
    N = 1024;
    a = (-N+1:N);
    a(N) = 1/100;
    a = a./(N-1);
    sig = sin(1.5./(i));
    sig = sig(513:1536);
elseif strcmp(Name,'cusp2'),
    N = 64;
    a = (1:N)./N;
    x = (1-sqrt(a)) + a/2 -.5;
    M = 8*N;
    sig = zeros(1,M);
    sig(M-1.5.*N+1:M-.5*N) = x;
    sig(M-2.5*N+2:M-1.5.*N+1) = reverse(x);
    sig(3*N+1:3*N + N) = .5*ones(1,N);
elseif strcmp(Name,'smoothcusp'),
    sig = MakeSignal('Cusp2');
    N = 64;
    M = 8*N;
    t = (1:M)/M;
    sigma = 0.01;
    g = exp(-.5.*(abs(t-.5)./sigma).^2)./sigma./sqrt(2*pi);
    g = fftshift(g);
    sig2 = iconv(g',sig)'/M;
elseif strcmp(Name,'piece-regular'),
    sig1=-15*MakeSignal('Bumps',n);
    t = (1:fix(n/12)) ./fix(n/12);
    sig2=-exp(4*t);
    t = (1:fix(n/7)) ./fix(n/7);
    sig5=exp(4*t)-exp(4);
    t = (1:fix(n/3)) ./fix(n/3);
    sigma=6/40;
    sig6=-70*exp(-((t-1/2).*(t-1/2))/(2*sigma^2));
    sig(1:fix(n/7))= sig6(1:fix(n/7));
    sig((fix(n/7)+1):fix(n/5))=0.5*sig6((fix(n/7)+1):fix(n/5));
    sig((fix(n/5)+1):fix(n/3))=sig6((fix(n/5)+1):fix(n/3));
    sig((fix(n/3)+1):fix(n/2))=sig1((fix(n/3)+1):fix(n/2));
    sig((fix(n/2)+1):(fix(n/2)+fix(n/12)))=sig2;
    sig((fix(n/2)+2*fix(n/12)):-1:(fix(n/2)+fix(n/12)+1))=sig2;
    sig(fix(n/2)+2*fix(n/12)+fix(n/20)+1:(fix(n/2)+2*fix(n/12)+3*fix(n/20)))=...
        -ones(1,fix(n/2)+2*fix(n/12)+3*fix(n/20)-fix(n/2)-2*fix(n/12)-fix(n/20))*25;
    k=fix(n/2)+2*fix(n/12)+3*fix(n/20);
    sig((k+1):(k+fix(n/7)))=sig5;
    diff=n-5*fix(n/5);
    sig(5*fix(n/5)+1:n)=sig(diff:-1:1);
    % zero-mean
    bias=sum(sig)/n;
    sig=bias-sig;
elseif strcmp(Name,'piece-polynomial'),
    t = (1:fix(n/5)) ./fix(n/5);
    sig1=20*(t.^3+t.^2+4);
    sig3=40*(2.*t.^3+t) + 100;
    sig2=10.*t.^3 + 45;
    sig4=16*t.^2+8.*t+16;
    sig5=20*(t+4);
    sig6(1:fix(n/10))=ones(1,fix(n/10));
    sig6=sig6*20;
    sig(1:fix(n/5))=sig1;
    sig(2*fix(n/5):-1:(fix(n/5)+1))=sig2;
    sig((2*fix(n/5)+1):3*fix(n/5))=sig3;
    sig((3*fix(n/5)+1):4*fix(n/5))=sig4;
    sig((4*fix(n/5)+1):5*fix(n/5))=sig5(fix(n/5):-1:1);
    diff=n-5*fix(n/5);
    sig(5*fix(n/5)+1:n)=sig(diff:-1:1);
    %sig((fix(n/20)+1):(fix(n/20)+fix(n/10)))=-ones(1,fix(n/10))*20;
    sig((fix(n/20)+1):(fix(n/20)+fix(n/10)))=ones(1,fix(n/10))*10;
    sig((n-fix(n/10)+1):(n+fix(n/20)-fix(n/10)))=ones(1,fix(n/20))*150;
    % zero-mean
    bias=sum(sig)/n;
    sig=sig-bias;
elseif strcmp(Name,'gaussian'),
    sig=GWN(n,beta);
    g=zeros(1,n);
    lim=alpha*n;
    mult=pi/(2*alpha*n);
    g(1:lim)=(cos(mult*(1:lim))).^2;
    g((n/2+1):n)=g((n/2):-1:1);
    g = rnshift(g,n/2);
    g=g/norm(g);
    sig=iconv(g,sig);
else
    disp(sprintf('MakeSignal: I don*t recognize <<%s>>',Name))
    disp('Allowable Names are:')
    disp('HeaviSine'),
    disp('Bumps'),
    disp('Blocks'),
    disp('Doppler'),
    disp('Ramp'),
    disp('Cusp'),
    disp('Crease'),
    disp('Sing'),
    disp('HiSine'),
    disp('LoSine'),
    disp('LinChirp'),
    disp('TwoChirp'),
    disp('QuadChirp'),
    disp('MishMash'),
    disp('WernerSorrows'),
    disp('Leopold'),
    disp('Sing'),
    disp('HiSine'),
    disp('LoSine'),
    disp('LinChirp'),
    disp('TwoChirp'),
    disp('QuadChirp'),
    disp('MishMash'),
    disp('WernerSorrows'),
    disp('Leopold'),
    disp('Riemann'),
    disp('HypChirps'),
    disp('LinChirps'),
    disp('Chirps'),
    disp('sineoneoverx'),
    disp('Cusp2'),
    disp('SmoothCusp'),
    disp('Gabor'),
    disp('Piece-Regular');
    disp('Piece-Polynomial');
    disp('Gaussian');
end

%
% Originally made by David L. Donoho.
% Function has been enhanced.

%
% Part of WaveLab Version 802
% Built Sunday, October 3, 1999 8:52:27 AM
% This is Copyrighted Material
% For Copying permissions see COPYING.m
% Comments? e-mail wavelab@stat.stanford.edu
%


function line_fekete_rule_test02 ( m )

%*****************************************************************************80
%
%% LINE_FEKETE_RULE_TEST02 seeks Fekete points in [-1,+1].
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    24 March 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    L Bos, N Levenberg,
%    On the calculation of approximate Fekete points: the univariate case,
%    Electronic Transactions on Numerical Analysis,
%    Volume 30, pages 377-397, 2008.
%
%  Parameters:
%
%    Input, integer M, the dimension of the polynomial space.
%
  if ( nargin < 1 )
    m = 5;
  end
  n = 1000;
  a = -1.0;
  b = +1.0;
  x = linspace ( a, b, n );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'LINE_FEKETE_RULE_TEST02:\n' );
  fprintf ( 1, '  Seek Fekete points in [%g,%g]\n', a, b );
  fprintf ( 1, '  using %d equally spaced sample points\n', n );
  fprintf ( 1, '  for polynomials of degree M = %d\n', m );
  fprintf ( 1, '  with the Chebyshev basis\n' );
  fprintf ( 1, '  and weight 1/sqrt(1-x^2).\n' );

  [ nf, xf, wf, vf ] = line_fekete_chebyshev ( m, a, b, n, x );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  NF = %d\n', nf );
  r8vec_print ( nf, xf, '  Estimated Fekete points XF:' );

  yf = ones ( size ( xf ) );
  plot ( xf, yf, '*' );
  title ( 'Estimated Fekete points' );
  grid on
  filename = 'line_fekete_rule_test02.png';
  print ( '-dpng', filename );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Saved plotfile as "%s"\n', filename );

  wf_sum = sum ( wf(1:nf) );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Sum(WF) = %g\n', wf_sum );

  return
end


%% Unimodal ODF Shapes
%
%%
% Also take a look at the page <SO3Kernels.html SO3Kernels>.
%
% In order to control the shape of unimodal ODF
% The classes @SO3Kernel and @S2Kernel are needed in MTEX to define the
% specific form of unimodal and fibre symmetric ODFs. It has to be passed
% as an argument when calling the methods <uniformODF.html uniformODF> or
% <fibreODF.html fibreODF>.
%
% A kernel is defined by specifying its name and its free parameter.
% Alternatively one can also specify the halfwidth of the kernel. Below you
% find a list of some important SO3Kernel functions supported by MTEX.

psi{1} = SO3AbelPoissonKernel(0.79);
psi{2} = SO3DeLaValleePoussinKernel(13);
psi{3} = SO3BumpKernel(35*degree);
psi{4} = SO3DirichletKernel(3);
psi{5} = SO3vonMisesFisherKernel(7.5);
psi{6} = SO3GaussWeierstrassKernel(0.07);
psi{7} = fibreVonMisesFisherKernel(7.2);
psi{8} = SO3SquareSingularityKernel(0.72);

%%
% Lets visualize these kernel functions as one dimensional sections through
% the orientation space

% the kernel on SO(3)
close;
figure('position',[100,100,1000,450])
hold all
for i = 1:numel(psi)
  plot(psi{i},'DisplayName',class(psi{i}));
end
hold off
legend(gca,'show','Location','eastoutside')

%%
% one dimensional sections through the corresponding PDF

close; figure('position',[100,100,1000,450])
hold all
for i = 1:numel(psi)
  plot(psi{i}.radon,'symmetric','DisplayName',class(psi{i}),'linewidth',2);
end
hold off

ylim([-5,20])

legend(gca,'show','Location','eastoutside')
%%
% the Fourier coefficients of the kernels

close; figure('position',[100,100,500,450])
hold all
for i = 1:numel(psi)
  plotSpektra(psi{i},'bandwidth',32,'linewidth',2,'DisplayName',class(psi{i}));
end
hold off
legend(gca,'show')


function value = r4_binom ( n, m )

%*****************************************************************************80
%
%% R4_BINOM evaluates the binomial coefficient using R4 arithmetic.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 September 2011
%
%  Author:
%
%    Original FORTRAN77 version by Wayne Fullerton.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Wayne Fullerton,
%    Portable Special Function Routines,
%    in Portability of Numerical Software,
%    edited by Wayne Cowell,
%    Lecture Notes in Computer Science, Volume 57,
%    Springer 1977,
%    ISBN: 978-3-540-08446-4,
%    LC: QA297.W65.
%
%  Parameters:
%
%    Input, integer N, M, the arguments.
%
%    Output, real VALUE, the binomial coefficient.
%
  persistent sq2pil
  persistent bilnmx
  persistent fintmx

  sq2pil = 0.91893853320467274;

  if ( isempty ( bilnmx ) )
    bilnmx = log ( r4_mach ( 2 ) );
    fintmx = 0.9 / r4_mach ( 3 );
  end

  if ( n < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R4_BINOM - Fatal error!\n' );
    fprintf ( 1, '  N < 0.\n' );
    error ( 'R4_BINOM - Fatal error!' )
  end

  if ( m < 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R4_BINOM - Fatal error!\n' );
    fprintf ( 1, '  M < 0.\n' );
    error ( 'R4_BINOM - Fatal error!' )
  end

  if ( n < m )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'R4_BINOM - Fatal error!\n' );
    fprintf ( 1, '  N < M.\n' );
    error ( 'R4_BINOM - Fatal error!' )
  end

  k = min ( m, n - m );

  if ( k <= 20 && k * log ( max ( n, 1 ) ) <= bilnmx )

    value = 1.0;

    for i = 1 : k
      value = value * ( n - i + 1 ) / i;
    end

  else

    if ( k < 9 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'R4_BINOM - Fatal error!\n' );
      fprintf ( 1, '  Result overflows.\n' );
      fprintf ( 1, '  N or M is too big.\n' );
      error ( 'R4_BINOM - Fatal error!' )
    end

    xn = n + 1;
    xk = k + 1;
    xnk = n - k + 1;

    corr = r4_lgmc ( xn ) - r4_lgmc ( xk ) - r4_lgmc ( xnk );

    value = xk * log ( xnk / xk ) ...
      - xn * r4_lnrel ( - ( xk - 1.0 ) / xn ) ...
      - 0.5 * log ( xn * xnk / xk ) + 1.0 - sq2pil + corr;

    if ( bilnmx < value )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'R4_BINOM - Fatal error!\n' );
      fprintf ( 1, '  Result overflows.\n' );
      fprintf ( 1, '  N or M is too big.\n' );
      error ( 'R4_BINOM - Fatal error!' )
    end

    value = exp ( value );

  end

  if ( value < fintmx )
    value = r4_aint ( value + 0.5 );
  end

  return
end


function [params,X,means,wX,wM,dwM] = pre_sphere(params, X, dim)
% Spheres the data with the eigenvalue decomposition of the covariance matrix
%   [params,X,means,wX,wM,dwM] = pre_sphere(params, X, dim)
%     X    Data to be sphered
%     dim  Requested result data dimension
%     wX   Sphered data
%     wM   Sphering matrix
%     dwM  De-sphering matrix

% Copyright (C) 2004, 2005 DSS MATLAB package team (dss@cis.hut.fi).
% Distributed by Laboratory of Computer and Information Science,
% Helsinki University of Technology. http://www.cis.hut.fi/projects/dss/.
% $Id$

if nargin<2
    params.name = 'PCA sphering';
    params.description = 'Spheres the data with the eigenvalue decomposition of the covariance matrix.';
    return;
end

if nargin<3; dim=0; end

[xdim,tdim] = size(X);
% removing the mean
means = mean(X,2);
X = X - repmat(means,1,tdim);

[wM, dwM] = dss_sphere(X, dim);

wX = wM * X;


% Applies a frequency shift to time domain samples
%
% @param samples Input samples (must be row or column vector)
% @param sample_rate Sample rate (in Hz) that the frequency offset should be relative to
% @param freq_offset Frequency offset (in Hz) to apply
% @return samples Frequency shifted time domain samples
function [samples] = apply_freq_offset_time_domain(samples, sample_rate, freq_offset)
    assert(isrow(samples) || iscolumn(samples), "Samples must be a row or column vector");

    if (isrow(samples))
        samples = samples .* exp(1j * 2 * pi * freq_offset / sample_rate * [0:length(samples)-1]);
    else
        samples = samples .* exp(1j * 2 * pi * freq_offset / sample_rate * [0:length(samples)-1].');
    end
end


function x = spm_invGcdf(F,h,l,tol)
% Inverse Cumulative Distribution Function (CDF) of Gamma distribution
% FORMAT x = spm_invGcdf(p,h,l,tol)
%
% F   - CDF (lower tail p-value)
% h   - Shape parameter (h>0)
% l   - Scale parameter (l>0)
% x   - Gamma ordinates at which CDF F(x)=F
% tol - tolerance [default 10^-6]
%__________________________________________________________________________
%
% spm_invGcdf implements the inverse Cumulative Distribution Function
% for Gamma distributions.
%
% Definition:
%--------------------------------------------------------------------------
% The Gamma distribution has shape parameter h and scale l, and is
% defined for h>0 & l>0 and for x in [0,Inf) (See Evans et al., Ch18,
% but note that this reference uses the alternative parameterisation of
% the Gamma with scale parameter c=1/l)
%
% The Cumulative Distribution Function (CDF) F(x) is the probability
% that a realisation of a Gamma random variable X has value less than
% x. F(x)=Pr{X<x}:  (See Evans et al., Ch18)
%
% Variate relationships: (Evans et al., Ch8 & Ch18)
%--------------------------------------------------------------------------
% For natural (strictly +ve integer) shape h this is an Erlang distribution.
%
% The Standard Gamma distribution has a single parameter, the shape h.
% The scale taken as l=1.
%
% The Chi-squared distribution with v degrees of freedom is equivalent
% to the Gamma distribution with scale parameter 1/2 and shape parameter v/2.
%
% Algorithm:
%--------------------------------------------------------------------------
% Interval bisection to find the inverse CDF to within tol.
%
% References:
%--------------------------------------------------------------------------
% Evans M, Hastings N, Peacock B (1993)
%       "Statistical Distributions"
%        2nd Ed. Wiley, New York
%
% Abramowitz M, Stegun IA, (1964)
%       "Handbook of Mathematical Functions"
%        US Government Printing Office
%
% Press WH, Teukolsky SA, Vetterling AT, Flannery BP (1992)
%       "Numerical Recipes in C"
%        Cambridge
%__________________________________________________________________________
% Copyright (C) 1992-2011 Wellcome Trust Centre for Neuroimaging

% Andrew Holmes
% $Id: spm_invGcdf.m 4182 2011-02-01 12:29:09Z guillaume $

%-Parameters
%--------------------------------------------------------------------------
Dtol   = 10^-8;
maxIt = 10000;

%-Format arguments, note & check sizes
%--------------------------------------------------------------------------
if nargin<4, tol = Dtol; end
if nargin<3, error('Insufficient arguments'), end

ad = [ndims(F);ndims(h);ndims(l)];
rd = max(ad);
as = [[size(F),ones(1,rd-ad(1))];...
      [size(h),ones(1,rd-ad(2))];...
      [size(l),ones(1,rd-ad(3))]];
rs = max(as);
xa = prod(as,2)>1;
if sum(xa)>1 && any(any(diff(as(xa,:)),1))
    error('non-scalar args must match in size');
end

%-Computation - Undefined and special cases
%--------------------------------------------------------------------------
%-Initialise result to zeros
x = zeros(rs);

%-Only defined for F in [0,1] & strictly positive h & l.
% Return NaN if undefined.
md = ( F>=0  &  F<=1  &  h>0  &  l>0 );
if any(~md(:))
    x(~md) = NaN;
    warning('Returning NaN for out of range arguments');
end

%-Special cases: x=0 when F=0, x=Inf when F=1
x(md & F==1) = Inf;

%-Compute where defined & not special case
%--------------------------------------------------------------------------
Q  = find( md  &  F>0  &  F<1 );
if isempty(Q), return, end
if xa(1), FQ=F(Q); FQ=FQ(:); else FQ=F*ones(length(Q),1); end
if xa(2), hQ=h(Q); hQ=hQ(:); else hQ=h*ones(length(Q),1); end
if xa(3), lQ=l(Q); lQ=lQ(:); else lQ=l*ones(length(Q),1); end
%-?Q=?Q(:) stuff is to avoid discrepant orientations of vector arguments!

%-Compute initial interval estimates [0,mean] & Grow to encompass F(QF);
%--------------------------------------------------------------------------
a  = zeros(length(Q),1);
b  = hQ./(2*lQ);
QQ = 1:length(Q);
while ~isempty(QQ)
    b(QQ) = 2*b(QQ);
    QQ    = find(spm_Gcdf(b,hQ,lQ) < FQ);
end

%-Interval bisection
%--------------------------------------------------------------------------
fa = spm_Gcdf(a,hQ,lQ)-FQ;
fb = spm_Gcdf(b,hQ,lQ)-FQ;
i  = 0;
xQ = a+(b-a)/2;
QQ = 1:length(Q);

while ~isempty(QQ) &&  i<maxIt
    fxQQ        = spm_Gcdf(xQ(QQ),hQ(QQ),lQ(QQ))-FQ(QQ);
    mQQ         = fa(QQ).*fxQQ > 0;
    a(QQ(mQQ))  = xQ(QQ(mQQ));   fa(QQ(mQQ))  = fxQQ(mQQ);
    b(QQ(~mQQ)) = xQ(QQ(~mQQ));  fb(QQ(~mQQ)) = fxQQ(~mQQ);
    xQ(QQ)      = a(QQ) + (b(QQ)-a(QQ))/2;
    QQ          = QQ( (b(QQ)-a(QQ))>tol );
    i           = i+1;
end

if i==maxIt
    warning('convergence criteria not reached - maxIt reached');
end

x(Q) = xQ;


function [Y,Cb,Cr]=RGB2YCbCr(img_rgb)
R=img_rgb(:,:,1);
G=img_rgb(:,:,2);
B=img_rgb(:,:,3);

% rgbתihs
Y   = 0.257*R+0.564*G+0.098*B+16;
Cb = -0.148*R-0.291*G+0.439*B+128;
Cr  = 0.439*R-0.368*G-0.071*B+128;

end


function E = quaternion2EulXYZ(Q)
% Euler/Tait-Bryan angles; x-y-z extrinsic convention
%
% Parameters
% ------------
%   Q : 1 x 4 float vector
%       Quaternion
%
% Returns
% ------------
%   E : 1 x 3 float vector
%       Euler angles equivalent of the quaternion (radian)
%       E(1) rotation about x, E(2) rotation about y, E(3) rotation about z
%
E = zeros(3,1);
E(1) = atan2((2*(Q(1)*Q(2)+Q(3)*Q(4))), (1-2*(Q(2)^2+Q(3)^2)));
E(2) = asin(  2*(Q(1)*Q(3)-Q(4)*Q(2)));
E(3) = atan2((2*(Q(1)*Q(4)+Q(2)*Q(3))), (1-2*(Q(3)^2+Q(4)^2)));
end


function  R = MatrixExp3(so3mat)
% *** CHAPTER 3: RIGID-BODY MOTIONS ***
% Takes a 3x3 so(3) representation of exponential coordinates.
% Returns R in SO(3) that is achieved by rotating about omghat by theta
% from an initial orientation R = I.
% Example Input:
%
% clear; clc;
% so3mat = [[0, -3, 2]; [3, 0, -1]; [-2, 1, 0]];
% R = MatrixExp3(so3mat)
%
% Output:
% R =
%   -0.6949    0.7135    0.0893
%   -0.1920   -0.3038    0.9332
%    0.6930    0.6313    0.3481

omgtheta = so3ToVec(so3mat);
if NearZero(norm(omgtheta))
    R = eye(3);
else
    [omghat, theta] = AxisAng3(omgtheta);
    omgmat = so3mat / theta;
    R = eye(3) + sin(theta) * omgmat + (1 - cos(theta)) * omgmat * omgmat;
end
end


function [ux,u,m] = applypca2(X)
% [ux,u,m] = applypca(X)
%
% finds principal components of
%
% input:
%  x  input data (each column is an input vector)
%
% output:
%  ux  data projected onto pca (1st row=leading principal component)
%  u   the orthogonal matrix with all principal components
%  m   mean of the data
%
% You can recover the original data with
%
% xoriginal=u*ux+repmat(m,1,size(ux,2));
%
% See also pca
%
% copyright by Kilian Q. Weinberger, 2006

% [u,v]=pca(X);
[d,N]  = size(X);

m = mean(X,2);
X = X - m*ones(1,N); % remove mean from data

cc = cov(X',1); % compute covariance
[cvv,cdd] = eig(cc); % compute eignvectors
[zz,ii] = sort(diag(-cdd)); % sort according to eigenvalues
u = cvv(:,ii); % pick leading eigenvectors

ux=u'*X;


function poly_product_test ( )

%*****************************************************************************80
%
%% POLY_PRODUCT_TEST tests POLY_PRODUCT.
%
%  Location:
%
%    http://people.sc.fsu.edu/~jburkardt/m_src/triangle_integrals/poly_product_test.m
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 April 2015
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'POLY_PRODUCT_TEST:\n' );
  fprintf ( 1, '  POLY_PRODUCT computes the product of two X,Y polynomials.\n' );
%
%  P1 = ( 1 + 2 x + 3 y )
%  P2 = ( 4 + 5 x )
%  P3 = 4 + 13x + 12y + 10x^2 + 15xy + 0y^2
%
  d1 = 1;
  p1 = [ 1.0, 2.0, 3.0 ];
  fprintf ( 1, '\n' );
  poly_print ( d1, p1, '  p1(x,y)' );

  d2 = 1;
  p2 = [ 4.0, 5.0, 0.0 ];
  fprintf ( 1, '\n' );
  poly_print ( d2, p2, '  p2(x,y)' );

  [ d3, p3 ] = poly_product ( d1, p1, d2, p2 );
  fprintf ( 1, '\n' );
  poly_print ( d3, p3, '  p3(x,y) = p1(x,y) * p2(x,y)' );

  dc = 2;
  pc = [ 4.0, 13.0, 12.0, 10.0, 15.0, 0.0 ];
  fprintf ( 1, '\n' );
  poly_print ( dc, pc, '  Correct answer: p3(x,y)=p1(x,y)*p2(x,y)' );
%
%  P1 = ( 1 - 2 x + 3 y - 4x^2 + 5xy - 6y^2)
%  P2 = ( 7 + 3x^2 )
%  P3 = 7
%    - 14x + 21y
%    - 25 x^2 + 35 xy -  42y^2
%    -  6x^3  + 9x^2y +  0xy^2   + 0y^3
%    - 12x^4 + 15x^3y - 18x^2y^2 + 0 xy^3 + 0y^4
%
  d1 = 2;
  p1 = [ 1.0, -2.0, 3.0 -4.0, +5.0, -6.0 ];
  fprintf ( 1, '\n' );
  poly_print ( d1, p1, '  p1(x,y)' );

  d2 = 2;
  p2 = [ 7.0, 0.0, 0.0, 3.0, 0.0, 0.0 ];
  fprintf ( 1, '\n' );
  poly_print ( d2, p2, '  p2(x,y)' );

  [ d3, p3 ] = poly_product ( d1, p1, d2, p2 );
  fprintf ( 1, '\n' );
  poly_print ( d3, p3, '  p3(x,y) = p1(x,y) * p2(x,y)' );

  dc = 4;
  pc = [ ...
      7.0, ...
    -14.0,  21.0, ...
    -25.0, +35.0, -42.0, ...
     -6.0,   9.0,   0.0, 0.0, ...
    -12.0, +15.0, -18.0, 0.0, 0.0 ];
  fprintf ( 1, '\n' );
  poly_print ( dc, pc, '  Correct answer: p3(x,y)=p1(x,y)*p2(x,y)' );

  return
end


function g=comp_pchirp(L,n)
%COMP_PCHIRP  Compute periodic chirp
%   Usage:  g=comp_pchirp(L,n);
%
%   `pchirp(L,n)` returns a periodic, discrete chirp of length *L* that
%   revolves *n* times around the time-frequency plane in frequency. *n* must be
%   an integer number.
%
%   This is a computational routine. Do not call it unless you have
%   verified the input parameters.

%   AUTHOR : Peter L. Søndergaard
%   TESTING: OK
%   REFERENCE: OK

l= (0:L-1).';
X = mod(mod(mod(n*l,2*L).*l,2*L)*(L+1),2*L);
g = exp(pi*1i*X/L);


function filteredM = tsmooth(m,fSize,fSd)
%
% filteredM = tsmooth(m,fSize,fSd)
%
%AUTHOR:  Engel, Wandell
%DATE:    1993-4
%PURPOSE:
%   Take a set of time series values in the columns of m and smooth
% each one by convolution with a Gaussian kernel of support fSize and
% standard deviation of fSd.
%

kernel = mkGaussKernel([1 fSize],[1 fSd]);
s = size(m);

% 7/09/97 Lea updated to 5.0
filteredM = [];

for i = (1:s(2))
 tmp = evenConv(m(:,i),kernel);
% tmp = mrDeTrend(tmp);
 filteredM = [filteredM;tmp];
end


function [A_hat E_hat iter] = exact_alm_rpca(D, lambda, tol, maxIter)

% Oct 2009
% This matlab code implements the augmented Lagrange multiplier method for
% Robust PCA.
%
% D - m x n matrix of observations/data (required input)
%
% lambda - weight on sparse error term in the cost function
%
% tol - tolerance for stopping criterion.
%     - DEFAULT 1e-7 if omitted or -1.
%
% maxIter - maximum number of iterations
%         - DEFAULT 1000, if omitted or -1.
%
% Initialize A,E,Y,u
% while ~converged
%   minimize
%     L(A,E,Y,u) = |A|_* + lambda * |E|_1 + <Y,D-A-E> + mu/2 * |D-A-E|_F^2;
%   Y = Y + \mu * (D - A - E);
%   \mu = \rho * \mu;
% end
%
% Minming Chen, October 2009. Questions? v-minmch@microsoft.com ;
% Arvind Ganesh (abalasu2@illinois.edu)
%
% Copyright: Perception and Decision Laboratory, University of Illinois, Urbana-Champaign
%            Microsoft Research Asia, Beijing

% addpath PROPACK;

[m n] = size(D);

if nargin < 2
    lambda = 1 / sqrt(m);
end

if nargin < 3
    tol = 1e-7;
elseif tol == -1
    tol = 1e-7;
end

if nargin < 4
    maxIter = 1000;
elseif maxIter == -1
    maxIter = 1000;
end

% initialize
Y = sign(D);
%norm_two = lansvd(Y, 1, 'L');
norm_two = svdex(Y, 1, struct('svd_solver','svds'));
norm_inf = norm( Y(:), inf) / lambda;
dual_norm = max(norm_two, norm_inf);
Y = Y / dual_norm;

A_hat = zeros( m, n);
E_hat = zeros( m, n);
dnorm = norm(D, 'fro');
tolProj = 1e-6 * dnorm;
total_svd = 0;
mu = .5/norm_two; % this one can be tuned
rho = 6;          % this one can be tuned

iter = 0;
converged = false;
stopCriterion = 1;
sv = 5;
svp = sv;

% excelly
temp_T = D - A_hat + (1/mu)*Y;
temp_E = max( temp_T - lambda/mu,0) + min( temp_T + lambda/mu,0);
[U dummy dummy] = svdex(D - temp_E + (1/mu)*Y, 1, struct('svd_solver','svds'));
while ~converged
    iter = iter + 1;

    % solve the primal problem by alternative projection
    primal_converged = false;
    primal_iter = 0;
    sv = sv + round(n * 0.1);
    while primal_converged == false

        temp_T = D - A_hat + (1/mu)*Y;
        temp_E = max( temp_T - lambda/mu,0) + min( temp_T + lambda/mu,0);

        [U diagS V] = svdex(D - temp_E + (1/mu)*Y, sv, struct('init',U(:,1)));
        % if choosvd(n, sv) == 1
        %     [U S V] = lansvd(D - temp_E + (1/mu)*Y, sv, 'L');
        % else
        %     [U S V] = svd(D - temp_E + (1/mu)*Y, 'econ');
        % end
        % diagS = diag(S);
        svp = length(find(diagS > 1/mu));
        if svp < sv
            sv = min(svp + 1, n);
        else
            sv = min(svp + round(0.05*n), n);
        end
        temp_A = U(:,1:svp)*diag(diagS(1:svp)-1/mu)*V(:,1:svp)';

        if norm(A_hat - temp_A, 'fro') < tolProj && norm(E_hat - temp_E, 'fro') < tolProj
            primal_converged = true;
        end
        A_hat = temp_A;
        E_hat = temp_E;
        primal_iter = primal_iter + 1;
        total_svd = total_svd + 1;

    end

    Z = D - A_hat - E_hat;
    Y = Y + mu*Z;
    mu = rho * mu;

    %% stop Criterion
    stopCriterion = norm(Z, 'fro') / dnorm;
    if stopCriterion < tol
        converged = true;
    end

    disp(['Iteration' num2str(iter) ' #svd ' num2str(total_svd) ' r(A) ' num2str(svp)...
        ' |E|_0 ' num2str(length(find(abs(E_hat)>0)))...
        ' stopCriterion ' num2str(stopCriterion)]);

    if ~converged && iter >= maxIter
        disp('Maximum iterations reached') ;
        converged = 1 ;
    end
end

if nargin == 5
    fclose(fid);
end


function determ = ris_determinant ( n )

%*****************************************************************************80
%
%% RIS_DETERMINANT returns the determinant of the Ris matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Output, real DETERM the determinant.
%
  top = 1.0;
  for i = 1 : n
    for j = i + 1 : n
      top = top * ( 4 * ( i - j ) * ( i - j ) );
    end
  end

  bottom = 1.0;
  for i = 1 : n
    for j = 1 : n
      bottom = bottom * ( 3 + 2 * n - 2 * i - 2 * j );
    end
  end

  determ = top / bottom;

  return
end


% Procedure to get nodal stresses using Extrapolation method
function [sigma] = Extrapolation(stressGP)

%--------------------------------------------------------------------------
% Purpose : Extrapolating stress at Gaussian points to get nodal stress
%           values
%
% Synopsis : sigma = extrapolation(stressGP)
%
% Variable Description :
%       sigma - Stress at nodal points
%       stressGP - Stressat gaussian points
%--------------------------------------------------------------------------
% Extrapolation matrix obtained from Shape Functions
% Stress points are at (-1,-1),(-1,1),(1,1),1,-1)
explmt = [1+sqrt(3)/2    -1/2          1-sqrt(3)/2    -1/2;
           -1/2       1-sqrt(3)/2          -1/2    1+sqrt(3)/2 ;
          1-sqrt(3)/2    -1/2          1+sqrt(3)/2    -1/2;
           -1/2       1+sqrt(3)/2         -1/2     1-sqrt(3)/2 ] ;

sigmax = explmt*stressGP(:,1) ;
sigmay = explmt*stressGP(:,2) ;
sigmaxy = explmt*stressGP(:,3) ;

sigma = [sigmax sigmay sigmaxy];


function CRB = crb_general_det_1d(design, wavelength, doas, P_est, noise_var, snapshot_count)
%CRB_GENERAL_DET_1D CRB for general 1D arrays based on the deterministic
%(conditional) model, in radians.
%Inputs:
%   design - Array design.
%   wavelength - Wavelength.
%   doas - DOA vector in radians.
%   P_est - Estimated source covariance matrix from deterministic source
%           signals.
%   noise_var - Noise power.
%   snapshot_count - (Optional) number of snapshots. Default is one.
%Reference:
%   [1] P. Stoica and A. Nehorai, "Performance study of conditional and
%       unconditional direction-of-arrival estimation," IEEE Transactions
%       on Acoustics, Speech and Signal Processing, vol. 38, no. 10,
%       pp. 1783-1795, Oct. 1990.
if design.dim ~= 1
    error('1D array expected.');
end
if nargin <= 5
    snapshot_count = 1;
end
[A, D] = steering_matrix(design, wavelength, doas);
[m, k] = size(A);
H = D'*(eye(m) - A/(A'*A)*A')*D;
CRB = real(H .* P_est.');
CRB = eye(k) / CRB * (noise_var / snapshot_count / 2);
end


function z = makebw(or,ph)
% function z = makebw(or,ph)
%
% Create BWT wavelets
% Version 1.2
%
% Arguments:
%  or: Orientation { -1 (DC), 0, 45, 90, 135 }
%  ph: Phase { 0 (odd), 90 (even) }
%
% Result:
%  z: The selected wavelet, or NaN if the parameters are not recognized
%
% Citation:
%  Willmore B, Prenger RJ, Wu MC and Gallant JL (2008). The Berkeley
%  Wavelet Transform: A biologically-inspired orthogonal wavelet transform.
%  Neural Computation 20:6, 1537-1564
%
% The article is available at:
%  <http://dx.doi.org/10.1162/neco.2007.05-07-513>
%
% Copyright (c) 2008 Ben Willmore
%
% Permission is hereby granted, free of charge, to any person
% obtaining a copy of this software and associated documentation
% files (the "Software"), to deal in the Software without
% restriction, including without limitation the rights to use,
% copy, modify, merge, publish, distribute, sublicense, and/or sell
% copies of the Software, and to permit persons to whom the
% Software is furnished to do so, subject to the following
% conditions:
%
% The above copyright notice and this permission notice shall be
% included in all copies or substantial portions of the Software.
%
% THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
% EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
% OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
% NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
% HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
% WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
% FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
% OTHER DEALINGS IN THE SOFTWARE.

z = nan;

if (or==-1)
    z = ones(3,3)/sqrt(9);

elseif (or==0)
    if (ph==0)
        z = [-1 0 1; -1 0 1; -1 0 1]/sqrt(6);
    elseif (ph==90)
        z = [-1 2 -1; -1 2 -1; -1 2 -1]/sqrt(18);
    end

elseif (or==45)
    if (ph==0)
        z = [-1 1 0; 1 0 -1; 0 -1 1]/sqrt(6);
    elseif (ph==90)
        z = [-1 -1 2; -1 2 -1; 2 -1 -1]/sqrt(18);
    end

elseif (or==90)
    if (ph==0)
        z = [-1 -1 -1; 0 0 0; 1 1 1]/sqrt(6);
    elseif (ph==90)
        z = [-1 -1 -1; 2 2 2; -1 -1 -1]/sqrt(18);
    end

elseif (or==135)
    if (ph==0)
        z = [0 -1 1; 1 0 -1; -1 1 0]/sqrt(6);
    elseif (ph==90)
        z = [2 -1 -1; -1 2 -1; -1 -1 2]/sqrt(18);
    end

end

if (isnan(z))
    disp('Wavelet parameters not recognized');
end


function data = outlier(N, r, dist, outliers, noise)

    if nargin < 1
        N = 600;
    end
    if nargin < 2
        r = 20;
    end
    if nargin < 3
        dist = 30;
    end
    if nargin < 4
        outliers = 0.04;
    end
    if nargin < 5
        noise = 5;
    end

    N1 = round(N * (.5-outliers));
    N2 = N1;
    N3 = round(N * outliers);
    N4 = N-N1-N2-N3;

    phi1 = rand(N1,1) * pi;
    r1 = sqrt(rand(N1,1))*r;
    P1 = [-dist + r1.*sin(phi1) r1.*cos(phi1) zeros(N1,1)];

    phi2 = rand(N2,1) * pi;
    r2 = sqrt(rand(N2,1))*r;
    P2 = [dist - r2.*sin(phi2) r2.*cos(phi2) 3*ones(N2,1)];

    P3 = [rand(N3,1)*noise dist+rand(N3,1)*noise 2*ones(N3,1)];

    P4 = [rand(N4,1)*noise -dist+rand(N4,1)*noise ones(N4,1)];

    data = [P1; P2; P3; P4];

end


%% RATE OF CONVERGENCE OF MIXED FINITE ELEMENT METHOD (RT0-P0) FOR POISSON EQUATION
%
% This example is to show the rate of convergence of RT0-P0 mixed finite
% element approximation of the Poisson equation on the unit square with the
% following boundary conditions:
%
% - pure Dirichlet boundary condition.
% - Pure Neumann boundary condition.
% - mxied boundary condition.
%
% The basis, data structure and numerical test is summarized in <a
% href="matlab:ifem PoissonRT0femrate">PoissonRT0femrate</a>.
%
% The optimal rates of convergence for u and sigma are observed, namely,
% 1st order for L2 norm of u, L2 norm of sigma and H(div) norm of sigma.
% The 2nd order convergent rates between two discrete functions ||uI-uh||
% and ||sigmaI-sigmah|| are known as superconvergence.
%
% Triangular preconditioned GMRES (default solver) and Uzawa preconditioned
% CG converges uniformly in all cases. Traingular preconditioner is two
% times faster than PCG although GMRES is used.
% See also PoissonBDM1mfemrate, Poissonfemrate, Poissonafemrate
%
% Copyright (C) Long Chen. See COPYRIGHT.txt for details.

%% Setting
[node,elem] = squaremesh([0,1,0,1],0.25);
mesh = struct('node',node,'elem',elem);
option.L0 = 1;
option.maxIt = 4;
option.printlevel = 1;
option.elemType = 'RT0';
option.solver = 'tri';
pde = sincosNeumanndata;

%% Pure Neumann boundary condition.
mesh.bdFlag = setboundary(node,elem,'Neumann');
mfemPoisson(mesh,pde,option);

%% Pure Dirichlet boundary condition.
mesh.bdFlag = setboundary(node,elem,'Dirichlet');
mfemPoisson(mesh,pde,option);

%% Mix Dirichlet and Neumann boundary condition.
option.solver = 'uzawapcg';
mesh.bdFlag = setboundary(node,elem,'Dirichlet','~(x==0)','Neumann','x==0');
mfemPoisson(mesh,pde,option);


function loc_2d_shapes(NumOfSamples)
% Create Localized 2-d data from uniform distribution.
% Data are bount to the rectangle between points A(0,6) and B(4,10),
% a circle with center at C(7.5,2.5) and radius R = 2,
% and an isosceles triangle defined by the intesection points
% of the lines y=6, y=2x-5 and y=-2x+25.
% Every local set has the same number of samples (NumOfSamples/4).

clc;
input_dims = 2;

% Initialize Sample counters.
k1 = 0; k2 = 1; k3 = 0; k4 = 0;

% Initialize Data
Data = [7.5; 2.5;];

while size(Data,2)<NumOfSamples

In1 = 10*rand(input_dims,1);

% Apply Mask to input Data
if (In1(1,1)<4)&&(In1(1,1)>0)&&(In1(2,1)<10)&&(In1(2,1)>6)&&(k1<=NumOfSamples/4)
    Data = [Data In1];
    k1 = k1 + 1;
elseif norm(In1-[7.5; 2.5;])<=2&&(k2<=NumOfSamples/4)
    Data = [Data In1];
    k2 = k2 + 1;
elseif (In1(2,1)>6)&&(In1(2,1)<2*In1(1,1)-5)&&(In1(2,1)<-2*In1(1,1)+25)&&(k3<=NumOfSamples/4)
    Data = [Data In1];
    k3 = k3 + 1;
elseif (abs(In1(2,1)-In1(1,1))<.1)&&(In1(2,1)<5)&&(In1(2,1)>.5)&&(In1(1,1)<5)&&(In1(1,1)>.5)&&(k4<=NumOfSamples/4)
    Data = [Data In1];
    k4 = k4 + 1;
end
end

a = randperm(NumOfSamples);
Data = Data(:,a);

save local_uniform_2d Data;
plot(Data(1,:),Data(2,:),'.','MarkerSize',1);
grid on;
clear;


function y = jed_to_year_hebrew ( jed )

%*****************************************************************************80
%
%% JED_TO_YEAR_HEBREW: the year in the Hebrew calendar when a JED occurred.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 June 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Edward Richards,
%    Algorithm H,
%    Mapping Time, The Calendar and Its History,
%    Oxford, 1999, page 331.
%
%  Parameters:
%
%    Input, real JED, the Julian Ephemeris Date.
%
%    Output, integer Y, the year in the Hebrew calendar that
%    included the JED.  If the input JED is less than the epoch of the Hebrew
%    calendar, then Y is always returned as -1.
%
  jed_epoch = epoch_to_jed_hebrew ( );

  if ( jed < jed_epoch )
    y = -1;
    return
  end
%
%  Using integer arithmetic in this computation may cause overflow.
%
%  Compute the number of months elapsed up to the date.
%
  m = 1 + floor ( ( 25920.0 * ( jed - jed_epoch + 2.5 ) ) / 765433.0 );
%
%  Estimate the number of years represented by these months.
%
  y = 19 * floor ( m / 235 ) + floor ( ( 19 * ( i4_modp ( m, 235 ) - 2 ) ) / 235 ) + 1;
  y = max ( y, 1 );
%
%  Determine the JED of the first day of that year.
%
  jed2 = new_year_to_jed_hebrew ( y );
%
%  We might have been off by 1 year.
%
  if ( jed < jed2 )
    y = y - 1;
  end

  return
end


% Builds and solves a simple least-squares problem using cvx

echo on

n = 100;
A = randn(2*n,n);
b = randn(2*n,1);

cvx_begin
   variable x1(n)
   minimize( norm( A*x1-b ) )
cvx_end
v1 = cvx_optval;

cvx_begin
   variable x2(n)
   minimize( sum_square( A*x2-b ) )
cvx_end
v2 = cvx_optval;

% The differences between the unsquared and squared versions should be small
norm( x1 - x2 ) / norm( x1 ) %#ok
abs( v1 - sqrt(v2) ) / v1 %#ok

echo off


%% AMG TEST II: Different boundary conditions
%
% We consider the linear finite element discretization of Poisson equation
% on the unstructured mesh with Dirichlet or Neumann boundary conditions.

%%
load lakemesh
%% Dirichlet problem
[N,itStep,time,err] = amgtest(node,elem,1);
%%
colHeaders = {'Unknowns','Iterations','Time(sec)','Error'};
makeHtmlTable([N itStep time err],[],[],colHeaders,[],6);
%%
r = showrate(N.*log(N),time,3);
xlabel('NlogN'); ylabel('Time');
title(['Complexity is (NlogN)^{' num2str(r) '}'] );

%% Neumann problem
[N,itStep,time,err] = amgtest(node,elem,0);
%%
colHeaders = {'Unknowns','Iterations','Time(sec)','Error'};
makeHtmlTable([N itStep time err],[],[],colHeaders,[],6);
%%
r = showrate(N.*log(N),time,3);
xlabel('NlogN'); ylabel('Time');
title(['Complexity is (NlogN)^{' num2str(r) '}'] );


% ASTROTIK by Francesco Santilli
% R2BP (Restricted Two Bodies Problem)
% M2E inverts the keplerian equation for an elliptical orbit and finds
% eccentric anomaly with a numeric iterative method.
%
% Usage: E = M2E(M,e)
%
% where: M(k) = mean anomaly [rad]
%        e = eccentricity [-] (0<e<1)
%        E(k) = eccentric anomaly [rad]

function E = M2E(M,e)

    if ~(nargin == 2)
        error('Wrong number of input arguments.')
    end

    K = check(M,1);
    check(e,0)

    if (e<=0 || e>=1)
        error('e must be in the range (0,1)')
    end

    E = zeros(K,1);
    for k = 1:K
        F = @(E) E-e*sin(E)-M(k);
        E1 = M(k)-e-eps;
        E2 = M(k)+e+eps;
        E(k) = regulafalsi(F,E1,E2);
    end

end


function [ num_int, pint ] = plane_normal_triangle_int_3d ( pp, normal, t )

%*****************************************************************************80
%
%% PLANE_NORMAL_TRIANGLE_INT_3D: intersection ( normal plane, triangle ) in 3D.
%
%  Discussion:
%
%    The normal form of a plane in 3D is:
%
%      PP is a point on the plane,
%      N is a normal vector to the plane.
%
%    There may be 0, 1, 2 or 3 points of intersection returned.
%
%    If two intersection points are returned, then the entire line
%    between them comprises points of intersection.
%
%    If three intersection points are returned, then all points of
%    the triangle intersect the plane.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 May 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real PP(3), a point on the plane.
%
%    Input, real NORMAL(3), a normal vector to the plane.
%
%    Input, real T(3,3), the vertices of the triangle.
%
%    Output, integer NUM_INT, the number of intersection points returned.
%
%    Output, real PINT(3,3), the coordinates of the
%    intersection points.
%
  dim_num = 3;

  num_int = 0;
  pint = [];
%
%  Compute the signed distances between the vertices and the plane.
%
  d = - normal(1:dim_num) * pp(1:dim_num)';

  dist1 = normal(1:dim_num) * t(1:dim_num,1) + d;
  dist2 = normal(1:dim_num) * t(1:dim_num,2) + d;
  dist3 = normal(1:dim_num) * t(1:dim_num,3) + d;
%
%  Consider any zero distances.
%
  if ( dist1 == 0.0 )

    num_int = num_int + 1;
    pint(1:dim_num,num_int) = t(1:dim_num,1);

  end

  if ( dist2 == 0.0 )

    num_int = num_int + 1;
    pint(1:dim_num,num_int) = t(1:dim_num,2);

  end

  if ( dist3 == 0.0 )

    num_int = num_int + 1;
    pint(1:dim_num,num_int) = t(1:dim_num,3);

  end
%
%  If 2 or 3 of the nodes intersect, we're already done.
%
  if ( 2 <= num_int )
    return
  end
%
%  If one node intersects, then we're done unless the other two
%  are of opposite signs.
%
  if ( num_int == 1 )

    if ( dist1 == 0.0 )

      [ num_int, pint ] = plane_imp_triangle_int_add_3d ( t(1:dim_num,2), ...
        t(1:dim_num,3), dist2, dist3, num_int, pint );

    elseif ( dist2 == 0.0 )

      [ num_int, pint ] = plane_imp_triangle_int_add_3d ( t(1:dim_num,1), ...
        t(1:dim_num,3), dist1, dist3, num_int, pint );

    elseif ( dist3 == 0.0 )

      [ num_int, pint ] = plane_imp_triangle_int_add_3d ( t(1:dim_num,1), ...
        t(1:dim_num,2), dist1, dist2, num_int, pint );

    end

    return

  end
%
%  All nodal distances are nonzero, and there is at least one
%  positive and one negative.
%
  if ( dist1 * dist2 < 0.0 & dist1 * dist3 < 0.0 )

    [ num_int, pint ] = plane_imp_triangle_int_add_3d ( t(1:dim_num,1), ...
      t(1:dim_num,2), dist1, dist2, num_int, pint );

    [ num_int, pint ] = plane_imp_triangle_int_add_3d ( t(1:dim_num,1), ...
      t(1:dim_num,3), dist1, dist3, num_int, pint );

  elseif ( dist2 * dist1 < 0.0 & dist2 * dist3 < 0.0 )

    [ num_int, pint ] = plane_imp_triangle_int_add_3d ( t(1:dim_num,2), ...
      t(1:dim_num,1), dist2, dist1, num_int, pint );

    [ num_int, pint ] = plane_imp_triangle_int_add_3d ( t(1:dim_num,2), ...
      t(1:dim_num,3), dist2, dist3, num_int, pint );

  elseif ( dist3 * dist1 < 0.0 & dist3 * dist2 < 0.0 )

    [ num_int, pint ] = plane_imp_triangle_int_add_3d ( t(1:dim_num,3), ...
      t(1:dim_num,1), dist3, dist1, num_int, pint );

    [ num_int, pint ] = plane_imp_triangle_int_add_3d ( t(1:dim_num,3), ...
      t(1:dim_num,2), dist3, dist2, num_int, pint );

  end

  return
end


function a = sweet3 (  )

%*****************************************************************************80
%
%% SWEET3 returns the SWEET3 matrix.
%
%  Example:
%
%      8    4    1    6    2    3
%      4    8    4    1    6    2
%    -34    4    8    4    1    6
%      5  -34    4    8    4    1
%      3    5  -34    4    8    4
%      1    3    5  -34    4    8
%
%  Properties:
%
%    A is Toeplitz: constant along diagonals.
%
%    A is generally not symmetric: A' /= A.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Per Hansen, Tony Chan,
%    FORTRAN Subroutines for General Toeplitz Systems,
%    ACM Transactions on Mathematical Software,
%    Volume 18, Number 3, September 1992, pages 256-273.
%
%    Douglas Sweet,
%    The use of pivoting to improve the numerical performance of
%    Toeplitz solvers,
%    In "Advanced Algorithms and Architectures for Signal Processing",
%    Edited by J M Speiser,
%    Proceedings SPIE 696, 1986, pages 8-18.
%
%  Parameters:
%
%    Output, real A(6,6), the matrix.
%
  n = 6;

  v = [ 1.0, 3.0, 5.0, -34.0, 4.0, 8.0, 4.0, ...
    1.0, 6.0, 2.0, 3.0 ];

  a = zeros ( n, n );

  for i = 1 : n
    for j = 1 : n

      a(i,j) = v ( j - i + 6 );

    end
  end

  return
end


function PhiX = polynomial(X, degree, varargin)
    params = inputParser;
    params.addRequired('X');
    params.addRequired('degree');
    params.addParameter('bias', true);
    params.parse(X, degree, varargin{:});
    results = params.Results;
    bias = results.bias;
    [rows, dim] = size(X);
    if bias
        num_features = 1;
    else
        num_features = 0;
    end
    for deg=1:degree
        num_features = num_features + nchoosek(dim+deg-1, deg);
    end
    PhiX = zeros(rows, num_features);
    if bias
        PhiX(:, 1) = 1;
    end
    for r=1:rows
        x = X(r, :);
        if bias
            s = 2;
        else
            s = 1;
        end
        for deg=1:degree
            num_features = nchoosek(dim+deg-1, deg);
            e = s + num_features -1;
            % form all k degree combinations
            combs = spx.discrete.combinations_with_replacement(x, deg);
            features = prod(combs, 2)';
            PhiX(r, s:e) = features;
            s = s + num_features;
        end
    end
end


function [all_theta] = oneVsAll(X, y, num_labels, lambda)
%ONEVSALL trains multiple logistic regression classifiers and returns all
%the classifiers in a matrix all_theta, where the i-th row of all_theta
%corresponds to the classifier for label i
%   [all_theta] = ONEVSALL(X, y, num_labels, lambda) trains num_labels
%   logisitc regression classifiers and returns each of these classifiers
%   in a matrix all_theta, where the i-th row of all_theta corresponds
%   to the classifier for label i

% Some useful variables
m = size(X, 1);
n = size(X, 2);

% You need to return the following variables correctly
all_theta = zeros(num_labels, n + 1);

% Add ones to the X data matrix
X = [ones(m, 1) X];

% ====================== YOUR CODE HERE ======================
% Instructions: You should complete the following code to train num_labels
%               logistic regression classifiers with regularization
%               parameter lambda.
%
% Hint: theta(:) will return a column vector.
%
% Hint: You can use y == c to obtain a vector of 1's and 0's that tell use
%       whether the ground truth is true/false for this class.
%
% Note: For this assignment, we recommend using fmincg to optimize the cost
%       function. It is okay to use a for-loop (for c = 1:num_labels) to
%       loop over the different classes.
%
%       fmincg works similarly to fminunc, but is more efficient when we
%       are dealing with large number of parameters.
%
% Example Code for fmincg:
%
%     % Set Initial theta
%     initial_theta = zeros(n + 1, 1);
%
%     % Set options for fminunc
%     options = optimset('GradObj', 'on', 'MaxIter', 50);
%
%     % Run fmincg to obtain the optimal theta
%     % This function will return theta and the cost
%     [theta] = ...
%         fmincg (@(t)(lrCostFunction(t, X, (y == c), lambda)), ...
%                 initial_theta, options);
%

initial_theta = zeros(n + 1, 1);

options = optimset('GradObj', 'on', 'MaxIter', 50);

for c = 1:num_labels
    all_theta(c,:) = fmincg (@(t)(lrCostFunction(t, X, (y == c), lambda)), initial_theta, options);

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

end

github-repos/MATLAB/Borye-machine-learning-coursera-1/machine-learning-coursera-1-033fdc2e6da393eeb1179a09aafe92362021effb/Week 4 Assignments/Multi-class Classification and Neural Networks/mlclass-ex3/oneVsAll.m


function rank = rgf_rank ( m, f )

%*****************************************************************************80
%
%% RGF_RANK ranks a restricted growth function.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    26 January 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Donald Kreher, Douglas Simpson,
%    Combinatorial Algorithms,
%    CRC Press, 1998,
%    ISBN: 0-8493-3988-X,
%    LC: QA164.K73.
%
%  Parameters:
%
%    Input, integer M, the domain of the RGF is the integers
%    from 1 to M.  M must be positive.
%
%    Input, integer F(M), the restricted growth function.
%
%    Output, integer RANK, the rank of the restricted growth
%    function.
%

%
%  Check.
%
  ierror = rgf_check ( m, f );

  if ( ierror ~= 0 )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'RGF_RANK - Fatal error!\n' );
    fprintf ( 1, '  The input array is illegal!\n' );
    fprintf ( 1, '  IERROR = %d\n', ierror );
    error ( 'RGF_RANK - Fatal error!' );
  end
%
%  Get the generalized restricted growth function table.
%
  offset = 1;
  d = rgf_g_table ( m );

  rank = 0;
  j = 1;
  for i = 2 : m
    rank = rank + ( f(i) - 1 ) * d(m-i+offset,j+offset);
    j = max ( j, f(i) );
  end

  return
end


function [f,t]=enframe(x,win,inc)
%ENFRAME split signal up into (overlapping) frames: one per row. [F,T]=(X,WIN,INC)
%
%	F = ENFRAME(X,LEN) splits the vector X(:) up into
%	frames. Each frame is of length LEN and occupies
%	one row of the output matrix. The last few frames of X
%	will be ignored if its length is not divisible by LEN.
%	It is an error if X is shorter than LEN.
%
%	F = ENFRAME(X,LEN,INC) has frames beginning at increments of INC
%	The centre of frame I is X((I-1)*INC+(LEN+1)/2) for I=1,2,...
%	The number of frames is fix((length(X)-LEN+INC)/INC)
%
%	F = ENFRAME(X,WINDOW) or ENFRAME(X,WINDOW,INC) multiplies
%	each frame by WINDOW(:)
%
%   The second output argument, T, gives the time in samples at the centre
%   of each frame. T=i corresponds to the time of sample X(i).
%

nx=length(x(:));
nwin=length(win);
if (nwin == 1)
   len = win;
else
   len = nwin;
end
if (nargin < 3)
   inc = len;
end
nf = fix((nx-len+inc)/inc);
f=zeros(nf,len);
indf= inc*(0:(nf-1)).';
inds = (1:len);
f(:) = x(indf(:,ones(1,len))+inds(ones(nf,1),:));
if (nwin > 1)
    w = win(:)';
    f = f .* w(ones(nf,1),:);
end
if nargout>1
    t=(1+len)/2+indf;
end


function funhan = piecewise(varargin)
%PIECEWISE Piecewise-defined functions.
%
%  F = PIECEWISE(COND1,DEFN1,...CONDn,DEFNn,DEFAULT) returns a callable
%  function F that applies different definitions according to supplied
%  conditions. For a given X, F(X) will test COND1 and apply DEFN1 if true,
%  etc. If all conditions fail, then DEFAULT is applied. For any particular
%  input, the first condition to match is the only one applied.
%
%  Each condition should be either a:
%    * function handle evaluating to logical values, or
%    * vector [a b] representing membership in the half-open interval [a,b).
%  Each definition should be either a:
%    * function handle, or
%    * scalar value.
%  The DEFAULT definition is optional; if omitted, it will be set to NaN.
%
%  All function definitions can accept multiple input variables, but they
%  all must accept the same number as in the call to F. They also should
%  all be vectorized, returning results the same size and shape as their
%  inputs. Complex inputs will work if the definitions are set up
%  accordingly; in that case, intervals will be tested using only Re parts.
%
%  The special syntax F() displays all the conditions and definitions for F.
%
%  Examples:
%     f = piecewise([-1 1],@(x) 1+cos(pi*x),0);   % a "cosine bell"
%     ezplot(f,[-2 2])
%     g = piecewise(@(x) sin(x)<0.5,@sin,@(x) 1-sin(x));
%     ezplot(g,[-2*pi 2*pi])
%     h = piecewise(@(x,y) (x<0)|(y<0),@(x,y) sin(x-y));  % defined on L
%     ezsurf(h,[-1 1])
%     chi = piecewise(@(x,y,z) x.^2+y.^2+z.^2<1,1,0); % characteristic func
%     [ triplequad(chi,-1,1,-1,1,-1,1), 4/3*pi ]
%         ans =
%                4.1888    4.1888
%
%  See also FUNCTION_HANDLE.

% Copyright (c) 2007 by Toby Driscoll.
% Version 1, 6 Aug 2007.

% If an even number of inputs was given, no default value exists.
if rem(nargin,2)==0
  default = NaN;
else
  default = varargin{end};
end

% Number of condition/definition pairs.
numdefs = floor(nargin/2);
condn = varargin(1:2:2*numdefs);
defn = varargin(2:2:2*numdefs);

funhan = @piecewisefun;   % this is the return value

  % This is the defintion of the returned function.
  function f = piecewisefun(varargin)
  % Special syntax to display the function.
  if nargin==0
    fprintf('\nPiecewise function with conditions/definitions:\n')
    for k = 1:numdefs
      fprintf('  If %s : %s\n',funcornum2str(condn{k}),funcornum2str(defn{k}))
    end
    fprintf('  Otherwise : %s\n\n',funcornum2str(default))
    return
  end

  % Normal call
  f = zeros(size(varargin{1}));
  mask = false(size(f));
  for k = 1:numdefs
    % Determine the values satistfying condition k.
    if isa(condn{k},'function_handle')
      newpts = logical( condn{k}(varargin{:}) );
    else
      newpts = (varargin{1}>=condn{k}(1)) & (varargin{1}<condn{k}(2));
    end
    newpts = newpts & (~mask);
    % Evaluate definition k
    if isa(defn{k},'function_handle')
      % Tricky part: Extract subset of points from each variable.
      sref.type = '()';
      sref.subs = {newpts};
      extract = @(x) subsref(x,sref);
      x = cellfun(extract,varargin,'uniformoutput',false);
      f(newpts) = defn{k}(x{:});
    else
      f(newpts) = defn{k};  % scalar expansion
    end
    mask = mask | newpts;
  end

  % Default case
  if isa(default,'function_handle')
    sref.subs = {~mask};
    extract = @(x) subsref(x,sref);
    x = cellfun(extract,varargin,'uniformoutput',false);
    f(~mask) = default(x{:});
  else
    f(~mask) = default;  % scalar expansion
  end

  end

  % This subfunction converts a function or scalar to its native char form,
  % and a 2-vector into an interval notation.
  function s = funcornum2str(f)
    if isa(f,'function_handle')
      s = char(f);
    elseif numel(f)==1
      s = num2str(f);
    else
      s = [ 'in [' num2str(f(1)) ',' num2str(f(2))  ')' ];
    end
  end

end


function a = antihadamard ( n )

%*****************************************************************************80
%
%% ANTIHADAMARD returns the ANTIHADAMARD matrix.
%
%  Discussion:
%
%    An Anti-Hadamard matrix is one whose elements are all 0 or 1,
%    and for which the Frobenius norm of the inverse is as large as
%    possible.  This routine returns a matrix for which the Frobenius norm
%    of the inverse is large, though not necessarily maximal.
%
%  Formula:
%
%    if ( I = J )
%      A(I,J) = 1
%    else if ( I < J and mod ( I+J, 2 ) = 1 )
%      A(I,J) = 1
%    else
%      A(I,J) = 0
%
%  Example:
%
%    N = 5
%
%    1 1 0 1 0
%    0 1 1 0 1
%    0 0 1 1 0
%    0 0 0 1 1
%    0 0 0 0 1
%
%  Properties:
%
%    A is generally not symmetric: A' /= A.
%
%    A is Toeplitz: constant along diagonals.
%
%    A is upper triangular.
%
%    A is integral: int ( A ) = A.
%
%    A is a zero-one matrix.
%
%    The inverse of A is integral.
%
%    det ( A ) = 1.
%
%    A is unimodular.
%
%    LAMBDA(1:N) = 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 September 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Ronald Graham, Neal Sloane,
%    Anti-Hadamard Matrices,
%    Linear Algebra and Applications,
%    Volume 62, 1984, pages 113-137.
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Output, real A(N,N), the matrix.
%
  a = zeros ( n, n );

  for i = 1 : n
    for j = 1 : n

      if ( j < i )
        a(i,j) = 0.0;
      elseif ( i == j )
        a(i,j) = 1.0;
      elseif ( mod ( i+j, 2 ) == 1 )
        a(i,j) = 1.0;
      else
        a(i,j) = 0.0;
      end

    end
  end

  return
end


function [X, R] = imstack2vectors(S, MASK)
%IMSTACK2VECTORS Extracts vectors from an image stack.
%   [X, R] = imstack2vectors(S, MASK) extracts vectors from S, which
%   is an M-by-N-by-n stack array of n registered images of size
%   M-by-N each (see Fig. 11.24). The extracted vectors are arranged
%   as the rows of array X. Input MASK is an M-by-N logical or
%   numeric image with nonzero values (1s if it is a logical array)
%   in the locations where elements of S are to be used in forming X
%   and 0s in locations to be ignored. The number of row vectors in X
%   is equal to the number of nonzero elements of MASK. If MASK is
%   omitted, all M*N locations are used in forming X.  A simple way
%   to obtain MASK interactively is to use function roipoly. Finally,
%   R is an array whose rows are the 2-D coordinates containing the
%   region locations in MASK from which the vectors in S were
%   extracted to form X.

%   Copyright 2002-2004 R. C. Gonzalez, R. E. Woods, & S. L. Eddins
%   Digital Image Processing Using MATLAB, Prentice-Hall, 2004
%   $Revision: 1.6 $  $Date: 2003/11/21 14:37:21 $

% Preliminaries.
[M, N, n] = size(S);
if nargin == 1
   MASK = true(M, N);
else
   MASK = MASK ~= 0;
end

% Find the set of locations where the vectors will be kept before
% MASK is changed later in the program.
[I, J] = find(MASK);
R = [I, J];

% Now find X.

% First reshape S into X by turning each set of n values along the third
% dimension of S so that it becomes a row of X. The order is from top to
% bottom along the first column, the second column, and so on.
Q = M*N;
X = reshape(S, Q, n);

% Now reshape MASK so that it corresponds to the right locations
% vertically along the elements of X.
MASK = reshape(MASK, Q, 1);

% Keep the rows of X at locations where MASK is not 0.
X = X(MASK, :);


function convex_hull ( prefix )

%*****************************************************************************80
%
%% CONVEX_HULL: an algorithm for the convex hull of a set of 2D points.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    13 February 2011
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CONVEX_HULL:\n' );
  fprintf ( 1, '  MATLAB version.\n' );

  if ( nargin < 1 )
    prefix = 'random points'
    n = 25;
    xy = rand ( n, 2 );
  else
    node_filename = strcat ( prefix, '_nodes.txt' );
    xy = load ( node_filename );
  end
%
%  Plot the initial data.
%
  figure ( 1 )
  clf
  plot ( xy(:,1), xy(:,2), 'b.', 'MarkerSize', 20 );
  grid on
  axis tight
  axis equal
  xlabel ( '<--- X --->' );
  ylabel ( '<--- Y --->' );
  title ( prefix );
%
%  Plot the convex hull as computed by MATLAB.
%
  figure ( 2 )
  clf
  k = convhull ( xy(:,1), xy(:,2) );
  plot ( xy(:,1), xy(:,2), 'b.', 'MarkerSize', 20 );
  hold on
  plot( xy(k,1), xy(k,2), 'r-', 'LineWidth', 2 );
  grid on
  axis tight
  axis equal
  xlabel ( '<--- X --->' );
  ylabel ( '<--- Y --->' );
  title ( sprintf ( '%s by MATLAB convhull', prefix ) );
  hold off
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'CONVEX_HULL:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );

  fprintf ( 1, '\n' );
  timestamp ( );

  return
end
function timestamp ( )

%*****************************************************************************80
%
%% TIMESTAMP prints the current YMDHMS date as a timestamp.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 February 2003
%
%  Author:
%
%    John Burkardt
%
  t = now;
  c = datevec ( t );
  s = datestr ( c, 0 );
  fprintf ( 1, '%s\n', s );

  return
end


function exp_test ( )

%*****************************************************************************80
%
%% EXP_TEST tests R4_EXP and R8_EXP.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 September 2011
%
%  Author:
%
%    John Burkardt
%
  addpath ( '../test_values' );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'EXP_TEST:\n' );
  fprintf ( 1, '  Test EXP_VALUES, R4_EXP, R8_EXP\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '             X          EXP(X)\n' );
  fprintf ( 1, '                     R4_EXP(X)        Diff\n' );
  fprintf ( 1, '                     R8_EXP(X)        Diff\n' );

  n_data = 0;

  while ( 1 )

    [ n_data, x, fx1 ] = exp_values ( n_data );

    if ( n_data == 0 )
      break
    end

    fx2 = r4_exp ( single ( x ) );
    fx3 = r8_exp ( x );

    fprintf ( 1, '\n' );
    fprintf ( 1, '  %14.4f  %14.6g\n', x, fx1 );
    fprintf ( 1, '                  %14.6g  %14.6g\n', fx2, abs ( fx1 - fx2 ) );
    fprintf ( 1, '                  %14.6g  %14.6g\n', fx3, abs ( fx1 - fx3 ) );

  end

  rmpath ( '../test_values' );

  return
end


function rational_quadratic_test ( )

%*****************************************************************************80
%
%% RATIONAL_QUADRATIC_TEST examines the rational_quadratic correlation.
%
%  Discussion:
%
%    This code is based substantially on a document by Toby Driscoll.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    25 February 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Toby Driscoll,
%    Mercer's theorem and the Karhunen-Loeve expansion,
%    Oxford University Mathematical Institute,
%    http://www2.maths.ox.ac.uk/chebfun/examples/stats/pdf/MercerKarhunenLoeve.pdf
%
  rmpath ( '/usr/local/matlab/toolbox/datafeed/datafeed' )
  addpath ( '../chebfun' )

  timestamp ( );
  fprintf ( 1, '\n' );
  fprintf ( 1, 'RATIONAL_QUADRATIC_TEST:\n' );
  fprintf ( 1, '  Demonstrate Mercer''s theorem and the KL expansion\n' );
  fprintf ( 1, '  for the rational_quadratic kernel.\n' );
%
%  Set the interval.
%
  a = 0.0;
  b = +10.0;
  fprintf ( 1, '  Using interval [%g,%g]\n', a, b );
  s_num = 21;
  s_vec = linspace ( a, b, s_num );
%
%  FRED is a function from the CHEBFUN library.
%
%  It constructs a "chebop" representing the Fredholm integral operator
%  with kernel K for functions in domain [A,B].
%
  F = fred ( @rational_quadratic_correlation, domain ( [ a, b ] ) );
%
%  EIGS has been extended to be able to compute the eigenvalues Lambda
%  and eigenfunctions Psi of the Fredholm integral operator represented by F.
%
%  The "LM" switch requests that we return the eigenvalues of largest magnitude.
%
%  Each Psi is a CHEBFUN, that is, it takes a real number argument.
%
  eigen_num = 20;
  [ Psi, Lambda ] = eigs ( F, eigen_num, 'lm' );

  eigen_found = length ( Lambda );
  fprintf ( 1, '  Requested %d eigenmodes, computed %d\n', eigen_num, eigen_found );
  eigen_num = min ( eigen_num, eigen_found );
%
%  Print the eigenvalues.
%
  lambda_vec = diag ( Lambda );
  fprintf ( 1, '\n' );
  fprintf ( 1, '   I       Lambda(I)\n' );
  fprintf ( 1, '\n' );
  for i = 1 : eigen_num
    fprintf ( 1, '  %2d  %10g\n', i, lambda_vec(i) );
  end
%
%  Plot the eigenvalues.
%
  figure ( 1 )
  clf
  plot ( lambda_vec, 'Linewidth', 2 );
  hold on
  plot ( lambda_vec, 'b.', 'Markersize', 20 );
  title ( 'rational_quadratic: Mercer eigenvalues' );
  xlabel ( '<--- N --->')
  grid on
  print -dpng 'rational_quadratic_figure1.png'
%
%  Plot selected eigenfunctions.
%
  figure ( 2 )
  subplot ( 4, 1, 1 )
  plot ( Psi(:,1), 'Linewidth', 2 );
  title ( 'rational_quadratic: Mercer eigenfunction PSI(1)' )
  grid on
  subplot ( 4, 1, 2 )
  plot ( Psi(:,2), 'Linewidth', 2 );
  title ( 'rational_quadratic: Mercer eigenfunction PSI(2)' )
  grid on
  subplot ( 4, 1, 3 )
  plot ( Psi(:,5), 'Linewidth', 2 );
  title ( 'rational_quadratic: Mercer eigenfunction PSI(5)' )
  grid on
  subplot ( 4, 1, 4 )
  plot ( Psi(:,10), 'Linewidth', 2 );
  title ( 'rational_quadratic: Mercer eigenfunction PSI(10)' )
  grid on
  print -dpng 'rational_quadratic_figure2.png'
%
%  Orthonormality check.
%
  ptp = Psi' * Psi;
  error_frobenius = r8mat_is_identity ( eigen_num, ptp );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Frobenius norm of I - Psi'' * Psi = %g\n', error_frobenius );
%
%  K(S,S) should be exactly 1.
%  Because we are using a truncated representation of K, our estimate of K(S,S)
%  will be smaller than 1.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Truncated estimate of K(s,s) = 1 for S in the interval.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '        S        K(s,s) estimate\n' );
  fprintf ( 1, '\n' );
  for i = 1 : 21
    s = s_vec(i);
    ptlp = Psi(s,:) * Lambda * Psi(s,:)';
    fprintf ( 1, '  %10g  %14g\n', s, ptlp );
  end
%
%  Look at eigenvalue decay.
%
  x = diff ( log ( ( 1:eigen_num ) ) );
  y = diff ( log ( lambda_vec ) )';
  c = y ./ x;
  figure ( 3 );
  clf
  plot ( c, 'Linewidth', 2 );
  grid on
  hold on
  plot ( c, 'b.', 'Markersize', 20 );
  xlabel ( '<-- N -->' )
  title ( 'rational_quadratic: Eigenvalue decay rate' );
  print -dpng 'rational_quadratic_figure3.png'
%
%  Look at eigenvalue sum.
%
%  The trace of K(s,s) over [a,b] should be the integral of 1 over [a,b],
%  that is, b - a.
%
%  We compare the trace to the partial sums of the lambda's to see how much
%  of the variance of the process we have captured.
%
  trace_K = b - a;
  lambda_cum = cumsum ( lambda_vec ) / trace_K;
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Index    Cumulative Eigenvalue sum\n' );
  fprintf ( 1, '\n' );
  for i = 1 : eigen_num
    fprintf ( 1, '  %5d  %10g\n', i, lambda_cum(i) );
  end
%
%  We decide to use the first 10 eigenfunctions.
%
  eigen_use = 10;
%
%  Find 400 realizations of the process by selecting, for each realization,
%  10 random parameters Z in the truncated KL expansion.
%
  Z = randn ( eigen_use, 400 );
  X = Psi(:,1:eigen_use) * ( sqrt ( Lambda(1:eigen_use,1:eigen_use) ) * Z );
%
%  Plot 40 of the realizations;
%  Plot their mean, computed from all 400.
%
  figure ( 4 )
  clf
  plot ( X(:,1:40) )
  mu = sum ( X, 2 ) / 400;
  hold on;
  plot ( mu, 'k', 'linewidth', 3 );
  title ( 'rational_quadratic: 40 Random Realizations X(t,omega), and their Mean.' )
  print -dpng 'rational_quadratic_figure4.png'
%
%  Estimate the covariance from the data.
%
  figure ( 5 )
  clf
  [ S, T ] = meshgrid ( s_vec, s_vec );
  C = cov ( X(s_vec,:)' );
  mesh ( S, T, C );
  hold on;
  D = rational_quadratic_correlation ( S, T );
  plot3 ( S, T, D, 'k.', 'markersize', 10 )
  title ( 'rational_quadratic: Covariance K(S,T) (dots), and Estimate from Realizations (mesh)' )
  print -dpng 'rational_quadratic_figure5.png'
%
%  Using just 10 functions in the expansion,
%  reduce the correlation length,
%  and examine the sum of the lambda's.
%
  if ( 0 )
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Use a fixed number of eigenfunctions = %d\n', eigen_use );
  fprintf ( 1, '  but vary the correlation length RHOBAR.\n' );
  fprintf ( 1, '  (We used RHOBAR = 1 above.)\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The sum of the eigenvalues, divided by (B-A),\n' );
  fprintf ( 1, '  discloses the relative amount of the total variation\n' );
  fprintf ( 1, '  that is captured by the truncated expansion.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '      RHOBAR      VARSUM\n' );
  fprintf ( 1, '\n' );
  rhobar = 4.0;
  for i = 1 : 10
    K = @ ( s, t ) exp ( - abs ( s - t ) / rhobar );
    F = fred ( K, domain ( [ a, b ] ) );
    lambda_vec = eigs ( F, 20, 'lm' );
    varsum = sum ( lambda_vec(1:eigen_use) ) / ( b - a );
    fprintf ( 1, '  %10g  %10g\n', rhobar, varsum );
    rhobar = rhobar / 2.0;
  end

  end
%
%  Terminate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'RATIONAL_QUADRATIC_TEST:\n' );
  fprintf ( 1, '  Normal end of execution.\n' );
  fprintf ( 1, '\n' );
  timestamp ( );

  rmpath ( '../chebfun' )

  return
end


function J = lotkaObjectiveFCN(u,x,Ts,N,xref,u0,p,Q,R,Ru)
%% Cost function of nonlinear MPC for Lotka-Volterra system
%
% Inputs:
%   u:      optimization variable, from time k to time k+N-1
%   x:      current state at time k
%   Ts:     controller sample time
%   N:      prediction horizon
%   xref:   state references, constant from time k+1 to k+N
%   u0:     previous controller output at time k-1
%
% Output:
%   J:      objective function cost
%

%% Nonlinear MPC design parameters

%% Cost Calculation
% Set initial plant states, controller output and cost
xk = x;
uk = u(1);
J = 0;
% Loop through each prediction step
for ct=1:N
    % Obtain plant state at next prediction step
    xk1 = rk4u(@sparseGalerkinControl_Discrete,xk,uk,Ts,1,[],p);

    % Accumulate state tracking cost from x(k+1) to x(k+N)
    J = J + (xk1-xref)'*Q*(xk1-xref);
    % Accumulate MV rate of change cost from u(k) to u(k+N-1)
    if ct==1
        J = J + (uk-u0)'*R*(uk-u0) + uk'*Ru*uk;
    else
        J = J + (uk-u(ct-1))'*R*(uk-u(ct-1)) + uk'*Ru*uk;
    end
    % Update xk and uk for the next prediction step
    xk = xk1;
    if ct<N
        uk = u(ct+1);
    end
end


%JTRAJ Compute a joint space trajectory
%
% [Q,QD,QDD] = JTRAJ(Q0, QF, M) is a joint space trajectory Q (MxN) where the joint
% coordinates vary from Q0 (1xN) to QF (1xN).  A quintic (5th order) polynomial is used
% with default zero boundary conditions for velocity and acceleration.
% Time is assumed to vary from 0 to 1 in M steps.  Joint velocity and
% acceleration can be optionally returned as QD (MxN) and QDD (MxN) respectively.
% The trajectory Q, QD and QDD are MxN matrices, with one row per time step,
% and one column per joint.
%
% [Q,QD,QDD] = JTRAJ(Q0, QF, M, QD0, QDF) as above but also specifies
% initial QD0 (1xN) and final QDF (1xN) joint velocity for the trajectory.
%
% [Q,QD,QDD] = JTRAJ(Q0, QF, T) as above but the number of steps in the
% trajectory is defined by the length of the time vector T (Mx1).
%
% [Q,QD,QDD] = JTRAJ(Q0, QF, T, QD0, QDF) as above but specifies initial and
% final joint velocity for the trajectory and a time vector.
%
% Notes::
% - When a time vector is provided the velocity and acceleration outputs
%   are scaled assumign that the time vector starts at zero and increases
%   linearly.
%
% See also QPLOT, CTRAJ, SerialLink.jtraj.

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

function [qt,qdt,qddt] = jtraj(q0, q1, tv, qd0, qd1)
    if length(tv) > 1
        tscal = max(tv);
        t = tv(:)/tscal;
    else
        tscal = 1;
        t = (0:(tv-1))'/(tv-1); % normalized time from 0 -> 1
    end

    q0 = q0(:);
    q1 = q1(:);

    if nargin == 3
        qd0 = zeros(size(q0));
        qd1 = qd0;
    elseif nargin == 5
        qd0 = qd0(:);
        qd1 = qd1(:);
    else
        error('incorrect number of arguments')
    end

    % compute the polynomial coefficients
    A = 6*(q1 - q0) - 3*(qd1+qd0)*tscal;
    B = -15*(q1 - q0) + (8*qd0 + 7*qd1)*tscal;
    C = 10*(q1 - q0) - (6*qd0 + 4*qd1)*tscal;
    E = qd0*tscal; % as the t vector has been normalized
    F = q0;

    tt = [t.^5 t.^4 t.^3 t.^2 t ones(size(t))];
    c = [A B C zeros(size(A)) E F]';

    qt = tt*c;

    % compute optional velocity
    if nargout >= 2
        c = [ zeros(size(A)) 5*A 4*B 3*C  zeros(size(A)) E ]';
        qdt = tt*c/tscal;
    end

    % compute optional acceleration
    if nargout == 3
        c = [ zeros(size(A))  zeros(size(A)) 20*A 12*B 6*C  zeros(size(A))]';
        qddt = tt*c/tscal^2;
    end


function DEM_demo_convolution
% DEM demo for linear deconvolution:  This demo considers the deconvolution
% of the responses of a single-input-multiple output input-state-output
% model (DCM) to disclose the input or causes.  It focuses on estimating the
% causes and hidden states: The notes provide a comparative evaluation with
% extended Kalman filtering (see script after return).

% get a simple convolution model
%==========================================================================
M       = spm_DEM_M('convolution model');
M(1).V  = exp(8);                             % error precision
M(1).W  = exp(8);                             % error precision

% and generate data
%==========================================================================
N       = 32;                                 % length of data sequence
U       = exp(-([1:N] - 12).^2/(2.^2));       % this is the Gaussian cause;
DEM     = spm_DEM_generate(M,U,{},{[] 16},{16});

% invert model
%==========================================================================
DEM     = spm_DEM(DEM);

% overlay true values
%--------------------------------------------------------------------------
spm_DEM_qU(DEM.qU,DEM.pU)

return

% explore dimensions (n)
%==========================================================================
for i = 1:8
    DEM.M(1).E.n = i;
    DEM.M(1).E.d = 3;

    D{i}   = spm_DEM(DEM);
    F(i)   = D{i}.F;
    Sx(i)  = sum(sum((D{i}.qU.x{1} - DEM.pU.x{1}).^2));
    Sv(i)  = sum(sum((D{i}.qU.v{2} - DEM.pU.v{2}).^2));
end

% plot
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 1');

subplot(2,1,1)
bar(Sv)
xlabel('n - 1 (d = 2)')
ylabel('sum squared error (causal states)')
title('embedding dimension (n)','FontSize',16)
axis square

d     = [1 8];
for i = 1:length(d)
    subplot(2,length(d),i + length(d))
    hold on
    plot([1:N],D{d(i)}.qU.v{2})
    plot([1:N],DEM.pU.v{2},'linewidth',2,'color',[1 1 1]/2)
    hold off
    axis square
    xlabel('time')
    title(sprintf('n = %i',d(i)),'FontSize',16)
    if i == 1, a = axis; else, axis(a); end
end

% and d
%--------------------------------------------------------------------------
clear D F Sx Sv
for i = 1:6
    DEM.M(1).E.n = 15;
    DEM.M(1).E.d = i - 1;

    D{i}   = spm_DEM(DEM);
    F(i)   = D{i}.F;
    Sx(i)  = sum(sum((D{i}.qU.x{1} - DEM.pU.x{1}).^2));
    Sv(i)  = sum(sum((D{i}.qU.v{2} - DEM.pU.v{2}).^2));

end

% plot
%--------------------------------------------------------------------------
spm_figure('GetWin','Figure 2'); clf

subplot(2,2,1)
bar(Sx)
xlabel('d (n = 15)')
ylabel('sum squared error (hidden states)')
title('embedding dimension (d)','FontSize',16)
axis square
set(gca,'XTickLabel',[0:7])

subplot(2,2,2)
bar(F - min(F) + 32)
xlabel('d (n = 15)')
ylabel('log-evidence')
title('log-evidence','FontSize',16)
axis square
set(gca,'XTickLabel',[0:7])

d     = [1 4];
for i = 1:length(d)
    subplot(2,length(d),i + length(d))
    hold on
    plot([1:N],D{d(i)}.qU.x{1})
    plot([1:N],DEM.pU.x{1},'linewidth',2,'color',[1 1 1]/2)
    hold off
    axis square
    xlabel('time')
    title(sprintf('d = %i',d(i) - 1),'FontSize',16)
    if i == 1, a = axis; else, axis(a); end
end

% Comparison with EKF
%==========================================================================
clear SSE
for i = 1:8

    % i.i.d.
    %----------------------------------------------------------------------
    DEM      = spm_DEM_generate(M,U,{},{[] 16});
    DEM      = spm_DEM(DEM);

    % serial correlations
    %----------------------------------------------------------------------
    D0          = DEM;
    D0.M(1).E.s = 0;
    D0          = spm_DEM(D0);

    % extended kalman filter - i.i.d.
    %----------------------------------------------------------------------
    e_x      = spm_ekf(DEM.M,DEM.Y);
    d_x      = DEM.qU.x{1};
    t_x      = DEM.pU.x{1};
    i_x      = D0.qU.x{1};

    SSE(i,1) = sum(sum((e_x - t_x).^2));
    SSE(i,2) = sum(sum((i_x - t_x).^2));
    SSE(i,3) = sum(sum((d_x - t_x).^2));

end

spm_figure('GetWin','Figure 3');

subplot(2,1,1)
plot(1:N,i_x,'g',1:N,d_x,'r',1:N,e_x,'b',1:N,t_x,'k')
legend('DEM(0)',' ','DEM(0.5)',' ','EKF',' ','true',' ')
xlabel('time')
title({'hidden states','comparison with EKF'},'FontSize',16)
axis square

subplot(2,1,2)
plot(SSE','k:'), hold on
plot(SSE','k.','Markersize',16), hold off
set(gca,'Xtick',[1 2 3],'XLim',[0 4])
set(gca,'Xticklabel',{'EKF','DEM(0)','DEM(0.5)'})
title('sum of squared error (hidden states)','FontSize',16)
axis square

% Show equivalence when causes are not structured
%==========================================================================
M(1).E.s  = 0;
M(1).pE.h = eye(2);
M(1).pC   = [];

M(2).v    = [0;0];
M(2).V    = eye(2);
U         = [U; 0*U];

DEM       = spm_DEM_generate(M,U,{},{[] 16});
DEM       = spm_DEM(DEM);
e_x       = spm_ekf(DEM.M,DEM.Y);
d_x       = DEM.qU.x{1};

spm_figure('GetWin','Figure 4');

subplot(2,1,1)
plot(1:N,d_x,'r',1:N,e_x,'b')
legend('DEM(0)',' ','EKF',' ')
xlabel('time')
title({'hidden states';'no correlations'},'FontSize',16)
axis square

% Show equivalence when causes are removed
%--------------------------------------------------------------------------
DEM.M(1).pE.h = sparse(2,2);
DEM.M(1).W    = speye(2);

DEM           = spm_DEM(DEM);
e_x           = spm_ekf(DEM.M,DEM.Y);
d_x           = DEM.qU.x{1};

spm_figure('GetWin','Figure 4');

subplot(2,1,2)
plot(1:N,d_x,'r',1:N,e_x,'b')
legend('DEM(0)',' ','EKF',' ')
xlabel('time')
title({'hidden states';'no causes'},'FontSize',16)
axis square


function [v,beta,s] = house (x)
%HOUSE find a Householder reflection.
% real or complex case.
% Example:
%   [v,beta,s] = house (x)
% See also: cs_demo

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

v = x ;
s = norm (x) ;
if (s == 0)
    beta = 0 ;
    v (1) = 1 ;
else
    if (x (1) ~= 0)
        s = sign (x (1)) * s ;
    end
    v (1) = v (1) + s ;
    beta = 1 / real (conj (s) * v (1)) ;
end
s = - s ;


function [x, istop, itn, normr, normAr, normA, condA, normx]...
   = lsmr(A, b, lambda, atol, btol, conlim, itnlim, localSize, show)

% LSMR   Iterative solver for least-squares problems.
%   X = LSMR(A,B) solves the system of linear equations A*X=B. If the system
%   is inconsistent, it solves the least-squares problem min ||b - Ax||_2.
%   A is a rectangular matrix of dimension m-by-n, where all cases are
%   allowed: m=n, m>n, or m<n. B is a vector of length m.
%   The matrix A may be dense or sparse (usually sparse).
%
%   X = LSMR(AFUN,B) takes a function handle AFUN instead of the matrix A.
%   AFUN(X,1) takes a vector X and returns A*X. AFUN(X,2) returns A'*X.
%   AFUN can be used in all the following syntaxes.
%
%   X = LSMR(A,B,LAMBDA) solves the regularized least-squares problem
%      min ||(B) - (   A    )X||
%          ||(0)   (LAMBDA*I) ||_2
%   where LAMBDA is a scalar.  If LAMBDA is [] or 0, the system is solved
%   without regularization.
%
%   X = LSMR(A,B,LAMBDA,ATOL,BTOL) continues iterations until a certain
%   backward error estimate is smaller than some quantity depending on
%   ATOL and BTOL.  Let RES = B - A*X be the residual vector for the
%   current approximate solution X.  If A*X = B seems to be consistent,
%   LSMR terminates when NORM(RES) <= ATOL*NORM(A)*NORM(X) + BTOL*NORM(B).
%   Otherwise, LSMR terminates when NORM(A'*RES) <= ATOL*NORM(A)*NORM(RES).
%   If both tolerances are 1.0e-6 (say), the final NORM(RES) should be
%   accurate to about 6 digits. (The final X will usually have fewer
%   correct digits, depending on cond(A) and the size of LAMBDA.)
%   If ATOL or BTOL is [], a default value of 1.0e-6 will be used.
%   Ideally, they should be estimates of the relative error in the
%   entries of A and B respectively.  For example, if the entries of A
%   have 7 correct digits, set ATOL = 1e-7. This prevents the algorithm
%   from doing unnecessary work beyond the uncertainty of the input data.
%
%   X = LSMR(A,B,LAMBDA,ATOL,BTOL,CONLIM) terminates if an estimate
%   of cond(A) exceeds CONLIM. For compatible systems Ax = b,
%   conlim could be as large as 1.0e+12 (say).  For least-squares problems,
%   conlim should be less than 1.0e+8. If CONLIM is [], the default value
%   is CONLIM = 1e+8. Maximum precision can be obtained by setting
%   ATOL = BTOL = CONLIM = 0, but the number of iterations may then be
%   excessive.
%
%   X = LSMR(A,B,LAMBDA,ATOL,BTOL,CONLIM,ITNLIM) terminates if the
%   number of iterations reaches ITNLIM.  The default is ITNLIM = min(m,n).
%   For ill-conditioned systems, a larger value of ITNLIM may be needed.
%
%   X = LSMR(A,B,LAMBDA,ATOL,BTOL,CONLIM,ITNLIM,LOCALSIZE) runs LSMR
%   with reorthogonalization on the last LOCALSIZE v_k's (v-vectors
%   generated by the Golub-Kahan bidiagonalization). LOCALSIZE = 0 or []
%   runs LSMR without reorthogonalization. LOCALSIZE = Inf specifies
%   full reorthogonalization of the v_k's.  Reorthogonalizing only u_k or
%   both u_k and v_k are not an option here, because reorthogonalizing all
%   v_k's makes the u_k's close to orthogonal. Details are given in the
%   submitted SIAM paper.
%
%   X = LSMR(A,B,LAMBDA,ATOL,BTOL,CONLIM,ITNLIM,LOCALSIZE,SHOW) prints an
%   iteration log if SHOW=true. The default value is SHOW=false.
%
%   [X,ISTOP] = LSMR(A,B,...) gives the reason for termination.
%      ISTOP  = 0 means X=0 is a solution.
%             = 1 means X is an approximate solution to A*X = B,
%                 according to ATOL and BTOL.
%             = 2 means X approximately solves the least-squares problem
%                 according to ATOL.
%             = 3 means COND(A) seems to be greater than CONLIM.
%             = 4 is the same as 1 with ATOL = BTOL = EPS.
%             = 5 is the same as 2 with ATOL = EPS.
%             = 6 is the same as 3 with CONLIM = 1/EPS.
%             = 7 means ITN reached ITNLIM before the other stopping
%                 conditions were satisfied.
%
%   [X,ISTOP,ITN] = LSMR(A,B,...) gives ITN = the number of LSMR iterations.
%
%   [X,ISTOP,ITN,NORMR] = LSMR(A,B,...) gives an estimate of the residual
%   norm: NORMR = norm(B-A*X).
%
%   [X,ISTOP,ITN,NORMR,NORMAR] = LSMR(A,B,...) gives an estimate of the
%   residual for the normal equation: NORMAR = NORM(A'*(B-A*X)).
%
%   [X,ISTOP,ITN,NORMR,NORMAR,NORMA] = LSMR(A,B,...) gives an estimate of
%   the Frobenius norm of A.
%
%   [X,ISTOP,ITN,NORMR,NORMAR,NORMA,CONDA] = LSMR(A,B,...) gives an estimate
%   of the condition number of A.
%
%   [X,ISTOP,ITN,NORMR,NORMAR,NORMA,CONDA,NORMX] = LSMR(A,B,...) gives an
%   estimate of NORM(X).
%
%   LSMR uses an iterative method requiring matrix-vector products A*v
%   and A'*u.  For further information, see
%      D. C.-L. Fong and M. A. Saunders,
%      LSMR: An iterative algorithm for sparse least-squares problems,
%      SIAM J. Sci. Comput., submitted 1 June 2010.
%      See http://www.stanford.edu/~clfong/lsmr.html.
%
% 08 Dec 2009: First release version of LSMR.
% 09 Apr 2010: Updated documentation and default parameters.
% 14 Apr 2010: Updated documentation.
% 03 Jun 2010: LSMR with local and/or full reorthogonalization of v_k.
% 10 Mar 2011: Bug fix in reorthgonalization. (suggested by David Gleich)

% David Chin-lung Fong            clfong@stanford.edu
% Institute for Computational and Mathematical Engineering
% Stanford University
%
% Michael Saunders                saunders@stanford.edu
% Systems Optimization Laboratory
% Dept of MS&E, Stanford University.
%-----------------------------------------------------------------------

  % Initialize.

  if isa(A,'numeric')
    explicitA = true;
  elseif isa(A,'function_handle')
    explicitA = false;
  else
    error('SOL:lsmr:Atype','%s','A must be numeric or a function handle');
  end

  msg = ['The exact solution is  x = 0                              '
         'Ax - b is small enough, given atol, btol                  '
         'The least-squares solution is good enough, given atol     '
         'The estimate of cond(Abar) has exceeded conlim            '
         'Ax - b is small enough for this machine                   '
         'The least-squares solution is good enough for this machine'
         'Cond(Abar) seems to be too large for this machine         '
         'The iteration limit has been reached                      '];

  hdg1 = '   itn      x(1)       norm r    norm A''r';
  hdg2 = ' compatible   LS      norm A   cond A';
  pfreq  = 20;   % print frequency (for repeating the heading)
  pcount = 0;    % print counter

  % Determine dimensions m and n, and
  % form the first vectors u and v.
  % These satisfy  beta*u = b,  alpha*v = A'u.

  u    = b;
  beta = norm(u);
  if beta > 0
    u  = u/beta;
  end

  if explicitA
    v = A'*u;
    [m n] = size(A);
  else
    v = A(u,2);
    m = size(b,1);
    n = size(v,1);
  end

  minDim = min([m n]);

  % Set default parameters.

  if nargin < 3 || isempty(lambda)   , lambda    = 0;          end
  if nargin < 4 || isempty(atol)     , atol      = 1e-6;       end
  if nargin < 5 || isempty(btol)     , btol      = 1e-6;       end
  if nargin < 6 || isempty(conlim)   , conlim    = 1e+8;       end
  if nargin < 7 || isempty(itnlim)   , itnlim    = minDim;     end
  if nargin < 8 || isempty(localSize), localSize = 0;          end
  if nargin < 9 || isempty(show)     , show      = false;      end

  if show
    fprintf('\n\nLSMR            Least-squares solution of  Ax = b')
    fprintf('\nVersion 1.11                          09 Jun 2010')
    fprintf('\nThe matrix A has %8g rows  and %8g cols', m,n)
    fprintf('\nlambda = %16.10e', lambda )
    fprintf('\natol   = %8.2e               conlim = %8.2e', atol,conlim)
    fprintf('\nbtol   = %8.2e               itnlim = %8g'  , btol,itnlim)
  end

  alpha = norm(v);
  if alpha > 0
    v = (1/alpha)*v;
  end

  % Initialization for local reorthogonalization.

  localOrtho = false;
  if localSize > 0
    localPointer    = 0;
    localOrtho      = true;
    localVQueueFull = false;

    % Preallocate storage for the relevant number of latest v_k's.

    localV = zeros(n, min([localSize minDim]));
  end

  % Initialize variables for 1st iteration.

  itn      = 0;
  zetabar  = alpha*beta;
  alphabar = alpha;
  rho      = 1;
  rhobar   = 1;
  cbar     = 1;
  sbar     = 0;

  h    = v;
  hbar = zeros(n,1);
  x    = zeros(n,1);

  % Initialize variables for estimation of ||r||.

  betadd      = beta;
  betad       = 0;
  rhodold     = 1;
  tautildeold = 0;
  thetatilde  = 0;
  zeta        = 0;
  d           = 0;

  % Initialize variables for estimation of ||A|| and cond(A).

  normA2  = alpha^2;
  maxrbar = 0;
  minrbar = 1e+100;

  % Items for use in stopping rules.
  normb  = beta;
  istop  = 0;
  ctol   = 0;         if conlim > 0, ctol = 1/conlim; end;
  normr  = beta;

  % Exit if b=0 or A'b = 0.

  normAr = alpha * beta;
  if normAr == 0, disp(msg(1,:)); return, end

  % Heading for iteration log.

  if show
    test1 = 1;
    test2 = alpha/beta;
    fprintf('\n\n%s%s'      , hdg1 , hdg2   )
    fprintf('\n%6g %12.5e'  , itn  , x(1)   )
    fprintf(' %10.3e %10.3e', normr, normAr )
    fprintf('  %8.1e %8.1e' , test1, test2  )
  end

  %------------------------------------------------------------------
  %     Main iteration loop.
  %------------------------------------------------------------------
  while itn < itnlim
    itn = itn + 1;

    % Perform the next step of the bidiagonalization to obtain the
    % next beta, u, alpha, v.  These satisfy the relations
    %      beta*u  =  A*v  - alpha*u,
    %      alpha*v  =  A'*u - beta*v.

    if explicitA
      u = A*v    - alpha*u;
    else
      u = A(v,1) - alpha*u;
    end
    beta = norm(u);

    if beta > 0
      u = (1/beta)*u;
      if localOrtho
        localVEnqueue(v);    % Store old v for local reorthogonalization of new v.
      end
      if explicitA
        v = A'*u   - beta*v;
      else
        v = A(u,2) - beta*v;
      end

      if localOrtho
        v = localVOrtho(v);  % Local-reorthogonalization of new v.
      end
      alpha  = norm(v);
      if alpha > 0,  v = (1/alpha)*v; end
    end

    % At this point, beta = beta_{k+1}, alpha = alpha_{k+1}.

    % Construct rotation Qhat_{k,2k+1}.

    alphahat = norm([alphabar lambda]);
    chat     = alphabar/alphahat;
    shat     = lambda/alphahat;

    % Use a plane rotation (Q_i) to turn B_i to R_i.

    rhoold   = rho;
    rho      = norm([alphahat beta]);
    c        = alphahat/rho;
    s        = beta/rho;
    thetanew = s*alpha;
    alphabar = c*alpha;

    % Use a plane rotation (Qbar_i) to turn R_i^T to R_i^bar.

    rhobarold = rhobar;
    zetaold   = zeta;
    thetabar  = sbar*rho;
    rhotemp   = cbar*rho;
    rhobar    = norm([cbar*rho thetanew]);
    cbar      = cbar*rho/rhobar;
    sbar      = thetanew/rhobar;
    zeta      =   cbar*zetabar;
    zetabar   = - sbar*zetabar;

    % Update h, h_hat, x.

    hbar      = h - (thetabar*rho/(rhoold*rhobarold))*hbar;
    x         = x + (zeta/(rho*rhobar))*hbar;
    h         = v - (thetanew/rho)*h;

    % Estimate of ||r||.

    % Apply rotation Qhat_{k,2k+1}.
    betaacute =   chat* betadd;
    betacheck = - shat* betadd;

    % Apply rotation Q_{k,k+1}.
    betahat   =   c*betaacute;
    betadd    = - s*betaacute;

    % Apply rotation Qtilde_{k-1}.
    % betad = betad_{k-1} here.

    thetatildeold = thetatilde;
    rhotildeold   = norm([rhodold thetabar]);
    ctildeold     = rhodold/rhotildeold;
    stildeold     = thetabar/rhotildeold;
    thetatilde    = stildeold* rhobar;
    rhodold       =   ctildeold* rhobar;
    betad         = - stildeold*betad + ctildeold*betahat;

    % betad   = betad_k here.
    % rhodold = rhod_k  here.

    tautildeold   = (zetaold - thetatildeold*tautildeold)/rhotildeold;
    taud          = (zeta - thetatilde*tautildeold)/rhodold;
    d             = d + betacheck^2;
    normr         = sqrt(d + (betad - taud)^2 + betadd^2);

    % Estimate ||A||.
    normA2        = normA2 + beta^2;
    normA         = sqrt(normA2);
    normA2        = normA2 + alpha^2;

    % Estimate cond(A).
    maxrbar       = max(maxrbar,rhobarold);
    if itn>1
      minrbar     = min(minrbar,rhobarold);
    end
    condA         = max(maxrbar,rhotemp)/min(minrbar,rhotemp);

    % Test for convergence.

    % Compute norms for convergence testing.
    normAr  = abs(zetabar);
    normx   = norm(x);

    % Now use these norms to estimate certain other quantities,
    % some of which will be small near a solution.

    test1   = normr /normb;
    test2   = normAr/(normA*normr);
    test3   =      1/condA;
    t1      =  test1/(1 + normA*normx/normb);
    rtol    = btol + atol*normA*normx/normb;

    % The following tests guard against extremely small values of
    % atol, btol or ctol.  (The user may have set any or all of
    % the parameters atol, btol, conlim  to 0.)
    % The effect is equivalent to the normAl tests using
    % atol = eps,  btol = eps,  conlim = 1/eps.

    if itn >= itnlim,   istop = 7; end
    if 1 + test3  <= 1, istop = 6; end
    if 1 + test2  <= 1, istop = 5; end
    if 1 + t1     <= 1, istop = 4; end

    % Allow for tolerances set by the user.

    if  test3 <= ctol,  istop = 3; end
    if  test2 <= atol,  istop = 2; end
    if  test1 <= rtol,  istop = 1; end

    % See if it is time to print something.

    if show
      prnt = 0;
      if n     <= 40       , prnt = 1; end
      if itn   <= 10       , prnt = 1; end
      if itn   >= itnlim-10, prnt = 1; end
      if rem(itn,10) == 0  , prnt = 1; end
      if test3 <= 1.1*ctol , prnt = 1; end
      if test2 <= 1.1*atol , prnt = 1; end
      if test1 <= 1.1*rtol , prnt = 1; end
      if istop ~=  0       , prnt = 1; end

      if prnt
	if pcount >= pfreq
	  pcount = 0;
          fprintf('\n\n%s%s'    , hdg1 , hdg2  )
	end
	pcount = pcount + 1;
        fprintf('\n%6g %12.5e'  , itn  , x(1)  )
        fprintf(' %10.3e %10.3e', normr, normAr)
        fprintf('  %8.1e %8.1e' , test1, test2 )
        fprintf(' %8.1e %8.1e'  , normA, condA )
      end
    end

    if istop > 0, break, end
  end % iteration loop

  % Print the stopping condition.

  if show
    fprintf('\n\nLSMR finished')
    fprintf('\n%s', msg(istop+1,:))
    fprintf('\nistop =%8g    normr =%8.1e'     , istop, normr )
    fprintf('    normA =%8.1e    normAr =%8.1e', normA, normAr)
    fprintf('\nitn   =%8g    condA =%8.1e'     , itn  , condA )
    fprintf('    normx =%8.1e\n', normx)
  end

% end function lsmr

%---------------------------------------------------------------------
% Nested functions.
%---------------------------------------------------------------------

  function localVEnqueue(v)

  % Store v into the circular buffer localV.

    if localPointer < localSize
      localPointer = localPointer + 1;
    else
      localPointer = 1;
      localVQueueFull = true;
    end
    localV(:,localPointer) = v;

  end % nested function localVEnqueue

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

  function vOutput = localVOrtho(v)

  % Perform local reorthogonalization of V.

    vOutput = v;
    if localVQueueFull
      localOrthoLimit = localSize;
    else
      localOrthoLimit = localPointer;
    end
    for localOrthoCount = 1:localOrthoLimit
      vtemp   = localV(:, localOrthoCount);
      vOutput = vOutput - (vOutput'*vtemp)*vtemp;
    end

  end % nested function localVOrtho

end % function lsmr