function h=plotgauss2d(mu, Sigma)
% PLOTGAUSS2D Plot a 2D Gaussian as an ellipse with optional cross hairs
% h=plotgauss2(mu, Sigma)
%

h = plotcov2(mu, Sigma);
return;

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

% PLOTCOV2 - Plots a covariance ellipse with major and minor axes
%            for a bivariate Gaussian distribution.
%
% Usage:
%   h = plotcov2(mu, Sigma[, OPTIONS]);
%
% Inputs:
%   mu    - a 2 x 1 vector giving the mean of the distribution.
%   Sigma - a 2 x 2 symmetric positive semi-definite matrix giving
%           the covariance of the distribution (or the zero matrix).
%
% Options:
%   'conf'    - a scalar between 0 and 1 giving the confidence
%               interval (i.e., the fraction of probability mass to
%               be enclosed by the ellipse); default is 0.9.
%   'num-pts' - the number of points to be used to plot the
%               ellipse; default is 100.
%
% This function also accepts options for PLOT.
%
% Outputs:
%   h     - a vector of figure handles to the ellipse boundary and
%           its major and minor axes
%
% See also: PLOTCOV3

% Copyright (C) 2002 Mark A. Paskin

function h = plotcov2(mu, Sigma, varargin)

if size(Sigma) ~= [2 2], error('Sigma must be a 2 by 2 matrix'); end
if length(mu) ~= 2, error('mu must be a 2 by 1 vector'); end

[p, ...
 n, ...
 plot_opts] = process_options(varargin, 'conf', 0.9, ...
					'num-pts', 100);
h = [];
holding = ishold;
if (Sigma == zeros(2, 2))
  z = mu;
else
  % Compute the Mahalanobis radius of the ellipsoid that encloses
  % the desired probability mass.
  k = conf2mahal(p, 2);
  % The major and minor axes of the covariance ellipse are given by
  % the eigenvectors of the covariance matrix.  Their lengths (for
  % the ellipse with unit Mahalanobis radius) are given by the
  % square roots of the corresponding eigenvalues.
  if (issparse(Sigma))
    [V, D] = eigs(Sigma);
  else
    [V, D] = eig(Sigma);
  end
  % Compute the points on the surface of the ellipse.
  t = linspace(0, 2*pi, n);
  u = [cos(t); sin(t)];
  w = (k * V * sqrt(D)) * u;
  z = repmat(mu, [1 n]) + w;
  % Plot the major and minor axes.
  L = k * sqrt(diag(D));
  h = plot([mu(1); mu(1) + L(1) * V(1, 1)], ...
	   [mu(2); mu(2) + L(1) * V(2, 1)], plot_opts{:});
  hold on;
  h = [h; plot([mu(1); mu(1) + L(2) * V(1, 2)], ...
	       [mu(2); mu(2) + L(2) * V(2, 2)], plot_opts{:})];
end

h = [h; plot(z(1, :), z(2, :), plot_opts{:})];
if (~holding) hold off; end

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

% CONF2MAHAL - Translates a confidence interval to a Mahalanobis
%              distance.  Consider a multivariate Gaussian
%              distribution of the form
%
%   p(x) = 1/sqrt((2 * pi)^d * det(C)) * exp((-1/2) * MD(x, m, inv(C)))
%
%              where MD(x, m, P) is the Mahalanobis distance from x
%              to m under P:
%
%                 MD(x, m, P) = (x - m) * P * (x - m)'
%
%              A particular Mahalanobis distance k identifies an
%              ellipsoid centered at the mean of the distribution.
%              The confidence interval associated with this ellipsoid
%              is the probability mass enclosed by it.  Similarly,
%              a particular confidence interval uniquely determines
%              an ellipsoid with a fixed Mahalanobis distance.
%
%              If X is an d dimensional Gaussian-distributed vector,
%              then the Mahalanobis distance of X is distributed
%              according to the Chi-squared distribution with d
%              degrees of freedom.  Thus, the Mahalanobis distance is
%              determined by evaluating the inverse cumulative
%              distribution function of the chi squared distribution
%              up to the confidence value.
%
% Usage:
%
%   m = conf2mahal(c, d);
%
% Inputs:
%
%   c    - the confidence interval
%   d    - the number of dimensions of the Gaussian distribution
%
% Outputs:
%
%   m    - the Mahalanobis radius of the ellipsoid enclosing the
%          fraction c of the distribution's probability mass
%
% See also: MAHAL2CONF

% Copyright (C) 2002 Mark A. Paskin

function m = conf2mahal(c, d)

m = chi2inv(c, d); % matlab stats toolbox


function [ p, seed ] = triangle_reference_sample ( n, seed )

%*****************************************************************************80
%
%% TRIANGLE_REFERENCE_SAMPLE returns random points in the reference triangle.
%
%  Diagram:
%
%       3
%    s  |\
%    i  | \
%    d  |  \
%    e  |   \  side 2
%       |    \
%    3  |     \
%       |      \
%       1-------2
%
%         side 1
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    07 December 2006
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of points to generate.
%
%    Input, integer SEED, a seed for the random number generator.
%
%    Output, real P(2,N), random points in the triangle.
%
%    Output, integer SEED, a seed for the random number generator.
%
  dim_num = 2;

  for j = 1 : n

    [ r, seed ] = r8_uniform_01 ( seed );
%
%  Interpret R as a percentage of the triangle's area.
%
%  Imagine a line L, parallel to side 1, so that the area between
%  vertex 1 and line L is R percent of the full triangle's area.
%
%  The line L will intersect sides 2 and 3 at a fraction
%  ALPHA = SQRT ( R ) of the distance from vertex 1 to vertices 2 and 3.
%
    alpha = sqrt ( r );
%
%  Now choose, uniformly at random, a point on the line L.
%
    [ beta, seed ] = r8_uniform_01 ( seed );

    p(1,j) = ( 1.0 - beta ) * alpha;
    p(2,j) =         beta   * alpha;

  end

  return
end


function z = r8vec_convolution ( m, x, n, y )

%*****************************************************************************80
%
%% R8VEC_CONVOLUTION returns the convolution of two R8VEC's.
%
%  Discussion:
%
%    An R8VEC is a vector of R8's.
%
%    The I-th entry of the convolution can be formed by summing the products
%    that lie along the I-th diagonal of the following table:
%
%    Y3 : 3   4   5   6   7
%    Y2 : 2   3   4   5   6
%    Y1 : 1   2   3   4   5
%       +------------------
%        X1  X2  X3  X4  X5
%
%    which will result in:
%
%    Z = ( X1 * Y1,
%          X1 * Y2 + X2 * Y1,
%          X1 * Y3 + X2 * Y2 + X3 * Y1,
%                    X2 * Y3 + X3 * Y2 + X4 * Y1,
%                              X3 * Y3 + X4 * Y2 + X5 * Y1,
%                                        X4 * Y3 + X5 * Y2,
%                                                  X5 * Y3 )
%
%  Example:
%
%    Input:
%
%      X = (/ 1, 2, 3, 4 /)
%      Y = (/ -1, 5, 3 /)
%
%    Output:
%
%      Z = (/ -1, 3, 10, 17, 29, 12 /)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    05 May 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the dimension of X.
%
%    Input, real X(M), the first vector to be convolved.
%
%    Input, integer N, the dimension of Y.
%
%    Input, real Y(N), the second vector to be convolved.
%
%    Output, real Z(M+N-1), the convolution of X and Y.
%
  z(1:m+n-1,1) = 0.0;

  for j = 1 : n
    z(j:j+m-1,1) = z(j:j+m-1,1) + x(1:m,1) * y(j,1);
  end

  return
end


function y = fcube ( x )

%*****************************************************************************80
%
%% FCUBE sets the cubic data values.
%
%  Discussion:
%
%    Y(X) = ( ( X + 2 ) * X + 3 ) * X + 4
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 February 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the argument.
%
%    Output, real Y, the value of the function.
%
  y = ( ( (       1.0E+00 ) ...
                * x + 2.0E+00 ) ...
                * x + 3.0E+00 ) ...
                * x + 4.0E+00;

  return
end


function [ x, w ] = gen_laguerre_ss_compute ( n, alpha )

%*****************************************************************************80
%
%% GEN_LAGUERRE_SS_COMPUTE computes a generalized Gauss-Laguerre quadrature rule.
%
%  Discussion:
%
%    The integral:
%
%      Integral ( 0 <= X < +oo ) EXP ( - X ) * X^ALPHA * F(X) dX
%
%    The quadrature rule:
%
%      Sum ( 1 <= I <= N ) W(I) * F ( X(I) )
%
%    The integral:
%
%      Integral ( 0 <= X < +oo ) X^ALPHA * F(X) dX
%
%    The quadrature:
%
%      Sum ( 1 <= I <= N ) W(I) * EXP ( X(I) ) * F ( X(I) )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    02 June 2010
%
%  Author:
%
%    Original FORTRAN77 version by Arthur Stroud, Don Secrest.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Arthur Stroud, Don Secrest,
%    Gaussian Quadrature Formulas,
%    Prentice Hall, 1966,
%    LC: QA299.4G3S7.
%
%  Parameters:
%
%    Input, integer N, the order.
%    N must be at least 1.
%
%    Input, real ALPHA, the exponent of the X factor.
%    Set ALPHA = 0.0 for the simplest rule.
%    ALPHA must be nonnegative.
%
%    Output, real X(N), the abscissas.
%
%    Output, real W(N), the weights.
%
  x = zeros ( n, 1 );
  w = zeros ( n, 1 );
%
%  Set the recursion coefficients.
%
  for i = 1 : n
    b(i) = ( alpha + 2 * i - 1 );
  end

  for i = 1 : n
    c(i) = ( i - 1 ) * ( alpha + i - 1 );
  end

  cc = gamma ( alpha + 1.0 ) * prod ( c(2:n) );

  for i = 1 : n
%
%  Compute an estimate for the root.
%
    if ( i == 1 )

      xval = ( 1.0 + alpha ) * ( 3.0 + 0.92 * alpha ) ...
        / ( 1.0 + 2.4 * n + 1.8 * alpha );

    elseif ( i == 2 )

      xval = xval + ( 15.0 + 6.25 * alpha ) / ( 1.0 + 0.9 * alpha + 2.5 * n );

    else

      r1 = ( 1.0 + 2.55 * ( i - 2 ) ) / ( 1.9 * ( i - 2 ) );

      r2 = 1.26 * ( i - 2 ) * alpha / ( 1.0 + 3.5 * ( i - 2 ) );

      ratio = ( r1 + r2 ) / ( 1.0 + 0.3 * alpha );

      xval = xval + ratio * ( xval - x(i-2) );

    end
%
%  Use iteration to find the root.
%
    [ xval, dp2, p1 ] = gen_laguerre_ss_root ( xval, n, alpha, b, c );
%
%  Set the abscissa and weight.
%
    x(i) = xval;
    w(i) = ( cc / dp2 ) / p1;

  end

  return
end


clear all;
close all;
more off;
addpath('tools');

epsilon = 1e-5;

x1 = [1.1 0.9 1]';
x2 = [2.2 1.9]';
z  = [1.3 -0.4]';

% get the analytic Jacobian
[e, A, B] = linearize_pose_landmark_constraint(x1, x2, z);

% check the error vector
e_true = [0.135804 0.014684]';
if (norm(e - e_true) > epsilon)
  disp('Your error function seems to return a wrong value');
  disp('Result of your function'); disp(e);
  disp('True value'); disp(e_true);
else
  disp('The computation of the error vector appears to be correct');
end

% compute it numerically
delta = 1e-6;
scalar = 1 / (2*delta);

% test for x1
ANumeric = zeros(2,3);
for d = 1:3
  curX = x1;
  curX(d) += delta;
  err = linearize_pose_landmark_constraint(curX, x2, z);
  curX = x1;
  curX(d) -= delta;
  err -= linearize_pose_landmark_constraint(curX, x2, z);

  ANumeric(:, d) = scalar * err;
end

diff = ANumeric - A;
if max(max(abs(diff))) > epsilon
  disp('Error in the Jacobian for x1');
  disp('Your analytic Jacobian'); disp(A);
  disp('Numerically computed Jacobian'); disp(ANumeric);
  disp('Difference'); disp(diff);
else
  disp('Jacobian for x1 appears to be correct');
end

% test for x2
BNumeric = zeros(2,2);
for d = 1:2
  curX = x2;
  curX(d) += delta;
  err = linearize_pose_landmark_constraint(x1, curX, z);
  curX = x2;
  curX(d) -= delta;
  err -= linearize_pose_landmark_constraint(x1, curX, z);

  BNumeric(:, d) = scalar * err;
