clear all; close all; clc

% A=randn(10,10,10);
% model=parafac(A,3);
% [A1,A2,A3]=fac2let(model)

x=-5:0.1:5; y=-6:0.1:6; t=0:0.1:10*pi;
[X,Y,T]=meshgrid(x,y,t);
A=exp(-(X.^2+0.5*Y.^2)).*(cos(2*T))+ ...
    (sech(X).*tanh(X).*exp(-0.2*Y.^2)).*sin(T);

for j=1:length(t)
  pcolor(x,y,A(:,:,j)), shading interp, caxis([-1 1]), drawnow
end

figure(1)
for j=1:8
  subplot(2,4,j)
  pcolor(x,y,A(:,:,8*j-3)), colormap(hot), shading interp, caxis([-1 1]), axis off
end

figure(2)
model=parafac(A,2);
[A1,A2,A3]=fac2let(model);
subplot(3,1,1), plot(y,A1,'Linewidth',[2])
subplot(3,1,2), plot(x,A2,'Linewidth',[2])
subplot(3,1,3), plot(t,A3,'Linewidth',[2])

subplot(3,1,1), set(gca,'Xtick',[-6 0 6],'Fontsize',[15])
subplot(3,1,2), set(gca,'Xtick',[-5 0 5],'Fontsize',[15])
subplot(3,1,3), set(gca,'Xlim',[0 10*pi],'Xtick',[0 5*pi 10*pi],'Xticklabels',{'0','5\pi','10\pi'},'Fontsize',[15])


function sliderc(r,L1,rec,sp)
% SLIDERC Slider Crank Mechanism
% sliderc(r,L1,rec) plots the slider crank mechanism specified and rotates it,
% where:
%
% r   : Radius of crank
% L1  : Length of piston
% rec : Side lengths of slider
% sp  : Speed of rotation
%
% Example 1:
%
% sliderc(1,3,[1,.5],.5)
%
%
% Example 2:
%
% sliderc(2,5,[5,.5],2)
%
%
% Uses the function Draw a Circle by Zhenhai Wang on the MATLAB File Exchange.
%
% numandina@gmail.com

L2=rec(1); % get horizontal side of slider
L3=rec(2); % get vertical side of slider
g=circle([0 0],r,100); % draw the crank
grid on
hold on
t=0; % originally, the angle theta is zero

xL1=r*cos(t); % the first piston end x location
yL1=r*sin(t); % the first piston end y location

xL2=r*cos(t)+(L1^2-yL1^2)^(1/2); % the second piston end x location (Pythagoras theorem)
yL2=0;                           % the second piston end y location

ln=line([xL1 xL2],[yL1 yL2]); % draw the piston
set([ln,g],'linewidth',3,'color','k') % format the lines
plot(0,0,'k+') % centre of crank
set(gcf,'color','w','menubar','none') % more formatting

while true % rotate indefinitely

	% piston start and end x and y locations, just like before
	xL1=r*cos(t);
	yL1=r*sin(t);
	xL2=r*cos(t)+(L1^2-yL1^2)^(1/2);
	yL2=0;

	set(ln,'xdata',[xL1 xL2],'ydata',[yL1 yL2]); % draw the piston accordingly

	% put coloured points at piston start and end locations and update them
	delete(findobj('marker','.'))
	plot([xL1 xL2],[yL1 yL2],'r.')

	% slider location points

	xp1=xL2; % first end x location is the same as the piston second end x location, since they are connected
	xp2=xL2+L2; % second end x location depends on the horizontal side length

	yp1=yL2-L3/2; %	the vertical side lengths determine the rest of the slider shape.
	yp2=yL2+L3/2;

	gr=[.5 .5 .5]; % RGB colour vector, [1 1 1] is white, [0 0 0] is black, so [.5 .5 .5] is grey

 	pl=patch([xp1,xp1,xp2,xp2],[yp1,yp2,yp2,yp1],gr); % draw the slider and paint it grey

 	axis([-(r+1) r+L1+L2+1 min([-(r+1),-(L3/2+1)]) max([r+1,L3/2+1])]) % adjust the axis limits
	axis equal % so that the circle would appear proper
	drawnow()  % update axes
 	delete(pl) % delete slider so it would be updated in next iteration
	t=t+.1*sp; % adjust theta for next iteration
end

end


function [mu,varOut,numVals] = knuthMeanVar(val,newCalcFlag)
% function [mu,varOut] = knuthMeanVar(val,newCalcFlag)
%
% Knuth method for mean and standard deviation (like ITK)
%
% Input: value to add to the existing mean and variance calculation.
%
% Output: Mean and variance (var(xV,1))
%
% Example:
% newCalcFlag = true;
% knuthMeanVar([],true);
% valV = [1:10,100:105];
% for i = 1:length(valV)
%     [mu,varOut,numVals] = knuthMeanVar(valV(i));
% end
%
% To start a new calculation:
% newCalcFlag = true;
% knuthMeanVar([],newCalcFlag);
% valV = [1:5];
% for i = 1:length(valV)
%     [mu,varOut,numVals] = knuthMeanVar(valV(i));
% end
%
% APA, 6/24/2019

if exist('newCalcFlag','var') && newCalcFlag
    clear persistent n
    clear persistent muPrev
    clear persistent varN
end

persistent n;
persistent muPrev;
persistent varN;

if isempty(val)
    numVals = n;
    mu = muPrev;
    varOut = varN/n;
    return;
end
if isempty(muPrev)
    muPrev = 0;
    varN = 0;
    n = 0;
end
n = n + 1;
mu = muPrev + (val - muPrev)/n;
varN = varN + (val-mu)*(val-muPrev);
muPrev = mu;
varOut = varN/n;
numVals = n;


function determ = circulant2_determinant ( n )

%*****************************************************************************80
%
%% CIRCULANT2_DETERMINANT returns the determinant of the CIRCULANT2 matrix.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 November 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the order of the matrix.
%
%    Output, real DETERM, the determinant.
%
  w = c8vec_unity ( n );

  lambda = zeros ( n, 1 );
  lambda(1:n) = n;
  for i = n-1 : -1 : 1
    lambda(1:n) = lambda(1:n) .* w(1:n) + i;
  end
%
%  First eigenvalue is "special".
%
  determ = real ( lambda(1) );
%
%  Eigenvalues 2, 3     through ( N + 1 ) / 2 are paired with complex conjugates.
%
  for i = 2 : floor ( ( n + 1 ) / 2 )
    determ = determ * ( abs ( lambda(i) ) )^2;
  end
%
%  If N is even, there is another unpaired eigenvalue.
%
  if ( mod ( n, 2 ) == 0 )
    determ = determ * real ( lambda((n/2)+1) );
  end

  return
end


function y = wprctile(x, p, w)
% WPRCTILE Percentiles of a weighted sample.
%
%    Description
%      Y = PRCTILE(X, P, W) returns percentiles of the values in X. P
%       is a scalar or a vector of percent values. W is a vector of
%       unnormalized weights for samples. Length of W has to be same
%       as length of X. X need to be a a vector. Y is the same size as
%       P, and Y(i) contains the P(i)-th percentile.
%
%      Example
%       y = prctile(x,50,w); % the median of x given sample weights w
%
%    See also wmean, prctile

% BUGS: Accepts only vector valued X

% Copyright (c) 2000-2010 Aki Vehtari

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

x=sort(x);
p=p./100;
y=p;
ww=cumsum(w);ww=ww./ww(end);
for j=1:length(p)
  wi=min(find(ww>=p(j)));
  if wi==1
    y(j)=x(1);
  else
    w1=ww(wi-1);x1=x(wi-1);
    y(j)=x1+(x(wi)-x1).*(p(j)-w1)./(ww(wi)-w1);
  end
end


function d=sqdist(a,b)
% SQDIST - computes squared Euclidean distance matrix
%          computes a rectangular matrix of pairwise distances
% between points in A (given in columns) and points in B

% NB: very fast implementation taken from Roland Bunschoten

