%% Rotation Demonstration: Discontinous Data
%
% A demonstration of how the timestep affects the error of the splitting
% method for a rotation. Similar results can be expected in general,
% although nonlinear sharpening may give smaller errors.
%
% The equation we solve is
%
% $$u_t - y u_x + x u_y = 0, \qquad u(x,y,0)=u_0(x,y)$$
%
% where the initial function u0 is discontinuous. To solve the equation, we
% will use Godunov splitting and front tracking to approximate the 1-D
% solution operators.

%% Initial setup
T     = 4*pi;
xmin  = -1.5; xmax=1.5;
N     = 128;
h     = (xmax-xmin)/N;
x     = xmin+h*(0:N);
y     = 0.5*(x(1:end-1)+x(2:end));
[X,Y] = meshgrid(y,y);
u0    = adiscontnuous_function(X,Y);
surfl(X,Y,u0); shading interp, view(-15,60), colormap(gray), axis tight

%% Error versus CFL number
% First we study the pointwise and the L1 error for CFL numbers 2.^(0:5)
colormap(jet)
for i=1:6
	nu=2^(i-1);
	err= abs( rotrack(u0,y,y,nu,T) - u0);
   subplot(2,3,i), pcolor(y,y,err), axis equal image, shading interp;
   set(colorbar('West'),'XColor',[1 1 1],'YColor',[1 1 1]);
   caxis([0 2]), title(['CFL = ', num2str(nu)]);
   set(gca,'XTick',[],'YTick',[])
   xlabel(['L1 error: ', num2str(h*h*sum(abs(err(:))))]);
	drawnow;
end;
%%
% The error is determined by two error mechanisms that work in opposite
% directions. The splitting error increases with increasing splitting
% steps, whereas the smoothing error caused by the projection operator
% increases with decreasing splitting steps. The total splitting error is
% therefore a convex function of the time step, and in the figure above,
% the minimum error is observed for CFL number 16.

%% Testing convergence
% We choose the best CFL number vu=16, and check for convergence rate.
% To this end, we use only one lap (it takes too long otherwise).
T    = 2*pi;
Lerr = zeros(1,7);
wbar = waitbar(0,'Convergence study');
for i=1:7,
	N    = 2^(i+3);
	h    = (xmax-xmin)/N; x=xmin+h*(1:N);
	[X,Y]=meshgrid(x,x);
	u0   = adiscontnuous_function(X,Y);
	err  = abs(rotrack(u0,x,x,nu,T) - u0);
	Lerr(i)=sum(err(:)) / sum(abs(u0(:)));
	waitbar(i/7,wbar);
end;
close(wbar);
clf, semilogy(1:7,Lerr,'o--');
set(gca,'XTick',1:7,'XTickLabel',2.^((1:7)+3))
xlabel('Grid size'); ylabel('Log(error)'); axis tight
title('Errors for \nu = 16');


% Chapter 3 - Complex Iterative Maps.
% Program 3b - The Mandelbrot Set.
% Thanks to Steve Lord from The MathWorks for his help.
% Copyright Birkhauser 2013. Stephen Lynch.

% Vectorized program.
% Plot the Mandelbrot set in black and white (Figure 3.2).
Nmax = 50; scale = 0.005;
xmin = -2.4; xmax  = 1.2;
ymin = -1.5; ymax  = 1.5;

% Generate x and y coordinates and z complex values
[x,y]=meshgrid(xmin:scale:xmax,ymin:scale:ymax);
z = x+1i*y;

% Generate w accumulation matrix and k counting matrix
w = zeros(size(z));
k = zeros(size(z));

N = 0;
while N<Nmax && ~all(k(:))
    w = w.^2+z;
    N = N+1;
    k(~k & abs(w)>4) = N;
end
k(k==0) = Nmax;
figure
s = pcolor(x, y, mod(k, 2));
colormap([0 0 0;1 1 1])
set(s,'edgecolor','none')

axis([xmin xmax -ymax ymax])
fsize=15;
set(gca,'XTick',xmin:0.4:xmax,'FontSize',fsize)
set(gca,'YTick',-ymax:0.5:ymax,'FontSize',fsize)
xlabel('Re z','FontSize',fsize)
ylabel('Im z','FontSize',fsize)
% End of Program 3b


function value = r8_cin ( x )

%*****************************************************************************80
%
%% R8_CIN evaluates the alternate cosine integral Cin of an R8 argument.
%
%  Discussion:
%
%    CIN(X) = gamma + log(X)
%      + integral ( 0 <= T <= X ) ( cos ( T ) - 1 ) / T  dT
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 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, real X, the argument.
%
%    Output, real VALUE, the cosine integral Cin evaluated at X.
%
  persistent cincs
  persistent eul
  persistent ncin
  persistent xmin

  eul = 0.57721566490153286060651209008240;

  if ( isempty ( ncin ) )

    cincs = [ ...
      0.37074501750909688741654801228564992, ...
     -0.05893574896364446831956864397363697, ...
      0.00538189642113569124048745326203340, ...
     -0.00029860052841962135319594906563410, ...
      0.00001095572575321620077031054467306, ...
     -0.00000028405454877346630491727187731, ...
      0.00000000546973994875384912457861806, ...
     -0.00000000008124187461318157083277452, ...
      0.00000000000095868593117706609013181, ...
     -0.00000000000000920266004392351031377, ...
      0.00000000000000007325887999017895024, ...
     -0.00000000000000000049143726675842909, ...
      0.00000000000000000000281577746753902, ...
     -0.00000000000000000000001393986788501, ...
      0.00000000000000000000000006022485646, ...
     -0.00000000000000000000000000022904717, ...
      0.00000000000000000000000000000077273, ...
     -0.00000000000000000000000000000000233 ]';

    ncin = r8_inits ( cincs, 18, 0.1 * r8_mach ( 3 ) );
    xmin = sqrt ( r8_mach ( 1 ) );

  end

  absx = abs ( x );

  if ( absx <= xmin )
    value = 0.0;
  elseif ( absx <= 4.0 )
    value = r8_csevl ( ( x * x - 8.0 ) * 0.125, cincs, ncin ) * x * x;
  else
    [ f, g ] = r8_sifg ( absx );
    sinx = sin ( absx );
    value = - f * sinx + g * cos ( absx ) + log ( absx ) + eul;
  end

  return
end


function [ xstar, seed ] = rk2_tv_step ( x, t, h, q, fv, gv, seed )

%*****************************************************************************80
%
%% RK2_TV_STEP takes one step of a stochastic Runge Kutta scheme.
%
%  Discussion:
%
%    The Runge-Kutta scheme is second-order, and suitable for time-varying
%    systems.
%
%    d/dx X(t,xsi) = F ( X(t,xsi), t ) + G ( X(t,xsi), t ) * w(t,xsi)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    21 June 2010
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Jeremy Kasdin,
%    Runge-Kutta algorithm for the numerical integration of
%    stochastic differential equations,
%    Journal of Guidance, Control, and Dynamics,
%    Volume 18, Number 1, January-February 1995, pages 114-120.
%
%    Jeremy Kasdin,
%    Discrete Simulation of Colored Noise and Stochastic Processes
%    and 1/f^a Power Law Noise Generation,
%    Proceedings of the IEEE,
%    Volume 83, Number 5, 1995, pages 802-827.
%
%  Parameters:
%
%    Input, real X, the value at the current time.
%
%    Input, real T, the current time.
%
%    Input, real H, the time step.
%
%    Input, real Q, the spectral density of the input white noise.
%
%    Input, external real FV, the name of the deterministic
%    right hand side function.
%
%    Input, external real GV, the name of the stochastic
%    right hand side function.
%
%    Input/output, integer SEED, a seed for the random
%    number generator.
%
%    Output, real XSTAR, the value at time T+H.
%
  a21 = 1.0;
  a31 = 0.5;
  a32 = 0.5;

  q1 = 2.0;
  q2 = 2.0;

  t1 = t;
  x1 = x;
  [ n1, seed ] = r8_normal_01 ( seed );
  w1 = n1 * sqrt ( q1 * q / h );
  k1 = h * fv ( t1, x1 ) + h * gv ( t1, x1 ) * w1;

  t2 = t1 + a21 * h;
  x2 = x1 + a21 * k1;
  [ n2, seed ] = r8_normal_01 ( seed );
  w2 = n2 * sqrt ( q2 * q / h );
  k2 = h * fv ( t2, x2 ) + h * gv ( t2, x2 ) * w2;

  xstar = x1 + a31 * k1 + a32 * k2;

  return
end


function fluxlimiter(N, method)
% N = number of grid points

% grid spacing on interval (-1,1)
dx = 2./N;

% location of cell centers
x = linspace(-1+0.5*dx, 1-0.5*dx, N);

% advection speed
u = 1;

% cfl condition + safety margin
dt = 0.25*dx;

% final time
FinalTime = 2;
Nsteps = ceil(FinalTime/dt);
dt = FinalTime/Nsteps;
lam = u*dt/dx;

% initial condition
q = fluxlimiterexact(x);

% time step
for n=1:Nsteps

  qp1  = [q(2:N),q(1)];    % q_{m+1}
  qm1 = [q(N),q(1:N-1)]; % q_{m-1}
  qm2 = [qm1(N), qm1(1:N-1)]; % q_{m-2}

  % compute limited cell average jumps based on method choice
  % theta_{i-1/2} = (q_{i-1}-q_{i-2})/(q_{i}-q_{i-1})
  tol = 1e-10;
  dq1 = q-qm1;
  dq2 = qm1-qm2;
  % have to test in case dq1 is small because of finite precision effects

  % are both dq1, and dq2 small (smooth)
  ids = find(abs(dq1)+abs(dq2)<tol); % both jumps small
  thetaL(ids) = 1;

  % is dq1 small and dq2 relatively large
  ids = find(abs(dq1)<tol & abs(dq2)>tol );
  thetaL(ids) = 100; % choose theta large

  % hopefully the remainder
  ids = find(abs(dq1)>= tol);
  thetaL(ids) = dq2(ids)./dq1(ids);

  if(strcmp(method,'minmod'))
      phiL = minmod(1, thetaL);
  elseif(strcmp(method,'MC'))
      phiL = max(0, min(min(0.5*(1+thetaL),2), 2*thetaL));
  elseif(strcmp(method,'vanleer'))
      phiL = (thetaL+abs(thetaL))./(1+abs(thetaL));
  elseif(strcmp(method,'superbee'))
      phiL = max(0, max(min(1,2*thetaL), min(2,thetaL)));
  elseif(strcmp(method,'Fromm'))
      phiL = 0.5*(1+thetaL);
  elseif(strcmp(method,'BeamWarming'))
      phiL = thetaL; % upwind
  elseif(strcmp(method,'LaxWendroff'))
      phiL = ones(size(q)); % downwind
  elseif(strcmp(method, 'upwind'))
      phiL = zeros(size(q));
  elseif(strcmp(method, 'custom'))

      phiL = customphi(thetaL);
  else
      disp('WARNING: wrong choice of flux limiter');
  end
  phiR = [phiL(2:N), phiL(1)];

  % update cell averages
  q = q - lam*(q-qm1) - 0.5*lam*(1-lam)*(phiR.*(qp1-q) -phiL.*(q-qm1));

  if(mod(n,40)==0)
    plot(x,q, '.');
    hold on; plot(x-0.5*dx, phiL, 'g-'); hold off;pause(0.02);
  end
end

plot(x, q, 'b.');
% have gone one exact period
hold on; plot(x, fluxlimiterexact(x), 'r-'); hold off;
hold on; plot(x-0.5*dx, phiL, 'g-'); hold off;
xlabel('x');
legend(sprintf('%s N=%d T=%f', method, N, FinalTime), ...
           'Exact solution', sprintf('flux limiter function: phi_{%s}', method));
axis([-1 1 -1 3])


% hohmann.m          July 9, 2013

% Hohmann two impulse orbit transfer between
% planar and non-coplanar circular orbits

% includes three-dimensional orbit graphics
% and graphical primer vector analysis

% Orbital Mechanics with MATLAB

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

global rtd dtr pvi pvdi

global mu req hn1 hn2 hn3 dinc

% astrodynamic and utility constants

om_constants;

% Brent root-finding tolerance

rtol = 1.0e-8;

% request inputs

clc; home;

fprintf('\nHohmann Orbit Transfer Analysis\n');

while (1)

    fprintf('\n\nplease input the initial altitude (kilometers)\n');

    alt1 = input('? ');

    if (alt1 > 0.0)
        break;
    end

end

while (1)

    fprintf('\n\nplease input the final altitude (kilometers)\n');

    alt2 = input('? ');

    if (alt2 > 0.0)
        break;
    end

end

while (1)

    fprintf('\n\nplease input the initial orbital inclination (degrees)');
    fprintf('\n(0 <= inclination <= 180)\n');

    inc1 = input('? ');

    if (inc1 >= 0.0 && inc1 <= 180.0)
        break;
    end

end

while (1)

    fprintf('\n\nplease input the final orbital inclination (degrees)');
    fprintf('\n(0 <= inclination <= 180)\n');

    inc2 = input('? ');

    if (inc2 >= 0.0 && inc2 <= 180.0)
        break;
    end

end

% convert orbit inclinations to radians

inc1 = inc1 * dtr;

inc2 = inc2 * dtr;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% solve the orbit transfer problem %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% calculate total inclination change (radians)

dinc = abs(inc2 - inc1);

% compute geocentric radii of initial and final orbits (kilometers)

r1 = req + alt1;

r2 = req + alt2;

% compute "normalized" radii

hn1 = sqrt(2.0 * r2 / (r2 + r1));

hn2 = sqrt(r1 / r2);

hn3 = sqrt(2.0 * r1 / (r2 + r1));

% compute "local circular velocity" of initial and final orbits (km/sec)