end

diff = BNumeric - B;
if max(max(abs(diff))) > epsilon
  disp('Error in the Jacobian for x2');
  disp('Your analytic Jacobian'); disp(B);
  disp('Numerically computed Jacobian'); disp(BNumeric);
  disp('Difference'); disp(diff);
else
  disp('Jacobian for x2 appears to be correct');
end


function y = maxstar(x, w)
% maxstar   Log of a sum of exponentials.
%   For vectors, maxstar(x) is equivalent to log(sum(exp(x))).
%   For matrices, maxstar(x) is a row vector and maxstar operates on
%   each column of x. For N-D arrays, maxstar(x) operates along the
%   first non-singleton dimension.
%
%   maxstar(x,w) is the log of a weighted sum of exponentials,
%   equivalent to log(sum(w.*exp(x))). Vectors w and x must be
%   the same length. For matrix x, the weights w can be input as
%   a matrix the same size as x, or as a vector of the same length
%   as columns of x. Weights may be zero or negative, but the result
%   sum(w.*exp(x)) must be greater than zero.
%
%   Acts on first dimensions

if nargin<2
    w = [];
else
    if ~isvector(w)
        error('maxstar: w must be a vector')
    end
    if length(w) ~= size(x,1)
        error('maxstar: weight does not match x')
    end
end
%%
w = w(:);
szx = size(x);
if isempty(w)
    % no weight
    m = max(x);
    y = m + log(sum(exp(bsxfun(@minux,x,m))));
else
    % Move the weight into the exponent xw and find
    % m = max(xw) over terms with positive weights
    wpos = w>0;
    xw = bsxfun(@plus, x(wpos,:), log(w(wpos)));
    m = max(xw);
    exwp = exp( bsxfun(@minus, xw, m) );
    wneg = w<0;
    exwn = exp( x(wneg,:) + bsxfun(@minus,log(-w(wneg)), m) );
    y = m + log(sum(exwp,1) - sum(exwn,1));
end
y = reshape(y,[szx(2:end) 1]);


%% Data import
%Copyright (c) 2011, The MathWorks, Inc.

% Import tumor-weight profile for drugs A & B
ds = dataset('xlsfile', 'Data.xls') ;

% Convert drug type column to nominal
ds.Drug = nominal(ds.Drug)               ;

%% Curve fitting

% Initial estimates for [L0, L1, k1, k2_A and k2_B]
p0 = [0.11 , .149 , 1.836, 0.0154, 0.00732] ;

% Estimate parameters
fh   = @(p , t ) objFcn(p , t , ds.Drug) ;% Function handle
tic
pFit = lsqcurvefit(fh , p0 , ds.Time , ds.TumorWeight ) ;
toc
%% Display results & plot

disp([' Drug-independent parameters: '          , ...
        num2str(pFit(1)), ' (L0), '             , ...
        num2str(pFit(2)), ' (L1) and '          , ...
        num2str(pFit(3)), ' (k1) '              ]) ;

disp([' Drug-dependent parameters: ' , ...
        num2str(pFit(4)), ' (k2 - Drug A) and '              , ...
        num2str(pFit(5)), ' (k2 - Drug B)'   ]) ;

% Plot observations
figure; hold on  ;
idx_A = ds.Drug == 'A' ;
plot(ds.Time(idx_A) , ds.TumorWeight(idx_A) , 'ro')  ;
plot(ds.Time(~idx_A), ds.TumorWeight(~idx_A), 'bo')  ;

% Plot predictions
yPred = fh(pFit, ds.Time) ;
plot(ds.Time(idx_A)  , yPred(idx_A) , 'r:')  ;
plot(ds.Time(~idx_A) , yPred(~idx_A), 'b:')  ;

xlabel('Time (days)')
ylabel('Tumor Weight (milligram)')
title('Tumor growth profile')
legend({'Drug A', 'Drug B'})


function x = half_normal_cdf_inv ( cdf, a, b )

%*****************************************************************************80
%
%% HALF_NORMAL_CDF_INV inverts the Half Normal CDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real CDF, the value of the CDF.
%    0.0 <= CDF <= 1.0.
%
%    Input, real A, B, the parameters of the PDF.
%    0.0 < B.
%
%    Output, real X, the corresponding argument.
%
  if ( cdf < 0.0 | 1.0 < cdf )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'HALF_NORMAL_CDF_INV - Fatal error!\n' );
    fprintf ( 1, '  CDF < 0 or 1 < CDF.\n' );
    error ( 'HALF_NORMAL_CDF_INV - Fatal error!' );
  end

  cdf2 = 0.5 * ( cdf + 1.0 );

  x = normal_cdf_inv ( cdf2, a, b );

  return
end


function [ xdp, ddp ] = dif_basis_derivk ( nd, xd, k )

%*****************************************************************************80
%
%% DIF_BASIS_DERIVK: Lagrange basis K-th derivative difference tables.
%
%  Discussion:
%
%    Given ND points XD, a Lagrange basis polynomial L(J)(X) is associated
%    with each point XD(J).
%
%    This function computes a table DDP(*,*) whose J-th column contains
%    the difference table for the K-th derivative of L(J)(X).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 June 2013
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Carl deBoor,
%    A Practical Guide to Splines,
%    Springer, 2001,
%    ISBN: 0387953663,
%    LC: QA1.A647.v27.
%
%  Parameters:
%
%    Input, integer ND, the number of data points.
%
%    Input, real XD(ND), the X values upon which the
%    Lagrange basis polynomials are to be based.
%
%    Input, integer K, the index of the derivative.
%
%    Output, real XDP(ND-K), the X values upon with
%    the derivative difference table is based.  In fact, these are
%    all 0.
%
%    Output, real DDP(ND-K,ND), the divided difference
%    tables for all the Lagrange basis polynomials.  Column J of DDP
%    contains the table for basis polynomial associated with XD(J).
%
  ddp = zeros ( nd - k, nd );
%
%  Process the vectors one column at a time.
%
  for j = 1 : nd
%
%  Set the data.
%
    yd(1:nd,1) = 0.0;
    yd(j,1) = 1.0;
%
%  Compute the divided difference table.
%
    dd = data_to_dif ( nd, xd, yd );
%
%  Compute the divided difference table for the derivative.
%
    [ xdp, ddp(1:nd-k,j) ] = dif_derivk_table ( nd, xd, dd, k );

  end

  return
end


function [ iters, endpt ] = hooke ( nvars, startpt, rho, eps, itermax, f )

%*****************************************************************************80
%
%% HOOKE seeks a minimizer of a scalar function of several variables.
%
%  Discussion:
%
%    This routine find a point X where the nonlinear objective function
%    F(X) has a local minimum.  X is an N-vector and F(X) is a scalar.
%    The objective function F(X) is not required to be differentiable
%    or even continuous.  The program does not use or require derivatives
%    of the objective function.
%
%    The user supplies three things:
%    1) a subroutine that computes F(X),
%    2) an initial "starting guess" of the minimum point X,
%    3) values for the algorithm convergence parameters.
%
%    The program searches for a local minimum, beginning from the
%    starting guess, using the Direct Search algorithm of Hooke and
%    Jeeves.
%
%    This program is adapted from the Algol pseudocode found in the
%    paper by Kaupe, and includes improvements suggested by Bell and Pike,
%    and by Tomlin and Smith.
%
%    The algorithm works by taking "steps" from one estimate of
%    a minimum, to another (hopefully better) estimate.  Taking
%    big steps gets to the minimum more quickly, at the risk of
%    "stepping right over" an excellent point.  The stepsize is
%    controlled by a user supplied parameter called RHO.  At each
%    iteration, the stepsize is multiplied by RHO  (0 < RHO < 1),
%    so the stepsize is successively reduced.
%
%    Small values of rho correspond to big stepsize changes,
%    which make the algorithm run more quickly.  However, there
%    is a chance (especially with highly nonlinear functions)
%    that these big changes will accidentally overlook a
%    promising search vector, leading to nonconvergence.
%
%    Large values of RHO correspond to small stepsize changes,
%    which force the algorithm to carefully examine nearby points
%    instead of optimistically forging ahead.  This improves the
%    probability of convergence.
%
%    The stepsize is reduced until it is equal to (or smaller
%    than) EPS.  So the number of iterations performed by
%    Hooke-Jeeves is determined by RHO and EPS:
%
%      RHO^(number_of_iterations) = EPS
%
%    In general it is a good idea to set RHO to an aggressively
%    small value like 0.5 (hoping for fast convergence).  Then,
%    if the user suspects that the reported minimum is incorrect
%    (or perhaps not accurate enough), the program can be run
%    again with a larger value of RHO such as 0.85, using the
%    result of the first minimization as the starting guess to
%    begin the second minimization.
%
%    Normal use:
%    (1) Code your function F() in the C language;
%    (2) Install your starting guess;
%    (3) Run the program.
%
%    If there are doubts about the result, the computed minimizer
%    can be used as the starting point for a second minimization attempt.
%
%    To apply this method to data fitting, code your function F() to be
%    the sum of the squares of the errors (differences) between the
%    computed values and the measured values.  Then minimize F()
%    using Hooke-Jeeves.
%
%    For example, you have 20 datapoints (T(i), Y(i)) and you want to
%    find A, B and C so that:
%
%      A*t*t + B*exp(t) + C*tan(t)
%
%    fits the data as closely as possible.  Then the objective function
%    F() to be minimized is just
%
%      F(A,B,C) = sum ( 1 <= i <= 20 )
%        ( y(i) - A*t(i)*t(i) - B*exp(t(i)) - C*tan(t(i)) )^2.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 February 2008
%
%  Author:
%
%    ALGOL original by Arthur Kaupe.
%    C version by Mark Johnson.
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    M Bell, Malcolm Pike,
%    Remark on Algorithm 178: Direct Search,
%    Communications of the ACM,
%    Volume 9, Number 9, September 1966, page 684.
%
%    Robert Hooke, Terry Jeeves,
%    Direct Search Solution of Numerical and Statistical Problems,
%    Journal of the ACM,
%    Volume 8, Number 2, April 1961, pages 212-229.
%
%    Arthur Kaupe,
%    Algorithm 178:
%    Direct Search,
%    Communications of the ACM,
%    Volume 6, Number 6, June 1963, page 313.
%
%    FK Tomlin, LB Smith,
%    Remark on Algorithm 178: Direct Search,
%    Communications of the ACM,
%    Volume 12, Number 11, November 1969, page 637-638.
%
%  Parameters:
%
%    Input, integer NVARS, the number of spatial dimensions.
%
%    Input, real STARTPT(NVARS), the user-supplied
%    initial estimate for the minimizer.
%
%    Input, real RHO, a user-supplied convergence parameter
%    which should be set to a value between 0.0 and 1.0.  Larger values
%    of RHO give greater probability of convergence on highly nonlinear
%    functions, at a cost of more function evaluations.  Smaller
%    values of RHO reduce the number of evaluations and the program
%    running time, but increases the risk of nonconvergence.
%
%    Input, real EPS, the criterion for halting
%    the search for a minimum.  When the algorithm
%    begins to make less and less progress on each
%    iteration, it checks the halting criterion: if
%    the stepsize is below EPS, terminate the
%    iteration and return the current best estimate
%    of the minimum.  Larger values of EPS (such
%    as 1.0e-4) give quicker running time, but a
%    less accurate estimate of the minimum.  Smaller
%    values of EPS (such as 1.0e-7) give longer
%    running time, but a more accurate estimate of
%    the minimum.
%
%    Input, integer ITERMAX, a limit on the number of iterations.
%
%    Input, function handle F, the name of the function routine,
%    which should have the form:
%      function value = f ( x, n )
%
%    Output, integer ITERS, the number of iterations taken.
%
%    Output, real ENDPT(NVARS), the estimate for the
%    minimizer, as calculated by the program.
%
  verbose = 0;

  for i = 1 : nvars
    newx(i) = startpt(i);
  end

  for i = 1 : nvars
    xbefore(i) = startpt(i);
  end

  for i = 1 : nvars
    if ( startpt(i) == 0.0 )
      delta(i) = rho;
    else
      delta(i) = rho * abs ( startpt(i) );
    end
  end

  funevals = 0;
  steplength = rho;
  iters = 0;
  fbefore = f ( newx, nvars );
  funevals = funevals + 1;
  newf = fbefore;

  while ( iters < itermax & eps < steplength )

    iters = iters + 1;

    if ( verbose )

      fprintf ( 1, '\n' );
      fprintf ( 1, '  FUNEVALS = %d, F(X) = %e\n', funevals, fbefore );
      for i = 1 : nvars
        fprintf ( 1, '  %8d  %e\n', i, xbefore(i) );
      end

    end