aa = sum(a.*a,1); bb = sum(b.*b,1); ab = a'*b;
d = abs(repmat(aa',[1 size(bb,2)]) + repmat(bb,[size(aa,2) 1]) - 2*ab);


function q=rotMat2Quat(R,handed)
%%ROTMAT2QUAT Get a unit quaternion corresponding to a particular rotation
%             matrix. The quaternion can be chosen to support standard
%             right-handed quaternion multiplication rules, or non-standard
%             left-handed rules that some authors choose to use.
%
%INPUTS:  M A 3X3 orthonormal real rotation matrix.
%    handed The handedness of the quaternion. If omitted, it is assumed
%           that the quaternion is right-handed (the standard). Possible
%           values are:
%           'right' The default if omitted. The quaternion multiplication
%                   is assumed right-handed (standard).
%           'left'  The quaternion multiplication is assumed left-handed.
%                   This is used in someplaces, including the reference
%                   from Shuster, below.
%
%OUTPUTS: q A 4X1 unit quaternion corresponding to the rotation matrix. The
%           quaternion is ordered [cos(theta/2);sin(theta/2)u'] where u is
%           a unit vector for the axis of rotation and theta is the
%           counterclockwise (right-handed) or clockwise (left-handed)
%           rotation angle about that unit vector.
%
%The formulae for converting a rotation matrix are from [1], where a minor
%change has been performed to support both right and left-handed
%quaternions. Both q and -q represent the same rotations. Here, the
%solution is chosen so that when considering a left-handed rotation, the
%sign of the largest magnitude element is positive. When considering a
%right handed rotation, the sign of the largest element may or may not be
%positive.
%
%A quaternion of form q(1)+i*q(2)+j*q(3)+k*q(4) that obeys right-handed
%multiplication rules supports the following rules for multiplication of i,
%j, and k, where an item in a row is multiplied by an item in the column to
%get the result:
%  i,  j, k
%i -1, k,-j
%j -k,-1, i
%k  j,-i,-1
%On the other hand, left-handed multiplication rules flip the signs of the
%off-diagonal terms:
%  i,  j, k
%i -1,-k, j
%j  k,-1,-i
%k -j, i,-1
%
%REFERENCES:
%[1] W. F. Phillips, C. E. Hailey, and G. A. Gebert, "Review of attitude
%    representations used for aircraft kinematics," Journal of Aircraft,
%    vol. 38, no. 4, pp. 718-737, Jul. - Aug. 2001.
%
%August 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<2||isempty(handed))
    handed='right';
end

R11=R(1,1);
R22=R(2,2);
R33=R(3,3);
R12=R(1,2);
R21=R(2,1);
R13=R(1,3);
R31=R(3,1);
R23=R(2,3);
R32=R(3,2);

%The square of the quaternion...
q2=(1/4)*[1+R11+R22+R33;
          1+R11-R22-R33;
          1-R11+R22-R33;
          1-R11-R22+R33];

q2Max=max(q2);

if(q2Max==q2(1))
    q0=0.5*sqrt(1+R11+R22+R33);
    q1=(1/(4*q0))*(R23-R32);
    q2=(1/(4*q0))*(R31-R13);
    q3=(1/(4*q0))*(R12-R21);
elseif(q2Max==q2(2))
    q1=0.5*sqrt(1+R11-R22-R33);
    q0=(1/(4*q1))*(R23-R32);
    q2=(1/(4*q1))*(R12+R21);
    q3=(1/(4*q1))*(R31+R13);
elseif(q2Max==q2(3))
    q2=0.5*sqrt(1-R11+R22-R33);
    q0=(1/(4*q2))*(R31-R13);
    q1=(1/(4*q2))*(R12+R21);
    q3=(1/(4*q2))*(R23+R32);
else
    q3=0.5*sqrt(1-R11-R22+R33);
    q0=(1/(4*q3))*(R12-R21);
    q1=(1/(4*q3))*(R31+R13);
    q2=(1/(4*q3))*(R23+R32);
end

%The above implementation provides a left-handed quaternion, so the sign of
%the vector part needs to be flipped if the standard right-handed system is
%used.
switch(handed)
    case 'right'
        q=[q0;-q1;-q2;-q3];
    case 'left'
        q=[q0;q1;q2;q3];
    otherwise
        error('Invalid handedness provided.')
end
end

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


function h = qqPlot(varargin)
%QQPLOT Quantile-quantile plot with patch option
%
%   IOSR.STATISTICS.QQPLOT(Y) displays a quantile-quantile plot of the
%   sample quantiles of Y versus theoretical quantiles from a normal
%   distribution. If the distribution of Y is normal, the plot will be
%   close to linear.
%
%   IOSR.STATISTICS.QQPLOT(X,Y) displays a quantile-quantile plot of two
%   samples. If the samples come from the same distribution, the plot will
%   be linear.
%
%   The inputs X and Y should be numeric and have an equal number of
%   elements; every element is treated as a member of the sample.
%
%   The plot displays the sample data with the plot symbol 'x'.
%   Superimposed on the plot is a dashed straight line connecting the first
%   and third quartiles.
%
%   IOSR.STATISTICS.QQPLOT(...,MODE) allows the appearance of the plot to
%   be configured. With MODE='line' (default), the plot appears as
%   described above. With MODE='patch', the data are plotted as a patch
%   object, with the area bound by the x-distribution and the linear fit
%   shaded grey. With mode='both' the two appearances are combined.
%
%   IOSR.STATISTICS.QQPLOT(...,MODE,METHOD) and
%   IOSR.STATISTICS.QQPLOT(...,[],METHOD) allows the method for calculating
%   the quartiles, used for the fit line, to be specified. The default is
%   'R-8'. Type 'help iosr.statistics.quantile' for more information. The
%   latter form of the function call uses the default mode.
%
%   H = IOSR.STATISTICS.QQPLOT(...) returns a two- or three-element vector
%   of handles to the plotted object. The nature of the handles depends
%   upon the mode. In all cases, the first handle is to the sample data,
%   the second handle is to the fit line. With MODE='patch' or MODE='both',
%   there is third handle to the patch object.
%
%   Example
%
%      % Display Q-Q plots for the rand and randn functions
%      figure
%      subplot(2,1,1)
%      iosr.statistics.qqPlot(rand(20),'patch')
%      subplot(2,1,2)
%      h = iosr.statistics.qqPlot(randn(20),'patch');
%      set(h(3),'FaceColor','r') % change fill color
%
%   See also IOSR.STATISTICS.QUANTILE, IOSR.STATISTICS.BOXPLOT.

%   Copyright 2016 University of Surrey.

    %% determine X and Y

    IXn = cellfun(@(x) isnumeric(x) & ~isempty(x),varargin);

    switch sum(IXn)
        case 0
            error('iosr:qqPlot:noData','No input data specified')
        case 1
            % compare to normal distrbution
            Y = get_input_sample(varargin,IXn);
            p = (.5:length(Y))/length(Y);
            X = sqrt(2)*erfinv(2*p - 1);
            x_label = 'Standard normal quantiles';
            y_label = 'Sample quantiles';
        case 2
            % compare to input data distribution
            Y = get_input_sample(varargin,find(IXn,1,'last'));
            X = get_input_sample(varargin,find(IXn,1,'first'));
            assert(isequal(size(X),size(Y)), 'iosr:quantile:invalidInput', 'Input data must be the same size')
            x_label = 'X quantiles';
            y_label = 'Y quantiles';
        otherwise
            error('iosr:qqPlot:unkonwnInput','Unknown input specified')
    end

    %% determine mode and method

    % find inputs
    IXc = cellfun(@(x) ischar(x) | isempty(x),varargin);
    switch sum(IXc)
        case 0
            mode = [];
            method = [];
        case 1
            mode = varargin{IXc};
            method = [];
        case 2
            mode = varargin{find(IXc,1,'first')};
            method = varargin{find(IXc,1,'last')};
        otherwise
            error('iosr:qqPlot:unknownString','Unknown string specified')
    end

    % defaults
    if isempty(mode)
        mode = 'line';
    end
    if isempty(method)
        method = 'R-8';
    end

    %% calculate fit to first and third quartile

    % quartiles
    q1x = iosr.statistics.quantile(X,.25,[],method);
    q3x = iosr.statistics.quantile(X,.75,[],method);
    q1y = iosr.statistics.quantile(Y,.25,[],method);
    q3y = iosr.statistics.quantile(Y,.75,[],method);

    % slope
    slope = (q3y-q1y)./(q3x-q1x);
    centerx = (q1x+q3x)/2;
    centery = (q1y+q3y)/2;

    % fit
    maxx = max(X);
    minx = min(X);
    maxy = centery + slope.*(maxx - centerx);
    miny = centery - slope.*(centerx - minx);

    % lines
    X_fit = linspace(minx,maxx,length(X));
    Y_fit = linspace(miny,maxy,length(X));

    %% plot data

    hold on

    if strcmpi(mode,'patch') || strcmpi(mode,'both')
        Hp = patch([X fliplr(X_fit)],[Y fliplr(Y_fit)],[0.5 0.5 0.5]);
        set(Hp,'edgecolor','none')
    else
        Hp = NaN;
    end

    switch lower(mode)
        case 'line'
            linestyle = 'xk';
        case 'patch'
            linestyle = '-k';
        case 'both'
            linestyle = 'xk';
        otherwise
            error('iosr:qqPlot:unknownMode','Unknown mode specified')
    end

    H = plot(X,Y,linestyle,X_fit,Y_fit,'--k');

    hold off

    % axis labels
    xlabel(x_label)
    ylabel(y_label)

    box on; axis tight
    set(gca,'layer','top')

    % return handle
    if nargout>0
        h = H;
        if isobject(Hp) || ishandle(Hp)
            h = [h; Hp];
        end
    end

end

function Z = get_input_sample(z,IX)
%GET_INPUT_SAMPLE get sample, order, and convert to vector
    Z = z{IX};
    Z = sort(Z(:))';
end


function linplus_test62 ( )

%*****************************************************************************80
%
%% TEST62 tests R8VM_DET.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 March 2009
%
%  Author:
%
%    John Burkardt
%
  n = 10;
  seed = 123456789;

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST62\n' );
  fprintf ( 1, '  R8VM_DET, determinant of a Vandermonde matrix.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Matrix order N = %d\n', n );
%
%  Set the matrix.
%
  [ a, seed ] = r8vm_random ( n, n, seed );

  r8vm_print ( n, n, a, '  The Vandermonde matrix:' );
%
%  Copy the matrix into a general array.
%
  a2 = r8vm_to_r8ge ( n, n, a );
%
%  Compute the determinant.
%
  det = r8vm_det ( n, a );

  fprintf ( 1, '\n' );
  fprintf ( 1, '  R8VM_DET computes the determinant = %14.6e\n', det );
%
%  Factor the general matrix.
%
  [ a2_lu, pivot, info ] = r8ge_fa ( n, a2 );
%
%  Compute the determinant.
%
  det = r8ge_det ( n, a2_lu, pivot );

  fprintf ( 1, '  R8GE_DET computes the determinant = %14.6e\n', det );

  return
end


function quadrule_test13 ( )

%*****************************************************************************80
%
%% TEST13 tests LAGUERRE_EK_COMPUTE and LAGUERRE_SUM.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    23 April 2011
%
%  Author:
%
%    John Burkardt
%
  order_max = 20;

  nfunc = func_set ( 'COUNT', 'DUMMY' );

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST13\n' );
  fprintf ( 1, '  LAGUERRE_EK_COMPUTE computes a Gauss-Laguerre rule;\n' );
  fprintf ( 1, '  LAGUERRE_SUM carries it out.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Quadrature order will vary.\n' );
  fprintf ( 1, '  Integrand will vary.\n' );
  fprintf ( 1, '  The weight function is EXP ( - X ).\n' );
  fprintf ( 1, '\n' );

  a = 1.0;

  fprintf ( 1, '  The integration interval is [ %f, +oo ).\n', a );
  fprintf ( 1, '\n' );

  for ilo = 1 : 5 : nfunc

    ihi = min ( ilo + 4, nfunc );

    fprintf ( 1, '\n' );
    fprintf ( 1, '    ' );
    for i = ilo : ihi
      fprintf ( '%14s', fname(i) );
    end
    fprintf ( 1, '\n' );
    fprintf ( 1, '\n' );

    for norder = 1 : order_max

      fprintf ( 1, '  %2d', norder );

      for i = ilo : ihi

        func_set ( 'SET', i );

        [ xtab, weight ] = laguerre_ek_compute ( norder );

        result(i) = laguerre_sum ( @func, a, norder, xtab, weight );

        fprintf ( 1, '  %12f', result(i) );

      end

      fprintf ( 1, '\n' );

    end

  end

  return
end


function [tm,D2]=momftfr(tfr,tmin,tmax,time);
%MOMFTFR Frequency moments of a time-frequency representation.
%	[TM,D2]=MOMFTFR(TFR,TMIN,TMAX,TIME) computes the frequeny
%	moments of a time-frequency representation.
%
%	TFR    : time-frequency representation ([Nrow,Ncol]size(TFR)).
%	TMIN   : smallest column element of TFR taken into account
%	                            (default : 1)
%	TMAX   : highest column element of TFR taken into account
%	                            (default : Ncol)
%	TIME   : true time instants (default : 1:Ncol)
%	TM     : averaged time          (first order moment)
%	D2     : squared time duration  (second order moment)
%
%	Example :
%	 sig=fmlin(128,0.1,0.4);
%	 [tfr,t,f]=tfrwv(sig); [tm,D2]=momftfr(tfr);
%	 subplot(211); plot(f,tm); subplot(212); plot(f,D2);
%
%	See also MOMTTFR, MARGTFR.

%	F. Auger, August 1995.
%	Copyright (c) 1996 by CNRS (France).
%
%  This program is free software; you can redistribute it and/or modify
%  it under the terms of the GNU General Public License as published by
%  the Free Software Foundation; either version 2 of the License, or
%  (at your option) any later version.
%
%  This program is distributed in the hope that it will be useful,
%  but WITHOUT ANY WARRANTY; without even the implied warranty of
%  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
%  GNU General Public License for more details.
%
%  You should have received a copy of the GNU General Public License
%  along with this program; if not, write to the Free Software
%  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA

[tfrrow,tfrcol]=size(tfr);
if (nargin==1),
 tmin=1; tmax=tfrcol; time=tmin:tmax;
elseif (nargin==2),
 tmax=tfrcol; time=tmin:tmax;
elseif (nargin==3),
 time=tmin:tmax;
end;

if (tmin>tmax)|(tmin<=0)|(tmax>tfrcol),
 error('1<=TMIN<=TMAX<=Ncol');
end;

E  = sum(tfr(:,tmin:tmax).');
tm = (time    * tfr(:,tmin:tmax).' ./E).';
D2 = (time.^2 * tfr(:,tmin:tmax).' ./E).' - tm.^2;


function d = distToEpipolarLine(F, p1, p2)
%% Given a fundamental matrix F, compute the matched point pair to their
% corresponding epipolarlines' distances, then sum the two distances.
% Extracted from Matlab built-in function estimateFundamentalMatrix.m

% Very important: F must map a point p1 to an epipolar line in p2 image space, not the other way around.
% More details: http://stackoverflow.com/questions/26582960/sampson-error-for-five-point-essential-matrix-estimation

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

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

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

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

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

% convert to homogenious
p1 = [p1, ones(size(p1,1),1)]';
p2 = [p2, ones(size(p2,1),1)]';

% euclidean distance
d = sum(p2.*(F*p1), 1) .^ 2;

% sampson distance
epl1 = F * p1;
epl2 = F' * p2;
d = d ./ (epl1(1,:).^2 + epl1(2,:).^2 + epl2(1,:).^2 + epl2(2,:).^2);
end


function [ fx, fy ] = f01_f1 ( n, x, y )

%*****************************************************************************80
%
%% F01_F1 returns first derivatives of function 1.
%
%  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 FX(N,1), FY(N,1), the derivative values.
%
  t1(1:n,1) = exp ( - ( ( 9.0 * x(1:n,1) - 2.0 ).^2 ...
                    + ( 9.0 * y(1:n,1) - 2.0 ).^2 ) / 4.0 );
  t2(1:n,1) = exp ( - ( ( 9.0 * x(1:n,1) + 1.0 ).^2 ) / 49.0 ...
                    - ( 9.0 * y(1:n,1) + 1.0 ) / 10.0 );
  t3(1:n,1) = exp ( - ( ( 9.0 * x(1:n,1) - 7.0 ).^2 ...
                    + ( 9.0 * y(1:n,1) - 3.0 ).^2 ) / 4.0 );
  t4(1:n,1) = exp ( -   ( 9.0 * x(1:n,1) - 4.0 ).^2 ...
                    - ( 9.0 * y(1:n,1) - 7.0 ).^2 );

  fx(1:n,1) = ...
    - 3.375           * ( 9.0 * x(1:n,1) - 2.0 ) * t1 ...
    - ( 27.0 / 98.0 ) * ( 9.0 * x(1:n,1) + 1.0 ) * t2 ...
    - 2.25            * ( 9.0 * x(1:n,1) - 7.0 ) * t3 ...
    + 3.6             * ( 9.0 * x(1:n,1) - 4.0 ) * t4;

  fy(1:n,1) = ...
    - 3.375 * ( 9.0 * y(1:n,1) - 2.0 ) * t1 ...
    - 0.675                          * t2 ...
    - 2.25  * ( 9.0 * y(1:n,1) - 3.0 ) * t3 ...
    + 3.6   * ( 9.0 * y(1:n,1) - 7.0 ) * t4;

  return
end


% This is an approximation of random numbers wrt to wrapped Normal Distrib
%
% Any neater, faster, more precise and accurate generator is very welcome.
%
% Copyright (c) 2012 University of Crete - Computer Science Department (UOC-CSD)
%
% License
%  This file is under the LGPL license,  you can
%  redistribute it and/or modify it under the terms of the GNU Lesser General
%  Public License as published by the Free Software Foundation, either version 3
%  of the License, or (at your option) any later version. This file is
%  distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
%  without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
%  PARTICULAR PURPOSE. See the GNU Lesser General Public License for more
%  details.
%
% This function is part of the Covarep project: http://covarep.github.io/covarep
%
% Author
%  Gilles Degottex <degottex@csd.uoc.gr>
%

function wgr = wrappednormrnd(mu, sigma, n, pregen)

    if nargin<4; pregen=false; end

    % The CDF is symetric around the mean. Thus, just need zero-mean random values
    % of unitary std and scale them wrt sigma.
    wgr = wrappednormrndunitystd(n, pregen);

    wgr = sigma.*wgr;

    wgr = wrap(wgr + mu);

return

function wgr = wrappednormrndunitystd(n, pregen)

    if nargin<2; pregen=0; end

    if pregen==1
        global wrappednormrnd;

        % If run for the first time, initialize the cumulative distrib fns
        if isempty(wrappednormrnd) || ~isfield(wrappednormrnd, 'icdf')
            fprintf('wrappednormrnd.m: Pre-compute the cumulative distribution function once for all ... ');

            xs = -pi:(2*pi)/1e6:pi;
            xs = xs(1:end-1);
            cdf = wrappednormcdf(xs, 0, 1);
            % Resample the inverse CDF with 1 million uniform steps
            % This also assumes that we don't need a thinner resolution of the distribution
            wrappednormrnd.icdf_ys = (0:1e-4:1);
            wrappednormrnd.icdf = interp1(cdf, xs, wrappednormrnd.icdf_ys, 'linear', 'extrap').';

            fprintf('done.\n');
            if nargin==0; return; end
        end

        % wgr = interp1(wrappednormrnd.icdf_ys, sigma*wrappednormrnd.icdf, rand(n,1), 'linear', 'extrap');
        % The following is a speed-up version of the above line.
        ri = (length(wrappednormrnd.icdf)-1)*rand(n,1)+1;
        rfi = floor(ri);
        rci = ceil(ri);
        wgr = wrappednormrnd.icdf(rfi);
        wgr = wgr + (ri-rfi).*(wrappednormrnd.icdf(rci)-wgr);

        % % Or a faster sampling (implies only 1e4 different random values can be generated).
        % idx = round(length(wrappednormrnd.icdf)*rand(n,1))+1;
        % idx = max(1,min(length(wrappednormrnd.icdf),idx));
        % wgr = wrappednormrnd.icdf(idx);
    else
        % The non-optimized random generator
        xs = -pi:(2*pi)/1e6:pi;
        xs = xs(1:end-1);
        cdf = wrappednormcdf(xs, 0, 1);

        wgr = interp1(cdf, xs, rand(n,1), 'linear', 'extrap');
    end

return


function [ u_i ] = rotate_b2i( u_b, phi, theta, psi )
% ROTATE_B2I - Rotation of an object (vector/matrix) from the body to the
% inertial 3D-space
%
%
%

    cr = cos(phi);
    cp = cos(theta);
    cy = cos(psi);
    sr = sin(phi);
    sp = sin(theta);
    sy = sin(psi);

    % Rotation matrix from inertial frame to body frame

    Rbi = [cp*cy, sr*sp*cy-cr*sy, cr*sp*cy+sr*sy;...
            cp*sy, sr*sp*sy+cr*cy, cr*sp*sy-sr*cy;...
            -sp,   sr*cp,          cr*cp];

    u_i = Rbi*u_b;

end


% Steady state transport solutions for one Pe=0 and various values of Da2
x = [0:0.01:1];
Da2 = 8; mu1 = sqrt(Da2); mu2 = -mu1;
s = mu2*exp(mu2)-mu1*exp(mu1);
c = (mu2*exp(mu2)*exp(mu1*x)-mu1*exp(mu1)*exp(mu2*x))./s
for Da2 = [4 2 1 0.5 0.25 0.125];
    s = sqrt(Da2); mu1 = s; mu2 = -s;
    s = mu2*exp(mu2)-mu1*exp(mu1);
    c = [c;(mu2*exp(mu2)*exp(mu1*x)-mu1*exp(mu1)*exp(mu2*x))./s];
end
plot (x,c);
legend('Da_2=8','Da_2=4','Da_2=2','Da_2=1','Da_2=0.5','Da_2=0.25','Da_2=0.125');
xlabel ('x/L [-]'); ylabel ('c/c_{in} [-]');


function varargout = gallery(name)
%CHEB.GALLERY   Chebfun example functions.
%   F = CHEB.GALLERY(NAME) returns a chebfun or a quasimatrix corresponding to
%   NAME.  See the listing below for available names.
%
%   For example,  plot(cheb.gallery('zigzag'))  plots a degree 10000 polynomial
%   that doesn't look like a polynomial, and  plot(cheb.gallery('gamma'))  shows
%   a chebfun with poles. For details of how each function is constructed, try
%   type +cheb/gallery  or  edit cheb.gallery.
%
%   [F,FA] = CHEB.GALLERY(NAME) also returns the anonymous function FA used to
%   define the function. Some gallery functions are generated by operations
%   beyond the usual Chebfun constructor (e.g. by solving ODEs), so FA in those
%   cases simply evaluates the chebfun.
%
%   CHEB.GALLERY with no input argument returns a random function from the
%   gallery.
%
%   CHEB.GALLERY with no output argument creates a plot of the selected
%   function.
%
%   airy         Airy Ai function on [-40,40]
%   bessel       Bessel function J_0 on [-100,100]
%   bump         C-infinity function with compact support
%   blasius      Blasius function on [0,10]
%   chirp        Sine with exponentially increasing frequency
%   daubechies   Approximation to Daubechies phi_2 wavelet scaling function
%   erf          Error function on [-10,10]
%   fishfillet   Wild oscillations from Extreme Extrema example
%   gamma        Gamma function on [-4,4]
%   gaussian     Gaussian function on [-Inf,Inf]
%   jitter       A piecewise constant function generated by ROUND
%   kahaner      Challenging integrand with four spikes
%   motto        Chebfun motto (Gilbert Strang)
%   random       Polynomial interpolant through random data in Chebyshev points
%   rose         A complex-valued sinusoid
%   runge        Runge function
%   seismograph  Tanh plus growing oscillation
%   Si           Sine integral on [-50,50]
%   sinefun1     As smooth as it looks
%   sinefun2     Not as smooth as it looks
%   spikycomb    25 peaks, each sharper than the last
%   stegosaurus  max(wiggly, x/10)
%   vandercheb   Chebyshev-Vandermonde quasimatrix
%   vandermonde  Vandermonde quasimatrix
%   wiggly       One of the Chebfun team's favorites
%   wild         An iteratively defined function on [-1 1]
%   zigzag       Degree 10000 polynomial that looks piecewise linear
%
%   Gallery functions are subject to change in future releases of Chebfun.
%
% See also CHEB.GALLERYTRIG, CHEB.GALLERY2, CHEB.GALLERY3, CHEB.GALLERYDISK, CHEB.GALLERYSPHERE.

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

% If the user did not supply an input, return a random function from the
% gallery.
if ( nargin == 0 )
    % NOTE [AB, 2014/12/01]: This is not a particularly scalable way of
    % implementing cheb.gallery. A better approach would be that each gallery
    % function lived in a separate m-file, and could return it's name,
    % description, and the required outputs, F and FA. That way, we would get
    % rid of the switch statement below. Furthermore, this would make the
    % testing of gallery easier, as we wouldn't have to update the list of
    % input options there.
    names = {'airy', 'bessel', 'blasius', 'bump', ...
        'chirp', 'daubechies', 'erf', 'fishfillet', 'gamma', 'gaussian', ...
        'jitter', 'kahaner', 'motto', 'random', 'rose', 'runge', ...
        'seismograph', 'Si', 'sinefun1', 'sinefun2', 'spikycomb', ...
        'stegosaurus', 'vandercheb', 'vandermonde', 'wiggly', 'wild', 'zigzag'};
    name = names{randi(length(names))};
end

% If nargout == 0, then the function is plotted. Each function in the gallery
% has its own plotting preferences.
ylims = [];
axispref = {};

% The main switch statement.
switch lower(name)

    % Airy Ai function on [-40,40]:
    case 'airy'
        fa = @airy;
        f = chebfun(fa, [-40 40]);
        ylims = [-.75 .75];

    % Bessel function with parameter 0 on [-100,100]:
    case 'bessel'
        fa = @(x) besselj(0, x);
        f = chebfun(fa, [-100 100]);
        ylims = [-.5 1.1];

    % Blasius function on [0,10]:
    case 'blasius'
        op = @(u) 2*diff(u,3) + u.*diff(u,2);
        bc  = @(x,u) [u(0); feval(diff(u),0); feval(diff(u),10)-1];
        N = chebop(op, [0 10], bc);
        N.init = chebfun([3.55542; 4.25907; 0.43669; -0.21367;
                0.06382; -0.00118; -0.00865; 0.00306], [0 10], 'coeffs');
        f = N\0;
        fa = @(x) f(x);
        ylims = [-.5 9];

    % Bump function:
    case 'bump'
        fa = @(x) (abs(x) < 1).*exp(-1./(1-x.^2));
        f = chebfun(fa, [-2 2]);

    % Sine with exponentially increasing frequency:
    case 'chirp'
        fa = @(x) sin(x.*exp(x));
        f = chebfun(fa, [0 5]);
        ylims = [-2 2];

    % Approx to Daubechies phi_2 wavelet scaling function:
    case 'daubechies'
        f = daubechies(10);
        fa = @(x) f(x);
        ylims = [-.5 1.5];

    % Error function on [-10,10]:
    case 'erf'
        fa = @erf;
        f = chebfun(fa, [-10 10]);

    % Wild oscillations from Extreme Extrema example:
    case 'fishfillet'
        fa = @(x) cos(x).*sin(exp(x));
        f = chebfun(fa, [0 6]);
        ylims = [-1.2 1.2];

    % Gamma function on [-4,4]:
    case 'gamma'
        fa = @gamma;
        f = chebfun(fa, [-4 4], 'blowup', 'on', 'splitting', 'on');

    % Gaussian function on [-Inf,Inf]:
    case 'gaussian'
        fa = @(x) exp(-x.^2/2)/sqrt(2*pi);
        f = chebfun(fa, [-Inf Inf]);
        ylims = [-.05 .45];

    % A piecewise constant function generated by ROUND:
    case 'jitter'
        fa = @(x) round(exp(x)*2.*sin(8*x));
        f = chebfun(fa, 'splitting', 'on');
        ylims = [-5 6];

    % Challenging integrand with four spikes:
    case 'kahaner'
        fa = @(x) sech(10*(x-0.2)).^2 + sech(100*(x-0.4)).^4 + ...
         sech(1000*(x-0.6)).^6 + sech(1000*(x-0.8)).^8;
        f = chebfun(fa, [0 1]);
        ylims = [-.1 1.2];

    % (Scribbled) Chebfun motto by Gilbert Strang:
    case 'motto'
        f = exp(3i*scribble('there is no fun like chebfun'));
        fa = @(x) f(x);
        axispref = {'equal', [-1 1 -1 1]*1.2, 'off'};

    % Polynomial interpolant through random data in Chebyshev points:
    case 'random'
        f = chebfun(rand(100,1));
        fa = @(x) f(x);

    % Rose curve:
    case 'rose'
        m = 5;
        n = 4;
        fa = @(t) cos(m/n*t).*cos(t) + 1i*cos(m/n*t).*sin(t);
        f = chebfun(fa, [0, 8*pi], 'trig');
        axispref = {'equal', [-1 1 -1 1]*1.2};

    % Runge function:
    case 'runge'
        fa = @(x) 1./(1 + x.^2);
        f = chebfun(fa, [-5 5]);
        ylims = [0 1.2];

    % Tanh plus growing oscillation from ATAP, Chapter 5:
    case 'seismograph'
        fa = @(x) tanh(20*sin(12*x)) + .02*exp(3*x).*sin(300*x);
        f = chebfun(fa);

    % Sine integral:
    case 'si'
        f = cumsum(chebfun(@(x) sin(x)./(x), [-50, 50]));
        fa = @(x) f(x);

    % As smooth as it looks:
    case 'sinefun1'
        fa = @(x) (1.75 + sin(50*x));
        f = chebfun(fa);

    % Not as smooth as it looks:
    case 'sinefun2'
        fa = @(x) (1.75 + sin(50*x)).^1.0001;
        f = chebfun(fa);

    % 25 peaks, each sharper than the last from ATAP, Chapter 18:
    case 'spikycomb'
        fa = @(x) exp(x).*sech(4*sin(40*x)).^exp(x);
        f = chebfun(fa);

    % max(wiggly, x/10):
    case 'stegosaurus'
        fa = @(x) max(sin(x)+sin(x.^2), x/10);
        f = chebfun(fa, [0 10], 'splitting', 'on');
        ylims = [-.2 2.2];

    % Chebyshev-Vandermonde quasimatrix:
    case 'vandercheb'
        f = chebpoly(0:5);
        f = simplify(cheb2quasi(f));
        fa = @(x) f(x);
        ylims = [-1.2 1.2];

    % Vandermonde quasimatrix:
    case 'vandermonde'
        fa = @(x) x.^(0:5);
        f = chebfun(fa, 'vectorize');
        f = simplify(cheb2quasi(f));
        ylims = [-1.2 1.2];

    % One of the Chebfun team's favorites:
    case 'wiggly'
        fa = @(x) sin(x) + sin(x.^2);
        f = chebfun(fa, [0 10]);
        ylims = [-2.4 2.4];

    % An example from one of the first Chebfun papers (Trefethen, 2007):
    case 'wild'
        fa = @(x) wild(x); % Defined below.
        f = chebfun(fa, [-1 1]);
        ylims = [5.5 9.5];

    % Degree 10000 polynomial that looks piecewise linear from ATAP appendix:
    case 'zigzag'
        f = cumsum(chebfun(@(t) sign(sin(100*t./(2-t))), 10000));
        fa = @(x) f(x);

    % Raise an error if the input is unknown.
    otherwise
        error('CHEB:GALLERY:unknown:unknownFunction', ...
            'Unknown function.')
end

% Only return something if there is an output argument.
if ( nargout > 0 )
    varargout = {f, fa};
else
    % Otherwise, plot the function.
    plot(f)
    title([name ', length = ' num2str(length(f))])
    if ( ~isempty(ylims) )
        ylim(ylims)
    end
    if ( ~isempty(axispref) )
        axis(axispref{:})
    end
    shg
end

end

function f = daubechies(n)
% nth order polynomial approximation to Daubechies scaling function phi_2
[x, phi] = daub(n);
f = chebfun([phi'; 0], [0 3], 'equi');

    function [x, phi] = daub(n)
        s = sqrt(3);
        if ( n == 0 )
            % Base case of recursion.
            x = 0:2;
            phi = [0, (1+s)/2, (1-s)/2];
        else
            c = [1+s, 3+s, 3-s, 1-s]/4;
            % Recursively call the daub() method.
            [x2, phi2] = daub(n-1);
            pp = [phi2, 0*phi2];
            N = 2^(n-1);
            ii = 1:6*N;
            phi = c(1)*pp(ii);
            for k = 2:4
                ii = ii([(5*N + 1):6*N, 1:5*N]);
                phi = phi + c(k)*pp(ii);
            end
            x = [x2, x2+3]/2;
        end
    end

end

function s = wild(x)
% The 'wild' function from Computing Numerically with Functions, Trefethen 2007.
f = sin(pi*x);
s = f;
for j = 1:15
    f = (3/4)*(1 - 2*f.^4);
    s = s + f;
end
end


function [Epb Eav Etl hst] = entropia_f(I,Iclass)

% Entropy (E) of intensity image

% 06/08/2011    - Version 4.0F

% Formula: E = -sum(p.*log2(p));

% Author:           Aristidis D. Vaiopoulos
% Acknowledgement:  Mathworks MATLAB entropy function

% If image class is not forced by user, detect and use native class
if nargin == 1;
    Iclass = class(I);
end
% Construct the bin values and convert image according to Iclass
switch Iclass
   case {'logical'}
%       cs = 1;
      cl = 2^1;
      I = logical(I);
      bv = [0 1];
   case {'uint8'}
%       cs = 2;
      cl = 2^8;
      I = im2uint8(I);
      bv = 0:(2^8)-1;
   case {'int16'}
%       cs = 3;
      cl = 2^16;
      I = im2int16(I);
      bv = -( (2^16)/2 ):( (2^16)/2-1 );
   case {'uint16'}
%       cs = 4;
      cl = 2^16;
      I = im2uint16(I);
      bv = 0:(2^16)-1;
   case {'single','double'}
%       cs = 5;
      cl = 2^16;
      I = im2uint16(I);
      bv = 0:(2^16)-1;
    otherwise
%       cs = -1;
      disp('-Unsupported data class.')
    return;

end
% Transpose bin values
bv = bv';

% Find the number of bands
bands = size(I);
if length(bands) == 3
    bands = bands(1,3);
else
    bands = 1;
end
% Calculate histogram counts per band
p = zeros(cl,bands);
for b = 1:bands
    p(:,b) = imhist(I(:,:,b),cl);
end

% Give histogram
if nargout == 4
    hst = [bv p];
end
% normalize p so that sum(p) is one.
p = p ./ (numel(I)/bands);
% normalize p for whole image.
pt = sum(p,2)/bands;
% logarithmization
lp = log2(p);
lpt= log2(pt);
% nullify -Inf values due to logarithmization of zeros in p
lp(isinf(lp)) = 0;
lpt(isinf(lpt)) = 0;
% Entropy per band
Epb = -sum(p.*lp,1)';
% Average Entropy
Eav = mean(Epb);
% Total Entropy (whole image)
Etl = -sum(pt.*lpt);

end


% 2D Lattice Boltzmann (BGK) model of a fluid.
%  c4  c3   c2    D2Q9 model. At each timestep, particle densities propagate
%    \  |  /      outwards in the directions indicated in the figure. An
%  c5 -c9 - c1    equivalent 'equilibrium' density is found, and the densities
%    /  |  \      relax towards that state, in a proportion governed by omega.
%  c6  c7   c8
%                  Iain Haslam, April 2006.

omega=1.0; density=1.0; t1=4/9; t2=1/9; t3=1/36; c_squ=1/3; nx=1; ny=101;
F=repmat(density/9,[nx ny 9]); F_EQ=F; msize=nx*ny; qi=[0:msize:msize*7];
bound=zeros(nx,ny); bound(:,[1 ny])=1;%bound([14:19],[14:16])=1;
obs=find(bound);
bounds_to_bounce=[obs+qi(1) obs+qi(2) obs+qi(3) obs+qi(4) ...
	obs+qi(5) obs+qi(6) obs+qi(7) obs+qi(8)];
bounds_bounced=[obs+qi(5) obs+qi(6) obs+qi(7) obs+qi(8) ...
	obs+qi(1) obs+qi(2) obs+qi(3) obs+qi(4)];
avu=1; prevavu=1; ts=0; deltaU=1e-9; numactivenodes=sum(sum(1-bound));

while (ts<100000 & 1e-10<abs((prevavu-avu)/avu)) | ts<100
    ts=ts+1;

    % Propagate
	F(:,:,4)=F([2:nx 1],[ny 1:ny-1],4);    F(:,:,3)=F(:,[ny 1:ny-1],3);
	F(:,:,2)=F([nx 1:nx-1],[ny 1:ny-1],2); F(:,:,5)=F([2:nx 1],:,5);
	F(:,:,1)=F([nx 1:nx-1],:,1);           F(:,:,6)=F([2:nx 1],[2:ny 1],6);
	F(:,:,7)=F(:,[2:ny 1],7);              F(:,:,8)=F([nx 1:nx-1],[2:ny 1],8);
	BOUNCEDBACK=F(bounds_to_bounce); %Densities bouncing back at next timestep

    % Relax; calculate equilibrium state with equivalent speed and density to F
	DENSITY = sum(F,3);
	UX=(sum(F(:,:,[1 2 8]),3)-sum(F(:,:,[4 5 6]),3))./DENSITY;
	UY=(sum(F(:,:,[2 3 4]),3)-sum(F(:,:,[6 7 8]),3))./DENSITY;

	%UX(1,2:ny-1)=UX(1,2:ny-1)+deltaU;
	UX=UX+deltaU;
	UX(obs)=0; UY(obs)=0; DENSITY(obs)=0;
	U_SQU=UX.^2+UY.^2; U_C2=UX+UY; U_C4=-UX+UY; U_C6=-U_C2; U_C8=-U_C4;

	% stationary
	F_EQ(:,:,9)=t1*DENSITY.*(1-U_SQU/(2*c_squ));

	% nearest-neighbours
	F_EQ(:,:,1)=t2*DENSITY.*(1+UX/c_squ+0.5*(UX/c_squ).^2-U_SQU/(2*c_squ));
	F_EQ(:,:,3)=t2*DENSITY.*(1+UY/c_squ+0.5*(UY/c_squ).^2-U_SQU/(2*c_squ));
	F_EQ(:,:,5)=t2*DENSITY.*(1-UX/c_squ+0.5*(UX/c_squ).^2-U_SQU/(2*c_squ));
	F_EQ(:,:,7)=t2*DENSITY.*(1-UY/c_squ+0.5*(UY/c_squ).^2-U_SQU/(2*c_squ));

	% next-nearest neighbours
	F_EQ(:,:,2)=t3*DENSITY.*(1+U_C2/c_squ+0.5*(U_C2/c_squ).^2-U_SQU/(2*c_squ));
	F_EQ(:,:,4)=t3*DENSITY.*(1+U_C4/c_squ+0.5*(U_C4/c_squ).^2-U_SQU/(2*c_squ));
	F_EQ(:,:,6)=t3*DENSITY.*(1+U_C6/c_squ+0.5*(U_C6/c_squ).^2-U_SQU/(2*c_squ));
	F_EQ(:,:,8)=t3*DENSITY.*(1+U_C8/c_squ+0.5*(U_C8/c_squ).^2-U_SQU/(2*c_squ));
	F=omega*F_EQ+(1-omega)*F;
	F(bounds_bounced)=BOUNCEDBACK;
	prevavu=avu;avu=sum(sum(UX))/numactivenodes; ts=ts+1;
end
%The bounceback boundary condition means that the actual boundary lies
%mid-way between the open and closed nodes. The width of the channel is
%therefore ny-2.
L=(ny-2)/2; arrindices=[-(L-0.5):(L-0.5)];     EXACT=-(arrindices.^2-L^2);
tau = 1/omega;

%Calculate the analytical solution's peak velocity
temp_F         = density * deltaU / tau;
temp_viscosity = (tau-0.5)/3;
calcfactor     = temp_F/(2*temp_viscosity);
CALC           = [UX(1,2:ny-1)];
EXACT          = EXACT*calcfactor;
figure;
plot(arrindices,abs(1-(EXACT./CALC)));
maxerror=max(abs(1-(EXACT./CALC)));
title('Relative error of LBM solution across channel width');
fprintf('RESULTS\n')
fprintf('ts=%d, maxerror=%g\n',ts, maxerror);

%Calculate relative proportional error in centre of channel
[maxcalc, maxindex] = max(CALC);
maxerrorcentre = (EXACT(maxindex)-CALC(maxindex))/EXACT(maxindex)
figure;plot(arrindices,EXACT);
hold on;
plot(arrindices,CALC,'o');
title('Comparison of analytical with LBM results');
xlabel('location in channel'); ylabel('speed')


function D=Delta(Time_to_Maturity,Stock_Value,Stock_Volatility,Strike,Riskfree_Rate)

Time_to_Maturity=max(Time_to_Maturity,10^(-9));  % avoid division by zero message

d1 = 1/(Stock_Volatility*sqrt(Time_to_Maturity))*( log(Stock_Value/Strike) + ...
                (Riskfree_Rate* Stock_Volatility^2/2)*Time_to_Maturity  );

D =normcdf(d1,0,1);


function c=ref_dstiii(f)
%REF_DSTIII  Reference Discrete Sine Transform type III
%   Usage:  c=ref_dstiii(f);
%
%   This is the inverse of DSTII

L=size(f,1);
W=size(f,2);

% Create weights.
w=ones(L,1);
w(L)=1/sqrt(2);
w=w*sqrt(2/L);

% Create transform matrix.
F=zeros(L);

for m=0:L-1
  for n=0:L-1
    F(m+1,n+1)=w(n+1)*sin(pi*(n+1)*(m+.5)/L);
  end;
end;

% Compute coefficients.
c=F*f;


% VL_DEMO_GMM_2D_RAND  Demonstrate clustering points with a GMM

%% Create a random set of points
numPoints = 5000 ;
dimension = 2 ;
numClusters = 20 ;
data = rand(dimension, numPoints) ;

%% Learn a GMM: fit the points at maximum likelihood
vl_twister('state',0) ;
[means, covariances, priors] = ...
    vl_gmm(data, numClusters, ...
           'MaxNumIterations', 1000, ...
           'Verbose') ;

figure(1) ; clf ; hold on
plot(data(1,:),data(2,:),'r.');
for i=1:numClusters
  vl_plotframe([means(:,i)' covariances(1,i) 0 covariances(2,i)], ...
               'Color','blue','LineWidth',2);
end

title('GMM: Gaussian mixture initialized choosing random points') ;
axis equal ; axis off ;
vl_demo_print('gmm_2d_rand',0.6);


function y = tapas_autocorr(x)
% USAGE:
%     Y = tapas_autocorr(X)
%
% INPUT:
%     X - n-by-m matrix of m time series (columns) of length n
%
% OUTPUT:
%     Y - n-by-m matrix with m columns of autocorrelation coefficients for lags n-1
% --------------------------------------------------------------------------------------------------
% Copyright (C) 2016 Christoph Mathys, TNU, UZH & ETHZ
%
% This file is part of the HGF toolbox, which is released under the terms of the GNU General Public
% Licence (GPL), version 3. You can redistribute it and/or modify it under the terms of the GPL
% (either version 3 or, at your option, any later version). For further details, see the file
% COPYING or <http://www.gnu.org/licenses/>.

% Length of time series
n = size(x,1);

% De-mean time series
x = x - ones(size(x))*diag(mean(x));

% Get the autocovariance
f = fft(x);
fsq = f.*conj(f);
y = ifft(fsq)/n;

% Get the autocorrelation (the next line is equivalent to y = y*diag(1./y(1,:));)
y = y*diag(1./var(x,1));

end


function legendre_exactness ( n, x, w, p_max )

%*****************************************************************************80
%
%% LEGENDRE_EXACTNESS investigates exactness of Legendre quadrature.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    16 May 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of points in the rule.
%
%    Input, real X(N), the quadrature points.
%
%    Input, real W(N), the quadrature weights.
%
%    Input, integer P_MAX, the maximum exponent.
%    0 <= P_MAX.
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Quadrature rule for Legendre integral.\n' );
  fprintf ( 1, '  Rule of order N = %d\n', n );
  fprintf ( 1, '  Degree          Relative Error\n' );
  fprintf ( 1, '\n' );

  for p = 0 : p_max

    s = legendre_integral ( p );

    v(1:n,1) = x(1:n) .^ p;

    q = w' * v;

    if ( s == 0.0 )
      e = abs ( q - s );
    else
      e = abs ( ( q - s ) / s );
    end

    fprintf ( 1, '  %6d  %24.16f\n', p, e );

  end

  return
end


function plterrel(xo,yo,C,s,ltype)
% PLTERREL  Plots an error ellipse on screen.
%   Note 1: x & y represent north & south (opposite of
%   normal MatLab convention).  Note 2: use a square
%   aspect ratio for plot; i.e., use axis('square')
%   command.  Non-vectorized.
% Version: 18 Jan 96
% Useage:  plterrel(xo,yo,C,s,ltype)
% Input:   xo - x (north) origin of ellipse
%          yo - y (east) origin of ellipse
%          C  - covariance matrix (2x2)
%          s  - ellipse scale factor (default=1)
%          ltype - line type for error ellipse
%                  (default=solid,red)
% Output:  Plotted error ellipse centred at (xo,yo)

% Copyright (c) 2011, Michael R. Craymer
% All rights reserved.
% Email: mike@craymer.com

if nargin<3
  error('Too few input arguements');
end
if nargin<4
  s=1;             % Define default scale factor
end
if nargin<5
  ltype='-r';      % Define default color red
end
dt=0.1;            % Angular resolution
t=[(0:dt:2*pi)';0];
[V,D]=eig(C);      % Eigenvalues & vectors
r=sqrt(diag(D));   % Length of axes
x=s*r(1)*cos(t);
y=s*r(2)*sin(t);
xx=xo+[x y]*V(1,:)';
yy=yo+[x y]*V(2,:)';
plot(yy,xx,ltype);  % Switch x (north) & y (east)


function  fa = cmd3(a, om, omt)
% calculate the growth factor using 1/(ah)^3
% t0 is (1 - om)/om with om the relative matter density

%fa = (a ./ (1 + a .^3 * t0)) .^ 1.5;

fa = (a ./ (om + (omt - om) .* a.^3  + (1 - omt) .* a))  .^1.5;


function [intersects, edgeIndices] = intersectLinePolyline(line, poly, varargin)
%INTERSECTLINEPOLYLINE Intersection points between a line and a polyline.
%
%   P = intersectLinePolyline(LINE, POLY)
%   Returns the intersection points of the lines LINE with polyline POLY.
%   LINE is a 1-by-4 row vector containing parametric representation of the
%   line (in the format [x0 y0 dx dy], see the function 'createLine' for
%   details).
%   POLY is a NV-by-2 array containing coordinates of the polyline vertices
%   P is a K-by-2 array containing the coordinates of the K intersection
%   points.
%
%   P = intersectLinePolyline(LINE, POLY, TOL)
%   Specifies the tolerance for geometric tests. Default is 1e-14.
%
%   [P INDS] = intersectLinePolyline(...)
%   Also returns the indices of edges involved in intersections. INDS is a
%   K-by-1 column vector, such that P(i,:) corresponds to intersection of
%   the line with the i-th edge of the polyline. If the intersection occurs
%   at a polyline vertex, the index of only one of the two neighbor edges
%   is returned.
%   Note that due to numerical approximations, the use of function
%   'isPointOnEdge' may give results not consistent with this function.
%
%
%   Examples
%   % compute intersections between a square and an horizontal line
%     poly = [0 0;10 0;10 10;0 10];
%     line = [5 5 1 0];
%     intersectLinePolyline(line, poly)
%     ans =
%           10     5
%     % also return indices of edges
%     [inters inds] = intersectLinePolyline(line, poly)
%     inters =
%           10     5
%     inds =
%           2
%
%   % compute intersections between a square and a diagonal line
%     poly = [0 0;10 0;10 10;0 10];
%     line = [5 5 1 1];
%     intersectLinePolyline(line, poly)
%     ans =
%            0     0
%           10    10
%
%   See also
%   lines2d, polylines2d, intersectLines, intersectLinePolygon
%

% ------
% Author: David Legland
% E-mail: david.legland@inrae.fr
% Created: 2003-10-31
% Copyright 2003-2022 INRA - TPV URPOI - BIA IMASTE

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

% create the array of edges
N = size(poly, 1);
edges = [poly(1:N-1, :) poly(2:N, :)];

% compute intersections with supporting lines of polyline edges
supportLines = edgeToLine(edges);
intersects = intersectLines(line, supportLines, tol);

% find edges that are not parallel to the input line
inds = find(isfinite(intersects(:, 1)));

% compute position of intersection points on corresponding lines
pos = linePosition(intersects(inds, :), supportLines(inds, :), 'diag');

% and keep only intersection points located on edges
b = pos > -tol & pos < 1+tol;
inds = inds(b);
intersects = intersects(inds, :);

% remove multiple vertices (can occur for intersections located at polyline
% vertices)
[intersects, I, J] = unique(intersects, 'rows'); %#ok<ASGLU>

if nargout > 1
    % return indices of edges involved in intersection
    % (in case of intersection located at a vertex, only one of the
    % neighbor edges is returned)
    edgeIndices = inds(I);
end


function [N, D] = ratpolyfit(x,y,kn,kd)
% Ratpolyfit Rational Polynomial Fitting
%
%  This programs finds 2 polynomials N(x) and D(x),
%  of user given order kn and kd respectively,
%  such that N(xi)/D(xi) ~= y(xi) in a least squares sense.
%
%usage: [N, D] = ratpolyfit(x, y, kn, kd)
%
%note: kn and kd must be large enough to get a good fit.
%      usually, kn=kd gives good results
%
%note: If you "overfit" the data, then you will usually have pole-zero
%      cancellations and/or poles and zeros with a very large magnitude.
%      If that happens, then reduce the values of kn and/or kd
%
%note: Often, if you have a good fit, you will find that your polynomials
%      have roots where the real function has zeros and poles.
%
%note: Polynomial curve fitting becomes ill conditioned
%      as the range of x increases and as kn and kd increase
%
%note: If you think that your function goes to infinity at some x value
%      then make sure y(xi) is set equal to Inf at that point.
%      The program will compensate for all +/- Inf values
%
%For example, here's a rational polynomial approximation to the Gamma function:
%   x=[-5:1/32:5]'; y=gamma(x);
%   [N, D]=ratpolyfit(x,y,10,10);
%   figure(1); plot(roots(N),'ob'); hold on; plot(roots(D),'xr'); grid on
%   yy=polyval(N,x)./polyval(D,x);
%   figure(2);plot(x,y,'b', x,yy,'dr'); grid on; axis([min(x) max(x) -25 25]);
%
%other demos are in the text listing at the end of this program
%
%tested under version 6.5 (R13)
%
%see also: polyfit, padefit, polyval, vander
%

%Paul Godfrey
%pgodfrey@conexant.com
%May 31, 2006

%default length if none given
if exist('kn','var')
   kn=round(real(kn));
   if kn<0, kn=0; end
else
   kn=5;
end
if exist('kd','var')
   kd=round(real(kd));
   if kd<0, kd=0;end
else
   kd=5;
end

x=x(:);
y=y(:);

% we must remove +/- Inf values from y first
% and insert those as separate poles at the end
p=find(~isfinite(y)); % find Nan 0/0 and Inf a/0 values
% or find abs(y) values > than some really big number

dinf=[];
while length(p)>0
   y=y.*(x-x(p(1))); %adjust remaining y values
   y(p(1))=[]; % remove bad y value, now a NaN
   dinf=[dinf; x(p(1))]; % remember where pole was
   x(p(1))=[]; % now remove that x value too
   if kd>0, kd=kd-1; end; %reduce expected order of den
   p=find(~isfinite(y)); % have all Inf values been removed yet?
end

yy=length(y);
an=ones(yy,kn+1);
for k=kn:-1:1
    an(:,k)=x.*an(:,k+1); % form vandermonde matrix
end

ad=ones(yy,kd+1);
for k=kd:-1:1
    ad(:,k)=x.*ad(:,k+1);
end
for k=1:yy
    ad(k,:)=y(k)*ad(k,:);
end

% A is basically N-y*D
A=[an -ad]; % LS solution is in the null space of A

[u,s,v]=svd(A); % null space is in the cols of V
ND=v(:,end); % use the "most null" vector

N=ND(1:kn+1).';
D=ND(kn+2:end).';

D1=D(1);
if D1==0; D1=1; end
% we have to make the D polynomial monic since thats
% what poly makes, so we have to first adjust N
N=N/D1;
D=D/D1;
% and then add the removed +/- Inf poles back in
D=poly([dinf; roots(D)]);

Dmax=max(abs(D));
if Dmax==0; Dmax=1; end
% normalize max Den value to be +/- 1
N=N/Dmax;
D=D/Dmax;

return

%a demo of this program is
clc
clear all
close all
x=[-5:1/16:5]';

y=gamma(x);
[N, D]=ratpolyfit(x,y,10,10);
yy=polyval(N,x)./polyval(D,x);
figure(1);plot(x,y,'b', x,yy,'dr');
grid on; axis([min(x) max(x) -15 15]);

y=erf(x);
[N, D]=ratpolyfit(x,y,10,10);
yy=polyval(N,x)./polyval(D,x);
figure(2);plot(x,y,'b', x,yy,'dr');
grid on; axis([min(x) max(x) -1.5 1.5]);

y=1./(1+x.*x);
[N, D]=ratpolyfit(x,y,10,10);
yy=polyval(N,x)./polyval(D,x);
figure(3);plot(x,y,'b', x,yy,'dr');
grid on; axis([min(x) max(x) 0 1]);

y=tan(x);
[N, D]=ratpolyfit(x,y,10,10);
yy=polyval(N,x)./polyval(D,x);
figure(4);plot(x,y,'b', x,yy,'dr');
grid on; axis([min(x) max(x) -5 5]);

y=exp(-x.*x);
[N, D]=ratpolyfit(x,y,10,10);
yy=polyval(N,x)./polyval(D,x);
figure(5);plot(x,y,'b', x,yy,'dr');
grid on; axis([min(x) max(x) 0 1]);

y=zeta(x);
[N, D]=ratpolyfit(x,y,10,10);
yy=polyval(N,x)./polyval(D,x);
figure(6);plot(x,y,'b', x,yy,'dr');
grid on; axis([min(x) max(x) -2 2]);

y=psi(x);
[N, D]=ratpolyfit(x,y,10,10);
yy=polyval(N,x)./polyval(D,x);
figure(7);plot(x,y,'b', x,yy,'dr');
grid on; axis([min(x) max(x) -5 5]);

x=x-min(x);

y=log(x);
[N, D]=ratpolyfit(x,y,10,10);
yy=polyval(N,x)./polyval(D,x);
figure(8);plot(x,y,'b', x,yy,'dr');
grid on; axis([min(x) max(x) -5 5]);

x=(x-min(x))*pi/2;

y=besselj(0,x);
[N, D]=ratpolyfit(x,y,10,10);
yy=polyval(N,x)./polyval(D,x);
figure(9);plot(x,y,'b', x,yy,'dr');
grid on; axis([min(x) max(x) -0.5 1]);

y=sin(x)./x;
[N, D]=ratpolyfit(x,y,10,10);
yy=polyval(N,x)./polyval(D,x);
figure(10);plot(x,y,'b', x,yy,'dr');
grid on; axis([min(x) max(x) -0.5 1]);

return


function [ nin, pi ] = triangle_line_imp_int_2d ( a, b, c, t, nin, pi )

%*****************************************************************************80
%
%% TRIANGLE_LINE_IMP_INT_2D finds where an implicit line intersects a triangle in 2D.
%
%  Discussion:
%
%    An implicit line is the set of points ( X, Y ) satisfying
%
%      A * X + B * Y + C = 0
%
%    where at least one of A and B is not zero.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    18 January 2007
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, real A, B, C, determine the equation of the line:
%    A*X + B*Y + C = 0.
%
%    Input, real T(2,3), the triangle vertices.
%
%    Output, integer NIN, the number of points of intersection
%    of the line with the triangle.  NIN may be 0, 1, 2 or 3.
%
%    Output, real PI(2,3), contains the intersection points.
%
  dim_num = 2;

  nin = 0;

  for i = 1 : 3

    j = i4_wrap ( i+1, 1, 3 );
%
%  Get the implicit form of the line through vertices I and I+1.
%
    [ a1, b1, c1 ] = line_exp2imp_2d ( t(1:2,i), t(1:2,j) );
%
%  Seek an intersection with the original line.
%
    [ ival, p ] = lines_imp_int_2d ( a, b, c, a1, b1, c1 );
%
%  If there is an intersection, determine if it lies between the two vertices.
%
    if ( ival == 1 )

      test1 = ( p(1:dim_num)'  - t(1:dim_num,i) )' ...
        * ( t(1:dim_num,j) - t(1:dim_num,i) );
      test2 = ( t(1:dim_num,j) - t(1:dim_num,i) )' ...
        * ( t(1:dim_num,j) - t(1:dim_num,i) );

      if ( 0 <= test1 & test1 <= test2 )
        nin = nin + 1;
        pi(1:dim_num,nin) = p(1:dim_num)';
      end

    end

  end

  return
end


function coeffs = fitPolynomialTransform2d(pts, ptsRef, degree)
%FITPOLYNOMIALTRANSFORM2D Coefficients of polynomial transform between two point sets.
%
%   COEFFS = fitPolynomialTransform2d(PTS, PTSREF, DEGREE)
%
%   Example
%
%   See also
%     polynomialTransform2d, fitAffineTransform2d

% ------
% Author: David Legland
% E-mail: david.legland@nantes.inra.fr
% Created: 2013-11-05, using Matlab 7.9.0.529 (R2009b)
% Copyright 2013-2022 INRA - Cepia Software Platform

%% Extract data

% ensure degree is valid
if nargin < 3
    degree = 3;
end

% polygon coordinates
xi = pts(:,1);
yi = pts(:,2);
nCoords = size(pts, 1);

% check inputs have same size
if size(ptsRef, 1) ~= nCoords
    error('fitPolynomialTransform2d:sizeError', ...
        'input arrays must have same number of points');
end

%% compute coefficient matrix

% number of coefficients of polynomial transform
nCoeffs = prod(degree + [1 2]) / 2;

% initialize matrix
A1 = zeros(nCoords, nCoeffs);

% iterate over degrees
iCoeff = 0;
for iDegree = 0:degree

    % iterate over binomial coefficients of a given degree
    for k = 0:iDegree
        iCoeff = iCoeff + 1;
        A1(:, iCoeff) = ones(nCoords, 1) .* power(xi, iDegree-k) .* power(yi, k);
    end
end

% concatenate matrix for both coordinates
A = kron(A1, [1 0;0 1]);

%% solve linear system that minimizes least squares

% create the vector of expected values
b = ptsRef';
b = b(:);

% solve the system
coeffs = (A \ b)';


function [ nfactor, factor, exponent, nleft ] = i4_factor ( n )

%*****************************************************************************80
%
%% I4_FACTOR factors an integer into prime factors.
%
%  Formula:
%
%    N = NLEFT * Product ( 1 <= I <= NFACTOR ) FACTOR(I)**EXPONENT(I).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    31 July 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the integer to be factored.  N may be positive,
%    negative, or 0.
%
%    Output, integer NFACTOR, the number of prime factors of N discovered
%    by the routine.
%
%    Output, integer FACTOR(NFACTOR), the prime factors of N.
%
%    Output, integer EXPONENT(NFACTOR).  EXPONENT(I) is the power of
%    the FACTOR(I) in the representation of N.
%
%    Output, integer NLEFT, the factor of N that the routine could not
%    divide out.  If NLEFT is 1, then N has been completely factored.
%    Otherwise, NLEFT represents factors of N involving large primes.
%
  nfactor = 0;

  factor = [];
  exponent = [];

  nleft = n;

  if ( n == 0 )
    return
  end

  if ( abs ( n ) == 1 )
    nfactor = 1;
    factor(1) = 1;
    exponent(1) = 1;
    return;
  end
%
%  Find out how many primes we stored.
%
  maxprime = prime ( -1 );
%
%  Try dividing the remainder by each prime.
%
  for i = 1 : maxprime

    p = prime ( i );

    if ( mod ( abs ( nleft ), p ) == 0 )

      nfactor = nfactor + 1;
      factor(nfactor) = p;
      exponent(nfactor) = 0;

      while ( 1 )

        exponent(nfactor) = exponent(nfactor) + 1;
        nleft = floor ( nleft / p );

        if ( mod ( abs ( nleft ), p ) ~= 0 )
          break;
        end

      end

      if ( abs ( nleft ) == 1 )
        break;
      end

    end

  end

  return
end


% function to implement the Iterated Extended Kalman Filter (IEKF)
% Inputs:
%  OBSn - the observations (with noise)
%  xest - initial state space estimates
% Ouputs:
%  Xp - predicted states
function Xp = f_IEKF(OBSn,xest)

load avar % r1,r2, L, and T

tol = .1; % tolerance for iterations
diff = 1;
count = 0;

F = [1 T 0 0 0 0 0;
    0 1 0 0 0 0 0;
    0 0 1 T 0 0 0;
    0 0 0 1 0 0 0;
    0 0 0 0 1 0 T;
    0 0 0 0 0 1 T;
    0 0 0 0 0 0 1]; % state transition matrix

n = size(OBSn,2); % number of observations

Xp = zeros(7,n);  % make room

Pkp1 = 1e10*eye(7); %xest*xest'; %.1*ones(7,7);

%Pkp1 = xest*xest';
%Pkp1 = F*Pkp1*F';

%Pkp1 = (10*randn(7,1))*(10*randn(7,1)).';
%Pkp1 = F*Pkp1*F';

% for each observation
for i = 1:n;

    % if this is the first iteration the prior predicted estimate is xest
    % if this is not the first run the prior estimate is in Xp
    if i == 1
        xkm1 = xest;
    else
        xkm1 = Xp(:,i-1);
    end

    Pkm1 = Pkp1; % conditional covariance from last iteration

    % iterations are started with the predicted estimate from the last run
    xkn = xkm1;
    while ~(diff < tol || count > 9)

        count = count + 1;

        H = [gradest(@(x)f_h1(x),xkn); gradest(@(x)f_h2(x),xkn)];

        R = (.01*randn(2,1))*(.01*randn(2,1)).';

        Rdiag = diag(R); R = diag(Rdiag);

        K = Pkm1*H'*(H*Pkm1*H'+R)^-1;

        xkn_temp = xkm1 + K*(OBSn(:,i)-f_h(xkn)-H*(xkm1-xkn));

        diff = norm(abs(xkn_temp-xkn));

        fprintf('diff = %g \n',diff)

        xkn = xkn_temp;

    end

    H = [gradest(@(x)f_h1(x),xkn); gradest(@(x)f_h2(x),xkn)];

    Pkk = (eye(7)-K*H)*Pkm1;

    Pkp1 = F*Pkk*F';

    Xp(:,i) = F*xkn;

    clc;

    fprintf('i = %g; Count is %g \n',i,count)

    count = 0;

    diff = 1;

end


function [MinPixVal, MaxPixVal]=max_min_pix_value(im_array);
% function [MinPixVal, MaxPixVal]=max_min_pix_value(im_array);
%
% function for calculating most often occuring Min and Max intensity values
% in images
% Perfromed by calculating means of pixels within the moving boxes of 5x5 pixel size throughout the
% image
% three most minimum values are averaged to get MinPixVal
% three most maximum values are averaged to get MaxPixVal
% input im_array:
% can be one image or array of images
%
% written by K.Artyushkova
% 102003

% Kateryna Artyushkova
% Postdoctoral Scientist
% Department of Chemical and Nuclear Engineering
% The University of New Mexico
% (505) 277-0750
% kartyush@unm.edu

[n,m,p]=size(im_array);
for k=1:p
    a=im_array(:,:,k);
    i=1:5:n-4;
    [e,f]=size(i);
    j=1:5:m-4;
    [g,h]=size(j);
    for I=1:f
        for J=1:h
            b(I,J)=mean(mean(a(i(I):i(I)+4,j(J):j(J)+4)));
        end
    end
    [r,p]=size(b);
    c=reshape(b,[r*p 1]);
    d=sort(c);
    MinPixVal(k)=mean(d(1:3));
    MaxPixVal(k)=mean(d((r*p-2):r*p));
end


function inv = cauchy_inv(x,m,s)
%---------------------------------------------------
% PURPOSE:
% Estimates the Inverse of the Cumulative Distribution Function of the
% Cauchy-Lorentz Distribution for a series of x values, with m location
% parameter and s scale parameter
%---------------------------------------------------
% USAGE:
% inv = cauchy_inv(x,m,s)
% where:
% x = (n x 1) or (1 x n) vector
% m = (n x 1) or (1 x n) vector
% s = (n x 1) or (1 x n) vector
%---------------------------------------------------
% RETURNS:
% cdf = (n x 1) or (1 x n) vector containing the probability for each
% element of x with corresponding location and scale parameters
%---------------------------------------------------
% Author:
% Alexandros Gabrielsen, a.gabrielsen@city.ac.uk
% Date: 06/2010
%---------------------------------------------------

if nargin == 0
    error('Data, Location Parameter, Scale Parameter')
end

inv = m + s.*tan(pi*(x-1/2));

end


function N = R1(rad)
% Suppose right handed coordinate frames A and B have the same x axis,
% and rotate the A frame along the x axis by rad obtains the B frame, then
% a point in B, p_B, can be transformed from its coordinates in A frame by
% p_B = R1(rad) * p_A.
N = [1 0 0;0 cos(rad) sin(rad);0 -sin(rad) cos(rad)];


function posteriori = slposterioritrue(condprops, nums, priori, op)
%SLPOSTERIORITRUE Computes the posteriori that samples belong to true class
%
% $ Syntax $
%   - posteriori = slposterioritrue(condprops, nums, priori)
%   - posteriori = slposterioritrue(condprops, nums, priori, 'log')
%
% $ Arguments $
%   - condprods:      the conditional probabilities of classes: C x n
%   - nums:           the number of samples belong to the classes: 1 x C
%   - priori:         the prior probabilities of classes: 1 x C
%   - posteriori:     the resulting posterior probabilities: 1 x n
%
% $ Description $
%   - posteriori = slposteriori(condprops, nums, priori) Computes the
%     posterior probability that the samples belong to the true class
%     according to the given conditional probabilities of all samples
%     to all classes and the priori of the classes.
%     In the input argument, each column in condprops represent the
%     conditional probabilities of that sample belong to all the
%     C classes. The samples from the same underlying classes should be
%     put together and the ownership is given by nums.
%     The priori can be given by an 1 x C row vector or [], which
%     means that all classes have equal prior.
%
%   - posteriori = slposterioritrue(condprops, nums, priori, 'log') means
%     that the input condprops are given by its logarithm.
%
% $ History $
%   - Created by Dahua Lin, on Sep 16th, 2006
%

%% parse and verify input arguments

if nargin < 2
    raise_lackinput('slposterioritrue', 2);
end

if ~isnumeric(condprops) || ndims(condprops) ~= 2
    error('sltoolbox:invalidarg', ...
        'The condprops should be a 2D numeric matrix');
end
[C, n] = size(condprops);

if ~isequal(size(nums), [1, C])
    error('sltoolbox:sizmismatch', ...
        'The nums should be an 1 x C row vector');
end
if nargin < 3 || isempty(priori)
    priori = [];
else
    if ~isequal(size(priori), [1, C])
        error('sltoolbox:sizmismatch', ...
            'The priori should be a an 1 x C row vector');
    end
end

if nargin >= 4 && strcmpi(op, 'log')
    is_log = true;
else
    is_log = false;
end

%% compute

if is_log
    if isempty(priori)
        P = sladdvec(condprops, -max(condprops, [], 1), 2);
    else
        P = sladdvec(condprops, log(priori)', 1);
        P = sladdvec(P, -max(condprops, [], 1), 2);
    end
    P = exp(P);
else
    if isempty(priori)
        P = condprops;
    else
        P = slmulvec(condprops, priori', 1);
    end
end

tP = sum(P, 1);
[sp, ep] = slnums2bounds(nums);
posteriori = zeros(1, n);

for k = 1 : C
    sk = sp(k);
    ek = ep(k);
    posteriori(sk:ek) = P(k, sk:ek);
end
posteriori = posteriori ./ tP;


function [center,rad,v1n,v2nb] = circlefit3d(p1,p2,p3)
% circlefit3d: Compute center and radii of circles in 3d which are defined by three points on the circumference
% usage: [center,rad,v1,v2] = circlefit3d(p1,p2,p3)
%
% arguments: (input)
%  p1, p2, p3 - vectors of points (rowwise, size(p1) = [n 3])
%               describing the three corresponding points on the same circle.
%               p1, p2 and p3 must have the same length n.
%
% arguments: (output)
%  center - (nx3) matrix of center points for each triple of points in p1,  p2, p3
%
%  rad    - (nx1) vector of circle radii.
%           if there have been errors, radii is a negative scalar ( = error code)
%
%  v1, v2 - (nx3) perpendicular vectors inside circle plane
%
% Example usage:
%
%  (1)
%      p1 = rand(10,3);
%      p2 = rand(10,3);
%      p3 = rand(10,3);
%      [center, rad] = circlefit3d(p1,p2,p3);
%      % verification, result should be all (nearly) zero
%      result(:,1)=sqrt(sum((p1-center).^2,2))-rad;
%      result(:,2)=sqrt(sum((p2-center).^2,2))-rad;
%      result(:,3)=sqrt(sum((p3-center).^2,2))-rad;
%      if sum(sum(abs(result))) < 1e-12,
%       disp('All circles have been found correctly.');
%      else,
%       disp('There had been errors.');
%      end
%
%
% (2)
%       p1=rand(4,3);p2=rand(4,3);p3=rand(4,3);
%       [center,rad,v1,v2] = circlefit3d(p1,p2,p3);
%       plot3(p1(:,1),p1(:,2),p1(:,3),'bo');hold on;plot3(p2(:,1),p2(:,2),p2(:,3),'bo');plot3(p3(:,1),p3(:,2),p3(:,3),'bo');
%       for i=1:361,
%           a = i/180*pi;
%           x = center(:,1)+sin(a)*rad.*v1(:,1)+cos(a)*rad.*v2(:,1);
%           y = center(:,2)+sin(a)*rad.*v1(:,2)+cos(a)*rad.*v2(:,2);
%           z = center(:,3)+sin(a)*rad.*v1(:,3)+cos(a)*rad.*v2(:,3);
%           plot3(x,y,z,'r.');
%       end
%       axis equal;grid on;rotate3d on;
%
%
% Author: Johannes Korsawe
% E-mail: johannes.korsawe@volkswagen.de
% Release: 1.0
% Release date: 26/01/2012

% Default values
center = [];rad = 0;v1n=[];v2nb=[];

% check inputs
% check number of inputs
if nargin~=3,
    fprintf('??? Error using ==> cirlefit3d\nThree input matrices are needed.\n');rad = -1;return;
end
% check size of inputs
if size(p1,2)~=3 || size(p2,2)~=3 || size(p3,2)~=3,
    fprintf('??? Error using ==> cirlefit3d\nAll input matrices must have three columns.\n');rad = -2;return;
end
n = size(p1,1);
if size(p2,1)~=n || size(p3,1)~=n,
    fprintf('??? Error using ==> cirlefit3d\nAll input matrices must have the same number or rows.\n');rad = -3;return;
end
% more checks are to follow inside calculation

% Start calculation
% v1, v2 describe the vectors from p1 to p2 and p3, resp.
v1 = p2 - p1;v2 = p3 - p1;
% l1, l2 describe the lengths of those vectors
l1 = sqrt((v1(:,1).*v1(:,1)+v1(:,2).*v1(:,2)+v1(:,3).*v1(:,3)));
l2 = sqrt((v2(:,1).*v2(:,1)+v2(:,2).*v2(:,2)+v2(:,3).*v2(:,3)));
if find(l1==0) | find(l2==0), %#ok<OR2>
    fprintf('??? Error using ==> cirlefit3d\nCorresponding input points must not be identical.\n');rad = -4;return;
end
% v1n, v2n describe the normalized vectors v1 and v2
v1n = v1;for i=1:3, v1n(:,i) = v1n(:,i)./l1;end
v2n = v2;for i=1:3, v2n(:,i) = v2n(:,i)./l2;end
% nv describes the normal vector on the plane of the circle
nv = [v1n(:,2).*v2n(:,3) - v1n(:,3).*v2n(:,2) , v1n(:,3).*v2n(:,1) - v1n(:,1).*v2n(:,3) , v1n(:,1).*v2n(:,2) - v1n(:,2).*v2n(:,1)];
if find(sum(abs(nv),2)<1e-5),
    fprintf('??? Warning using ==> cirlefit3d\nSome corresponding input points are nearly collinear.\n');
end
% v2nb: orthogonalization of v2n against v1n
dotp = v2n(:,1).*v1n(:,1) + v2n(:,2).*v1n(:,2) + v2n(:,3).*v1n(:,3);
v2nb = v2n;for i=1:3,v2nb(:,i) = v2nb(:,i) - dotp.*v1n(:,i);end
% normalize v2nb
l2nb = sqrt((v2nb(:,1).*v2nb(:,1)+v2nb(:,2).*v2nb(:,2)+v2nb(:,3).*v2nb(:,3)));
for i=1:3, v2nb(:,i) = v2nb(:,i)./l2nb;end

% remark: the circle plane will now be discretized as follows
%
% origin: p1                    normal vector on plane: nv
% first coordinate vector: v1n  second coordinate vector: v2nb

% calculate 2d coordinates of points in each plane
% p1_2d = zeros(n,2); % set per construction
% p2_2d = zeros(n,2);p2_2d(:,1) = l1; % set per construction
p3_2d = zeros(n,2); % has to be calculated
for i = 1:3,
    p3_2d(:,1) = p3_2d(:,1) + v2(:,i).*v1n(:,i);
    p3_2d(:,2) = p3_2d(:,2) + v2(:,i).*v2nb(:,i);
end

% calculate the fitting circle
% due to the special construction of the 2d system this boils down to solving
% q1 = [0,0], q2 = [a,0], q3 = [b,c] (points on 2d circle)
% crossing perpendicular bisectors, s and t running indices:
% solve [a/2,s] = [b/2 + c*t, c/2 - b*t]
% solution t = (a-b)/(2*c)

a = l1;b = p3_2d(:,1);c = p3_2d(:,2);
t = 0.5*(a-b)./c;
scale1 = b/2 + c.*t;scale2 = c/2 - b.*t;

% centers
center = zeros(n,3);
for i=1:3,
    center(:,i) = p1(:,i) + scale1.*v1n(:,i) + scale2.*v2nb(:,i);
end

% radii
rad = sqrt((center(:,1)-p1(:,1)).^2+(center(:,2)-p1(:,2)).^2+(center(:,3)-p1(:,3)).^2);


function [M, F, b, x, e_conn] =  assemb_co ( param )

%  The FEM equation for the steady state temperature distribution is
%
%    M_2 z^{ss} + F + b = 0

%
%% Initialization
%---------------------------------------------------------------------

  n_nodesx   =  param.nodesx;    % number of nodes in the x direction
  n_nodesy   =  param.nodesy;    % number of nodes in the y direction

  n_gauss    = 7;        % number of points used in Gauss integration
%
%%  Geometry Module
%-----------------------------------------------------------------------
%
%  Generate a mesh of the L \times W rectangle with quadratic elements
%
  n_nodes    = n_nodesx*n_nodesy;
  n_elements = 2*(n_nodesx-1)*(n_nodesy-1)/4;

  x_nodes = linspace(0.0, param.L, n_nodesx);
  y_nodes = linspace(0.0, param.W, n_nodesy);
%
%  Generate node coordinates.
%
  x = zeros(n_nodes, 2);
  for ii=1:n_nodesx
    x_i = x_nodes(ii); k = ii - n_nodesx;
    for jj=1:n_nodesy
      k = k + n_nodesx;
      x(k,:) = [ x_i  y_nodes(jj)];
    end
  end
%
%  Generate element connectivity
%
  e_conn = zeros(n_elements, 6);
  ie     = 0;
%
%  Template for odd numbered triangles
%
  elmt_odd = [ 0 ; 2+2*n_nodesx;   2*n_nodesx;
                    1+n_nodesx ; 1+2*n_nodesx; n_nodesx];
%
%  Template for even numbered triangles
%
  elmt_evn = [ 0 ;      2 ;    2+2*n_nodesx;
                         1;    2+n_nodesx ;    1+n_nodesx];

  for jj=1:2:n_nodesy-1
    for ii=1:2:n_nodesx-1
      k = ii + (jj-1)*n_nodesx;
      ie = ie + 1;
      e_conn(ie,:) = k + elmt_odd;
      ie = ie + 1;
      e_conn(ie,:) = k + elmt_evn;
    end
  end
%
%  Set up arrays for elements with faces on left/right boundary.
%
  Omega_left  =            1:(n_nodesx-1):n_elements;
  Omega_right = (n_nodesx-1):(n_nodesx-1):n_elements;
%
%  With Neumann/Robins bc all nodes are unknown.
%
  n_equations = n_nodes;
%
%  Begin the SPMD block:
%
  spmd
%
%  Set up codistributed structure
%
%  Column pointers and such for codistributed arrays.
%
    Vc  = codistributed.colon(1, n_equations);
    lP  = getLocalPart(Vc);
    lP_1 = lP(1); lP_end = lP(end); % first and last columns of M on this lab

    fid  = fopen(['out' int2str(labindex)], 'w');
    fprintf(fid, 'Lab %d : column range %d  %d \n', labindex, lP_1, lP_end);

    co_dist_Vc = getCodistributor(Vc);
    dPM = co_dist_Vc.Partition;
    col_shft = [0 cumsum(dPM(1:end-1))];
%
%  sparse arrays on each lab
%
    M_lab = sparse(n_equations, dPM(labindex));
    b_lab = sparse(dPM(labindex), 1); F_lab = b_lab;

    [n_elements, nel_dof] = size(e_conn);
    [r, s, w] = twod_gauss(n_gauss);
%
%  Build the finite element matrices - Begin loop over elements
%
    n_el_lab = 0;
    fprintf(fid, 'Lab %d : begin n_el loop \n', labindex);

    for n_el=1:n_elements
%
%  Which nodes are in this element?
%
      nodes_local = e_conn(n_el,:);
%
%  subset of nodes/columns on this lab
%
      lab_nodes_local = extract( nodes_local, lP_1, lP_end);

%     fprintf(fid, '\n \n n_el = %d \n', n_el);
%     fprintf(fid, 'lab_nodes_local has %d rows \n', size(lab_nodes_local, 1));

      if ~isempty(lab_nodes_local) % continue the calculation for this elmnt
        n_el_lab = n_el_lab + 1;
%       fprintf(fid, 'Lab %d : n_el loop location 1 \n', labindex);
%
%  coordinates, shape functions & gradients evaluated at the Gauss pts.
%
        x_local                   = x(nodes_local,:);
        [x_g, w_g, phi, p_x, p_y] = twod_shape(x_local, r, s, w);

%-------------------------------------------------------------------
%% Element contributions to the weak form of the equations
%  First we evaluate the surface integral (2D) terms
%-------------------------------------------------------------------
        M_loc =   twod_bilinear(param.k_x, p_x, p_x, w_g) + ...
                  twod_bilinear(param.k_y, p_y, p_y, w_g);
        src_g  =  source(x_g,  param); % distributed source term
        F_loc  =  twod_f_int( src_g , phi, w_g );
%
%  pre-allocate boundary arrays
%
        b_loc  =   zeros(nel_dof,1);
%       fprintf(fid, 'Lab %d : n_el loop location 2 \n', labindex);

%---------------------------------------------------------------------
%%  Add boundary terms
%---------------------------------------------------------------------
%
%  Add boundary-integral terms for left and right bounary elements
%  Left  boundary n_el = [1:2*(n_nodesx-1):n_elements
%  Right boundary n_el = [2*(n_nodesx-1):2*(n_nodesx-1):n_elements
%
        if any(Omega_left == n_el);
%
%  Here we impose Robin boundary term. The node points on the boundary
%  are nodes_local([3 6 1]) and  are at x_local([3 6 1],:)
%
          [phi_g1, ip, x_g1,  w_g1] = boundary(3, x_local, 5); % n_gauss=5
          alpha_g1 = steps(x_g1(:,2), param.yh, param.ep, param.alpha );
          phi_a    = diag(alpha_g1)*phi_g1;  % mult. each row by alpha(y_g)
          beta_g1  = steps(x_g1(:,2), param.yh, param.ep, param.beta );
%
%  1D integrals are from y = w to y = 0
%
          for ii=1:3
            for jj=1:3
              M_loc(ip(ii),ip(jj)) = M_loc(ip(ii),ip(jj)) + ...
                            oned_f_int( phi_g1(:, jj), phi_a(:,ii), w_g1 );
            end
            b_loc(ip(ii),1) = b_loc(ip(ii),1) + ...
                                  oned_f_int( beta_g1, phi_a(:,ii), w_g1 );
          end

        elseif any(Omega_right == n_el);
%
%  Here we impose boundary term for a specified flux
%  The node points on the boundary are nodes_local([2 3 5]) and
%  are at x_local([2 3 5],:)
%  We need \int f(y) \phi(L, y) dy along the right boundary.
%
          [phi_g1, ip,  ~  ,  w_g1] = boundary(2, x_local, 5); % n_gauss=5;
          for ii=1:3
             b_loc(ip(ii),1) = b_loc(ip(ii),1) + ...
                             oned_f_int( param.flux, phi_g1(:,ii), w_g1 );
          end
        end

%-----------------------------------------------------------------
%% Assemble contributions into the global system matrices
%-----------------------------------------------------------------
%  For M_lab we want all the rows (test functions) but only the
%  columns associated with nodes on this lab
%
        for n_t = 1:nel_dof               % local DOF  - test fcn
          t_glb  = nodes_local(n_t);     % global DOF - test fcn
          for n_u = 1:size(lab_nodes_local, 1)
            n_locj = lab_nodes_local(n_u, 1);  % local DOF in current n_el
            n_glbj = lab_nodes_local(n_u, 2) ...
                         - col_shft(labindex); % global DOF
            M_lab(t_glb,n_glbj) = M_lab(t_glb,n_glbj) ...
                                          +  M_loc(n_t,n_locj);
          end
%
%  Some triangles have nodes on more than one lab;
%  we want only the rows associated with nodes on this lab
%
          if t_glb >= lP_1 && t_glb <= lP_end
            t_loc = t_glb - col_shft(labindex);
            b_lab(t_loc,1)  = b_lab(t_loc,1)  + b_loc(n_t,1);
            F_lab(t_loc,1)  = F_lab(t_loc,1)  + F_loc(n_t,1);
          end

        end % for  n_t

      end % if not empty

    end  % n_el

    fprintf(fid,'Fraction of elements evaluated on this lab  is %8.6f \n',...
                                                      n_el_lab/n_elements);
%
%  Assemble the 'lab' parts in a codistributed format.
%  syntax for version R2009b
%
    codist_matrix = codistributor1d( 2, dPM, [n_equations, n_equations]);
    M = codistributed.build(M_lab, codist_matrix );
    codist_vector = codistributor1d( 1, dPM, [n_equations, 1]);
    b  = codistributed.build(b_lab , codist_vector );
    F  = codistributed.build(F_lab , codist_vector );

    fclose(fid);

  end
%
%  This terminates the SPMD block.
%
  return
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   [JX, Jq, Jphi]=compute_jacobians_5R(robot, q)
%
%   Compute Jacobians for the for the 5R planar parallel robot
%
%   JX is the 4x2 Jacobian with respect to the cartesian coordinates (x, y)
%   Jq is the 4x2 Jacobian with respect to the active joints (q1, q2)
%   Jphi is the 4x2 Jacobian with respect to the passive joints (phi1, phi2)
%
%   Author: Arturo Gil Aparicio arturo.gil@umh.es
%   Date: 13/09/2013
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

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

function [JX, Jq, Jphi]=compute_jacobians_5R(robot, q)

% The geometric parameters of this robot can be retrieved from
% the two DOF planar arms that form it
a1=eval(robot.robot1.DH.a);
a2=eval(robot.robot2.DH.a);

%Link lengths
l1=abs(a1(1));
l2=abs(a1(2));
l3=abs(a2(1));
l4=abs(a2(2));
%distance along X that separate both arms
L=robot.L;

%retrieve the active and passive joints from the vector q
q1=q(1);
phi1=q(2);
q2=q(3);
phi2=q(4);

%The loop closure equations for this robot can be written as:
%
% \begin{bmatrix}
% \Gamma_1\\
% \Gamma_2\\
% \Gamma_3\\
% \Gamma_4
% \end{bmatrix}=
% \begin{bmatrix}
% x-l_1\cos(q_1)-l_2\cos(q_1+\varphi_1)\\
% y-l_1\sin(q_1)-l_2\sin(q_1+\varphi_1)\\
% x-l_4\cos(q_2)-l_3\cos(q_2+\varphi_2)-L\\
% y-l_4\sin(q_2)-l_3\sin(q_2+\varphi_2)
% \end{bmatrix}

%In this matrix, JX11 is the partial derivative of Gamma1 with respect to x
%                JX12 is the partial derivative of Gamma1 with respect to y
%                JX21 is the partial derivative of Gamma2 with respect to x
%...
JX=[1   0;
    0   1;
    1   0;
    0   1];

%In this matrix, Jq11 is the partial der. of Gamma1 with respect to q1
%                Jq12 is the partial der. of Gamma1 with respect to q2
%                Jq21 is the partial der. of Gamma2 with respect to q1
%etc...
Jq = [l1*sin(q1)+l2*sin(q1+phi1)              0;
      -l1*cos(q1)-l2*cos(q1+phi1)             0;
      0                           l4*sin(q2)+l3*sin(q2+phi2);
      0                           -l4*cos(q2)-l3*cos(q2+phi2)];

%In this matrix, Jphi11 is the partial der. of Gamma1 with respect to phi1
%                Jphi12 is the partial der. of Gamma1 with respect to phi2
%                Jphi21 is the partial der. of Gamma2 with respect to phi1
%etc...
Jphi = [l2*sin(q1+phi1)              0;
       -l2*cos(q1+phi1)             0;
      0                           l3*sin(q2+phi2);
      0                           -l3*cos(q2+phi2)];


function   s1 = sigma_8(k, k1, psm1, h)
% sigma_8.m calculates  the integrant for the expected rms mass overdensity in a sphere of radius 8 Mpc/h.
% input : k, wavenumber in units h/Mpc and the vectors k1 resp psm1, the mass power spectrum as a
% function of k1
% output : the integrant to obtain sigma_8, the rms mass overdensity sampled with
% a sphere of radius 8/h Mpc.

% D Vangheluwe 15 july 2005

% radius of the sphere in units Mpc/h
r0 = 8/h;
x = k * r0;
% calculate the point in the mass power spectrum at k :
psm = spline(k1, psm1, k);

% calculate the integrant:
s1 = (3*(sin(x) - x .* cos(x)) ./ (x .^3)) .^2 .* psm .* k .^2;

return


function index = tileIndex3d(tile)
% Return the index of a 2-by-2-by-2 3D binary configuration tile.
%
%   INDEX = tileIndex3d(TILE)
%   Compute the index of a tile, given as a 2x2x2 binary image, by adding
%   powers of two mutliplied by values of the tile. The result is comprised
%   between 0 and 255.
%   Iterate on x, y, then z directions, corresponding to directions 2, 1,
%   and 3.
%
%   Example
%   tile = zeros([2 2 2]);
%   tile(1,1:2,1) = 1;
%   tileIndex3d(tile)
%       3
%
%   See also
%     createTile3d, tileIndex
%

% ------
% Author: David Legland
% e-mail: david.legland@inrae.fr
% Created: 2009-05-25,    using Matlab 7.7.0.471 (R2008b)
% Copyright 2009 INRA - Cepia Software Platform.

tile = tile>0;
index = ...
    tile(1,1,1) + 2*tile(1,2,1) + 4*tile(2,1,1) + 8*tile(2,2,1) + ...
    16*tile(1,1,2) + 32*tile(1,2,2) + 64*tile(2,1,2) + 128*tile(2,2,2);


function fx = p03_fun ( n, x )

%*****************************************************************************80
%
%% P03_FUN evaluates the integrand for problem 3.
%
%  Discussion:
%
%    D&R gives "exact" value as 13.628...
%    Mathematica returns        13.440045415012575106...
%    D&R gives Laguerre(16) as   0.44996932...
%
%  Integral:
%
%    exp ( -2 ) Integral ( 2 <= x < +oo ) 1 / ( x^1.01 ) dx
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    28 December 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Philip Davis, Philip Rabinowitz,
%    Methods of Numerical Integration,
%    Second Edition,
%    Dover, 2007,
%    ISBN: 0486453391,
%    LC: QA299.3.D28.
%
%  Parameters:
%
%    Input, integer N, the number of points.
%
%    Input, real X(N), the evaluation points.
%
%    Output, real FX(N), the function values.
%
  fx(1:n) = exp ( - 2.0 ) * 1.0 ./ x(1:n).^1.01;

  return
end


%% Analyzing Neural Time Series Data
% Matlab code for Chapter 25
% Mike X Cohen
%
% This code accompanies the book, titled "Analyzing Neural Time Series Data"
% (MIT Press). Using the code without following the book may lead to confusion,
% incorrect data analyses, and misinterpretations of results.
% Mike X Cohen assumes no responsibility for inappropriate or incorrect use of this code.

%% setup for figure 25.3

% generate a random signal of 3 seconds
srate = 1000;

randsig1 = randn(1,3*srate); % 3 seconds, with this sampling rate
randsig2 = randn(1,3*srate);

% now filter at 5 Hz
f       = 5; % frequency of wavelet in Hz
time    = -1:1/srate:1; % time for wavelet, from -1 to 1 second in steps of 1/sampling-rate
s       = 6/(2*pi*f); % width of Gaussian
wavelet = exp(2*pi*1i*f.*time) .* exp(-time.^2./(2*s^2));

% FFT parameters
n_wavelet            = length(wavelet);
n_data               = length(randsig1);
n_convolution        = n_wavelet+n_data-1;
half_of_wavelet_size = (length(wavelet)-1)/2;

% FFT of wavelet and EEG data
convolution_result_fft = ifft(fft(wavelet,n_convolution).*fft(randsig1,n_convolution),n_convolution)*sqrt(s)/10;
filtsig1  = real(convolution_result_fft(half_of_wavelet_size+1:end-half_of_wavelet_size));
anglesig1 = angle(convolution_result_fft(half_of_wavelet_size+1:end-half_of_wavelet_size));

convolution_result_fft = ifft(fft(wavelet,n_convolution).*fft(randsig2,n_convolution),n_convolution)*sqrt(s)/10;
filtsig2  = real(convolution_result_fft(half_of_wavelet_size+1:end-half_of_wavelet_size));
anglesig2 = angle(convolution_result_fft(half_of_wavelet_size+1:end-half_of_wavelet_size));

figure
for i=1:2
    subplot(2,1,i)
    eval([ 'plot(randsig' num2str(i) ')' ]) % "eval" can be useful for increasing control over commands
    hold on
    eval([ 'plot(filtsig' num2str(i) ',''r'')' ])
end

%% Figure 25.3

% initialize output correlation matrix
correlations = zeros(5,round(1000/f));

for i=1:round(1000/f)

    % correlation of unfiltered random signal
    temp = corrcoef(randsig1(1:end-i),randsig1(i+1:end));
    correlations(1,i) = temp(1,2);

    % correlation of filtered signal
    temp = corrcoef(filtsig1(1:end-i),filtsig1(i+1:end));
    correlations(2,i) = temp(1,2);

    % phase clustering
    correlations(3,i) = abs(mean(exp(1i*( angle(anglesig1(1:end-i)-anglesig1(i+1:end))))));

    % difference of correlations of filtered signal
    temp = corrcoef(filtsig2(1:end-i),filtsig2(i+1:end));
    correlations(4,i) = temp(1,2) - correlations(2,i);

    % difference of phase clusterings
    correlations(5,i) = abs(mean(exp(1i*( angle(anglesig2(1:end-i)-anglesig2(i+1:end)))))) - correlations(3,i);
end

figure
plot(correlations')
xlabel('Lag (ms)')
ylabel('Connectivity strength')
legend({'unfiltered';'power corr';'ISPC';'corr diffs';'ISPC diffs'})

%% end.


function [wh_predictors, betas, b, stat] = regress_best_subsets_ga(X, Y)
% :Usage:
% GA-based best subsets regression
%
% ::
%
%     [wh_predictors, betas, b_subset, stat_subset] = regress_best_subsets_ga(X, Y)
%
% :Inputs:
%
%   **Y:**
%        is outcome data
%
%   **X:**
%        is predictor matrix
%
%   **wh_predictors:**
%        is the primary outcome -- it is vector of which predictors to include in the model
%
% the objective criterion is AIC

seeds = get_seeds(X, Y);

% Example: get_AIC(X, Y, seeds(:, 1))

% The fitness function is higher when AIC is lower
% The 1/AIC transformation would introduce a nonlinear weighting function;
% so would -log(AIC)
% log(1/AIC) is approximately linear.

%fitness_fcn = @(wh_preds)  1 ./ get_AIC(X, Y, wh_preds)
fitness_fcn = @(wh_preds)  log(1 ./ get_AIC(X, Y, wh_preds));

% for i = 1:size(seeds, 2)
%     a(i) = get_AIC(X, Y, seeds(:, i));
%     f(i) = fitness_fcn(seeds(:, i));
% end

% Run GA

example_vec = {seeds(:, 1)}; % this is 'inputs', what will be passed to the objective function (ofun) in the GA

for i = 1:size(seeds, 2), gaseeds{i} = seeds(:, i); end

[best_params,fit,beff,in,isconverged] = tor_ga(300, 50, example_vec, fitness_fcn, 'discrete', 'seeds', gaseeds, 'genconverge', 5);

wh_predictors = logical(best_params{1});

 Xs = X(:, wh_predictors);
 [b, dev, stat] = glmfit(Xs, Y); % subset plus intercept first

 betas = zeros(size(X, 2) + 1, 1);
 betas(logical([1; wh_predictors])) = b;  % add the intercept always

end

function aic = get_AIC(X, Y, wh_preds)

    X = X(:, logical(wh_preds));
    [nobs, npreds] = size(X);

    [b, dev, stat] = glmfit(X, Y);
    RSS = stat.resid' * stat.resid;

    % AIC = 2k - 2ln(L),  k = npreds, L = likelihood
    % AIC = 2k + n(ln(2piRSS/n) + 1)
    %
    % simplifying to relative values (we need only relative AIC, not
    % absolute:

    npreds = sum(wh_preds) + 1;  % include intercept
    aic = 2 * npreds + nobs * log(RSS);

end

function seeds = get_seeds(X, Y)

[nobs, npreds] = size(X);
z = false(npreds, 1);

% seed the GA with stepwise regression results and univariate results (top
% 1, 2, ... k)

[b, dev, stat] = glmfit(X, Y);
b = abs(b(2:end));
t = abs(stat.t(2:end));

% Set of candidate subsets with descending order of abs magnitude of betas
[bs, wh] = sort(b, 1, 'descend');
bseeds = false(npreds);

for i = 1:npreds
    my_subset = z;
    my_subset(wh(1:i)) = true;
    bseeds(:, i) = my_subset;
end

% Set of candidate subsets with descending order of abs magnitude of
% t-values (last one is redundant with bseeds)
[ts, wh] = sort(t, 1, 'descend');
tseeds = false(npreds, npreds - 1);
for i = 1:npreds - 1
    my_subset = z;
    my_subset(wh(1:i)) = true;
    tseeds(:, i) = my_subset;
end

% single-predictor seeds with each variable
eseeds = logical(eye(npreds));

% stepwise seeds
pentervals = [.05:.05:.95];
for i = 1:length(pentervals)
    [b, se, p, inmodel] = stepwisefit(X, Y, 'penter', pentervals(i), 'display', 'off');
    fseeds(:, i) = inmodel';
end

seeds = [bseeds tseeds eseeds fseeds];

% random seeds
numseeds = max(100, 200 - size(seeds, 2)); % at least 100 seeds
rseeds = rand(npreds, numseeds);
rseeds = rseeds > .5;

seeds = [seeds rseeds];

end


function [G, tt]=gsp_swiss_roll(N,rand_state,param)
%GSP_SWISS_ROLL Initialize a swiss roll graph
%   Usage:  G = gsp_swiss_roll(N,rand_state,param);
%
%   Input parameters:
%         N          : Number of vertices.
%         s          : sigma ( default: sqrt(2/N))
%         thresh     : threshold (default: 1e-6)
%         rand_state : rand seed (default: 0)
%   Output parameters:
%         G     : Graph structure.
%
%   'gsp_create_swiss_roll(N,s,thresh,rand_state)' initializes a graph
%   structure containing the swiss roll graph
%
%   Example:::
%
%          G = gsp_swiss_roll(200);
%          gsp_plot_graph(G);
%

% Author : David I Shuman, Nathanael Perraudin
% Test: test_graphs

if nargin<1
    N = 200;
end

if nargin<2
   rand_state = 0;
end

gsp_reset_seed(rand_state);

a = 1;   % swiss roll goes from a*pi to b*pi
b = 4;
y = rand(2,N);
% uniform distribution along the manifold (in data space)
tt = sqrt((b*b-a*a)*y(1,:)+a*a);
tt = pi*tt;
% now tt should go from a*pi to b*pi
height = y(2,:);
x = [tt.*cos(tt)/b^2; height; tt.*sin(tt)/b^2];

if nargin<3
    param = struct;
end

if ~isfield(param,'k'), param.k = 6; end
G = gsp_nn_graph(x',param);
G.map_coord = y';
G.type = 'Swiss Roll';

end


function [suspicious_index lof] = LOF(A, k)
%
% Local Outlier Factor
% Authors: Markus M. Breunig, Hans-Peter Kriegel,
%          Raymond T. Ng, J?rg Sander
% Original paper :
% LOF: Identifying Density-Based Local Outliers
% e-mail : { breunig | kriegel | sander }
%          @dbs.informatik.uni-muenchen.de
%          rng@cs.ubc.ca
% Programmer: Yi-Ren Yeh(yirenyeh@gmail.com)
% modified by: Zi-Wen Gui(evan176@hotmail.com)
%
%
% Inputs
%   A: the data matrix, each row represents an instance
%   k: the number of nearest neighbors, specified as an integer or
%      as a fraction of the total number of data points
%
% Outputs
%   lof: the local outlier factor for each instance
%   suspicious_index: the ranking of instances according to their
%                     suspicious score
%                     For example, suspicious_index(i)=j means the
%                     ith instance is in jth position in the ranking
%

if k < 1
    [numrows ~] = size(A);
    k = round(k*numrows);
end

try
    %Find the nearest neighbors by "KDTree" for each elements
    [k_index, k_dist] = knnsearch(A,A,'k',k+1,'nsmethod','kdtree','IncludeTies',true);
    %Ignore first element(itself) at nearest neighbors
    k_index = cellfun(@(x) x(2:end),k_index,'UniformOutput',false);
    numneigh = cellfun('length',k_index);
    %Get k-distance
    k_dist1 = cell2mat(cellfun(@(x) x(end),k_dist,'UniformOutput',false));
    %Get row length of matrix A
    n = length(A(:,1));
    %Initialize lrd_value vector
    lrd_value = zeros(n,1);
    %Calculate lrd for each elements
    for i = 1:n
        lrd_value(i) = lrd(A, i, k_dist1, k_index, numneigh(i));
    end
    %Initialize lof vector
    lof = zeros(n,1);
    %Calculate LOF
    for i = 1:n
        lof(i) = sum(lrd_value(k_index{i})/lrd_value(i))/numneigh(i);
    end
    %Indices from sorting lof are the suspicious score rankings
    [~,suspicious_index]=sort(lof,'descend');

catch err
    if (strcmp(err.message, 'Invalid parameter name: IncludeTies.'))
        warning('MATLAB:LOF', 'Matlab not newest version? Falling back to old version.')
        [suspicious_index lof] = LOF_old(A, k);
    else
        rethrow(err)
    end

end

%=========================================================================
function lrd_value = lrd(A, index_p, k_dist,k_index, numneighbors)
%Calculate the reachability distance for nearest neighbors
Temp = repmat(A(index_p,:), numneighbors, 1) - A(k_index{index_p}, :);
Temp = sqrt(sum(Temp.^2,2));
reach_dist = max([Temp k_dist(k_index{index_p})],[],2);
%Calculate the local reachability density for each elements
lrd_value = numneighbors/sum(reach_dist);


function y = normpdfg(x,scale,shape,center,max)
% y = normpdfg(x,scale,shape,center,max)
%
% Evalute the generalized gaussian pdf at point(s) x
% the pdf is defined by the given scale (2*variance) and shape and mean
% (center)
%
% Note the variance of the distribution is scale/2
%
% As shape --> Inf we get a (step function) uniform distribution over [center-scale, center+scale]
% As shape --> 0 we get a delta function
% Beta = 1 is the Laplace
% Beta = 2 is gaussian
%
% (C) Tim Mullen, 2011. SCCN/INC UCSD

A = shape./(2.*scale.*gamma(1/shape));
C = max./A;

y = C.*A.*exp(-((abs(x-center)/scale).^shape));