v1 = sqrt(mu / r1);

v2 = sqrt(mu / r2);

% compute transfer orbit semimajor axis (kilometers)

smat = 0.5 * (r1 + r2);

% compute transfer orbit eccentricity (non-dimensional)

ecct = (max(r1, r2) - min(r1, r2)) / (r1 + r2);

% compute transfer orbit perigee and apogee radii and velocities

rp = smat * (1.0 - ecct);

ra = smat * (1.0 + ecct);

vt1 = sqrt(2.0 * mu * ra / (rp * (rp + ra)));

vt2 = sqrt(2.0 * mu * rp / (ra * (rp + ra)));

% compute transfer orbit period (seconds)

taut = 2.0 * pi * sqrt(smat^3 / mu);

tof = 0.5 * taut;

if (abs(dinc) == 0)

    % coplanar orbit transfer

    if (r2 > r1)

        % higher-to-lower transfer

        dv1 = vt1 - v1;

        dv2 = v2 - vt2;

    else

        % lower-to-higher transfer

        dv1 = v1 - vt2;

        dv2 = vt1 - v2;

    end

    dinc1 = 0;

    dinc2 = 0;

    inct = inc1;

else

    % non-coplanar orbit transfer

    [xroot, froot] = brent('hohmfunc', 0, dinc, rtol);

    % calculate delta-v's

    dinc1 = xroot;

    dinc2 = dinc - dinc1;

    dv1 = v1 * sqrt(1.0 + hn1 * hn1 - 2.0 * hn1 * cos(dinc1));

    dv2 = v1 * sqrt(hn2 * hn2 * hn3 * hn3 + hn2 * hn2 ...
        - 2.0 * hn2 * hn2 * hn3 * cos(dinc2));

    if (inc2 > inc1)

        inct = inc1 + dinc1;

    else

        inct = inc1 - dinc1;

    end

end

% print results

clc; home;

fprintf('\nHohmann Orbit Transfer Analysis');
fprintf('\n-------------------------------\n\n');

fprintf('initial orbit altitude            %10.4f kilometers \n\n', alt1);

fprintf('initial orbit radius              %10.4f kilometers \n\n', alt1 + req);

fprintf('initial orbit inclination         %10.4f degrees \n\n', inc1 * rtd);

fprintf('initial orbit velocity            %10.4f meters/second \n\n\n', 1000.0 * v1);

fprintf('final orbit altitude              %10.4f kilometers \n\n', alt2);

fprintf('final orbit radius                %10.4f kilometers \n\n', alt2 + req);

fprintf('final orbit inclination           %10.4f degrees \n\n', inc2 * rtd);

fprintf('final orbit velocity              %10.4f meters/second \n', 1000.0 * v2);

fprintf('\n\nfirst inclination change          %10.4f degrees\n\n', dinc1 * rtd);

fprintf('second inclination change         %10.4f degrees\n\n', dinc2 * rtd);

fprintf('total inclination change          %10.4f degrees\n\n\n', rtd * (dinc1 + dinc2));

fprintf('first delta-v                     %10.4f meters/second \n\n', 1000.0 * dv1);

fprintf('second delta-v                    %10.4f meters/second \n\n', 1000.0 * dv2);

fprintf('total delta-v                     %10.4f meters/second \n\n\n', 1000.0 * (dv1 + dv2));

fprintf('transfer orbit semimajor axis     %10.4f kilometers \n\n', smat);

fprintf('transfer orbit eccentricity       %10.8f \n\n', ecct);

fprintf('transfer orbit inclination        %10.4f degrees \n\n', rtd * inct);

fprintf('transfer orbit perigee velocity   %10.4f meters/second \n\n', 1000.0 * vt1);

fprintf('transfer orbit apogee velocity    %10.4f meters/second \n\n', 1000.0 * vt2);

fprintf('transfer orbit coast time         %10.4f seconds \n', tof);

fprintf('                                  %10.4f minutes \n', tof / 60.0);

fprintf('                                  %10.4f hours \n\n', tof / 3600.0);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% create trajectory graphics %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% load orbital elements arrays, create state vectors and plot orbits

oevi(1) = r1;
oevi(2) = 0.0;
oevi(3) = inc1;
oevi(4) = 0.0;
oevi(5) = 0.0;

% determine correct true anomaly (radians)

if (alt2 > alt1)

    oevi(6) = 0.0;

else

    oevi(6) = 180.0 * dtr;

end

[ri, vi] = orb2eci(mu, oevi);

oevti(1) = smat;
oevti(2) = ecct;
oevti(3) = inct;
oevti(4) = 0.0;
oevti(5) = 0.0;

% determine correct true anomaly (radians)

if (alt2 > alt1)

    oevti(6) = 0.0;

else

    oevti(6) = 180.0 * dtr;

end

[rti, vti] = orb2eci(mu, oevti);

oevtf(1) = smat;
oevtf(2) = ecct;
oevtf(3) = inct;
oevtf(4) = 0.0;
oevtf(5) = 0.0;

% determine correct true anomaly (radians)

if (alt2 > alt1)

    oevtf(6) = 180.0 * dtr;

else

    oevtf(6) = 0.0;

end

[rtf, vtf] = orb2eci(mu, oevtf);

oevf(1) = r2;
oevf(2) = 0.0;
oevf(3) = inc2;
oevf(4) = 0.0;
oevf(5) = 0.0;

% determine correct true anomaly (radians)

if (alt2 > alt1)

    oevf(6) = 180.0 * dtr;

else

    oevf(6) = 0.0;

end

[rf, vf] = orb2eci(mu, oevf);

% compute orbital periods

period1 = 2.0 * pi * oevi(1) * sqrt(oevi(1) / mu);

period2 = 2.0 * pi * oevti(1) * sqrt(oevti(1) / mu);

period3 = 2.0 * pi * oevf(1) * sqrt(oevf(1) / mu);

deltat1 = period1 / 300;

simtime1 = -deltat1;

deltat2 = 0.5 * period2 / 300;

simtime2 = -deltat2;

deltat3 = period3 / 300;

simtime3 = -deltat3;

for i = 1:1:301

    simtime1 = simtime1 + deltat1;

    simtime2 = simtime2 + deltat2;

    simtime3 = simtime3 + deltat3;

    % compute initial orbit "normalized" position vector

    [rwrk, vwrk] = twobody2 (mu, simtime1, ri, vi);

    rp1_x(i) = rwrk(1) / req;

    rp1_y(i) = rwrk(2) / req;

    rp1_z(i) = rwrk(3) / req;

    % compute transfer orbit position vector

    [rwrk, vwrk] = twobody2 (mu, simtime2, rti, vti);

    rp2_x(i) = rwrk(1) / req;

    rp2_y(i) = rwrk(2) / req;

    rp2_z(i) = rwrk(3) / req;

    % compute final orbit position vector

    [rwrk, vwrk] = twobody2 (mu, simtime3, rf, vf);

    rp3_x(i) = rwrk(1) / req;

    rp3_y(i) = rwrk(2) / req;

    rp3_z(i) = rwrk(3) / req;

end

figure(1);

% create axes vectors

xaxisx = [1 1.5];
xaxisy = [0 0];
xaxisz = [0 0];

yaxisx = [0 0];
yaxisy = [1 1.5];
yaxisz = [0 0];

zaxisx = [0 0];
zaxisy = [0 0];
zaxisz = [1 1.5];

figure (1);

hold on;

grid on;

% plot earth

[x y z] = sphere(24);

h = surf(x, y, z);

colormap([127/255 1 222/255]);

set (h, 'edgecolor', [1 1 1]);

% plot coordinate system axes

plot3(xaxisx, xaxisy, xaxisz, '-g', 'LineWidth', 1);

plot3(yaxisx, yaxisy, yaxisz, '-r', 'LineWidth', 1);

plot3(zaxisx, zaxisy, zaxisz, '-b', 'LineWidth', 1);

% plot initial orbit

plot3(rp1_x, rp1_y, rp1_z, '-r', 'LineWidth', 1.5);

plot3(rp1_x(1), rp1_y(1), rp1_z(1), 'ob');

% plot transfer orbit

plot3(rp2_x, rp2_y, rp2_z, '-b', 'LineWidth', 1.5);

plot3(rp2_x(end), rp2_y(end), rp2_z(end), 'ob');

% plot final orbit

plot3(rp3_x, rp3_y, rp3_z, '-g', 'LineWidth', 1.5);

xlabel('X coordinate (ER)', 'FontSize', 12);

ylabel('Y coordinate (ER)', 'FontSize', 12);

zlabel('Z coordinate (ER)', 'FontSize', 12);

title('Hohmann Transfer: Initial, Transfer and Final Orbits', 'FontSize', 16);

axis equal;

view(50, 20);

rotate3d on;

print -depsc -tiff -r300 hohmann1.eps

%%%%%%%%%%%%%%%%%%%%%%%%
% create primer graphics
%%%%%%%%%%%%%%%%%%%%%%%%

dvi = (vti - vi)';

dvf = (vf - vtf)';

% perform primer vector initialization

pviniz(tof, rti, vti, dvi, dvf);

% number of graphic data points

npts = 300;

% plot behavior of primer vector magnitude

dt = tof / npts;

for i = 1:1:npts + 1

    t = (i - 1) * dt;

    if (t == 0)

        % initial value of primer magnitude and derivative

        pvm = norm(pvi);

        pvdm = dot(pvi, pvdi) / pvm;

    else

        % primer vector and derivative magnitudes at time t

        [pvm, pvdm] = pvector(rti, vti, t);

    end

    % load data array

    x1(i) = t / 60.0;

    y1(i) = pvm;

    y2(i) = pvdm;

end

figure(2);

hold on;

plot(x1, y1, '-r', 'LineWidth', 1.5);

plot(x1(1), y1(1), 'or');

plot(x1(end), y1(end), 'or');

title('Primer Vector Analysis of the Hohmann Transfer', 'FontSize', 16);

xlabel('simulation time (minutes)', 'FontSize', 12);

ylabel('primer vector magnitude', 'FontSize', 12);

grid;

% create eps graphics file with tiff preview

print -depsc -tiff -r300 primer.eps;

% plot behavior of magnitude of primer derivative

figure(3);

hold on;

plot(x1, y2, '-r', 'LineWidth', 1.5);

plot(x1(1), y2(1), 'or');

plot(x1(end), y2(end), 'or');

title('Primer Vector Analysis of the Hohmann Transfer', 'FontSize', 16);

xlabel('simulation time (minutes)', 'FontSize', 12);

ylabel('primer derivative magnitude', 'FontSize', 12);

grid;

% create eps graphics file with tiff preview

print -depsc -tiff -r300 primer_der.eps;