%
%  Find best new point, one coordinate at a time.
%
    for i = 1 : nvars
      newx(i) = xbefore(i);
    end

    [ newf, newx, funevals ] = best_nearby ( delta, newx, fbefore, nvars, ...
    f, funevals );
%
%  If we made some improvements, pursue that direction.
%
    keep = 1;

    while ( newf < fbefore & keep == 1 )

      for i = 1 : nvars
%
%  Arrange the sign of DELTA.
%
        if ( newx(i) <= xbefore(i) )
          delta(i) = - abs ( delta(i) );
        else
          delta(i) = abs ( delta(i) );
        end
%
%  Now, move further in this direction.
%
        tmp = xbefore(i);
        xbefore(i) = newx(i);
        newx(i) = newx(i) + newx(i) - tmp;
      end

      fbefore = newf;
      [ newf, newx, funevals ] = best_nearby ( delta, newx, fbefore, nvars, ...
        f, funevals );
%
%  If the further (optimistic) move was bad...
%
      if ( fbefore <= newf )
        break;
      end
%
%  Make sure that the differences between the new and the old points
%  are due to actual displacements; beware of roundoff errors that
%  might cause NEWF < FBEFORE.
%
      keep = 0;

      for i = 1 : nvars
        if ( 0.5 * abs ( delta(i) ) < abs ( newx(i) - xbefore(i) ) )
          keep = 1;
          break
        end
      end

    end

    if ( eps <= steplength & fbefore <= newf )
      steplength = steplength * rho;
      for i = 1 : nvars
        delta(i) = delta(i) * rho;
      end
    end

  end

  for i = 1 : nvars
    endpt(i) = xbefore(i);
  end

  return
end


function varargout = modschaffer4(X)
% modified Schaffer function, #4
%
%   MODSCHAFFER4([x1, x2]) returns the value of the 4th Schaffer
%   function at the specified points. [x1] and [x2] may be vectors.
%   The search domain is
%
%               -100 < x_i < 100
%
%   The global minimum is
%
%               f(x1, x2) = f(0, 1.25313) = 0.292579.

% Author: Rody P.S. Oldenhuis
% Delft University of Technology
% E-mail: oldenhuis@dds.nl
% Last edited 20/Jul/2009

    % if no input is given, return dimensions, bounds and minimum
    if (nargin == 0)
        varargout{1} = 2;  % # dims
        varargout{2} = [-100, -100]; % LB
        varargout{3} = [+100, +100]; % UB
        varargout{4} = [0, 1.253131828927371e+000]; % solution
        varargout{5} = 2.925786320359805e-001; % function value at solution

    % otherwise, output function value
    else

        % keep values within the search interval
        X(X < -100) = inf;      X(X > 100) = inf;

        % split input vector X into x1, x2
        if size(X, 1) == 2
            x1 = X(1, :);        x2 = X(2, :);
        else
            x1 = X(:, 1);        x2 = X(:, 2);
        end

        % output function value
        varargout{1} = 0.5  + (cos(sin(abs(x1.^2 - x2.^2))).^2 - 0.5) ./ (1+0.001*(x1.^2 + x2.^2)).^2;

    end

end


function value = j_double_product_integral ( i, j, a, b )

%*****************************************************************************80
%
%% J_DOUBLE_PRODUCT_INTEGRAL: integral of J(i,x)*J(j,x)*(1-x)^a*(1+x)^b.
%
%  Discussion:
%
%    VALUE = integral ( -1 <= x <= +1 ) J(i,x)*J(j,x)*(1-x)^a*(1+x)^b dx
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 March 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer I, J, the polynomial indices.
%
%    Input, real A, B, the parameters.
%    -1 < A, B.
%
%    Output, real VALUE, the value of the integral.
%
  if ( i ~= j )
    value = 0.0;
  else
    value = 2^( a + b + 1.0 ) / ( 2 * i + a + b + 1 ) ...
      * gamma ( i + a + 1 ) * gamma ( i + b + 1 ) ...
      / r8_factorial ( i ) / gamma ( i + a + b + 1 );
  end

  return
end


function [mappedX, mapping] = em_pca(X, no_dims, max_iter)
%EMPCA Run an EM-based implementation of (probabilistic) PCA
%
%   [mappedX, mapping] = em_pca(X, no_dims)
%
% Performs probabilistic PCA on dataset X in order to reduce its
% dimensionality to no_dims. The dimensionality reduction is performed by
% means of an EM algorithm. The resulting low-dimensional counterpart of X
% is returned in mappedX. Information on the applied mapping (allowing for,
% e.g., out-of-sample extension) is returned in mapping.
%
%