function [theta] = normalEqn(X, y)
    %% NORMALEQN Computes the closed-form solution to linear regression
    %   NORMALEQN(X,y) computes the closed-form solution to linear
    %   regression using the normal equations.
    theta = pinv(X' * X) * X' * y;
end


%MAIN_singlePendulum.m
%
% This script runs a simple simulation of a single pendulum

% Use: EoM_Single_Pendulum to write the equations of motion

m = 1.0;  % (kg) pendulum mass
g = 9.81; % (m/s^2) gravity
l = 1.0; % (m) pendulum length

tSpan = [0,10]; %Simulation time interval

th0 = (pi/180)*(-178);  %Initial angle
w0 = 0;  %Initial angular rate
z0 = [th0;w0];

userFunc = @(t,z)singlePendulumRhs(t,z,g,l);

options = odeset(...
    'AbsTol',1e-8,...
    'RelTol',1e-8,...
    'Vectorized','on');

% Run the simulation!
sol = ode45(userFunc,tSpan,z0,options);

% Break apart solution for plotting
nPlot = 1000;
time = linspace(tSpan(1),tSpan(2),nPlot);
z = deval(sol,time); %Evaluate solution from ode45 at points in time
th = z(1,:);
w = z(2,:);
[energy, kinetic, potential] = singlePendulumEnergy(th,w,m,g,l);

% Plotting!

figure(1111);clf;

subplot(3,1,1);
plot(time,th,'k-','LineWidth',2);
xlabel('time (s)')
ylabel('angle (rad)');

subplot(3,1,2);
plot(time,w,'k-','LineWidth',2);
xlabel('time (s)')
ylabel('angle rate (rad/s)');

subplot(3,1,3);  hold on;
plot(time,energy,'k-','LineWidth',3);
plot(time,kinetic,'r-','LineWidth',2);
plot(time,potential,'b-','LineWidth',2);
xlabel('time (s)')
ylabel('energy (J)');
legend('total','kinetic','potential');


function [err, C] = gen_pos_err(std, n, type, absolute)
%GEN_POS_ERR Generates position errors. We first generate i.i.d. position errors
%for each sensor. We use the first sensor as the reference sensor and substract
%the the position error of the first sensor from the remaining sensors.
%Therefore, the resulting covariance matrix is not diagonal.
%Inputs:
%   std - Standard deviation of the position errors.
%   n - Number of sensors.
%   type - 'Gaussian' or 'Uniform'.
%   absolute - If set to true, will not use the first sensor as the reference
%              sensor and the position errors will be i.i.d.
%Outputs:
%   err - 2xn vector of position errors.
%   C - Covariance matrix. (2n-2) x (2n-2) if absolute is false, (2n) x (2n) if
%       absolute is true.
if nargin < 4
    absolute = false;
end
switch lower(type)
    case 'gaussian'
        err = randn(2, n)*std;
        if nargout > 1
            if absolute
                C = std^2*eye(n + n);
            else
                C = std^2*(eye(n - 1) + ones(n - 1));
                C = blkdiag(C, C);
            end
        end
    case 'uniform'
        b = sqrt(3)*std;
        err = unifrnd(-b, b, 2, n);
        if nargout > 1
            if absolute
                C = std^2*eye(n + n);
            else
                C = std^2*(eye(n - 1) + ones(n - 1));
                C = blkdiag(C, C);
            end
        end
    otherwise
        error('Unknow type "%s".', type);
end
if ~absolute
    err(1,2:end) = err(1,2:end) - err(1,1);
    err(2,2:end) = err(2,2:end) - err(2,1);
    err(1,1) = 0;
    err(2,1) = 0;
end
end


function Zf = filt2(Z,res,lambda,filtertype)
% filt2 performs a highpass, lowpass, bandpass, or bandstop 2D gaussian filter on gridded data such as
% topographic, atmospheric, oceanographic, or any kind of geospatial data. This function is designed to
% make it easy to remove features longer or shorter than a given characteristic wavelength. The
% input grid can contain NaNs!
%
%% Syntax
%
%  Zf = filt2(Z,res,lambda,filtertype)
%
%% Description
%
% Zf = filt2(Z,res,lambda,filtertype) filters 2D dataset Z that has resolution res,
% to an approximate wavelength lambda.  If the filtertype is 'lp' or 'hp' for lowpass
% or highpass, lambda must be a scalar value.  If the filtertype is 'bp' or 'bs' for
% bandpass or bandstop, lambda must be a two-element array of the two cutoff wavelengths.
%
%% Explanation of this type of filter
% For an explanation of this filter with pictures, type
%
%   showdemo filt2_documentation
%
% into your command window.
%
%% Example
% Consider a 100 km by 100 km elevation dataset that has a resolution of 200 m.  It has
% some long 25 km wavelength features aligned with north/south direction, some short
% ~5 km features oriented diagonally, and a considerable amount of random noise.  There's
% also a block of missing data. Here's what your datset looks like:
%
% res = 0.2; % 200 m resolution
% x = 0:res:100; % eastings from 0 to 100 km
% y = 0:res:100; % northings from 0 to 100 km
% [X,Y] = meshgrid(x,y);
%
% % Z contains 25 km features, ~5 km diagonal features, and noise:
% Z = cos(2*pi*X/25)+cos(2*pi*(X+Y)/7)+randn(size(X));
%
% % Z also has some missing data:
% Z(100:120,100:120) = nan;
%
% subplot(1,3,1)
% imagesc(x,y,Z);
% axis xy image
% caxis([-1 1])
% title ' original Z matrix '
% xlabel ' eastings (km) '
% ylabel ' northings (km) '
%
% % Get the lowpass-filtered version of Z:
% Zlow = filt2(Z,res,15,'lp');
%
% subplot(1,3,2)
% imagesc(x,y,Zlow);
% axis xy image
% caxis([-1 1])
% title ' 15 km lowpass filtered Z matrix '
% xlabel ' eastings (km) '
% ylabel ' northings (km) '
%
% % Get the highpass-filtered version of Z:
% Zhi = filt2(Z,res,15,'hp');
%
% subplot(1,3,3)
% imagesc(x,y,Zhi);
% axis xy image
% caxis([-1 1])
% title ' 15 km highpass filtered Z matrix '
% xlabel ' eastings (km) '
% ylabel ' northings (km) '
%
%% Author Info
% This function was written by Chad A. Greene of the University of Texas Institute for
% Geophysics in November 2016; however, all I did was repackage Carlos Adrian Vargas Aguilera's
% superb ndnanfilter function, which can be found here: http://www.mathworks.com/matlabcentral/fileexchange/20417.
% Many thanks to Carlos for his well-thought-out code and clear documentation.
%
% See also conv2, imgaussfilt, and imfilter.

%% Input checks

narginchk(4,4)
% assert(license('test','image_toolbox')==1,'Error: I''m sorry, the filt2 function requires the Image Processing Toolbox.')
assert(ismatrix(Z)==1,'Input error: Z must be a 2d matrix.')
%assert(isscalar(res)==1,'Input error: res must be a scalar value.')
assert(ismember(lower(filtertype),{'lp','hp','bp','bs'}),'Input error: filtertype must be ''hp'', ''lp'', ''bp'', or ''bs''.')
if lambda<=(2*max(res))
   warning('Nyquist says the wavelength should exceed two times the resolution of the dataset, which is an unmet condition based on these inputs. I''ll give you some numbers, but I would''t trust ''em if I were you.')
end

if ismember(lower(filtertype),{'bp','bs'})
   assert(numel(lambda)==2,'Input error: Wavelength lambda must be a two-element array for a bandpass filter.')
else
   assert(isscalar(lambda)==1,'Input error: Wavelength lambda must be a scalar for lowpass or bandpass filters.')
end

%% Design filter:

% 2*pi*sigma is the wavelength at which the amplitude is multiplied by a factor of about 0.6 (more exactly, exp(-0.5))
sigma = (lambda(1)./res) /(2*pi);
%f = fspecial('gaussian',2*ceil(2.6*sigma)+1,sigma);
% WJP implementation without image processing toolbox, subfunc at bottom of
% this function
f = gaussian2D([2*ceil(2.6*sigma(1))+1 2*ceil(2.6*sigma(end))+1], sigma);

%% Now filter the data

switch lower(filtertype)
   %WJP incorrect usage originally but happened to work if imfilter is available
   case 'lp'
      Zf = ndnanfilter(Z,f,size(f),[],[],{'replicate'}); % ndnanfilter is Carlos Adrian Vargas Aguilera's excellent function, which is included as a subfunction below.

   case 'hp'
      Zf = Z - ndnanfilter(Z,f,size(f),[],[],{'replicate'});

   case 'bp'
      Zf =  filt2(filt2(Z,res,max(lambda),'hp'),res,min(lambda),'lp');

   case 'bs'
      Zf = filt2(Z,res,max(lambda),'lp') - filt2(Z,res,min(lambda),'hp');

   otherwise
      error('No such filter type.')
end

end

function [Y,W] = ndnanfilter(X,HWIN,F,DIM,WINOPT,PADOPT,WNAN)
% NDNANFILTER   N-dimensional zero-phase digital filter, ignoring NaNs.
%
%   Syntax:
%         Y = ndnanfilter(X,HWIN,F);
%         Y = ndnanfilter(X,HWIN,F,DIM);
%         Y = ndnanfilter(X,HWIN,F,DIM,WINOPT);
%         Y = ndnanfilter(X,HWIN,F,DIM,WINOPT,PADOPT);
%         Y = ndnanfilter(X,HWIN,F,DIM,WINOPT,PADOPT,WNAN);
%     [Y,W] = ndnanfilter(...);
%
%   Input:
%     X      - Data to be filtered with/without NaNs.
%     HWIN   - Window function handle (or name) or numeric multidimensional
%              window to be used (without NaNs). See WINDOW for details.
%              Default:   @rectwin  or 'rectwin' (moving average).
%     F      - A vector specifying the semi-width of the window for each
%              dimension. The final window's width will be 2*F+1.
%              Default: 3 (i.e. a 1-dimensional window of width 6).
%     DIM    - If F is a single scalar, the window will be applied through
%              this dimension; otherwise, this will be ignored.
%              Default: columns (or the first non-singleton dimension).
%     WINOPT - Cell array specifying optional arguments for the window
%              function HWIN (in addition to the width).
%              Default: {} (window's defaults).
%     PADOPT - Cell array specifying the optional arguments for the
%              PADARRAY MATLAB's function (in addition to the array X and
%              the padsize: 2*F+1). If the function is not found, data is
%              padded with zeros or the specified value: try {mean(X(:))}
%              for example.
%              Default: {'replicate'} (repeats border elements of X).
%              Default: {0} (pads with zeros if PADARRAY not found).
%     WNAN   - Integer indicating NaNs treatment and program behaviour!:
%              0: Filters data and interpolates NaNs         (default).
%              1: Filters data but do not interpolates NaNs
%              2: "Do not filters data" but interpolates NaNs!
%              See the NOTEs below
%
%   Output:
%     Y      - Filtered X data (same size as X!).
%     W      - N-dimensional window with central symmetry generated by a
%              special subfunction called NDWIND. See the description below
%              for details.
%
%   Description:
%     This function applies a N-dimensional convolution of X with W, using
%     the MATLAB's IMFILTER or CONVN function. One important aspect of the
%     function is the generation of the N-dimensional window (W) from the
%     specified function and width, which cannot be done with MATLAB's
%     functions. Besides, unlike MATLAB's FILTER, FILTER2 and IMFILTER,
%     NaNs elements are taken into account (ignored).
%
%     The N-dimensional window is generated from rotating the 1-dimensional
%     output of the HWIN function, through each of the N-dimensions, and
%     then shrinking it through each of its axes in order to fit the
%     specified semi-widths (F). This is done in the included subfunction
%     named NDWIND. In this way, the window has central symmetry and do not
%     produce a phase shift on X data.
%
%     By default, the edges are padded with the values of X at the borders
%     with the PADARRAY MATLAB's function. In this way, the edges are
%     treated smoothly. When PADARRAY is not found, the program performs
%     zero-padding.
%
%   Notes:
%     * The use of semi-widths F's is to force the generated window to be
%       even and, therefore, the change of phase is null.
%     * The window function HWIN should output an even function, otherwise,
%       it won't generate an error but the user should be aware that this
%       program will consider only the last half of it.
%     * The function window should return a monotonically decreasing
%       result, this restriction is because I try to avoid the use of FZERO
%       function, for example, to find the expanding/shrinking factors.
%     * If the user has an already generated window, it can be used in HWIN
%       instead of a function handle or name.
%     * Accepts empty value for any input. When X is empty, the program can
%       be used as a N-dimensional window generator.
%     * NaNs elements surrounded by no-NaNs elements (which will depend on
%       window width) are the ones that will be interpolated. The others
%       are leaved untouched.
%     * When WNAN=2, the programs acts like an NAN-interpolat/GAP-filling,
%       leaving untouched the no-NaNs elements but the filtering is
%       perfomed anyway. I recomend the default behaviour (WNAN=0) in order
%       to keep the filtered data in the workspace, and then use the code
%       at the end of this function to get/remove the interpolated NaNs
%       (see the example).
%     * The program looks for the IMFILTER and PADARRAY functions from the
%       Image Processing Toolbox. If not found, then CONVN is used instead
%       (slower) and pads with zeros or the given value. In this latter
%       case, if border elements are NaNs, the window won't work properly.
%
%   Example:
%     FWIN = 'hamming';
%     F = [13 8];
%     N = 100;
%     Pnoise = 0.30;
%     PNaNs  = 0.20;
%     X = peaks(N);                                     % original
%     Y = X + ((rand(size(X))-0.5)*2)*max(X(:))*Pnoise; % add noise
%     Y(round(1 + (N^2-1).*rand(N^2*PNaNs,1))) = NaN;   % add NaNs
%     [Z0,W] = ndnanfilter(Y,FWIN,F);                   % filters
%     Z1 = Z0; Z2 = Y; inan = isnan(Y);
%     Z1(inan) = NaN;
%     Z2(inan) = Z0(inan);
%     subplot(231), imagesc(X), clim = caxis; axis equal tight
%                   title('Original data')
%     subplot(232), imagesc(Y),  caxis(clim), axis equal tight
%                   title('Data + NOISE + NaNs')
%     subplot(234), imagesc(Z0), caxis(clim), axis equal tight
%                   title('FILTERS + NaNs interpolation')
%     subplot(235), imagesc(Z1), caxis(clim), axis equal tight
%                   title('FILTERS ignoring NaNs')
%     subplot(236), imagesc(Z2), caxis(clim), axis equal tight
%                   title('GAP-filling with interpolated NaNs')
%     subplot(233), imagesc(-F(1):F(1),-F(2):F(2),W), axis equal tight,
%                    title([upper(FWIN) ' 2D window']), view(2)
%
%   See also: FILTER, FILTER2 and CONVN; WINDOW from the Signal Processing
%   Toolbox; and FWIND1, FWIND2, FSPECIAL, IMFILTER and PADARRAY from the
%   Image Processing Toolbox.

%   Copyright 2008 Carlos Adrian Vargas Aguilera
%   $Revision: 1.2 $  $Date: 2008/06/30 18:00:00 $

%   Written by
%   M.S. Carlos Adrian Vargas Aguilera
%   Physical Oceanography PhD candidate
%   CICESE
%   Mexico, 2008
%   nubeobscura@hotmail.com
%
%   Download from:
%   http://www.mathworks.com/matlabcentral/fileexchange/loadAuthor.do?objec
%   tType=author&objectId=1093874

%   1.0    Release (2008/06/23 10:30:00)
%   1.1    Fixed Bug adding an extra dimension of unitary width.
%   1.2    Fixed Bug with ynan.

% Use the IMFILTER function? (faster than CONVN):
yimfilter = (exist('imfilter','file')==2);

% Use the PADARRAY function (or zero padding):
ypadarray = (exist('padarray','file')==2);

% Check inputs and sets defaults of principal arguments:
if nargin<3 || nargin>7
 error('Filtern:IncorrectNumberOfInputs',...
  'At least three inputs are needed and less than 7.')
end
if isempty(HWIN)
 HWIN = 'rectwin';
end
if isempty(F)
 F = 3;
end
N = length(F);
S = size(X);
% Secondary arguments:
if N && (nargin<4 || isempty(DIM))
 DIM = find(S~=1,1);   %  DIM = min(find(S~=1));
 if isempty(DIM), DIM = 1; end
end
if nargin<5 || isempty(WINOPT)
 WINOPT = {};
end
if nargin<6 || isempty(PADOPT)
 if ypadarray
  PADOPT = {'replicate'};
 else
  PADOPT = {0};
 end
elseif ~ypadarray && ~isnumeric(PADOPT{1})
 PADOPT = {0};
end
if nargin<7 || isempty(WNAN)
 WNAN = 0;
end

% Selects the 1-dimensional filter or set a row vector:
if N==1
 a = zeros(1,DIM);
 a(DIM) = F;
 F = a;
 clear a
end

% Checks if the window input is a function or an array:
if ~isa(HWIN,'function_handle') && ~ischar(HWIN)
 W = HWIN;
else
 W = [];
end

% If no input data but two outputs then generates the window only:
if isempty(X)
 Y = [];
 if nargout==2 && ~isempty(W)
  W = ndwind(HWIN,F,WINOPT{:});
 end
 return
end

% Generates the window:
if isempty(W)
 W = ndwind(HWIN,F,WINOPT{:});
end

% Check for NaN's:
inan = isnan(X);
ynan = any(inan(:));                       % Bug fixed 30/jun/2008
if ynan
 X(inan) = 0;
else
 factor = sum(W(:));
end

% Filtering:
if yimfilter                                % Use IMFILTER (faster)
 if ~isa(X,'double')
  X = double(X);
 end
 if ~isa(W,'double')
  W = double(W);
 end
 if ynan
  Y = imfilter(X,W       ,PADOPT{:},'conv');
 else
  Y = imfilter(X,W/double(factor),PADOPT{:},'conv');
 end
else                                        % Use CONVN
 % Sets F and S of equal sizes.
 F = reshape(F,1,N);
 Nx = numel(S);
 if N<Nx
  F(N+1:Nx) = 0;
 elseif N>Nx
  S(Nx+1:N) = 1;
 end
 F2 = 2*F;
 % Pads the borders:
 if ypadarray
  ind    = padarray(false(S),F2,true     );    % Index of the padding.
  Y      = padarray(X       ,F2,PADOPT{:},'both');
 elseif length(PADOPT{1})==1
  ind2 = cell(N,1);
  for n = 1:N
   ind2{n} = F2(n) + (1:S(n)).';
  end
  ind          = true(2*F2+S);
  Y            = repmat(PADOPT{1},2*F2+S);
  ind(ind2{:}) = false;
  Y(ind2{:})   = X;
 else % No padding at all
  Y    = X;
  ind  = false(S);
  warning('Ndnanfilter:PaddingOption','Do not perfom any padding.')
 end
 % Convolutes both arrays:
 if ynan
  Y = convn(Y,W       ,'same');
 else
  Y = convn(Y,W/factor,'same');
 end
 %  Eliminates the padding:
 Y(ind) = [];
 Y      = reshape(Y,S);
end

% Estimates the averages when NaNs are present:
if ynan
 if yimfilter
  factor       = imfilter(double(~inan),W,PADOPT{:},'conv');
 else
  if ypadarray
   factor      = padarray(~inan,F2,PADOPT{:});
  elseif length(PADOPT{1})==1 % (won't work properly with NaNs at borders)
   factor          = ind;
   factor(ind2{:}) = ~inan;
  else
   factor = ~inan;
  end
  factor      = convn(factor,W,'same');
  factor(ind) = [];
  factor      = reshape(factor,S);
 end
 Y = Y./factor;
end

% What about NaNs?:
if     WNAN == 1       % Leave NaNs elements untouched!
 Y(inan) = NaN;
elseif WNAN == 2       % Leave no-NaNs elements untouched!!!
 X(inan) = Y(inan);
 Y = X;
end
end

function W = ndwind(HWIN,F,varargin)
% NDWIND Generate a N-Dimensional zero-phase window.
%
%   Syntax:
%     W = ndwind(HWIN,F);
%     W = ndwind(HWIN,F,OPT);
%
%   Input:
%     HWIN - Window function handle. See WINDOW for details. By default
%            uses: @rectwin (a rectangular window).
%     F    - A vector specifying the semiwidth of the window for each
%            dimension. The window's width will be 2*F+1. By default uses:
%            3 (i.e. a window of width 6).
%     OPT  - Cell array specifying optional arguments for the window
%            function. By default uses: {[]} (window's defaults).
%
%   Output:
%     W    - N-Dimensional window with central symmetry.
%
%   Description:
%     In the axes of each dimension, W has a 1-D window defined as
%              feval(HWIN,2*F(n)+1), n = 1,...,N.
%     That is, they are defined by the same window function but have
%     different widths. So, this program creates another widther window (at
%     least 201 points), with the same definition, and finds how much the
%     former windows should be expanded in order to fit the latter one.
%
%     Afterwards, the coordinates of every point are expanded accordingly
%     and the value of the window in those points are found by linear
%     interpolation with the bigger window.
%
%     In resume, it is like rotating this big window through every
%     dimension and then shrinking it through each of its axes to fix the
%     specified widths.
%
%   Notes:
%     * Because of the use of the semi-widths F's, all the generated
%       windows are even. Therefore the change of phase is null.
%     * The window function HWIN should output an even function, otherwise,
%       it won't generate an error but this program will consider only the
%       last half of it.
%     * The window should be monotonically decreasing.
%     * Instead of the handle window, it can be given as a string:
%       'hamming' instead of @hamming, for example.
%     * Uses the MATLAB's function FUNC2STR.
%
%   Example:
%     W = ndwind(@hamming,[3 2])
%     % Results:
%     W =
%
%              0         0    0.0800         0         0
%              0    0.1417    0.3100    0.1417         0
%              0    0.3966    0.7700    0.3966         0
%         0.0800    0.5400    1.0000    0.5400    0.0800
%              0    0.3966    0.7700    0.3966         0
%              0    0.1417    0.3100    0.1417         0
%              0         0    0.0800         0         0
%
%
%   See also: WINDOW from the Signal Processing Toolbox; and FWIND1,
%   FWIND2, and FSPECIAL from the Image Processing Toolbox.

%   Copyright 2008 Carlos Adrian Vargas Aguilera
%   $Revision: 1.1 $  $Date: 2008/06/26 19:30:00 $

%   Written by
%   M.S. Carlos Adrian Vargas Aguilera
%   Physical Oceanography PhD candidate
%   CICESE
%   Mexico, 2008
%   nubeobscura@hotmail.com
%
%   Download from:
%   http://www.mathworks.com/matlabcentral/fileexchange/loadAuthor.do?objec
%   tType=author&objectId=1093874

%   1.0    Release (2008/06/23 10:30:00)
%   1.1    Fixed Bug adding an extra dimension of unitary width.

% Check inputs:
if nargin<1 || isempty(HWIN)
 HWIN = 'rectwin';
end
if nargin<2 || isempty(F)
 F = 3;
end

% Rectangular wind?:
if isa(HWIN,'function_handle')
 HWIN = func2str(HWIN);
end
if strcmpi(HWIN,'rectwin')
 W = ones([2*F(:).'+1 1]);
 return
end

% Generate the BIG window (only the last half):
FBIG         = max([100; F(:)]);
BIGw         = feval(HWIN,2*FBIG+1,varargin{:});
BIGw(1:FBIG) = [];       % Deletes the first half.
rBIGw        = 0:FBIG;   % Window argument (distance).

% Axial windows widths:
N  = numel(F);
F  = reshape(F,1,N);
F  = [F 0];             % BUG fixed by adding an extra dimension.
N  = N+1;
F2 = 2*F+1;

% Pre-allocates the final window and the expanded axis:
W  = zeros(F2);
An = cell(N,1);
Ae = An;

% Generates the index and expanded axes:
for n = 1:N

 % Generate temporally the window in the n-axis:
 wn = feval(HWIN,F2(n),varargin{:});

 % Finds the expansion factors (Note: the window should tends to zero):
 if F(n)
  piv = wn(end);
  ind = (BIGw == piv);
  if ~any(ind)
   ind1 = (BIGw >= piv); ind1 = length(ind1(ind1));
   ind2 = (BIGw <= piv); ind2 = length(ind2(~ind2))+1;
   if ind2>FBIG+1
    r = rBIGw(ind1);
   else
    r = interp1(BIGw([ind1 ind2]), rBIGw([ind1 ind2]),piv);
   end
  else
   r = rBIGw(ind);
  end
  Ef = r/F(n);
 else
  Ef = 1;
 end

 % Reversed index and expanded n-axis (for the following grid):
 An{n} = (F(n):-1:0);
 Ae{n} = An{n}*Ef;

end

% Estimates the expanded distances outside the axes (only at the 1st
% quarter):
% Note: In a 2-Dimensional matrix, by the 1st quarter of a matrix I mean
% the first 1/4 piece of the matrix after you divided it throuh the middle
% row and column. In N-dimensions it would be the 1st 1/2^N part.
gride4      = cell(N,1);
[gride4{:}] = ndgrid(Ae{:});
R4          = sqrt(sum(reshape([gride4{:}],prod(F+1),N).^2,2));

% Generates the window and linear index in the 1st quarter:
grid4     = cell(N,1);
[grid4{:}]= ndgrid(An{:});
in        = (R4<=rBIGw(end));           % Looks for elements inside window.
W4        = zeros(F+1);                 % 1st quarter of the window.
W4(in)    = interp1(rBIGw,BIGw,R4(in)); % Interpolates the window values.
for n=1:N                               % Linear index on the 1st quarter.
 grid4{n} = flip(grid4{n}+1,n);
end
ind4      = sub2ind(F2,grid4{:});

% Index of permutations to fill the N-D window:
np = 2^N-1;
ip = zeros(1,np);
for n = 1:N
 ini  = 2^(n-1);
 step = ini*2;
 ip(ini:step:np) = n;
end

% Fills the N-D window by flipping W4 and the index:
ones4       = false(F2);    % Avoids using new FALSE function
ones4(ind4) = true;
W(ones4)    = W4;
for kp = ip
 W4         = flip(W4,kp);
 ones4      = flip(ones4,kp);
 W(ones4)   = W4;
end
end

function h = gaussian2D(siz, std)

    % create the grid of (x,y) values
    siz = (siz-1)./2;
    [x,y] = meshgrid(-siz(2):siz(2),-siz(1):siz(1));

    std = max(std);
    sr = siz(2)/siz(1);
    if sr > 1
        sy = sr^2; sx = 1;
    else
        sx = sr^2; sy = 1;
    end

    % analytic function
    h = exp(-(sx*x.*x + sy*y.*y)/(2*std*std));

    % truncate very small values to zero
    h(h<eps*max(h(:))) = 0;

    % normalize filter to unit L1 energy
    sumh = sum(h(:));
    if sumh ~= 0
        h = h/sumh;
    end
end


function [x0,y0,iout,jout] = intersections(x1,y1,x2,y2,robust)
%INTERSECTIONS Intersections of curves.
%   Computes the (x,y) locations where two curves intersect.  The curves
%   can be broken with NaNs or have vertical segments.
%
% Example:
%   [X0,Y0] = intersections(X1,Y1,X2,Y2,ROBUST);
%
% where X1 and Y1 are equal-length vectors of at least two points and
% represent curve 1.  Similarly, X2 and Y2 represent curve 2.
% X0 and Y0 are column vectors containing the points at which the two
% curves intersect.
%
% ROBUST (optional) set to 1 or true means to use a slight variation of the
% algorithm that might return duplicates of some intersection points, and
% then remove those duplicates.  The default is true, but since the
% algorithm is slightly slower you can set it to false if you know that
% your curves don't intersect at any segment boundaries.  Also, the robust
% version properly handles parallel and overlapping segments.
%
% The algorithm can return two additional vectors that indicate which
% segment pairs contain intersections and where they are:
%
%   [X0,Y0,I,J] = intersections(X1,Y1,X2,Y2,ROBUST);
%
% For each element of the vector I, I(k) = (segment number of (X1,Y1)) +
% (how far along this segment the intersection is).  For example, if I(k) =
% 45.25 then the intersection lies a quarter of the way between the line
% segment connecting (X1(45),Y1(45)) and (X1(46),Y1(46)).  Similarly for
% the vector J and the segments in (X2,Y2).
%
% You can also get intersections of a curve with itself.  Simply pass in
% only one curve, i.e.,
%
%   [X0,Y0] = intersections(X1,Y1,ROBUST);
%
% where, as before, ROBUST is optional.

% Version: 1.12, 27 January 2010
% Author:  Douglas M. Schwarz
% Email:   dmschwarz=ieee*org, dmschwarz=urgrad*rochester*edu
% Real_email = regexprep(Email,{'=','*'},{'@','.'})

% Theory of operation:
%
% Given two line segments, L1 and L2,
%
%   L1 endpoints:  (x1(1),y1(1)) and (x1(2),y1(2))
%   L2 endpoints:  (x2(1),y2(1)) and (x2(2),y2(2))
%
% we can write four equations with four unknowns and then solve them.  The
% four unknowns are t1, t2, x0 and y0, where (x0,y0) is the intersection of
% L1 and L2, t1 is the distance from the starting point of L1 to the
% intersection relative to the length of L1 and t2 is the distance from the
% starting point of L2 to the intersection relative to the length of L2.
%
% So, the four equations are
%
%    (x1(2) - x1(1))*t1 = x0 - x1(1)
%    (x2(2) - x2(1))*t2 = x0 - x2(1)
%    (y1(2) - y1(1))*t1 = y0 - y1(1)
%    (y2(2) - y2(1))*t2 = y0 - y2(1)
%
% Rearranging and writing in matrix form,
%
%  [x1(2)-x1(1)       0       -1   0;      [t1;      [-x1(1);
%        0       x2(2)-x2(1)  -1   0;   *   t2;   =   -x2(1);
%   y1(2)-y1(1)       0        0  -1;       x0;       -y1(1);
%        0       y2(2)-y2(1)   0  -1]       y0]       -y2(1)]
%
% Let's call that A*T = B.  We can solve for T with T = A\B.
%
% Once we have our solution we just have to look at t1 and t2 to determine
% whether L1 and L2 intersect.  If 0 <= t1 < 1 and 0 <= t2 < 1 then the two
% line segments cross and we can include (x0,y0) in the output.
%
% In principle, we have to perform this computation on every pair of line
% segments in the input data.  This can be quite a large number of pairs so
% we will reduce it by doing a simple preliminary check to eliminate line
% segment pairs that could not possibly cross.  The check is to look at the
% smallest enclosing rectangles (with sides parallel to the axes) for each
% line segment pair and see if they overlap.  If they do then we have to
% compute t1 and t2 (via the A\B computation) to see if the line segments
% cross, but if they don't then the line segments cannot cross.  In a
% typical application, this technique will eliminate most of the potential
% line segment pairs.

% Input checks.
error(nargchk(2,5,nargin))

% Adjustments when fewer than five arguments are supplied.
switch nargin
	case 2
		robust = true;
		x2 = x1;
		y2 = y1;
		self_intersect = true;
	case 3
		robust = x2;
		x2 = x1;
		y2 = y1;
		self_intersect = true;
	case 4
		robust = true;
		self_intersect = false;
	case 5
		self_intersect = false;
end

% x1 and y1 must be vectors with same number of points (at least 2).
if sum(size(x1) > 1) ~= 1 || sum(size(y1) > 1) ~= 1 || ...
		length(x1) ~= length(y1)
	error('X1 and Y1 must be equal-length vectors of at least 2 points.')
end
% x2 and y2 must be vectors with same number of points (at least 2).
if sum(size(x2) > 1) ~= 1 || sum(size(y2) > 1) ~= 1 || ...
		length(x2) ~= length(y2)
	error('X2 and Y2 must be equal-length vectors of at least 2 points.')
end

% Force all inputs to be column vectors.
x1 = x1(:);
y1 = y1(:);
x2 = x2(:);
y2 = y2(:);

% Compute number of line segments in each curve and some differences we'll
% need later.
n1 = length(x1) - 1;
n2 = length(x2) - 1;
xy1 = [x1 y1];
xy2 = [x2 y2];
dxy1 = diff(xy1);
dxy2 = diff(xy2);

% Determine the combinations of i and j where the rectangle enclosing the
% i'th line segment of curve 1 overlaps with the rectangle enclosing the
% j'th line segment of curve 2.
[i,j] = find(repmat(min(x1(1:end-1),x1(2:end)),1,n2) <= ...
	repmat(max(x2(1:end-1),x2(2:end)).',n1,1) & ...
	repmat(max(x1(1:end-1),x1(2:end)),1,n2) >= ...
	repmat(min(x2(1:end-1),x2(2:end)).',n1,1) & ...
	repmat(min(y1(1:end-1),y1(2:end)),1,n2) <= ...
	repmat(max(y2(1:end-1),y2(2:end)).',n1,1) & ...
	repmat(max(y1(1:end-1),y1(2:end)),1,n2) >= ...
	repmat(min(y2(1:end-1),y2(2:end)).',n1,1));

% Force i and j to be column vectors, even when their length is zero, i.e.,
% we want them to be 0-by-1 instead of 0-by-0.
i = reshape(i,[],1);
j = reshape(j,[],1);

% Find segments pairs which have at least one vertex = NaN and remove them.
% This line is a fast way of finding such segment pairs.  We take
% advantage of the fact that NaNs propagate through calculations, in
% particular subtraction (in the calculation of dxy1 and dxy2, which we
% need anyway) and addition.
% At the same time we can remove redundant combinations of i and j in the
% case of finding intersections of a line with itself.
if self_intersect
	remove = isnan(sum(dxy1(i,:) + dxy2(j,:),2)) | j <= i + 1;
else
	remove = isnan(sum(dxy1(i,:) + dxy2(j,:),2));
end
i(remove) = [];
j(remove) = [];

% Initialize matrices.  We'll put the T's and B's in matrices and use them
% one column at a time.  AA is a 3-D extension of A where we'll use one
% plane at a time.
n = length(i);
T = zeros(4,n);
AA = zeros(4,4,n);
AA([1 2],3,:) = -1;
AA([3 4],4,:) = -1;
AA([1 3],1,:) = dxy1(i,:).';
AA([2 4],2,:) = dxy2(j,:).';
B = -[x1(i) x2(j) y1(i) y2(j)].';

% Loop through possibilities.  Trap singularity warning and then use
% lastwarn to see if that plane of AA is near singular.  Process any such
% segment pairs to determine if they are colinear (overlap) or merely
% parallel.  That test consists of checking to see if one of the endpoints
% of the curve 2 segment lies on the curve 1 segment.  This is done by
% checking the cross product
%
%   (x1(2),y1(2)) - (x1(1),y1(1)) x (x2(2),y2(2)) - (x1(1),y1(1)).
%
% If this is close to zero then the segments overlap.

% If the robust option is false then we assume no two segment pairs are
% parallel and just go ahead and do the computation.  If A is ever singular
% a warning will appear.  This is faster and obviously you should use it
% only when you know you will never have overlapping or parallel segment
% pairs.

if robust
	overlap = false(n,1);
	warning_state = warning('off','MATLAB:singularMatrix');
	% Use try-catch to guarantee original warning state is restored.
	try
		lastwarn('')
		for k = 1:n
			T(:,k) = AA(:,:,k)\B(:,k);
			[unused,last_warn] = lastwarn;
			lastwarn('')
			if strcmp(last_warn,'MATLAB:singularMatrix')
				% Force in_range(k) to be false.
				T(1,k) = NaN;
				% Determine if these segments overlap or are just parallel.
				overlap(k) = rcond([dxy1(i(k),:);xy2(j(k),:) - xy1(i(k),:)]) < eps;
			end
		end
		warning(warning_state)
	catch err
		warning(warning_state)
		rethrow(err)
	end
	% Find where t1 and t2 are between 0 and 1 and return the corresponding
	% x0 and y0 values.
	in_range = (T(1,:) >= 0 & T(2,:) >= 0 & T(1,:) <= 1 & T(2,:) <= 1).';
	% For overlapping segment pairs the algorithm will return an
	% intersection point that is at the center of the overlapping region.
	if any(overlap)
		ia = i(overlap);
		ja = j(overlap);
		% set x0 and y0 to middle of overlapping region.
		T(3,overlap) = (max(min(x1(ia),x1(ia+1)),min(x2(ja),x2(ja+1))) + ...
			min(max(x1(ia),x1(ia+1)),max(x2(ja),x2(ja+1)))).'/2;
		T(4,overlap) = (max(min(y1(ia),y1(ia+1)),min(y2(ja),y2(ja+1))) + ...
			min(max(y1(ia),y1(ia+1)),max(y2(ja),y2(ja+1)))).'/2;
		selected = in_range | overlap;
	else
		selected = in_range;
	end
	xy0 = T(3:4,selected).';

	% Remove duplicate intersection points.
	[xy0,index] = unique(xy0,'rows');
	x0 = xy0(:,1);
	y0 = xy0(:,2);

	% Compute how far along each line segment the intersections are.
	if nargout > 2
		sel_index = find(selected);
		sel = sel_index(index);
		iout = i(sel) + T(1,sel).';
		jout = j(sel) + T(2,sel).';
	end
else % non-robust option
	for k = 1:n
		[L,U] = lu(AA(:,:,k));
		T(:,k) = U\(L\B(:,k));
	end

	% Find where t1 and t2 are between 0 and 1 and return the corresponding
	% x0 and y0 values.
	in_range = (T(1,:) >= 0 & T(2,:) >= 0 & T(1,:) < 1 & T(2,:) < 1).';
	x0 = T(3,in_range).';
	y0 = T(4,in_range).';

	% Compute how far along each line segment the intersections are.
	if nargout > 2
		iout = i(in_range) + T(1,in_range).';
		jout = j(in_range) + T(2,in_range).';
	end
end

% Plot the results (useful for debugging).
% plot(x1,y1,x2,y2,x0,y0,'ok');


function res = hbg(im,thresh)

% inputs
% im is the fourier transform of the image
% thresh is the cutoff circle radius

%outputs
% res is the boosted image

[r,c]=size(im);
d0=thresh;
d=zeros(r,c);
h=zeros(r,c);

for i=1:r
    for j=1:c
     d(i,j)=  sqrt( (i-(r/2))^2 + (j-(c/2))^2);
    end
end

A=1.75; % boost factor or coefficient

for i=1:r
    for j=1:c

         h(i,j)= 1-exp ( -( (d(i,j)^2)/(2*(d0^2)) ) );
         h(i,j)=(A-1)+h(i,j);
    end

end

for i=1:r
    for j=1:c
    res(i,j)=(h(i,j))*im(i,j);
    end
end


 function [proj, J, alpha, kb_m, d] = kaiser_bessel_xray(r, J, alpha, kb_m, d)
%function [proj, J, alpha, kb_m, d] = kaiser_bessel_xray(r, J, alpha, kb_m, d)
%
% X-ray transform of generalized Kaiser-Bessel function,
% See (A7) in lewitt:90:mdi, JOSA-A, Oct. 1990.
%
% in
%	r	[?]	radial locations in projection space (unitless)
%
% options
%	J		diameter of blob (a = J/2), default 4
%	alpha		shape parameter, default 10.83
%	kb_m		order parameter, default 2
%	d		dimension, default 2
%
% out
%	proj	[?]	x-ray transform values
%
% Copyright 2005-7-29, Jeff Fessler, The University of Michigan

if nargin < 1, ir_usage, end
if nargin == 1 && streq(r, 'test'), kaiser_bessel_xray_test, return, end

if ~isvar('J'), J = 4; end
if ~isvar('alpha') || isempty('alpha'), alpha = 10.83; end
if ~isvar('kb_m') || isempty('kb_m'), kb_m = 2; end
if ~isvar('d'), d = 2; end
tol = 1e-4;

%
% trick to yield inline functions
%
if ischar(r)
	kernel = sprintf('kaiser_bessel_xray(r, %d, %g, %g, %g)', ...
		J, alpha, kb_m, d);

	if streq(r, 'string')
		proj = kernel;
	elseif streq(r, 'inline')
		proj = inline(kernel, 'r');
	else
		error 'bad argument'
	end
return
end

%
% Check for validity of FT formula
%
persistent warned
if (kb_m < 0 || (abs(round(kb_m)-kb_m) > eps))
	if isempty(warned)
		printf([mfilename: 'kb_m=%g in kaiser_bessel_xray()'], kb_m)
		printf('validity of formula uncertain')
		warned = 1;
	end
end

a = J/2;
factor = a / besseli(kb_m, alpha) * sqrt(2*pi/alpha);
root = sqrt(1 - (r/a).^2);
nu = kb_m + 1/2;
proj = factor * root.^nu .* besseli(nu, alpha * root);
proj = reale(proj, tol);

%
% test
%
function kaiser_bessel_xray_test
x = linspace(-2.5,2.5,501)';
y = x;
[xx yy] = ndgrid(x, y);
r = sqrt(xx.^2 + yy.^2);
[proj J kb_m alpha] = kaiser_bessel_xray(x);
dx = x(2) - x(1);

f0 = kaiser_bessel(r, J, kb_m, alpha);
psum = sum(f0,2) * dx;

if im
	clf
	im(221, x, y, f0, 'blob'), grid
	subplot(222)
	plot(x, f0(:,y==0)), title 'profile', axis tight
	xtick([-2:1.0:2]), grid
	subplot(212)
	plot(x, proj, '-', x, psum, '--'), axis tight, title 'projection'
	legend('Analytical', 'Numerical')
end
max_percent_diff(proj, psum, mfilename)


function pdf = reciprocal_pdf ( x, a, b )

%*****************************************************************************80
%
%% RECIPROCAL_PDF evaluates the Reciprocal PDF.
%
%  Formula:
%
%    PDF(X)(A,B) = 1.0D+00 / ( X * LOG ( B / A ) )
%    for 0.0D+00 <= X
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    20 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real X, the argument of the PDF.
%
%    Input, real A, B, the parameters of the PDF.
%    0.0 < A <= B.
%
%    Output, real PDF, the value of the PDF.
%
  if ( x <= 0.0 )
    pdf = 0.0;
  elseif ( 0.0 < x )
    pdf = 1.0 / ( x * log ( b / a ) );
  end

  return
end


function [bmproc] = brownian(npoints, sigma)
% BROWNIAN generate and plot an aproximation to Brownian motion.
%   Generates a random walk with normally distributed jumps
%
% [bmproc] = brownian(npoints [, sigma])
%
% Inputs: npoints - length of the trajectory
%         sigma - optional, the norming constant (standard
%         deviation of B(1)). Default 1.
%
% Outputs: bmproc - trajectory of the process

% Authors: R.Gaigalas, I.Kaj
% v1.2 04-Oct-02

  % set default parameter values
  if (nargin==1)
    sigma = 1;
  end

  % generate a sample from a Gaussian distribution and sum up
  bmproc = [0 cumsum(sigma.*randn(1, npoints-1))];

  % plot the process
  plot([0:npoints-1], bmproc);


function g = p27_g ( n, x )

%*****************************************************************************80
%
%% P27_G evaluates the gradient for problem 27.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 January 2001
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of variables.
%
%    Input, real X(N), the values of the variables.
%
%    Output, real G(N), the gradient of the objective function.
%
  g = zeros ( n, 1 );

  r = sqrt ( x(1)^2 + x(2)^2 );

  if ( r == 0.0 )
    g(1) = 0.0;
    g(2) = 0.0;
    return
  end

  rx1 = x(1) / r;
  rx2 = x(2) / r;

  a = ( 1.0 + 0.001 * r^2 )^( -2 );
  ar = - 0.004 * r * ( 1.0 + 0.001 * r^2 )^( -3 );

  b = ( sin ( r ) )^2 - 0.5;
  br = sin ( 2.0 * r );

  g(1) = ( ar * b + a * br ) * rx1;
  g(2) = ( ar * b + a * br ) * rx2;

  return
end


function x = MW2cv(P,N)
%MW1CV   critical Mann-Whitney's U associated to a p-value.
%It obtain a Mann-Whitney's U of two random variables with continuous cumulative
%distribution associated to a p-value. This procedure is highly recommended for sample sizes
%7< nx & ny <=40. For nx & ny <=7 it is recommended to use the MW1cv function;
%otherwise, U-value may be a poor approximation.
%It works with a procedure to get the nearest cumulative distribution relative value to P.
%[Based on the Fortran77 algorithm AS 62 Appl. Statist. (1973)]
%
%   Syntax: function x = MW2cv(P,N)
%
%     Inputs:
%          P - cumulative probability value of interest.
%          N - 2-element vector of sample sizes for the two samples [].
% The input quantities should be scalars.
%     Outputs:
%          x - Mann-Whitney's U statistic.
%
%    Example: For two independent samples we are interested to get the
%             Mann-Whitney's statistic U with an associated cumulative
%             probability P = 0.95. Sample sizes are n1 = 36 and n2 = 14.
%
%                              P = 0.95; N = [36,14];
%
%     Calling on Matlab the function:
%             x = MW2cv(P,N)
%
%       Answer is:
%
%                 328
%

%  Created by A. Trujillo-Ortiz and R. Hernandez-Walls
%             Facultad de Ciencias Marinas
%             Universidad Autonoma de Baja California
%             Apdo. Postal 453
%             Ensenada, Baja California
%             Mexico.
%             atrujo@uabc.mx
%
%  May 23, 2003.
%
%  To cite this file, this would be an appropriate format:
%  Trujillo-Ortiz, A. and R. Hernandez-Walls. (2003). MW2cv: Critical Mann-Whitney's U
%    associated to a p-value: nx or ny >7. A MATLAB file. [WWW document]. URL http://
%    www.mathworks.com/matlabcentral/fileexchange/loadFile.do?objectId=3555&objectType=FILE
%
%  References:
%
%  Mann, H. B. and Whitney, D. R. (1947), On a test of whether one of two
%           random variables is stochastically larger than the other. Annals
%           of Mathematical Statistics, 18: 50-60.
%  Algorithm AS 62 (1973). Journal of Applied Statistics, 22(2):1-3.
%

if nargin <  2,
   error('Requires two input arguments.');
end

nmin = min(N);  %largest sample size.
nmax = max(N);  %smallest sample size.

if (nmin <= 7) & (nmax <= 7);
   fprintf('Warning: For nx and ny <= 7, the p-value may be a poor approximation.\n');
   fprintf('It is recommended to use the MW1cv function you can find on the\n');
   fprintf('Matlab>File Exchange Antonio Trujillo-Ortiz'' Author Page.\n');
   disp(' ');
   cont=input('Do you want to continue anyway (y/n):','s');

   if (cont=='y');
      disp('Here it goes.');

      mn1 = prod(N)+1;
      n1 = nmax+1;
      freq = [ones(n1,1); zeros(mn1-n1,1)];

      lwrk = floor((mn1+1)/2 + nmin);
      work = zeros(lwrk,1);

% Generate successively higher-order distributions
      in = nmax;
      for i = 2:nmin
         in = in+nmax;
         n1 = in+2;
         l = 1 + in/2;
         k = i;

% Generate complete distribution from outside inwards
         for j = 1:l
            k = k+1;
            n1 = n1-1;
            summ = freq(j) + work(j);
            freq(j) = summ;
            work(k) = summ - freq(n1);
            freq(n1) = summ;
         end;
      end;

      freq = freq/sum(freq);  % Make distribution relative

% Cumulative frequency distribution
      cumfreq = cumsum(freq);

%Location of the interested Mann-Whitney's U on all the possible U's for this 2-sample sizes.
%Here we are using a procedure to get the nearest fc value to P.
      cumfreq=cumfreq-P;
      u = find(abs(cumfreq)==min(abs(cumfreq(:))));

      UU = [0:length(freq)-1];  %vector of all the possible Mann-Whitney's U values.

%Association of the interested Mann-Whitney's U with its cumulative distribution.
      x = UU(u);
   else
   end
else

   mn1 = prod(N)+1;
   n1 = nmax+1;
   freq = [ones(n1,1); zeros(mn1-n1,1)];

   lwrk = floor((mn1+1)/2 + nmin);
   work = zeros(lwrk,1);

%Generate successively higher-order distributions
   in = nmax;
   for i = 2:nmin
      in = in+nmax;
      n1 = in+2;
      l = 1 + in/2;
      k = i;

%Generate complete distribution from outside inwards
      for j = 1:l
         k = k+1;
         n1 = n1-1;
         summ = freq(j) + work(j);
         freq(j) = summ;
         work(k) = summ - freq(n1);
         freq(n1) = summ;
      end;
   end;

   freq = freq/sum(freq);  % Make distribution relative

% Cumulative frequency distribution
   cumfreq = cumsum(freq);

%Location of the interested Mann-Whitney's U on all the possible U's for this 2-sample sizes.
%Here we are using a procedure to get the nearest fc value to P.
   cumfreq=cumfreq-P;
   u = find(abs(cumfreq)==min(abs(cumfreq(:))));

   UU = [0:length(freq)-1];  %vector of all the possible Mann-Whitney's U values.

%Association of the interested Mann-Whitney's U with its cumulative distribution.
   x = UU(u);
end


function varargout = holdertable(X)
% Holder table function
%
%   HOLDERTABLE([x1, x2]) returns the value of the Holder table
%   function at the specified points. [x1] and [x2] may be vectors.
%   The search domain is
%
%               -10 < x_i < 10
%
%   The four global minima are near the edges of the interval, and have a
%   function value of
%
%       f(x*) = -1.92085026.

% 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} = [-10, -10]; % LB
        varargout{3} = [+10, +10]; % UB
        varargout{4} = [+8.055023472141116e+000   +9.664590028909654e+000
                        -8.055023472141116e+000   +9.664590028909654e+000
                        +8.055023472141116e+000   -9.664590028909654e+000
                        -8.055023472141116e+000   -9.664590028909654e+000]; % solution
        varargout{5} = -1.920850256788675e+001; % function value at solution

    % otherwise, output function value
    else

        % keep values within the search interval
        X(X < -10) = inf;      X(X > 10) = 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} = -abs(sin(x1).*cos(x2).*exp(abs(1 - sqrt(x1.^2 + x2.^2)/pi)));

    end

end


function r = hammersley_sequence ( n )

%*****************************************************************************80
%
%% HAMMERSLEY_SEQUENCE computes N elements of a leaped Hammersley subsequence.
%
%  Discussion:
%
%    The DIM_NUM-dimensional Hammersley sequence is really DIM_NUM separate
%    sequences, each generated by a particular base.  If the base is
%    greater than 1, a standard 1-dimensional
%    van der Corput sequence is generated.  But if the base is
%    negative, this is a signal that the much simpler sequence J/(-BASE)
%    is to be generated.  For the standard Hammersley sequence, the
%    first spatial coordinate uses a base of (-N), and subsequent
%    coordinates use bases of successive primes (2, 3, 5, 7, 11, ...).
%    This program allows the user to specify any combination of bases,
%    included nonprimes and repeated values.
%
%    This routine selects elements of a "leaped" subsequence of the
%    Hammersley sequence.  The subsequence elements are indexed by a
%    quantity called STEP, which starts at 0.  The STEP-th subsequence
%    element is simply element
%
%      SEED(1:DIM_NUM) + STEP * LEAP(1:DIM_NUM)
%
%    of the original Hammersley sequence.
%
%
%    This routine "hides" a number of input arguments.  To specify these
%    arguments explicitly, use I4_TO_HAMMERSLEY instead.
%
%    All the arguments have default values.  However, if you want to
%    examine or change them, you may call the appropriate routine first.
%
%    * DIM_NUM, the spatial dimension,
%      Default: DIM_NUM = 1;
%      Required: 1 <= DIM_NUM is required.
%
%    * STEP, the subsequence index.
%      Default: STEP = 0.
%      Required: 0 <= STEP.
%
%    * SEED(1:DIM_NUM), the Hammersley sequence element corresponding to STEP = 0.
%      Default SEED = (0, 0, ... 0).
%      Required: 0 <= SEED(1:DIM_NUM).
%
%    * LEAP(1:DIM_NUM), the succesive jumps in the Hammersley sequence.
%      Default: LEAP = (1, 1, ..., 1).
%      Required: 1 <= LEAP(1:DIM_NUM).
%
%    * BASE(1:DIM_NUM), the Hammersley bases.
%      Default: BASE = (2, 3, 5, 7, 11, ... ) or (-N,2,3,5,7,11,...) if N is known.
%      Required: 1 < BASE(I) for any van der Corput dimension, or BASE(I) < 0
%      to generate the fractional sequence J/|BASE(I)|.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 July 2004
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    J M Hammersley,
%    Monte Carlo methods for solving multivariable problems,
%    Proceedings of the New York Academy of Science,
%    Volume 86, 1960, pages 844-874.
%
%    Ladislav Kocis and William Whiten,
%    Computational Investigations of Low-Discrepancy Sequences,
%    ACM Transactions on Mathematical Software,
%    Volume 23, Number 2, 1997, pages 266-294.
%
%  Parameters:
%
%    Input, integer N, the number of elements to compute.
%
%    Output, real R(DIM_NUM,N), the SEED-th through SEED+N-1th elements of
%    the Hammersley sequence.
%
  dim_num = hammersley_dim_num_get ( );
  step = hammersley_step_get ( );
  seed = hammersley_seed_get ( );
  leap = hammersley_leap_get ( );
  base = hammersley_base_get ( );

  r = i4_to_hammersley_sequence ( dim_num, n, step, seed, leap, base );

  step = step + n;

  hammersley_step_set ( step );

  return
end


function [E, E1, E2, Et, Et1, Et2] = testClassSeperability(N,MEAN1,STD1,MEAN2,STD2, PLOT)

%
% This function generates two random Gaussian sequences and computes
% both the theoretical error and the histogram-based method, using
% respectively the functions computeHistError() and theoreticalError().
%
%
% ARGUMENTS:
% N: number of samples per class
% MEAN1: average value of the samples (1st class)
% STD1: standard deviation of the samples (1st class)
% MEAN2: average value of the samples (2nd class)
% STD2: standard deviation of the samples (2nd class)
% PLOT: this is 1 if results are to be plotted
%
% --------------------------------------------
% Theodoros Giannakopoulos
% Dep. of Informatics and Telecommunications
% University of Athens, Greece
% http://www.di.uoa.gr/~tyiannak
% --------------------------------------------
%

if (N<100)
    fprintf('N has to be at lest 1000!\n');
    return;
end

% GENERATE DATASETS (normal distribution)
x1 = randn(N,1) * sqrt(STD1) + MEAN1;
x2 = randn(N,1) * sqrt(STD2) + MEAN2;

if (N<1000)
    nBins = 10;
else
    if (N<10000)
        nBins = 50;
    else
        nBins = 100;
    end
end

% CALCULATE HISTOGRAMS FOR BOTH CLASSES:
step = (max(MEAN1,MEAN2) - min(MEAN1,MEAN2)) / nBins;
X = min(MEAN1-3*STD1,MEAN2-3*STD2): step : max(MEAN1+3*STD1,MEAN2+3*STD2);
[H1, X1] = hist(x1, X);
[H2, X2] = hist(x2, X);

% COMPUTE THE ERROR PROBABILITIES FOR BOTH CLASSES using the historam method:
%
% N O T E : this function computes the error classification probability for
% the training data using the histograms as a pdf estimation method.
% Therefore, it can be used for any distribution of input data.
%

[E1, E2] = computeHistError(x1, x2);
E1 = 100* E1;
E2 = 100* E2;

% OVERALL ERROR PROBABILITY:
E = (E1 + E2) / 2;

if (PLOT==1)
    figure;
    subplot(2,1,1);
    str = sprintf('Estimated Errors: E1 = %.2f%%, E2 = %.2f%%, E = %.2f%%.', E1, E2, E);
    plot(X1, H1);
    hold on;
    plot(X2, H2,'r');
    legend('Class A','Class B');
    title(str);
    axis([min(MEAN1-3*STD1,MEAN2-3*STD2) max(MEAN1+3*STD1,MEAN2+3*STD2) 0 max([H1 H2])]);
    subplot(2,1,2);
    [Et, Et1, Et2, x] = theoreticalError(MEAN1, STD1, MEAN2, STD2);
    str = sprintf('Theoretical Errors: E1 = %.2f%%, E2 = %.2f%%, E = %.2f%%.', Et1, Et2, Et);
    title(str);
end


% %Question No: 5 (a)
% Determine the weights of a network with 4 input and 2 output units using
% Perceptron Learning Law for the following input-output pairs:

% Input: [1100]' [1001]' [0011]' [0110]'
% output: [11]' [10]' [01]' [00]'
% Discuss your results for different choices of the learning rate
% parameters.
% Use suitable values for the initial weights.

in=[1 1 0 0 -1;1 0 0 1 -1; 0 0 1 1 -1; 0 1 1 0 -1];
out=[1 1; 1 0; 0 1; 0 0];
eta=input('Enter the learning rate value = ');
it=input('Enter the number of iterations required = ');
wgt=input('Enter the weights,2 by 5 matrix(including weight for bias):\n');
for x=1:it
    for i=1:4
        s1=0;
        s2=0;
        for j=1:5
          s1=s1+in(i,j)*wgt(1,j);
          s2=s2+in(i,j)*wgt(2,j);
        end
        wi=eta*(out(i,1)-sign(s1))*in(i,:);
        wgt(1,:)=wgt(1,:)+wi;
        wi=eta*(out(i,2)-sign(s2))*in(i,:);
        wgt(2,:)=wgt(2,:)+wi;
    end
end
wgt


function [dist, t] = distancePointEdge3d(point, edge)
%DISTANCEPOINTEDGE3D Minimum distance between a 3D point and a 3D edge
%
%   DIST = distancePointEdge3d(POINT, EDGE);
%   Return the euclidean distance between edge EDGE and point POINT.
%   EDGE has the form: [x1 y1 z1 x2 y2 z2], and POINT is [x y z].
%
%   If EDGE is N-by-6 array, result is N-by-1 array computed for each edge.
%   If POINT is a N-by-3 array, the result is computed for each point.
%   If both POINT and EDGE are array, they must have the same number of
%   rows, and the result is computed for each couple point(i,:);edge(i,:).
%
%   [DIST POS] = distancePointEdge3d(POINT, EDGE);
%   Also returns the position of closest point on the edge. POS is
%   comprised between 0 (first point) and 1 (last point).
%
%   See also:
%   edges3d, points3d, distancePoints3d, distancePointLine3d
%

%   ---------
%   author : David Legland
%   INRA - CEPIA URPOI - MIA MathCell
%   created the 07/04/2004.
%

%   HISTORY
%   2005-06-24 rename, and change arguments sequence
%   2009-04-30 add possibility to return position of closest point
%   2011-04-14 add checkup for degenerate edges, improve speed, update doc

% direction vector of each edge
vl = edge(:, 4:6) - edge(:, 1:3);

% compute position of points projected on the supporting line
% (Size of t is the max number of edges or points)
t = linePosition3d(point, [edge(:,1:3) vl]);

% change position to ensure projected point is located on the edge
t(t < 0) = 0;
t(t > 1) = 1;

% difference of coordinates between projected point and base point
p0 = bsxfun(@plus, edge(:,1:3), [t .* vl(:,1) t .* vl(:,2) t .* vl(:,3)]);
p0 = bsxfun(@minus, point, p0);

% compute distance between point and its projection on the edge
dist = sqrt(sum(p0 .* p0, 2));


%  Computing an numerical Jordan decomposition
%                      X*J*inv(X)
%  of a given matrix A within a distance threshold theta, formulated under
%  a 'three-strikes principle'.  For details, see
%     Z. Zeng and T.Y. Li,  A numerical algorithm for computing the Jordan
%        Canonical Form, preprint, 2007
%
%  In a nutshell, let A be a matrix whose entries are given approximately
%  with error magnitude being small.  The exact JCF of A will be degraded.
%  However, it is possible to recover the underlying Jordan structure and
%  approximate X and J by NumericalJoranForm
%
%  Syntax:  There are several choices for either LHS or RHS
%        >>   LHS      =      RHS
%  ------------------- | --------------------------------------
%   [J,X]              |   RegularizedJCF(A)
%   [J,X,e,s,t]        |   RegularizedJCF(A,theta)
%                      |   RegularizedJCF(A,theta,tau,gap)
%
% Input:    A -- matrix whose AJCF is to be computed
%       theta -- (optional) distance threshold
%         tau -- (optional) deflation threshold, simple eigenvalues
%                  whose geometric condition numbers above tau will be deflated
%         gap -- (optional) singular value gap used in rank revealing
%
% Output:   J -- The numerical Jordan Canonical Form
%           X -- The principle vector matrix
%           e -- the list of distinct eigenvalues
%           s -- The Jordan block sizes of the eigenvalues
%           t -- the residuals (backward errors) and the staircase condition
%                  numbers of the eigenvalues


function [Afp,Bfp]=freeprecess(T,T1,T2,df)
%
%	Function simulates free precession and decay
%	over a time interval T, given relaxation times T1 and T2
%	and off-resonance df.  Times in ms, off-resonance in Hz.

phi = 2*pi*df*(T/1000);	% Off-resonance precession, radians.

% Relaxation exponentials
E1 = exp(-T/T1);
E2 = exp(-T/T2);

% Decay and phase due to off-resonance
Afp = [E2 0 0;0 E2 0;0 0 E1]*zrot(phi); % Mathieu, check order of matrix multiplication

% Regrowth
Bfp = [0 0 1-E1]';


function u=transsplit(u0,a,b,x,y,T,nstep)
%%
% Computes an approximation to the solution of
%
% $$ u_t+  a u_x + b u_y=0 $$
%
% with initial data u0 on the rectangle spanned by the vectors x and y. u0 must be
% of the size |[length(x),length(y)]|.
%
% Output is a 3d matrix of size |[lentgth(x),length(y),((T/dt)+1)]|.
%
%% Initial setup
dt=T/nstep; Nt=ceil(T/dt); dt=T/Nt;
dx=x(2)-x(1); dy=y(2)-y(1);
u=zeros(length(x),length(y),Nt+1);

%% Solving by Strang splitting
u(:,:,1) = u0;
u1 = transport(b,u(:,:,1),0.5*dt,dx,1);   % Transport in the x-direction
for i=2:Nt,
   u(:,:,i) = transport(a,u1,      dt,dy,2);   % Transport in the y-direction
   u1       = transport(b,u(:,:,i),dt,dx,1);   % Transport in the x-direction
end
u(:,:,Nt+1) = transport(a,u1,            dt,dy,2);   % Transport in the y-direction
u(:,:,Nt+1) = transport(b,u(:,:,Nt+1),0.5*dt,dx,1);   % Transport in the x-direction


%--------------------------------------------------------------------------
%   W = word(N, theta_tgt, window_N, lamda, position, theta_jam)
%--------------------------------------------------------------------------
%   功能：
%   正交投影加宽零陷抗干扰
%--------------------------------------------------------------------------
%   输入:
%           N                       阵元数
%           theta_tgt               波束指向 度
%           window_N                指向加窗
%           lambda                  波长
%           position                阵列坐标
%           theta_jam               干扰来向
%   输出：
%           W                       加权系数
%--------------------------------------------------------------------------
%   例子:
%   N = 64;
%   lambda = 1;                                                             %波长
%   dd = lambda/2;                                                          %阵元间距d = lambda/2
%   d = 0:dd:(N-1)*dd;                                                      %构建阵列坐标
%   W= word( 64, 0,@taylorwin, lambda, d, theta_jam);                       %生成权系数
%--------------------------------------------------------------------------
function  W = word(N, theta_tgt, window_N, lamda, position, theta_jam)
%--------------------------------------------------------------------------
%   指向角的导向矢量
%--------------------------------------------------------------------------
As = window_N(N).* ...
     exp(1j*2*pi*position*sind(theta_tgt)/lamda).';

%--------------------------------------------------------------------------
%   生成单位阵
%--------------------------------------------------------------------------
In= eye( N);
Ai = [Ai exp( 1j* 2* pi* position* sind( theta_jam(idx))/ lamda).'];

%--------------------------------------------------------------------------
%   干扰来向构造矩阵
%--------------------------------------------------------------------------
P =  In - Ai*( Ai'* Ai)^-1*Ai';                                             %干扰来向的投影矩阵的 正交子空间
W_temp=(As'*P)';
W= W_temp/sqrt( W_temp'* W_temp);                                           %归一化


function extended_rosenbrock_test ( )

%*****************************************************************************80
%
%% EXTENDED_ROSENBROCK_TEST works with the extended Rosenbrock function.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    04 January 2012
%
%  Author:
%
%    John Burkardt
%
  fprintf ( 1, '\n' );
  fprintf ( 1, 'EXTENDED_ROSENBROCK_TEST:\n' );
  fprintf ( 1, '  Test COMPASS_SEARCH with the extended Rosenbrock function.\n' );
  m = 4;
  delta_tol = 0.00001;
  delta = 0.3;
  k_max = 20000;

  x = [ - 1.2, 1.0,  -1.5, 1.2 ];
  r8vec_print ( m, x, '  Initial point X0:' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  F(X0) = %g\n', extended_rosenbrock ( m, x ) );

  [ x, fx, k ] = compass_search ( @extended_rosenbrock, m, x, delta_tol, delta, k_max );
  r8vec_print ( m, x, '  Estimated minimizer X1:' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  F(X1) = %g, number of steps = %d\n', fx, k );
%
%  Demonstrate correct minimizer.
%
  x = [ 1.0, 1.0, 1.0, 1.0 ];
  r8vec_print ( m, x, '  Correct minimizer X*:' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  F(X*) = %g\n', extended_rosenbrock ( m, x ) );

  return
end


function f = f10_f0 ( n, x, y )

%*****************************************************************************80
%
%% F10_F0 returns the value of function 10.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 January 2012
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of evaluation points.
%
%    Input, real X(N,1), Y(N,1), the evalution points.
%
%    Output, real F(N,1), the function values.
%
  t1(1:n,1) = sqrt ( ( 80.0 * x(1:n,1) - 40.0 ).^2 + ( 90.0 * y(1:n,1) - 45.0 ).^2 );
  t2(1:n,1) = exp ( - 0.04 * t1(1:n,1) );
  t3(1:n,1) = cos ( 0.15 * t1(1:n,1) );

  f(1:n,1) = t2(1:n,1) .* t3(1:n,1);

  return
end


function sc = ssd(I,J,SII,SJJ)

% SSD  Sum of Squared Differences coefficient
%   SSD(I,J) computes the SSD score of matrices I and J:
%
%     SSD = 1/N*sum((I-J).^2)
%
%   where N = prod(size(I))
%   and   size(I) = size(J)
%
%   SSD(I,J,SII,SJJ) accepts useful intermediate results
%   that permit to speed up the calculations. These are:
%
%     SII = sum(sum(I.*I))
%     SJJ = sum(sum(J.*J))
%
%   See also PATCHCORR, ZNCC, CENSUS

% (c) 2005 Joan Sola

if size(I) ~= size(J)
    error ('Matrices must be the same size.')
else
    switch nargin
        case {1,2}
            SII = sum(sum(I.*I));
            SJJ = sum(sum(J.*J));
        case 3
            SJJ = sum(sum(J.*J));
    end

    SIJ = sum(sum(I.*J));

    N   = numel(I);

    sc  = sqrt((SII+SJJ-2*SIJ)/(N+eps));

end
% ========== End of function - Start GPL license ==========

%   # START GPL LICENSE

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

%   SLAMTB is Copyright:
%   Copyright (c) 2008-2010, Joan Sola @ LAAS-CNRS,
%   Copyright (c) 2010-2013, Joan Sola,
%   Copyright (c) 2014-2015, Joan Sola @ IRI-UPC-CSIC,
%   SLAMTB is Copyright 2009
%   by Joan Sola, Teresa Vidal-Calleja, David Marquez and Jean Marie Codol
%   @ LAAS-CNRS.
%   See on top of this file for its particular copyright.

%   # END GPL LICENSE


function p = hexagon_shape_2d ( angle )

%*****************************************************************************80
%
%% HEXAGON_SHAPE_2D returns points on the unit regular hexagon in 2D.
%
%  Diagram:
%
%      120_____60
%        /     \
%    180/       \0
%       \       /
%        \_____/
%      240     300
%
%  Discussion:
%
%    The unit regular hexagon has radius 1.  The radius is the distance from
%    the center to any vertex, and it is also the length of any side.
%    An example of a unit hexagon is the convex hull of the points:
%
%      (   1,              0 ),
%      (   0.5,   sqrt (3)/2 ),
%      ( - 0.5,   sqrt (3)/2 ),
%      ( - 1,              0 ),
%      ( - 0.5, - sqrt (3)/2 ),
%      (   0.5, - sqrt (3)/2 ).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    12 May 2005
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real ANGLE, the angle, in degrees, of the point.
%
%    Output, real P(2,1), the coordinates of the point.
%

%
%  Ensure that 0 <= ANGLE < 360.
%
  angle2 = r8_modp ( angle, 360.0 );
%
%  y = - sqrt(3) * x + sqrt(3)
%
  if ( 0.0 <= angle2 & angle2 <= 60.0 )

    p(1,1) = sqrt ( 3.0 ) / ( tan_deg ( angle2 ) + sqrt ( 3.0 ) );
    p(2,1) = tan_deg ( angle2 ) * p(1,1);
%
%  y = sqrt(3) / 2
%
  elseif ( angle2 <= 120.0 )

    p(2,1) = sqrt ( 3.0 ) / 2.0;
    p(1,1) = cot_deg ( angle2 ) * p(2,1);
%
%  y = sqrt(3) * x + sqrt(3)
%
  elseif ( angle2 <= 180.0 )

    p(1,1) = sqrt ( 3.0 ) / ( tan_deg ( angle2 ) - sqrt ( 3.0 ) );
    p(2,1) = tan_deg ( angle2 ) * p(1,1);
%
%  y = - sqrt(3) * x - sqrt(3)
%
  elseif ( angle2 <= 240.0 )

    p(1,1) = - sqrt ( 3.0 ) / ( tan_deg ( angle2 ) + sqrt ( 3.0 ) );
    p(2,1) = tan_deg ( angle2 ) * p(1,1);
%
%  y = - sqrt(3) / 2
%
  elseif ( angle2 <= 300.0 )

    p(2,1) = - sqrt ( 3.0 ) / 2.0;
    p(1,1) = cot_deg ( angle2 ) * p(2,1);
%
%  y = sqrt(3) * x - sqrt(3)
%
  elseif ( angle2 <= 360.0 )

    p(1,1) = - sqrt ( 3.0 ) / ( tan_deg ( angle2 ) - sqrt ( 3.0 ) );
    p(2,1) = tan_deg ( angle2 ) * p(1,1);

  end

  return
end


function K = gaussian_correlation ( s, t )

%*****************************************************************************80
%
%% GAUSSIAN_CORRELATION evaluates the Gaussian correlation function.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    15 February 2012
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Petter Abrahamsen,
%    A Review of Gaussian Random Fields and Correlation Functions,
%    Norwegian Computing Center, 1997.
%
%  Parameters:
%
%    Input, real S(*), T(*), pairs of argument values.
%
%    Output, real K(*), the correlation function values
%
  K = exp ( - ( s - t ).^2 );

  return
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);
%

for c = 1:num_labels
    init_theta = zeros(n + 1, 1);
    options = optimset('GradObj', 'on', 'MaxIter', 50);
    [theta] = fmincg (@(t)(lrCostFunction(t, X, (y == c), lambda)), init_theta, options);
    all_theta(c,:) = theta';
end;

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

end


function b = isPointOnRay(point, ray, varargin)
%ISPOINTONRAY Test if a point belongs to a ray
%
%   B = isPointOnRay(PT, RAY);
%   Returns 1 if point PT belongs to the ray RAY.
%   PT is given by [x y] and RAY by [x0 y0 dx dy].
%
%   If PT is a N-by-2 array, and RAY is a M-by-4 array, then the result is
%   a N-by-M array containing the result of each pair-wise test.
%
%   B = isPointOnRay(PT, RAY, TOL);
%   Specifies the tolerance to use for testing if point is on the ray.
%
%   Example
%     ray = [10 20 3 4];
%     % test for a point on the ray
%     p1 = [16 28];
%     isPointOnRay(p1, ray)
%     ans =
%       logical
%        0
%     % test for a point on the supporting line but "before" the origin
%     p2 = [7 16];
%     isPointOnRay(p1, ray)
%     ans =
%       logical
%        0
%
%   See also:
%   rays2d, points2d, isPointOnLine, isPointOnEdge
%

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

%   HISTORY
%   07/07/2005 normalize condition to test if on the line and add support
%       of multiple rays or points
%   22/05/2009 rename to isPointOnRay, add psb to specify tolerance
%   26/01/2010 was drawing a line before making test

% extract computation tolerance
tol = 1e-14;
if ~isempty(varargin)
    tol = varargin{1};
end

% number of rays and points
Nr = size(ray, 1);
Np = size(point, 1);

% if several rays or several points, adapt sizes of arrays
x0 = repmat(ray(:,1)', Np, 1);
y0 = repmat(ray(:,2)', Np, 1);
dx = repmat(ray(:,3)', Np, 1);
dy = repmat(ray(:,4)', Np, 1);
xp = repmat(point(:,1), 1, Nr);
yp = repmat(point(:,2), 1, Nr);

% test if points belongs to the supporting line
b1 = abs((xp-x0).*dy - (yp-y0).*dx) ./ (dx.*dx + dy.*dy) < tol;

% check if points lie the good direction on the rays
ind     = abs(dx) > abs(dy);
t       = zeros(size(b1));
t(ind)  = (xp(ind) - x0(ind)) ./ dx(ind);
t(~ind) = (yp(~ind) - y0(~ind)) ./ dy(~ind);

% combine the two tests
b = b1 & (t >= 0);


function a = line_adj_eigen_right ( n )

%*****************************************************************************80
%
%% LINE_ADJ_EIGEN_RIGHT returns the right eigenvectors of the LINE_ADJ matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    06 April 2015
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of A.
%
%    Output, real A(N,N), the right eigenvector matrix.
%
  a = zeros ( n, n );

  for i = 1: n
    for j = 1 : n
      angle = ( i * j ) * pi / ( n + 1 );
      a(i,j) = sqrt ( 2.0 / ( n + 1 ) ) * sin ( angle );
    end
  end

  return
end


function dist = distancePoints(p1, p2, varargin)
%DISTANCEPOINTS Compute distance between two points.
%
%   D = distancePoints(P1, P2)
%   Return the Euclidean distance between points P1 and P2.
%
%   If P1 and P2 are two arrays of points, result is a N1-by-N2 array
%   containing distance between each point of P1 and each point of P2.
%
%   D = distancePoints(P1, P2, NORM)
%   Compute distance using the specified norm. NORM=2 corresponds to usual
%   euclidean distance, NORM=1 corresponds to Manhattan distance, NORM=inf
%   is assumed to correspond to maximum difference in coordinate. Other
%   values (>0) can be specified.
%
%   D = distancePoints(..., 'diag')
%   compute only distances between P1(i,:) and P2(i,:).
%
%   See also:
%   points2d, minDistancePoints, nndist, hausdorffDistance
%

% ------
% Author: David Legland
% e-mail: david.legland@nantes.inra.fr
% Copyright 2009 INRA - Cepia Software Platform.
% created the 24/02/2004.
%

%   HISTORY :
%   25/05/2004: manage 2 array of points
%   07/04/2004: add option for computing only diagonal.
%   30/10/2006: generalize to any dimension, and manage different norms
%   03/01/2007: bug for arbitrary norm, and update doc
%   28/08/2007: fix bug for norms 2 and infinite, in diagonal case

%% Setup options

% default values
diag = false;
norm = 2;

% check first argument: norm or diag
if ~isempty(varargin)
    var = varargin{1};
    if isnumeric(var)
        norm = var;
    elseif strncmp('diag', var, 4)
        diag = true;
    end
    varargin(1) = [];
end

% check last argument: diag
if ~isempty(varargin)
    var = varargin{1};
    if strncmp('diag', var, 4)
        diag = true;
    end
end

% number of points in each array and their dimension
n1  = size(p1, 1);
n2  = size(p2, 1);
d   = size(p1, 2);

if diag
    % compute distance only for apparied couples of pixels
    dist = zeros(n1, 1);

    if norm == 2
        % Compute euclidian distance. this is the default case
        % Compute difference of coordinate for each pair of point
        % and for each dimension. -> dist is a [n1*n2] array.
        for i = 1:d
            dist = dist + (p2(:,i)-p1(:,i)).^2;
        end
        dist = sqrt(dist);

    elseif norm == inf
        % infinite norm corresponds to maximal difference of coordinate
        for i = 1:d
            dist = max(dist, abs(p2(:,i)-p1(:,i)));
        end

    else
        % compute distance using the specified norm.
        for i = 1:d
            dist = dist + power((abs(p2(:,i)-p1(:,i))), norm);
        end
        dist = power(dist, 1/norm);
    end
else
    % compute distance for all couples of pixels
    dist = zeros(n1, n2);

    if norm == 2
        % Compute euclidian distance. This is the default case.
        % Compute difference of coordinate for each pair of point
        % and for each dimension. -> dist is a [n1*n2] array.
        for i = 1:d
            % equivalent to:
            % dist = dist + ...
            %   (repmat(p1(:,i), [1 n2])-repmat(p2(:,i)', [n1 1])).^2;
            dist = dist + bsxfun (@minus, p1(:,i), p2(:, i)').^2;
        end
        dist = sqrt(dist);

    elseif norm == inf
        % infinite norm corresponds to maximal difference of coordinate
        for i = 1:d
            dist = max(dist, abs(bsxfun (@minus, p1(:,i), p2(:, i)')));
        end

    else
        % compute distance using the specified norm.
        for i = 1:d
            % equivalent to:
            % dist = dist + power((abs(repmat(p1(:,i), [1 n2]) - ...
            %     repmat(p2(:,i)', [n1 1]))), norm);
            dist = dist + power(abs(bsxfun(@minus, p1(:,i), p2(:, i)')), norm);
        end
        dist = power(dist, 1/norm);
    end
end


function [ err ] = residual_KR_robust( X1, X2, imsize1, imsize2, paras, sigma )

% parameretes sigma indicates the distance scope for inliers with homopraphy matrix, in pixels

k1 = paras(1);
k2 = paras(2);
theta = paras(3:5);
% yaw = paras(3);
% pitch = paras(4);
% roll = paras(5);

K1 = [k1, 0, imsize1(2)/2;
     0, k1, imsize1(1)/2;
     0,  0, 1];
K2 = [k2, 0, imsize2(2)/2;
      0, k2, imsize2(1)/2;
      0,  0, 1];
theta_m = [0         -theta(3) theta(2)
           theta(3)  0         -theta(1)
           -theta(2) theta(1)  0];
R = expm(theta_m);
% Ry = [cos(yaw),     0,              -sin(yaw);
%       0,            1,              0;
%       sin(yaw),     0,              cos(yaw)] ;
% Rp = [1,            0               0;
%       0,            cos(pitch),     sin(pitch);
%       0,            -sin(pitch),    cos(pitch)] ;
% Rr = [cos(roll),    sin(roll),      0;
%       -sin(roll),   cos(roll),      0;
%       0,            0,              1] ;
% R = Rr * Rp * Ry;

err = residual_H(X1, X2, K1, K2, R);

outlier = (abs(err) > sigma);
err(outlier) = sign(err(outlier)) .* (sigma + sigma * log(abs(err(outlier))/sigma));
% err(outlier) = sign(err(outlier)) .* sqrt(2*sigma*abs(err(outlier)) - sigma*sigma);

end


function [xs,ys] = snakeIterate(alpha,beta,gamma,x,y,NI,Fx,Fy)
%SNAKEITERATE Iterative solution of the snake equation.
%   [XS,YS] = SNAKEITERATE(ALPHA,BETA,GAMMA,X,Y,NI,Fx,Fy) computes the
%   [XS,YS] coordinates of a segmentation snake using the iterative
%   solution in Eq.(12-7) of DIPUM3E. Vectors X and Y are the initial
%   coordinates of the snake (provided in sequential order). These
%   vectors are updated during iteration. ALPHA, BETA, and GAMMA are
%   parameters in Eq. (12-7) and (12-8), and Fx, Fy are the 2D force
%   arrays obtained, for example, using DIPUM3E function snakeForce.
%
%   This function is normally run within an outer loop with snake-point
%   respacing after each execution of the loop. NI controls the number
%   of iterations of Eq. (12-7) before the snake points are respaced. A
%   common value of NI is 1, indicating one execution of point respacing
%   after each iteration of Eq. (12-7).
%
%   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

% PRELIMINARIES.
K = numel(x);
% Multiply the forces by gamma.
Fx = gamma*Fx;
Fy = gamma*Fy;

% CONSTRUCT MATRIX A IN EQ. (12-8) FOR USE IN EQ. (12-7).
% First construct matrix D2 in Eq. (12-9).
a = -2*ones(K,1);
b = 1*ones(K-1,1);
D2 = diag(a) + diag(b,-1) + diag(b,1);
D2(1,K) = 1;
D2(K,1) = 1;
% Next construct D4 in Eq. (12-10).
a = 6*ones(K,1);
b = -4*ones(K-1,1);
c = 1*ones(K-2,1);
D4 = diag(a) + diag(b,-1) + diag(b,1) + diag(c,-2) + diag(c,2);
D4(1,K) = -4;
D4(K,1) = -4;
D4(1,K-1) = 1;
D4(K-1,1) = 1;
D4(2,K) = 1;
D4(K,2) = 1;
% Construct matrix A. The inverse operation is performed during
% iteration.
D = alpha*D2 - beta*D4;
A = eye(K) - D;

% ITERATIVE SOLUTION.
for I = 1:NI
	% Obtain the force vectors fx and fy in Eq. (12-7). These are
	% obtained from Fx and Fy, the 2-D array components of the force
	% field F, by interpolating values of Fx and Fy at the locations of
	% the current snake coordinates, x and y. The 0 in the following
	% function call avoids NaNs in areas where there are no points to
	% interpolate. The order y,x is because function interp2 works with
	% (col,row), as opposed to (row,col) as in the book.
	fx = interp2(Fx,y,x,'linear',0);
	fy = interp2(Fy,y,x,'linear',0);

	% Compute new values of x and y using Eq. (12-7). Note the use of x =
	% A\(x + fx), as opposed to x = inv(A)*(x + fx), and similarly for y.
	% The former method is faster and more accurate.
	x = A\(x + fx);
	y = A\(y + fy);
end

% FORM THE SNAKE.
xs = x;
ys = y;


function v = legendre_van ( m, a, b, n, x )

%*****************************************************************************80
%
%% LEGENDRE_VAN returns the LEGENDRE_VAN matrix for [A,B].
%
%  Discussion:
%
%    The LEGENDRE_VAN matrix is the Legendre Vandermonde-like matrix.
%
%    Normally, the Legendre polynomials are defined on -1 <= XI <= +1.
%    Here, we assume the Legendre polynomials have been defined on the
%    interval A <= X <= B, using the mapping
%      XI = ( - ( B - X ) + ( X - A ) ) / ( B - A )
%    so that
%      Lab(A,B;X) = L(XI).
%
%    if ( I = 1 ) then
%      V(1,1:N) = 1
%    else if ( I = 2 ) then
%      V(2,1:N) = XI(1:N)
%    else
%      V(I,1:N) = ( (2*I-1) * XI(1:N) * V(I-1,1:N) - (I-1)*V(I-2,1:N) ) / I
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 April 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, the number of rows of the matrix.
%
%    Input, real A, B, the limits of the interval.
%
%    Input, integer N, the number of columns of the matrix.
%
%    Input, real X(N), the abscissas.
%
%    Output, real V(M,N), the matrix.
%

%
%  Force X to be a row vector.
%
  x = ( x(:) .' );
%
%  Compute the normalized abscissas in [-1,+1].
%
  xi(1,1:n) = ( - 1.0 * ( b - x(1,1:n)     )   ...
                + 1.0 * (     x(1,1:n) - a ) ) ...
              /         ( b            - a );
%
%  Set up the matrix.
%
  v = zeros ( m, n );

  for i = 1 : m

    if ( i == 1 )
      v(i,1:n) = 1.0;
    elseif ( i == 2 )
      v(i,1:n) = xi(1,1:n);
    else
      v(i,1:n) = ( ( 2 * i - 1 ) * xi(1,1:n) .* v(i-1,1:n)   ...
                 + (   - i + 1 ) *              v(i-2,1:n) ) ...
                 / (     i );
    end

  end

  return
end