% This file is part of the Matlab Toolbox for Dimensionality Reduction.
% The toolbox can be obtained from http://homepage.tudelft.nl/19j49
% You are free to use, change, or redistribute this code in any way you
% want for non-commercial purposes. However, it is appreciated if you
% maintain the name of the original author.
%
% (C) Laurens van der Maaten, Delft University of Technology

    if ~exist('max_iter', 'var')
        max_iter = 200;
    end

    % Initialize some variables
    [n D] = size(X);                        % data dimensions
    Ez = zeros(no_dims, n);                 % expectation of latent vars
    Ezz = zeros(no_dims, no_dims, n);       % expectation of cov(z)
    Q = Inf;                                % log-likelihood
    mapping = struct;

    % Randomly initialize W and sigma
    W = rand(D, no_dims) * 2;               % factor loadings
    sigma2 = rand(1) * 2;                   % variance ^ 2
    % The covariance of the Gaussian is: C = W * W' + sigma2 * eye(D);

    % Make data zero-mean (possible because data mean is ML estimate for mu)
    mapping.mean = mean(X, 1);
    X = bsxfun(@minus, X, mapping.mean);

    % Compute data covariance and transpose data
    S = cov(X);
    X = X';

    % Perform EM iterations
    converged = 0;
    iter = 0;
    inW = W' * W;
    while ~converged && iter <= max_iter

        % Update iteration number
        iter = iter + 1;
        if rem(iter, 5) == 0
            fprintf('.');
        end

        % Perform E-step
        invM = inv(inW + sigma2 * eye(no_dims));
        for i=1:n
            Ez(:,i)    = invM * W' * X(:,i);
            Ezz(:,:,i) = sigma2 * invM + Ez(:,i) * Ez(:,i)';
        end

        % Perform M-step (maximize mapping W)
        Wp1 = zeros(D, no_dims);
        Wp2 = zeros(no_dims, no_dims);
        for i=1:n
            Wp1 = Wp1 + X(:,i) * Ez(:,i)';
            Wp2 = Wp2 + Ezz(:,:,i);
        end
        W = Wp1 / Wp2;
        inW = W' * W;

        % Perform M-step (maximize discarded variance sigma)
        normX = sum(X .^ 2, 1);
        sigma2 = 0;
        for i=1:n
            sigma2 = sigma2 + (normX(i) - 2 * Ez(:,i)' * W' * X(:,i) + trace(Ezz(:,:,i) * inW));
        end
        sigma2 = (1 / (n * D)) * sigma2;

        % Compute likelihood of new model
        oldQ = Q;
        if iter > 1
            invC = ((1 / sigma2) * eye(D)) - ((1 / sigma2) * W * invM * W');
            detC = det(sigma2 * eye(D)) * det(eye(no_dims) + W' * ((sigma2 .^ -1) * eye(D)) * W);
            Q = (-n / 2) * (D * log(2 * pi) + log(detC) + trace(invC * S));
        end

        % Stop condition to detect convergence
        if abs(oldQ - Q) < 1e-3
            converged = 1;
        end
    end

    % Compute mapped data
    disp(' ');
    mapping.M = (inW \ W')';
    mapping.sigma2 = sigma2;
    mappedX = X' * mapping.M;


function t = tvec_even3 ( nt )

%*****************************************************************************80
%
%% TVEC_EVEN3 computes an evenly spaced set of angles between 0 and 2*PI.
%
%  Discussion:
%
%    The angles begin with 0 and end with 2*PI.
%
%  Example:
%
%    NT = 4
%
%    T = ( 0, 2*PI/3, 4*PI/3 2*PI )
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 May 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer NT, the number of values to compute.
%
%    Output, real TVEC(NT), the evenly spaced angles, in radians.
%
  if ( nt == 1 )
    t(1) = pi;
  else
    t(1:nt) = ( 0:2:(2*nt-1) ) * pi / ( nt - 1 );
  end

  return
end


close all;
clearvars;
clc;
rng default;

degree = 5;

n = 10;
sigma = 0.25;
[x_train, t_train] = spx.data.synthetic.func.sinusoid('n', n, 'sigma', sigma);

n2 = 10;
[x_test, t_test] = spx.data.synthetic.func.sinusoid('n', n2, 'sigma', 0);

features_train = spx.ml.features.polynomial(x_train, degree);
model = spx.ml.models.linear.LinearRegression;
model.fit(features_train, t_train);
features_test = spx.ml.features.polynomial(x_test, degree);
y_test = model.predict(features_test);

p = polyfit(x_train, t_train, degree);
py_test = polyval(p,x_test);

fprintf('t_test: ');
spx.io.print.vector(t_test, 3);
fprintf('y_test: ');
spx.io.print.vector(y_test, 3);
fprintf('py_test: ');
spx.io.print.vector(py_test, 3);


function [VIn, MIn] = partition_distance(Cx, Cy)
%PARTITION_DISTANCE     Distance or similarity between community partitions
%
%   This function quantifies information-theoretic distance (normalized
%   variation of information) or similarity (normalized mutual information)
%   between community partitions.
%
%   VIn        = partition_distance(Cx);
%   VIn        = partition_distance(Cx, Cy);
%   [VIn, MIn] = partition_distance(Cx, Cy);
%
%   Inputs:
%       Cx,
%           Community partition vector or matrix of n rows and p columns,
%           n is the number of network nodes, and p is the number of input
%           community partitions (in the case of vector input p=1).
%
%       Cy (optional argument),
%           Community partition vector or matrix of n rows and q columns. n
%           is the number of nodes (must be equal to the number of nodes in
%           Cq) and q is the number of input community partitions (may be
%           different to the number of nodes in Cq). This argument may be
%           omitted, in which case, the partition distance is computed
%           between all pairwise partitions of Cx.
%
%   Outputs:
%       VIn,
%           Normalized variation of information ([p, q] matrix)
%
%       MIn,
%           Normalized mutual information ([p, q] matrix)
%
%   Notes:
%       Mathematical definitions.
%
%           VIn = [H(X) + H(Y) - 2MI(X, Y)]/log(n)
%           MIn = 2MI(X, Y) / [H(X) + H(Y)]
%
%           where H is the entropy and MI is the mutual information
%
%
%   Reference: Meila M (2007) J Multivar Anal 98, 873-895.
%
%
%   2011-2017, Mika Rubinov, UNSW, Janelia HHMI

%   Modification History:
%   Mar 2011: Original
%   Jan 2017: Added computation between input matrices.

s = (nargin==1);
if s
    Cy = Cx;
    d = 10.^ceil(log10(double(1 + max( Cx(:)) )));
else
    d = 10.^ceil(log10(double(1 + max([Cx(:);Cy(:)]) )));
end

if ~isequal([Cx(:);Cy(:)], int64([Cx(:);Cy(:)])) || min([Cx(:);Cy(:)])<=0
    error('Input partitions must contain only positive integers.')
end

[n, p] = size(Cx);
HX = zeros(p, 1);
for i = 1:p
    Px = nonzeros(accumarray(Cx(:, i), 1)) / n;                     % P(x)
    HX(i) = - sum(Px .* log(Px));                                   % H(x)
end

if s
    q = p;
    HY = HX;
else
    [n_, q] = size(Cy);
    assert(n == n_);
    HY = zeros(q, 1);
    for j = 1:q
        Py = nonzeros(accumarray(Cy(:, j), 1)) / n;                 % P(y)
        HY(j) = - sum(Py .* log(Py));                               % H(y)
    end
end

VIn = zeros(p, q);
MIn = zeros(p, q);
for i = 1:p
    j_idx = (s * (i - 1) + 1):q;
    for j = j_idx
        Pxy = nonzeros(accumarray(d*Cx(:, i) + Cy(:, j), 1)) / n; 	% P(x,y)
        Hxy = -sum(Pxy .* log(Pxy));                                % H(x,y)
        VIn(i, j) = (2 * Hxy - HX(i) - HY(j)) / log(n);             % VIn
        MIn(i, j) = 2 * (HX(i) + HY(j) - Hxy) / (HX(i) + HY(j));    % MIn
    end
    if s
        VIn(j_idx, i) = VIn(i, j_idx);
        MIn(j_idx, i) = MIn(i, j_idx);
    end
end


function x = log_normal_cdf_inv ( cdf, a, b )

%*****************************************************************************80
%
%% LOG_NORMAL_CDF_INV inverts the Lognormal CDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 February 1999
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real CDF, the value of the CDF.
%    0.0 <= CDF <= 1.0.
%
%    Input, real A, B, the parameters of the PDF.
%    0.0 < B.
%
%    Input, real X, the corresponding argument.
%
  if ( cdf < 0.0 | 1.0 < cdf )
    fprintf ( 1, '\n' );
    fprintf ( 1, 'LOG_NORMAL_CDF_INV - Fatal error!\n' );
    fprintf ( 1, '  CDF < 0 or 1 < CDF.\n' );
    error ( 'LOG_NORMAL_CDF_INV - Fatal error!' );
  end

  logx = normal_cdf_inv ( cdf, a, b );

  x = exp ( logx );

  return
end


function [ pn, dist, t ] = segment_point_near ( p1, p2, p )

%*****************************************************************************80
%
%% SEGMENT_POINT_NEAR finds the line segment point nearest a point.
%
%  Discussion:
%
%    A line segment is the finite portion of a line that lies between
%    two points.
%
%    The nearest point will satisfy the condition
%
%      PN = (1-T) * P1 + T * P2.
%
%    T will always be between 0 and 1.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 December 2010
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real P1(2,1), P2(2,1), the endpoints of the line segment.
%
%    Input, real P(2,1), the point whose nearest neighbor
%    on the line segment is to be determined.
%
%    Output, real PN(2,1), the point on the line segment which is
%    nearest the point (X,Y).
%
%    Output, real DIST, the distance from the point to the
%    nearest point on the line segment.
%
%    Output, real T, the relative position of the point (XN,YN)
%    to the points (X1,Y1) and (X2,Y2).
%

%
%  If the line segment is actually a point, then the answer is easy.
%
  if ( p1(1:2,1) == p2(1:2,1) )

    t = 0.0;

  else

    bot = sum ( ( p2(1:2,1) - p1(1:2,1) ).^2 );

    t = ( p(1:2,1) - p1(1:2,1) )' * ( p2(1:2,1) - p1(1:2,1) ) / bot;

    t = max ( t, 0.0 );
    t = min ( t, 1.0 );

  end

  pn(1:2,1) = p1(1:2,1) + t * ( p2(1:2,1) - p1(1:2,1) );

  dist = sqrt ( sum ( ( pn(1:2,1) - p(1:2,1) ).^2 ) );

  return
end


clear;
clc;
close all;

%% https://www.coursera.org/learn/spacecraft-dynamics-kinematics/lecture/UFIBv/4-1-example-of-devenports-q-method

%% setuo the true attitude states;
theta_true = deg2rad([30 20 -10]);
BNtrue = angle2dcm(theta_true(1), theta_true(2), theta_true(3));

v1N = [1 0 0];
v2N = [0 0 1];

v1B_true = [BNtrue*v1N'];
v2B_true = [BNtrue*v2N'];

%% setup the measured attitude states
v1B = [0.8190 -0.5282 0.2243];
v2B = [-0.3138 -0.1584 0.9363];
% v1B = v1B / norm(v1B);
% v2B = v2B / norm(v2B);

% v1B = [0.8273 0.5541 -0.0920];
% v2B = [-0.8285 0.5522 -0.0955];
% v1N = [-0.1517 -0.9669 0.2050];
% v2N = [-0.8383 0.4494 -0.3044];

%% Devenport_Q mehold
w1 = 1;
w2 = 1;
W =[w1 w2];

vB = [v1B; v2B];
vB = vB.*W';
vN = [v1N; v2N];

%B = w1* v1B'*v1N + w2*v2B'*v2N;
B = vB'*vN;

S = B +B';
sigma = B(1,1) + B(2,2) + B(3,3);
Z = [B(2,3)-B(3,2) B(3,1)-B(1,3) B(1,2)-B(2,1)]';

K  = [sigma Z'; Z S - sigma*eye(3) ];

[V D] = eig(K);
[val index] = max(diag(D));
beta_q = V(:,index)';

DEVENPORT_Q = quat2dcm(beta_q)

%% OLAE  optimzal linear attitude estimator
W = eye(6);

d = [v1B - v1N,  v2B - v2N];

S =[ skew_symmetric(v1B + v1N); skew_symmetric(v2B + v2N)];
qBar = ((S'*W*S)^-1)  * S'*W*d';
OLAE_result = rod2dcm(qBar');
OLAE_result


function [env] = env_secant(x_data, y_data, view, side)
% Function call: env_secant(x_data, y_data, view, side)
% Calculates the top envelope of data <y_data> over <x_data>.
% Method used: 'secant-method'
% env_secant() observates the max. slope of about <view> points,
% and joints them to the resulting envelope.
% An interpolation over original x-values is done finally.
% <side> ('top' or 'bottom') defines which side to evolve.
% Author: Andreas Martin, Volkswagen AG, Germany

side = strcmpi( {'top','bottom'}, side ) * [ 1 ; -1 ];

assert( view > 1, ...
       'Parameter <view> too small!' );
assert( ndims (x_data) == 2, ...
       'Parameter <x_data> has to be vector type!' );
assert( size (x_data, 1) == 1 || size (x_data, 2) == 1, ...
       'Parameter <x_data> has to be vector type (Nx1)!' );
assert( ndims (y_data) == 2, ...
       'Parameter <y_data> has to be vector type (Nx1)!' );
assert( size (y_data, 1) == 1 || size (y_data, 2) == 1, ...
       'Parameter <y_data> has to be vector type (Nx1)!' );
assert( length (x_data) == length (y_data), ...
       'Parameters <x_data> and <y_data> must have same length!' );
assert( side ~= 0, ...
       'Parameter <side> must be ''top'' or ''bottom''' );

y_data = y_data(:);
data_len = length( y_data );
x_new = [];
y_new = [];

i = 1;
while i < data_len;
    ii = i+1:min( i + view, data_len );
    [ m, idx ] = max( ( y_data(ii) - y_data(i) ) ./ (ii-i)' .* side );

    % Equidistant x_data assumed! Use next row instead, if not:
    %[ m, idx ] = max( ( y_data(ii) - y_data(i) ) ./ ( x_data(ii) - x_data(i) ) * side );

    % New max. slope: store new "observation point"
    i = i + idx;
    x_new = [ x_new x_data(i) ];
    y_new = [ y_new y_data(i) ];
end;

env = interp1( x_new, y_new, x_data, 'linear', 'extrap' );


function polynomial_dif_test ( )

%*****************************************************************************80
%
%% POLYNOMIAL_DIF_TEST tests POLYNOMIAL_DIF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 November 2013
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'POLYNOMIAL_DIF_TEST\n' );
  fprintf ( 1, '  POLYNOMIAL_DIF computes derivatives of a polynomial.\n' );

  m = 2;

  o1 = 4;
  c1 = [ 2.0, 3.0, 4.0, 5.0 ];
  e1 = [ 1, 10, 12, 32 ];
  title1 = '  P(X) =';
  fprintf ( 1, '\n' );
  polynomial_print ( m, o1, c1, e1, title1 );

  dif = [ 2, 1 ];

  [ o, c, e ] = polynomial_dif ( m, o1, c1, e1, dif );
  title = '  d3 P(X) dx1 dx1 dx2 =';
  fprintf ( 1, '\n' );
  polynomial_print ( m, o, c, e, title );

  return
end


function program_07 ( )

%*****************************************************************************80
%
%% PROGRAM_07: Monte Carlo integral estimate for arbitrary triangle.
%
%  Discussion:
%
%    The program
%    * reads a triangle T (defined by three points),
%    * reads a random number seed;
%    * reads integer exponents P and Q;
%    * reads N, the number of random values to generate;
%    * it then computes N random points in the triangle;
%    * evaluates X^P * Y^Q at each point, and averages
%      to estimate the integral over the triangle.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    03 February 2007
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'PROGRAM_07 - Monte Carlo estimate of\n' );
  fprintf ( 1, '  Integral x^p y^q\n' );
  fprintf ( 1, '  over an arbitrary triangle.\n' );
%
%  Get the triangle vertices.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Define a triangle T:\n' );

  t_v1 = input ( '  Enter [ T.v1.x, T.v1.y]: ' );
  t_v2 = input ( '  Enter [ T.v2.x, T.v2.y]: ' );
  t_v3 = input ( '  Enter [ T.v3.x, T.v3.y]: ' );
%
%  Get the random number seed.
%
  seed = input ( 'Enter a random number seed:  ' );
  rand ( 'state', seed );
%
%  Get the powers P and Q.
%
  p = input ( 'Enter the power P for X^P:  ' );
  q = input ( 'Enter the power Q for Y^Q:  ' );
%
%  Get the number of values to generate.
%
  n = input ( 'Enter the number of samples to generate:  ' );
%
%  Compute the area.
%
  t_area = 0.5 * ( t_v1(1) * ( t_v2(2) - t_v3(2) ) ...
                 + t_v2(1) * ( t_v3(2) - t_v1(2) ) ...
                 + t_v3(1) * ( t_v1(2) - t_v2(2) ) );
%
%  Generate sample points in the unit triangle,
%  map them to points in the given triangle,
%  evaluate the integrand, add to QUAD.
%
  quad = 0.0;

  for i = 1 : n

    r = rand;
    s = rand;

    xi1 =   1.0       - sqrt ( s );
    xi2 = ( 1.0 - r ) * sqrt ( s );
    xi3 =         r   * sqrt ( s );

    x = xi1 * t_v1(1) + xi2 * t_v2(1) + xi3 * t_v3(1);
    y = xi1 * t_v1(2) + xi2 * t_v2(2) + xi3 * t_v3(2);

    quad = quad + x^p * y^q;

  end
%
%  Normalize QUAD by the area and the number of points.
%
  quad = quad * t_area / n;
%
%  We didn't work out the exact integral, so all we have is
%  this estimate.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Estimated integral = %12.6f\n', quad );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'PROGRAM_07\n' );
  fprintf ( 1, '  Normal end of execution.\n' );

  return
end


function y = logGauss(X, mu, sigma)
% Compute log pdf of a Gaussian distribution.
% Input:
%   X: d x n data matrix
%   mu: d x 1 mean vector of Gaussian
%   sigma: d x d covariance matrix of Gaussian
% Output:
%   y: 1 x n probability density in logrithm scale y=log p(x)
% Written by Mo Chen (sth4nth@gmail.com).
d = size(X,1);
X = X-mu;
[U,p]= chol(sigma);
if p ~= 0
    error('ERROR: sigma is not PD.');
end
Q = U'\X;
q = dot(Q,Q,1);  % quadratic term (M distance)
c = d*log(2*pi)+2*sum(log(diag(U)));   % normalization constant
y = -(c+q)/2;


%% Mean-Shift Algorithm
% by Sylvain Bernhardt
% July 2008
%% Description
% Computes the mask of a Parzen window
% and its gradient in respect of the x-axis
% and the y-axis.
% The different types of kernel are:
% {Uniform,Triangular,Epanechnikov,Gaussian}
%
% [k,gx,gy] = Parzen_window(H,W,R,type,graph)
% with:
% k - the mask
% gx,gy - its gradients
% (H,W) - size of the mask
% type - its type
% graph - plot the masks if graph=1

function [k,gx,gy] = Parzen_window(H,W,R,type,graph)
k = zeros(H,W);

%% ----Uniform----
if strcmp(type,'Uniform')==1
    for i=1:H
        for j=1:W
            if (((2*i)/H-1)/R)^2+(((2*j)/W-1)/R)^2 <= 1
                k(i,j) = 1;
            end
        end
    end
end

%% ----Triangular----
if strcmp(type,'Triangular')==1
    Max = max(H,W);
    for z=1:round(R*Max/2)-1
        h = zeros(H,W);
        for i=1:H
            for j=1:W
                if ((i-(H/2))/(R*H/2-z*H/Max))^2+...
                        ((j-(W/2))/(R*W/2-z*W/Max))^2 <= 1
                    h(i,j) = 2/(Max*R);
                end
            end
        end
        k = k+h;
    end
end

%% ----Epanechnikov
if strcmp(type,'Epanechnikov')==1
    for i=1:H
        for j=1:W
            k(i,j) = (1-(2*i/(R*H)-1/R)^2-...
                (2*j/(R*W)-1/R)^2);
            if k(i,j) < 0
                k(i,j) = 0;
            end
        end
    end
end

%% ----Gaussian----
if strcmp(type,'Gaussian')==1
    sigmaH = (R*H/2)/3;
    sigmaW = (R*W/2)/3;
    % sigma = x/3 as a gaussian is almost equal to 0
    % from 3*sigma.
    for i=1:H
        for j=1:W
            k(i,j) = exp(-.5*((i-.5*H)^2/sigmaH^2+...
                (j-.5*W)^2/sigmaW^2));
        end
    end
end

%% Gradient of kernel
[gx,gy] = gradient(-k);

%% Plotting the window
if graph==1
    figure (4)
    scrsz = get(0,'ScreenSize');
    set(4,'Position',[scrsz(3)/4 scrsz(4)/4 ...
        scrsz(3)/1.5 scrsz(4)/1.5])
    subplot(2,2,1)
    mesh(k);
    surf(k);
    shading interp
    subplot(2,2,2)
    mesh(gx);
    surf(gx);
    shading interp
    subplot(2,2,3)
    mesh(gy);
    surf(gy);
    shading interp
end


function c=dsti(f,L,dim)
%DSTI  Discrete Sine Transform type I
%   Usage:  c=dsti(f);
%           c=dsti(f,L);
%           c=dsti(f,[],dim);
%           c=dsti(f,L,dim);
%
%   `dsti(f)` computes the discrete sine transform of type I of the
%   input signal *f*. If *f* is multi-dimensional, the transformation is
%   applied along the first non-singleton dimension.
%
%   `dsti(f,L)` zero-pads or truncates *f* to length *L* before doing the
%   transformation.
%
%   `dsti(f,[],dim)` or `dsti(f,L,dim)` applies the transformation along
%   dimension *dim*.
%
%   The transform is real (output is real if input is real) and orthonormal.
%
%   This transform is its own inverse.
%
%   Let f be a signal of length *L* and let `c=dsti(f)`. Then
%
%   ..                         L-1
%     c(n+1) = sqrt(2/(L+1)) * sum sin(pi*(n+1)*(m+1)/(L+1))
%                              m=0
%   .. math:: c\left(n+1\right)=\sqrt{\frac{2}{L+1}}\sum_{m=0}^{L-1}f\left(m+1\right)\sin\left(\frac{\pi \left(n+1\right)\left(m+1\right)}{L+1}\right)
%
%   The implementation of this functions uses a simple algorithm that requires
%   an FFT of length $2N+2$, which might potentially be the product of a large
%   prime number. This may cause the function to sometimes execute slowly.
%   If guaranteed high speed is a concern, please consider using one of the
%   other DST transforms.
%
%   Examples:
%   ---------
%
%   The following figures show the first 4 basis functions of the DSTI of
%   length 20:::
%
%     % The dsti is its own adjoint.
%     F=dsti(eye(20));
%
%     for ii=1:4
%       subplot(4,1,ii);
%       stem(F(:,ii));
%     end;
%
%   See also:  dcti, dstiii, dstiv
%
%   References: rayi90 wi94

%   AUTHOR: Peter L. Søndergaard
%   TESTING: TEST_PUREFREQ
%   REFERENCE: REF_DSTI

complainif_argnonotinrange(nargin,1,3,mfilename);

if nargin<3
  dim=[];
end;

if nargin<2
  L=[];
end;

[f,L,Ls,W,dim,permutedsize,order]=assert_sigreshape_pre(f,L,dim,'DSTI');

if ~isempty(L)
  f=postpad(f,L);
end;

if L==1
  c=f;

else

  c = comp_dst(f,1);

end;

c=assert_sigreshape_post(c,dim,permutedsize,order);


% This program compares the execution time of mgs.m to mgsfact.m
% on matrices with 200 rows and various numbers of columns ranging
% from 50 to 200 in steps of 10.
%
% The results are saved in a plot named timing.eps.
%
% Dianne O'Leary 09/2005

m = 200;
nsizes = 50:10:m;

disp('Comparing the new algorithm to the old.')
disp('Ratios of old execution times to new:')

for j=1:length(nsizes)
  n = nsizes(j)
  C = rand(m,n);
  tic
  [q,r] = mgs(C);
  t1(j) = toc;
  tic
  [q,r] = mgsfact(C);
  t2(j) = toc;
  disp(sprintf('For %d columns, ratio = %f',n,t1(j)/t2(j)))
end

semilogy(nsizes,t1,nsizes,t2)
xlabel('number of columns')
ylabel('time (sec)')
title('Times for matrices with 200 rows')
legend('Original algorithm','Modified algorithm')

print -depsc timing.eps


%% NUMERICAL LINEAR ALGEBRA WITH MATLAB
% This document looks at some important concepts in linear and shows how to
% solve some basic problems using MATLAB.

%% Matrix-Vector products
% Let's think about what a matrix-vector product actually does. It's more
% interesting that you think! Lets take a simple example. Consider the
% vector [1;0] multiplied by a matrix [1,1;1,1]
%%
figure('color',[1 1 1])
A=ones(2)
x=[1;0]
Ax=A*x
line([0 x(1)],[0 x(2)]), hold on, grid on
line([0,Ax(1)],[0,Ax(2)],'color',[1 0 0])
text(1.1,0,'x','fontweight','bold'), text(1.1,1.1,'A*x','fontweight','bold')
axis([-0.5 1.5 -0.5 1.5])
title('Action of a matrix A on a vector x')
%%
% We can see that the matrix multiplication resulted in a vector that was
% rotated by a certain angle and also stretched by a certain factor. This
% action (stretching and rotating) is extremely useful for switching from
% one co-ordinate system [x,y] to another [x', y']. Something that you will
% come across in special relativity and a number of other areas. To
% visualise what we mean by a co-ordinate transformation, consider the
% basis vectors x:=[1,0] and y:=[0,1], and a point p:=[0.2,0.1].
figure('color',[1 1 1])
xy=[1,0;0,1]
p=[0.5,0.3,0.1;0.4,0.2,0.0]
A=[-1 1;1 1]
xyp=A*xy; % Co-ordinate rotation step
Ap=A*p;
line([0 xy(1,1)],[0 xy(2,1)]), grid on, hold on
line([0,xy(1,2)],[0,xy(2,2)])
plot(p(1,:),p(2,:),'*','markersize',6,'color','b')
line([0, xyp(1,1)],[0 xyp(2,1)],'color',[1 0 0])
line([0, xyp(2,1)],[0,xyp(2,2)],'color',[1 0 0])
plot(Ap(1,:),Ap(2,:),'*','markersize',6,'color','r')
axis([-1.5 1.5 -1.5 1.5]), axis square
title('Changing the co-ordinate basis with matrix multiplication')

%% A rogue's gallery of useful matrices
% Matrices can be thought of as a 2-D array of data, or a collection of
% vectors. There are a number of important matrices that are extremely
% useful, and worth knowing about. We will use several of these later on in
% this lecture. Here are a few:
%%
% * Identity
% * Ones/Zeros
% * Upper/Lower Triangular
% * Symmetric
% * Vandermonde
% * Finite Difference
% * Rotation
%%
% The *Identity Matrix* or *I*
%%
I = eye(4)
%%
% A *matrix of ones or zeros*:
%%
O = ones(4)
Z = zeros(4)
%%
% *Upper and lower triangular matrices*. These are particularly important,
% as we shall see below, and consist of matrices that have zeros below, or
% above the _main diagonal_
%%
UT = triu(randn(4))
LT = tril(randn(4))
%%
% *Symmetric matrices* are matrices that have the same
% elements above he main diagonal as below the main diagonal:
%%
A = randn(4);
SYM = A*A'
%%
% The *Vandermonde matrix* is a matrix whose columns are polynomials in
% $$x$. This is useful in curve fitting and interpolation, for example, if
% I wanted to find the $$3^{rd}$ order interpolating polynomial for some
% data [x,y], then I would use the following matrix:
figure('color',[1 1 1])
x=linspace(0,1,5)';
IM = [x.^0, x.^1, x.^2, x.^3, x.^4]
plot(x,IM), axis([0 1 0 1.2]), grid on
title('First five powers of x')
%%
% Generally for interpolation, it's better to use a matrix whose columns
% are *orthogonal* polynomials, such as Legendre or Chebyshev, but this is
% beyond the remit of this course.
%%
% *Finite difference matrices* are extremely useful. They are matrices that
% approximate the derivative of a function on a grid. One way to calculate
% them is to fit an interpolating polynomial through the function, and then
% differentiate the polynomial to get what is called a finite difference
% stencil, while the other is to use a Taylor expansion to approximate
% the derivatives.
%%
% As an illustration, we will use the Taylor expansion.
%%
% <latex> {\Large
% $ f(x+\Delta x) = f(x) + \Delta x f'(x) + \frac{{\Delta x}^2f''(x)}{2!} +
% $}{\emph higher order terms} </latex>
%%
% Here the dash represents a derivative with respect to x. If we ignore
% terms above the first derivative, we see we can say that
%%
% <latex> {\Large
% $ f'(x) \approx \frac{f(x+ \Delta x) - f(x)}{\Delta x}$} </latex>
%%
% This is called the *forward difference* approximation. If we apply this
% formula to itself, we come up with an approximation for the second
% derivative of a function:
%%
% <latex>{\Large
% $ f''(x) \approx \frac{1}{\Delta x}\left(\frac{f(x+\Delta x+\Delta
% x)-f(x+\Delta x)-f(x+\Delta x)+f(x)}{\Delta x}\right)$}</latex>
%%
% or that, finally:
%%
% <latex> {\Large
% $ f''(x) \approx \frac{1}{\Delta x^2}\left(f(x+2\Delta x
% )-2f(x+\Delta x)+f(x)\right)$}</latex>
%%
% How to turn this into a matrix, however? If we imagine a function
% expressed on a grid of points $${x_1,x_2,\cdots,x_N}$ and replace the
% function evaluation $$f(x+\Delta x)$ with function evaluations at the
% grid points, then the second derivative can be expressed as the following
% matrix:
%%
N=5;
I=ones(N,1);
D2 = spdiags([I, -2*I, I],[-1:1],N,N);
full(D2)
%%
% Here we use the construct
%  spdiags
% to tell MATLAB that the matrix is sparse, i.e. is composed of mainly zero
% elements. In fact, this particular type of sparsity pattern is called
% *tridiagonal*. Consider the second row of this matrix multiplied by a
% vector which is the function evaluated on the grid of points $$x_i$,
% $$\left[f(x_0), f(x_1), f(x_2),\cdots,f(x_N)\right]^T$. We have the following
% result:
%%
% <latex>
% $ \left(\begin{array}{cccccc} 1&-2&1&0&\cdots&0 \end{array}\right)\times
% \left(\begin{array}{c} f(x_0) \\ f(x_1) \\ f(x_2) \\ \vdots \\
% f(x_N)\end{array}\right) = f(x_0)-2\times f(x_1)+f(x_2)$
% </latex>
%%
% That is, the result of multiplying the second row of the differentiation
% matrix by a vector representing our function on the grid, gives an
% approximation to the derivative of our function on the grid. As an
% example, let's calculate the second derivative of $$ f(x)=\exp^{\sin(x)}$
% on a grid of 21 points:
N=21; x=linspace(0,2*pi,N)'; % set up the grid
I=ones(N,1); dx=x(2)-x(1);
D2 = spdiags([I, -2*I, I],[-1:1],N,N)/dx^2; %2nd derivative matrix
f=@(x)(exp(sin(x))) % function
d2 = D2*f(x);       % numerical derivative
d2(1)=1; d2(N)=1;   % fix boundary points
figure('color',[1 1 1])
plot(x,f(x),x,d2), grid on, hold on
d2f=@(x)(cos(x).^2.*exp(sin(x))-sin(x).*exp(sin(x))) % actual second derivative
plot(x,d2f(x),'r-.')
title('Approximating a second derivative using a differentiation matrix');
legend('f(x)=exp^{sin(x)}','Approximate 2^{nd} deriv','Actual 2^{nd} deriv');
hold off
%% Systems of linear equations
% At its heart, the subject of linear algebra is concerned with how to
% solve the seemingly simple equation:
%
% $${\bf{A}}x=b$,
% where $${\bf{A}}$ is a matrix and $$x$ and $$b$ are vectors.
%%
%
% This might seem surprising, but this problem is right at the heart of
% almost all problems in scientific computing, from differential equations
% to eigenvalue problems to interpolation and curve fitting.

%%
% Of course, for a single scalar variable, x, we know what that means:
% $$ a\times x = y$
% from which we can deduce that
% $$ x = y/a$, for non-zero $$a$.
%%
% The question is how do we solve such a problem when we have more than one
% unknown variable, or, equivalently, when x is a vector? To investigate this
% question, let's consider the following electrical circuit:
%
[c,m]=imread('circuit2.gif','gif');

figure('color',[1 1 1]),
image(c), colormap(m), axis equal, axis off

%%
% We want to calculate the currents $$I_1,I_2,I_3$
%%
% Using Kirchoff's voltage laws we get the following three equations for
% the three unknown currents:
%%
% <latex>
% $\left\{\begin{array}{ccc}
% I_1 + 25(I_1-I_2)+50(I_1-I_3) & = & 10 \\
% 25(I_2-I_1) + 30I_2 + I_2 -I_3 & = & 0 \\
% 50(I_3-I_1)+I_3-I_2+55I_3 & = & 0 \end{array}\right . $
% </latex>
%%
% which, with a little re-arranging becomes:
%%
% <latex>
% $\left\{\begin{array}{ccc}
% 76I_1 - 25I_2-50I_3 & = & 10 \\
% -25I_1 +56I_2 -I_3 & = & 0 \\
% -50I_1-I_2+106I_3 & = & 0 \end{array}\right .$
% </latex>
%%
% Finally, we can gather all the coefficients into a matrix, and the
% unknowns into a vector, to write the single matrix linear equation:
%%
% <latex>
% $\left( {\begin{array}{ccc}
% 76 & -25 & -50 \\
% -25 & 56 & -1  \\
% -50& -1 & 106 \end{array}} \right)
% \times \left(\begin{array}{c}
% I_1\\
% I_2\\
% I_3 \end{array}\right) = \left( \begin{array}{c}
% 10 \\
% 0\\
% 0 \end{array}\right)$
% </latex>
%%
% In this way we have got the system of equations into the form that we
% described above, namely:
% $${\bf{A}}x=b$
%%
% Of course, for a small system (3 by 3 in this case) we can solve this by
% hand. For a larger system, however, we need a computer. The basic
% approach to solving such a system is an extension of that used by hand,
% namely we add and subtract multiples of one row to one another until we
% finally end up with an upper triangular matrix. This action
% of adding and subtrating multiples of one row to another is known as a
% basic row operation. Let us illustrate the process with MATLAB. Before we
% begin, we augment the right hand side vector to be an extra column in our
% matrix $$\bf A$, so it is now three rows by four columns.
A=[76 -25 -50 10;-25 56 -1 0; -50 -1 106 0]
rrefexample(A)
%%
% What is the advantage of reducing $${\bf A}$ to be upper triangular?
% Starting from the last row, we see that we now have an equation that
% depends only on $$I_3$, that is $$I_3 = 0.117$. The row above depends on
% $$I_3$ and $$I_2$ only, and we now know what $$I_3$ is! In this way, we
% work our way up through the rows substituting as we go, and in each row
% there is only a single unknown variable. This process is called _back
% substitution_. This entire process - that of reducing the matrix to a
% triangular form and solving via back substitution - is the way most
% computer packages solve these linear systems. In MATLAB, the shorthand
% way of solving such systems is with the backslash character ``\''
A=[76 -25 -50;-25 56 -1; -50 -1 106]
b =[10;0;0]
I= A\b
%%
% In the example above, we have performed a special case of what is a more
% general idea - that of transforming a matrix into a special form, so that
% the equations can be solved more easily. In general, the idea of
% transforming a matrix through a series of elementary row operations is a
% powerful one. May special types of _matrix factorisations_ exist, and two
% of the most important examples are:
%%
% *QR decomposition*
% In this case, a general matrix is written as the product of two matrices,
% one orthogonal and one upper triangular:
% $${\bf A = QR}$
% As the inverse of an orthogonal matrix is equal to its transpose, we find
% the solution to the general system $${\bf A}x=b$ can be calculated as:
%%
% <latex>
% $\begin{array}{l}
% {\bf A}x = b \\
% {\bf QR}x = b \\
% {\bf Q}({\bf R}x)=b \\
% {\bf R}x = {\bf Q^T} \times b \\
% x = {\bf R}\setminus{\bf Q^T} \times b \end{array}$
% </latex>
%%
% *LU Decomposition*
% In this case the matrix is decomposed into two, a lower and an upper
% triangular matrix. As both the matrices are triangular, the back (and
% forward) substitution process is very quick:
%%
% <latex>
% $\begin{array}{l}
% {\bf A}x = b \\
% {\bf LU}x = b \\
% {\bf L}({\bf U}x)=b \\
% {\bf L}x = {\bf U}\setminus b \\
% x = {\bf L}\setminus({\bf U}\setminus b) \end{array}$
% </latex>
%%
% As another example of using MATLAB to solve linear systems, let us
% perform a least squares fit on some experimental data. We have run an
% experiment to measure the distance fallen by an object in a given time.
% We know from the laws of motion that the distance is related to the
% initial velocity and position, and the acceleration through a quadratic
% relationship:
%%
% $$x(t) = x(0) + v(0)t + \frac{1}{2}gt^2$
%%
% We want to fit a quadratic line of best fit through the data, and hence
% work out a rough estimate for $$g$ the acceleration due to gravity.
load fall_data
t=data(:,1);
dist=data(:,2);
I=ones(size(t));
R=[I,t,t.^2];  % This is a VANDERMONDE matrix (see above)
coeffs = R\dist
g=2*coeffs(3)  % approximation for g (coeff is g/2)
plot(t,dist,'o',t,R*coeffs,'r'), grid on
title('Fitting a quadratic curve to experimental data')

%% Eigenvectors and Eigenvalues of a matrix
% As we have seen, the action of a mtrix on a vector is one of rotation and
% scaling. There are certain vectors, however, which in some sense
% _resonate_ with some fundamental property of the matrix, and do not get
% rotated. These special vectors (or directions in N-Dimensional space) are
% called *eigenvectors* and the scaling factor by which they are stretched
% are called the *eigenvalues*. Mathematically what we are saying is:
% $$\bf{A}x = \lambda x$
% Or visually we are saying:
evecPlot
%%
% Essentially, what we are saying is that (square) matrices have certain
% ``resonant modes'' or ``preferred'' co-ordinate directions, which we call
% _eigenvectors_ of the matrix. The eigenvectors are orthogonal to one
% another, and together form an orthogonal basis. If an $$N\times N$ matrix
% has N distinct eigenvalues, and correspondingly N orthogonal
% eigenvectors, then it is said to have *Rank* N, or to be *full rank*.

%%
% To make all this more concrete, let's consider an example from the
% mathematical theory of waves and vibration. In one dimension, the wave
% equation can be written as the following _partial differential equation_
% (don't worry about what this means, exactly, you'll learn more about all
% this later).
%%
% <latex>{\Large $\frac{\partial ^2 \Psi}{\partial t^2} = c\frac{\partial ^2
% \Psi}{\partial x^2}$}</latex>
%%
% We solve such a system by a process known as _separation of the
% variables_, and this involves writing the solution to the above equation
% as a product of two parts, one purely dependent on space and the other on
% time:
%%
% $${\Large\Psi\left(x,t\right) = \Theta\left(x\right)\Phi\left(t\right)}$
%%
% By substituting this form of the solution back into the differential
% equation, we get:
%%
% $$\ddot{\Phi}(t)\Theta(x)=c\Phi(t)\Theta''(x)$
%%
% or that
%%
% <latex> {\Large$\frac{\ddot{\Phi}(t)}{\Phi(t)} =
% c\frac{\Theta''(x)}{\Theta(x)} = -k^2 $}</latex>
%%
% The only way that the left hand side, which is completely independent of
% $$x$, can be equal to the right hand side, which is completely
% independent of $$t$, is if both sides are equal to a constant, $$-k^2$.
% We are interested in time independent solutions (standing waves) for this
% problem, so we can consider only the spatial parts of the equation, which
% can be written as:
%%
% <latex>
% {\Large$\frac{\partial ^2 \Theta(x)}{\partial x^2} = -k^2\Theta(x)
% $ }</latex>
%%
% This is an eigenvalue equation, exactly as we had above, and the
% solutions we are looking for are called *eigenfunctions*. This particular
% equation is important in physics and is known as the _Helmholtz
% equation_.
%%
% Using the second derivative differentiation matrix we described above
% (actually a more accurate version of it) and extending the notion to two
% dimensions, we transform a differential equation into a matrix eigenvalue
% equation. We can then use MATLAB to compute the eigenvalues and
% eigenvectors numerically to investigate the standing wave patterns, for
% example:
chladni

%   Copyright 2008-2009 The MathWorks, Inc.
%   $Revision: 35 $  $Date: 2009-05-29 15:27:34 +0100 (Fri, 29 May 2009) $


function a = lehmer ( m, n )

%*****************************************************************************80
%
%% LEHMER returns the LEHMER matrix.
%
%  Discussion:
%
%    This matrix is also known as the "Westlake" matrix.
%
%  Formula:
%
%    A(I,J) = min ( I, J ) / max ( I, J )
%
%  Example:
%
%    N = 5
%
%    1/1  1/2  1/3  1/4  1/5
%    1/2  2/2  2/3  2/4  2/5
%    1/3  2/3  3/3  3/4  3/5
%    1/4  2/4  3/4  4/4  4/5
%    1/5  2/5  3/5  4/5  5/5
%
%  Properties:
%
%    A is symmetric: A' = A.
%
%    Because A is symmetric, it is normal.
%
%    Because A is normal, it is diagonalizable.
%
%    A is positive definite.
%
%    A is totally nonnegative.
%
%    The inverse of A is tridiagonal.
%
%    The condition number of A lies between N and 4*N*N.
%
%    The family of matrices is nested as a function of N.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 October 2007
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Morris Newman, John Todd,
%    The evaluation of matrix inversion programs,
%    Journal of the Society for Industrial and Applied Mathematics,
%    Volume 6, Number 4, 1958, pages 466-476.
%
%    Solutions to problem E710, proposed by DH Lehmer: The inverse of
%    a matrix.
%    American Mathematical Monthly,
%    Volume 53, Number 9, November 1946, pages 534-535.
%
%    John Todd,
%    Basic Numerical Mathematics, Volume 2: Numerical Algebra,
%    Academic Press, 1977, page 154.
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns of A.
%
%    Output, real A(M,N), the matrix.
%
  a = zeros ( m, n );

  for i = 1 : m
    for j = 1 : n
      a(i,j) = ( min ( i, j ) ) / ( max ( i, j ) );
    end
  end

  return
end


function mortrage()
clc;clear all; close all;
% definitions
% P = principle amount
% J = Monthly intrest rate
% R = Total monthly payment
% R = J*P+ amount applied to principle
% new principle amount
% P+j*P-R => P(1+J)-R => P*m-R: m=1+J

% inputs
pri=input('Principle amount ');
Yr=input(' No of Year ' );
rate=input('Annual percentage rate ' );
peryear=1/12;
percent=1/100;
ttl_mnt=Yr*12;
syms m J P R A N;

% the principal after n payments can be written as
% P = A?mn ? R? (mn ? 1)/(m? 1).
solve(A*m^N - R*(m^N - 1)/(m - 1), R);
R = subs(ans, m, J + 1);
format bank;
 disp( '     Interest Rate      Payment')
%for rate= 6:0.2:12
    disp([rate, double(subs(R, [A, N, J], [pri, ttl_mnt, rate*percent*peryear]))])
%end
ttl_payment=subs(R, [A, N, J], [pri, ttl_mnt, rate*percent*peryear])*ttl_mnt;
disp('total amunt paid =')
disp(ttl_payment)


% circlecoords - returns the pixel coordinates of a circle defined by the
%                radius and x, y coordinates of its centre.
%
% Usage:
% [x,y] = circlecoords(c, r, imgsize,nsides)
%
% Arguments:
%	c           - an array containing the centre coordinates of the circle
%	              [x,y]
%   r           - the radius of the circle
%   imgsize     - size of the image array to plot coordinates onto
%   nsides      - the circle is actually approximated by a polygon, this
%                 argument gives the number of sides used in this approximation. Default
%                 is 600.
%
% Output:
%	x		    - an array containing x coordinates of circle boundary
%	              points
%   y		    - an array containing y coordinates of circle boundary
%                 points
%
% Author:
% Libor Masek
% masekl01@csse.uwa.edu.au
% School of Computer Science & Software Engineering
% The University of Western Australia
% November 2003

function [x,y] = circlecoords(c, r, imgsize,nsides)

    if nargin == 3
	nsides = 600;
    end

    nsides = round(nsides);

    a = [0:pi/nsides:2*pi];
    xd = (double(r)*cos(a)+ double(c(1)) );
    yd = (double(r)*sin(a)+ double(c(2)) );

    xd = round(xd);
    yd = round(yd);

    %get rid of -ves
    %get rid of values larger than image
    xd2 = xd;
    coords = find(xd>imgsize(2));
    xd2(coords) = imgsize(2);
    coords = find(xd<=0);
    xd2(coords) = 1;

    yd2 = yd;
    coords = find(yd>imgsize(1));
    yd2(coords) = imgsize(1);
    coords = find(yd<=0);
    yd2(coords) = 1;

    x = int32(xd2);
    y = int32(yd2);


%DEMO_PGAUSS  How to use PGAUSS
%
%   This script illustrates various properties of the Gaussian function.
%
%   .. figure::
%
%      Window+Dual+Tight
%
%      This figure shows an optimally centered Gaussian for a
%      given Gabor system, its canonical dual and tight windows
%      and the DFTs of these windows.
%
%   See also: pgauss

disp('Type "help demo_pgauss" to see a description of how this demo works.');

% A quick test: If the second input parameter to
% pgauss is not specified, the output will be
% invariant under an unitary DFT. Matlabs FFT is does not
% preserve the norm, so it must be scaled a bit.

L=128;
g=pgauss(L);

disp('');
disp('Test of DFT invariance: Should be close to zero.');
norm(g-dft(g))

% Setup parameters and length of signal.
% Note that it must hold that L=M*b=N*a for some integers
% b and N, and that a <= M
L=72;  % Length of signal.
a=6;   % Time shift.
M=9;   % Number of modulations.

% Calculate the frequency shift.
b=L/M;

% For this Gabor system, the optimally concentrated Gaussian
% is given by
g=pgauss(L,a/b);

% This is not invarient with respect to a DFT, but it is still
% real and whole point even
disp('');
disp('The function is WP even. The following should be 1.');
isevenfunction(g)

disp('Therefore, its DFT is real.');
disp('The norm of the imaginary part should be close to zero.');
norm(imag(dft(g)))

% Calculate the canonical dual.
gdual=gabdual(g,a,M);

% Calculate the canonical tight window.
gtight=gabtight(g,a,M);

% Plot them:

% Standard note on plotting:
%
% - All windows have real DFTs, but Matlab does not
%   always recoqnize this, so we have to filter away
%   the small imaginary part by calling REAL(...)
%
% - The windows are all centered around zero, but this
%   is not visually pleasing, so the window must be
%   shifted to the middle by an FFTSHIFT
%

gf_plot      = fftshift(real(dft(g)));
gdual_plot   = fftshift(gdual);
gdualf_plot  = fftshift(real(dft(gdual)));
gtight_plot  = fftshift(gtight);
gtightf_plot = fftshift(real(dft(gtight)));
figure(1);

subplot(3,2,1);
x=(1:L).';
plot(x,fftshift(g),'-',...
     x,circshift(fftshift(g),a),'-',...
     x,circshift(fftshift(g),-a),'-');
title('g=pgauss(72,6/8)');

subplot(3,2,2);
plot(gf_plot);
title('g, frequency domain');

subplot(3,2,3);
plot(gdual_plot);
title('Dual window of g');

subplot(3,2,4);
plot(gdualf_plot);
title('dual window, frequency domain');

subplot(3,2,5);
plot(gtight_plot);
title('Tight window generated from g');

subplot(3,2,6);
plot(gtightf_plot);
title('tight window, frequency domain');


function copl = isCoplanar(x,y,z,tol)
%ISCOPLANAR Tests input points for coplanarity in 3-space.
%
% COPL = isCoplanar(PTS)
% Tests the coplanarity of the input points in array PTS. Input array must
% be 4-by-3, each row containing coordinate of one point.
%
% COPL = isCoplanar(PTS, TOLERANCE)
% Specifies the tolerance value used for checking coplanarity. Default is
% zero.
%
%
% Example:
%   iscoplanar([1 2 -2; -3 1 -14; -1 2 -6; 1 -2 -8], eps)

%
% Adapted from a function originally written by Brett Shoelson, Ph.D.
% brett.shoelson@joslin.harvard.edu
% https://fr.mathworks.com/matlabcentral/fileexchange/46-iscoplanar-m
%

if nargin == 0
	error('Requires at least one input argument.');

elseif nargin == 1
    if size(x,2) == 3
        % Matrix of all x,y,z is input
        pts = x;
        tol = 0;
    else
        error('Invalid input.')
    end

elseif nargin == 2
	if size(x,2) == 3
		% Matrix of all x,y,z is input
		pts = x;
		tol = y;
	else
		error('Invalid input.')
	end
elseif nargin == 3
	% Compile a matrix of all x,y,z
	pts = [x y z];
	tol = 0;
else
	pts = [x y z];
end

if size(x, 1) < 4
    error('Requires at least four points to compute coplanarity');
end

% replace first point at the origin and compute SVD of the matrix
sv = svd(bsxfun(@minus, pts(2:end,:), pts(1,:)));
copl = sv(3) <= tol * sv(1);

% % Alterantive version that computes the rank of the matrix
% rnk = rank(bsxfun(@minus, pts(2:end,:), pts(1,:)), tol);
% copl = rnk <= size(pts, 2) - 1;

% % Old version:
% %Compare all 4-tuples of point combinations; {P1:P4} are coplanar iff
% %det([x1 y1 z1 1;x2 y2 z2 1;x3 y3 z3 1;x4 y4 z4 1])==0
% tmp = nchoosek(1:size(pts,1),4);
% for ii = 1:size(tmp,1)
% 	copl = abs(det([pts(tmp(ii, :), :) ones(4,1)])) <= tolerance;
% 	if ~copl
% 		break
% 	end
% end


function sig = signature(b,x0,y0)
%SIGNATURE Computes the signature of a boundary.
%   SIG = SIGNATURE(B,X0,Y0) computes the signature of boundary. B. A
%   signature is defined as the distance (DIST) from (X0,Y0) to the
%   boundary, as a function of angle. B is an np-by-2 matrix, with np >=
%   360, whose rows contain the (x,y) = (row,col) coordinates of the
%   boundary ordered in a clockwise or counterclockwise direction. If
%   (X0,Y0) is not included in the input argument, the centroid of the
%   boundary is used by default. SIG is a vector of size size 360-by-1,
%   indicating a signature resolution of one degree. The input must be a
%   one-pixel-thick boundary obtained, for example, by using function
%   bwboundaries.
%
%   If (X0,Y0) or the default centroid is outside the boundary, the
%   signature is not defined and an error is issued.
%
%   Copyright 2002-2020 Gatesmark
%
%   This function, and other functions in the DIPUM Toolbox, are based
%   on the theoretical and practical foundations established in the
%   book Digital Image Processing Using MATLAB, 3rd ed., Gatesmark
%   Press, 2020.
%
%   Book website: http://www.imageprocessingplace.com
%   License: https://github.com/dipum/dipum-toolbox/blob/master/LICENSE.txt

% Check dimensions of b.
[np,nc] = size(b);
if (np < 360 || nc ~= 2)
   error('b must be of size (>=360)-by-2.');
end

% Some boundary tracing programs, such as bwboundaries.m, result in a
% sequence in which the coordinates of the first and last points are the
% same. If this is the case, in b, eliminate the last point.
if isequal(b(1,:),b(np,:))
   b = b(1:np - 1,:);
   np = np - 1;
end

% If (x0,y0) is not specified, set the origin from which the signature
% is computed as the centroid.
if nargin == 1
   % Coordinates of the centroid.
   x0 = sum(b(:,1))/np;
   y0 = sum(b(:,2))/np;
end

% Check to see that (x0,y0) is inside the boundary.
IN = inpolygon(x0,y0,b(:,1),b(:,2));
if ~IN
   error('(x0,y0) or centroid is not inside the boundary.')
end

% Shift origin of coordinate system to (x0,y0).
b(:,1) = b(:,1) - x0;
b(:,2) = b(:,2) - y0;

% Convert the coordinates to polar. But first have to convert the given
% image coordinates, (x,y), to the coordinate system used by MATLAB for
% conversion between Cartesian and polar cordinates. Designate these
% coordinates by (xcart,ycart). The two coordinate systems are related
% as follows:  xcart = y and ycart = -x, where (x,y) = (row,col).
xcart = b(:,2);
ycart = -b(:,1);
[theta,rho] = cart2pol(xcart,ycart);

% Convert angles to degrees.
theta = theta.*(180/pi);

% Convert to all nonnegative angles.
j = theta == min(theta(:));
k = theta == max(theta(:));
theta = theta.*(0.5*abs(1 + sign(theta)))...
   - 0.5*(-1 + sign(theta)).*(360 + theta);

% Set the smallest angle to 0 and the largest angle to 359.
theta(j) = 0;
theta(k) = 359;

% Sort angles in ascending order and pair them with the corresponding
% rho
[theta,idx] = sort(theta);
rho(idx) = rho;
n = size(theta,1);

% Subsample the signature into 360 angle increments.
n = size(rho,1);
idx = round(linspace(1,n,360));
rho = rho(idx);
sig = rho;


%% bfs
load_gaimc_graph('bfs_example.mat') % use the dfs example from Boost
d = bfs(A,1)

%% bipartite_matching
A = rand(10,8); % bipartite matching between random data
[val mi mj] = bipartite_matching(A);
val

%% clustercoeffs
load_gaimc_graph('clique-10');
cc = clustercoeffs(A) % they are all equal! as we expect in a clique

%% dfs
load_gaimc_graph('dfs_example.mat') % use the dfs example from Boost
d = dfs(A,1)

%% dijkstra
% Find the minimum travel time between Los Angeles (LAX) and
% Rochester Minnesota (RST).
load_gaimc_graph('airports')
A = -A; % fix funny encoding of airport data
lax=247; rst=355;
[d pred] = dijkstra(A,lax);
fprintf('Minimum time: %g\n',d(rst));
% Print the path
fprintf('Path:\n');
path =[]; u = rst; while (u ~= lax) path=[u path]; u=pred(u); end
fprintf('%s',labels{lax});
for i=path; fprintf(' --> %s', labels{i}); end, fprintf('\n');

%% dirclustercoeffs
load_gaimc_graph('celegans'); % load the C elegans nervous system network
cc=dirclustercoeffs(A);
[maxval maxind]=max(cc)
labels(maxind) % most clustered vertex in the nervous system

%% graph_draw
load_gaimc_graph('dfs_example');
graph_draw(A,xy);

%% mst_prim
load_gaimc_graph('airports'); % A(i,j) = negative travel time
A = -A; % convert to travel time.
A = max(A,A'); % make the travel times symmetric
T = mst_prim(A);
gplot(T,xy); % look at the minimum travel time tree in the US

%% scomponents
% scomponents
load_gaimc_graph('cores_example'); % the graph A has three components
ci = scomponents(A)
ncomp = max(ci)               % should be 3
R = sparse(1:size(A,1),ci,1,size(A,1),ncomp); % create a restriction matrix
CG = R'*A*R;                  % create the graph with each component
                              % collapsed into a single node.

%% load_gaimc_graph
% equivalent to load('graphs/airports.mat') run from the gaimc directory
load_gaimc_graph('airports')
% equivalent to P=load('graphs/kt-7-2.mat') run from the gaimc directory
P=load_gaimc_graph('kt-7-2.mat')
% so you don't have to put the path in for examples!

%% largest_component
load_gaimc_graph('dfs_example')
[Acc p] = largest_component(A); % compute the largest component
xy2 = xy(p,:); labels2 = labels(p); % get component metadata
% draw original graph
subplot(1,2,1); graph_draw(A,xy,'labels',labels); title('Original');
% draw component
subplot(1,2,2); graph_draw(Acc,xy2,'labels',labels2); title('Component');

%% corenums
load_gaimc_graph('cores_example'); % the graph A has three components
corenums(A)

%% sparse_to_csr
A=sparse(6,6); A(1,1)=5; A(1,5)=2; A(2,3)=-1; A(4,1)=1; A(5,6)=1;
[rp ci ai]=sparse_to_csr(A);

%% csr_to_sparse
A=sparse(6,6); A(1,1)=5; A(1,5)=2; A(2,3)=-1; A(4,1)=1; A(5,6)=1;
[rp ci ai]=sparse_to_csr(A);
A2 = csr_to_sparse(rp,ci,ai);


% This is a small script to compute the Bethe approximation of a small
% factor graph. The factor graph is generated by taking the Bayes net of
% Lauritzen and Spiegelhalter (1988) and setting the following random
% variables as evidence
%
%   visit to Asia  = false
%   positive X-ray = true
%   smoking        = true
%
% For this particular set of evidence, the Bethe approximation is exact
% because the corresponding factor graph does not have any cycles.
%
% For more information, see: Lauritzen and Speigelhalter (1988). "Local
% computations with probabilities on graphical structures and their
% application to expert systems." Journal of the Royal Statistical
% Society, Series B, Vol. 50, No. 2, pp. 157-194.
%
% Copyright (C) 2007 Peter Carbonetto. All Rights Reserved.
% This code is published under the Eclipse Public License.
%
% Author: Peter Carbonetto
%         Dept. of Computer Science
%         University of British Columbia
%         May 19, 2007

verbose = true;

% The remaining random variables can either be true or false. They are:
%   1. tuberculosis
%   2. lung cancer
%   3. bronchitis
%   4. tuberculosis or lung cancer
%   5. dysponoea
nv = 5;             % The number of random variables.
nf = 6;             % The number of factors.
K  = 2*ones(nv,1);  % The number of possible discrete assignments to the
                    % random variable at each site.
C = cell(nf,1);     % The factor neighbourhoods.
f = cell(nf,1);     % The factors.

% Set up the factors.
% ------------------
% This is p(tuberculosis | visit to Asia = false).
C{1} = 1;
f{1} = [ 0.01 0.99 ]';

% This is p(lung cancer | smoking = true).
C{2} = 2;
f{2} = [ 0.1 0.9 ]';

% This is p(bronchitis | smoking = true).
C{3} = 3;
f{3} = [ 0.6 0.4 ]';

% This is p(tuberculosis or lung cancer | tuberculosis, lung cancer).
C{4}     = [4 1 2];
p        = zeros(2,2,2);
p(:,1,1) = [ 0.99 0.01 ]';
p(:,1,2) = [ 0.99 0.01 ]';
p(:,2,1) = [ 0.99 0.01 ]';
p(:,2,2) = [ 0.99 0.01 ]';
f{4}     = p;

% This is p(positive X-ray = true | tuberculosis or lung cancer).
C{5} = 4;
f{5} = [ 0.98 0.5 ]';

% This is p(dyspnoea | tuberculosis or lung cancer, bronchitis).
C{6}     = [5 4 3];
p        = zeros(2,2,2);
p(:,1,1) = [ 0.9 0.1 ]';
p(:,1,2) = [ 0.7 0.3 ]';
p(:,2,1) = [ 0.8 0.2 ]';
p(:,2,2) = [ 0.1 0.9 ]';
f{6}     = p;
clear p

% Set up the junction graph.
% -------------------------
% These are the large regions of the junction graph.
Rv{1} = [1 2 4];
Rf{1} = [1 2 4];
NR{1} = [1];

Rv{2} = [3 4 5];
Rf{2} = [3 5 6];
NR{2} = [1];

% These are the small regions of the junction graph.
Sv{1} = [4];
Sf{1} = [];
NS{1} = [1 2];

nr = length(Rv);  % The number of large regions.
ns = length(Sv);  % The number of small regions.

% Infer the region marginals.
% --------------------------
% Compute the junction graph approximation to the variational free
% energy. Since the junction graph is a tree in this case (trivially,
% because there are only two large regions), the junction graph
% approximation is exact.
qR = bopt(K,C,f,Rv,Rf,Sv,Sf,NS,verbose);

% Output the marginal probabilities.
% ---------------------------------
p = ndsum(qR{1},[2 3]);
fprintf('Pr(tuberculosis | evidence) = %0.2f \n', p(1));

p = ndsum(qR{1},[1 3]);
fprintf('Pr(lung cancer | evidence)  = %0.2f \n', p(1));

p = ndsum(qR{2},[2 3]);
fprintf('Pr(bronchitis | evidence)   = %0.2f \n', p(1));

p = ndsum(qR{2},[1 2]);
fprintf('Pr(dysponoea | evidence)    = %0.2f \n', p(1));


function [ fea, out ] = ex_convdiff3( varargin )
%EX_CONVDIFF3 1D Time dependent convection and diffusion equation example.
%
%   [ FEA, OUT ] = EX_CONVDIFF3( VARARGIN ) 1D time dependendt convection and diffusion equation on
%   a line with an infinately oscillating exact solution. Accepts the following property/value pairs.
%
%       Input       Value/{Default}        Description
%       -----------------------------------------------------------------------------------
%       w           scalar {1.5*pi}        Simulation parameter
%       U           scalar {2}             Simulation parameter
%       a           scalar {1}             Convection velocity
%       nu          scalar {0.1}           Diffusion coefficient
%       hmax        scalar {1/25}          Max grid cell size
%       dt          scalar {0.02}          Time step size
%       ischeme     scalar {3}             Time stepping scheme
%       sfun        string {sflag2}        Shape function
%       iplot       scalar 0/{1}           Plot solution (=1)
%                                                                                .
%       Output      Value/(Size)           Description
%       -----------------------------------------------------------------------------------
%       fea         struct                 Problem definition struct
%       out         struct                 Output struct

% Copyright 2013-2022 Precise Simulation, Ltd.

cOptDef = { ...
  'w',        1.5*pi; ...
  'U',        2; ...
  'a',        1; ...
  'nu',       0.1; ...
  'hmax',     1/25; ...
  'dt'        0.02; ...
  'ischeme'   3; ...
  'sfun',     'sflag1'; ...
  'iplot',    1; ...
  'tol',      3e-2; ...
  'fid',      1 };
[got,opt] = parseopt(cOptDef,varargin{:});
fid       = opt.fid;

w  = opt.w;
U  = opt.U;
a  = opt.a;
nu = opt.nu;
l1 = ['(',num2str(a),'+sqrt(',num2str(a^2),'+',num2str(4*nu*w),'*i))/',num2str(2*nu)];
l2 = ['(',num2str(a),'-sqrt(',num2str(a^2),'+',num2str(4*nu*w),'*i))/',num2str(2*nu)];
refsol  = ['(exp(',l1,'*x)-exp(',l2,'*x))/(exp(',l1,')-exp(',l2,'))*',num2str(U),'*exp(i*',num2str(w),'*t)'];
refsol0 = ['(exp(',l1,'*x)-exp(',l2,'*x))/(exp(',l1,')-exp(',l2,'))*',num2str(U)];

% Grid generation.
fea.grid = linegrid( 1/opt.hmax, 0, 1 );

% Problem definition.
fea.sdim  = { 'x' };
fea = addphys( fea, @convectiondiffusion );
fea.phys.cd.sfun = { opt.sfun };
fea.phys.cd.eqn.coef{2,4} = { opt.nu };
fea.phys.cd.eqn.coef{3,4} = { opt.a  };
fea = parsephys(fea);

% Parse and solve problem.
fea = parseprob( fea );
fea.bdr.d{1} = 0;
fea.bdr.d{2} = [num2str(U),'*cos(',num2str(w),'*t)'];
fea.bdr.n    = cell(1,2);
x = fea.grid.p';
if( strcmp( opt.sfun,'sflag2' ) )
  x = [ x; (x(2:end)+x(1:end-1))/2 ];
end
init = real( eval( refsol0 ) );
[fea.sol.u,tlist] = solvetime( fea, 'fid', fid, 'init', init, 'ischeme', opt.ischeme, 'tstep', opt.dt, 'tmax', 1 );

% Postprocessing.
if( opt.iplot>0 )
  figure;
  if( opt.iplot>1 )
    i_sol_list = 1:numel(tlist);
  else
    i_sol_list = numel(tlist);
  end
  [~,ix] = sort( x );
  for i_sol=i_sol_list
    t = tlist(i_sol);
    clf
    postplot( fea, 'surfexpr', 'c', 'solnum', i_sol );
    hold on
    u_r = real( eval( refsol ) );
    plot( sort(x), u_r(ix), 'r--' );
    title( ['Solution at time ',num2str(t)])
    xlabel( 'x' )
    drawnow
  end
end

% Error checking.
for i_sol=1:numel(tlist)
  u_i = fea.sol.u(:,i_sol);
  t   = tlist(i_sol);
  u_r = real( eval( refsol ) );
  errnm(i_sol) = norm( u_i - u_r )/norm( u_r );
end
out.err  = errnm;
out.pass = all( errnm<opt.tol );

if ( nargout==0 )
  clear fea out
end


function freq_grid = fftshiftfreqgrid(N,Fs)
%fftshiftfreqgrid Generate frequency grid

freq_res = Fs/N;
freq_grid = (0:N-1).'*freq_res;
Nyq = Fs/2;
half_res = freq_res/2;
if rem(N,2) % odd
    idx = 1:(N-1)/2;
    halfpts = (N+1)/2;
    freq_grid(halfpts) = Nyq-half_res;
    freq_grid(halfpts+1) = Nyq+half_res;
else
    idx = 1:N/2;
    hafpts = N/2+1;
    freq_grid(hafpts) = Nyq;
end

freq_grid = fftshift(freq_grid);
freq_grid(idx) = freq_grid(idx)-Fs;

end


% SubgradientOpt.m // Jon Lee
%
% Apply Subgradient Optimization to
%   z = min c'x
%        Ex  = h
%        Ax  = b
%         x >= 0
%
% NOTE: WE ONLY HANDLE PROBLEMS WITH BOUNDED SUBPROBLEM LPs.
%       IF WE ENCOUNTER AN UNBOUNDED SUBPROBLEM, THEN WE QUIT.

% generate a random example
n = 50; % number of variables
m1 = 15; % number of equations to relax
m2 = 10; % number of equations to keep

rng('default');
rng(1);  % set seed
E = rand(m1,n);  % random m1-by-n matrix
A = rand(m2,n);  % random m2-by-n matrix

w = rand(n,1);     % generate rhs's so problem is feasible
h = E*w;
b = A*w;

c = rand(n,1);     % generate random objective
% first we will calculate the optimal value z of the original LP, just to
% compare later
disp('Optimizing the original LP without Lagrangian relaxation');
[x,z,exitflag] = linprog(c,[],[],[E; A],[h; b],zeros(n,1),[]);
if (exitflag < 1)
     disp('fail 1: original LP (without using LR) did not solve correctly');
     return;
end;

disp('Optimal value of the original LP:');
z

disp('Starting Subgradient Optimization');

MAXIT = 100; % number of iterations

results1 = zeros(MAXIT,1);
results2 = zeros(MAXIT,1);

k = 0; % iteration counter
y = zeros(m1,1); % initialize y as you like
g = zeros(m1,1); % initialize g arbitrarily --- it will essentially be ignored
stepsize = 0;    % just to have us really start at the initialized y
bestlb = -Inf;
while k < MAXIT
   k = k + 1; % increment the iteration counter
   y = y + stepsize*g; % take a step in the direction of the subgradient
   % solve the Lagrangian
   [x,subval,exitflag,output,dualsol] = linprog((c'-y'*E)',[],[],A,b,zeros(n,1),[]);
   v = h'*y + (c'-y'*E)*x;  % calculate the value of the Lagrangian
   disp(v);
   bestlb = max(bestlb,v);
   results1(k)=k;
   results2(k)=v;
   g = h - E*x; % calculate the subgradient
   stepsize = 1/k; % calculate the step size
end

bestlb

z

clf;  % clear figure
% plot the sequence of MAXIT lower bounds
plot(results1,results2,'k--.','MarkerSize',15);
hold on;
% plot a horizontal line at height z
plot([1,MAXIT],[ z z ], 'r-','LineWidth',2.5)
axis tight;
xlabel('Iteration');
ylabel('Lagrangian lower bound');
legend('Lagrangian LB','z','Location','SouthEast');

print -djpeg SubgradientOpt.jpeg;

% Next, if y really solves the Lagrangian dual, and pi is the
% optimal solution to the Lagrangian subproblem corresponding to y,
% then y and pi should be an optimal dual solution to the given problem.
%
% Let's see if y and pi are nearly dual feasible.

pi = - dualsol.eqlin;
Total_Dual_Infeasibility = norm(min(c' - y'*E - pi'*A,zeros(1,n)))