%ZSAD Sum of absolute differences
%
% M = ZSAD(I1, I2) is the zero-mean sum of absolute differences between the
% two equally sized image patches I1 and I2.  The result M is a scalar that
% indicates image similarity, a value of 0 indicates identical pixel patterns
% and is increasingly positive as image dissimilarity increases.
%
% Notes::
% - The ZSAD similarity measure is invariant to changes in image brightness
%   offset.
%
% See also SAD, SSD, NCC, ISIMILARITY.

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

function m = zssd(w1, w2)

	w1 = w1 - mean(w1(:));
	w2 = w2 - mean(w2(:));

	m = abs(w1-w2);
    m = sum(m(:));


% Chapter 7 - Differential Equations.
% Program_7c - Solving Simple Differential Equations.
% Copyright Birkhauser 2013. Stephen Lynch.

% Chemical kinetics (Example 8).
% Note that INLINE will be removed in future releases.
% deqn=inline('.00713*(4-c(1))^2*(1-c(1)/2)','t','c');
k=0.00713;
deqn = @(t,c) [k*(4-c)^2*(1-c/2)];
[t,ca]=ode45(deqn,[0 400],0);
plot(t,ca(:,1))
axis([0 400 0 3])
fsize=15;
set(gca,'XTick',0:100:400,'FontSize',fsize)
set(gca,'YTick',0:1:3,'FontSize',fsize)
xlabel('t','FontSize',fsize)
ylabel('c(t)','FontSize',fsize)
hold off

% End of Program_7c.


function [upsilonvp, upsilon] = lfmvpComputeUpsilonMatrix(gamma, sigma2, t1, t2, mode)

% LFMVPCOMPUTEUPSILONMATRIX Upsilon matrix vel. pos. with t1, t2 limits
% FORMAT
% DESC computes a portion of the LFM kernel.
% ARG gamma : Gamma value for system.
% ARG sigma2 : length scale of latent process.
% ARG t1 : first time input (number of time points x 1).
% ARG t2 : second time input (number of time points x 1).
% ARG mode : operation mode, according to the derivative (mode 0,
% derivative wrt t1, mode 1 derivative wrt t2)
% RETURN upsilon : result of this subcomponent of the kernel for the given values.
%
% SEEALSO : lfmComputeUpsilonMatrix.F, lfmComputeH3.m
%
% COPYRIGHT : Mauricio A. Alvarez, 2010

% KERN

sigma = sqrt(sigma2);
gridt1 = repmat(t1, 1, length(t2));
gridt2 = repmat(t2', length(t1), 1);
timeGrid = gridt1 - gridt2;

if mode==0
    if nargout > 1
        upsilon = lfmComputeUpsilonMatrix(gamma, sigma2, t1, t2);
        upsilonvp = -gamma*upsilon + (2/(sqrt(pi)*sigma))*exp(-(timeGrid.^2)./sigma2);
    else
        upsilonvp = -gamma*lfmComputeUpsilonMatrix(gamma, sigma2, t1, t2) ...
            + (2/(sqrt(pi)*sigma))*exp(-(timeGrid.^2)./sigma2);
    end
else
    if nargout > 1
        upsilon = lfmComputeUpsilonMatrix(gamma, sigma2, t1, t2);
        upsilonvp = gamma*upsilon - (2/(sqrt(pi)*sigma))*exp(-(timeGrid.^2)./sigma2) ...
            + (2/(sqrt(pi)*sigma))*exp(-gamma*t1)*(exp(-(t2.^2)/sigma2)).';
    else
        upsilonvp = gamma*lfmComputeUpsilonMatrix(gamma, sigma2, t1, t2) ...
            - (2/(sqrt(pi)*sigma))*exp(-(timeGrid.^2)./sigma2) ...
            + (2/(sqrt(pi)*sigma))*exp(-gamma*t1)*(exp(-(t2.^2)/sigma2)).';
    end
end


function [m_CO2, y_CO2] = DuanSun(T, p, m_salt)
% This function solves the Duan-Sun model to calculate the solubility of
% CO2 in water and in the brine and the mole fraction of CO2 in the gas phase
% T is the temperature in [K]
% p is the pressure in [Pa]
% m is the salt molality in [mol/kg]
% Copyright (c) 2012-2016 Ali Akbar Eftekhari
% See the license file
% R = 0.08314467; % [bar.L/(mol.K)]
% Written by Ali A. Eftekhari at TU Delft

P = p/1e5; % convert Pa to bar
% Constants (Table 2):
% constants for the calculation of chemical potential of CO2 in the liquid phase
c_mu_CO2_L = [28.9447706, -0.0354581768, -4770.67077, 1.02782768e-5, ...
		  33.8126098, 9.04037140e-3, -1.14934031e-3, -0.307405726, ...
		  -0.0907301486, 9.32713393e-4, 0.0];
% Constants for the calculation of second and third order interaction parameters
c_lambda_CO2_Na = [-0.411370585, 6.07632013e-4, 97.5347708, 0.0, 0.0, ...
				   0.0, 0.0, -0.0237622469, 0.0170656236, 0.0, 1.41335834e-5];
c_zeta_CO2_NaCl = [3.36389723e-4, -1.98298980e-5, 0.0, 0.0, 0.0, ...
				    0.0, 0.0, 2.12220830e-3, -5.24873303e-3, 0.0, 0.0];

% using the above constants (Eq. 7)
par = @(c)(c(1)+c(2)*T+c(3)/T+c(4)*T^2+c(5)/(630-T)+c(6)*P+ ...
		   c(7)*P*log(T)+c(8)*P/T+c(9)*P/(630-T)+ ...
		   c(10)*P^2/(630-T)^2+c(11)*T*log(P));

mu_CO2_L = par(c_mu_CO2_L);
lambda_CO2_Na = par(c_lambda_CO2_Na);
zeta_CO2_NaCl = par(c_zeta_CO2_NaCl);

% activity coefficient of CO2 (Eq. 5)
m_Na = m_salt;
m_Cl = m_salt;
log_gama_CO2 = 2*lambda_CO2_Na*m_Na+zeta_CO2_NaCl*m_Na*m_Cl;

% calculation of the fugacity of CO2 using the equation of Duan
a = [8.99288497e-2, -4.94783127e-1, 4.77922245e-2, 1.03808883e-2, ...
     -2.82516861e-2, 9.49887563e-2, 5.20600880e-4, -2.93540971e-4, ...
     -1.77265112e-3, -2.51101973e-5, 8.93353441e-5, 7.88998563e-5, ...
     -1.66727022e-2, 1.39800000,     2.96000000e-2];
% CO2 critical properties
Tc = 304.25; % [K]
Pc = 7.39e6; % [Pa]
Vc = 8.314467*Tc/Pc;
Tr = T/Tc;
Pr = P*1.0e5/Pc;
Z = @(Vr)(1+(a(1)+a(2)/Tr^2+a(3)/Tr^3)/Vr + ...
          (a(4)+a(5)/Tr^2+a(6)/Tr^3)/Vr^2 + (a(7)+a(8)/Tr^2+a(9)/Tr^3)/Vr^4 ...
          + (a(10)+a(11)/Tr^2+a(12)/Tr^3)/Vr^5 + ...
          a(13)/(Tr^3*Vr^2)*(a(14)+a(15)/Vr^2)*exp(-a(15)/Vr^2));
Vr_guess = (8.314*T*0.9/(P*1.0e5))/Vc;
F = @(Vr)(Z(Vr)/Vr-Pr/Tr);
Vr = fzero(F, Vr_guess);

% fugacity coefficient of pure CO2
Z_num = Z(Vr);
log_phi_CO2 = Z_num - 1.0 - log(Z_num) + (a(1)+a(2)/Tr^2+a(3)/Tr^3)/Vr + ...
			(a(4)+a(5)/Tr^2+a(6)/Tr^3)/(2.0*Vr^2) + ...
			(a(7)+a(8)/Tr^2+a(9)/Tr^3)/(4.0*Vr^4) + ...
			(a(10)+a(11)/Tr^2+a(12)/Tr^3)/(5.0*Vr^5) + ...
			a(13)/(2.0*Tr^3*a(15))*(a(14)+1.0-(a(14)+1.0+a(15)/Vr^2)*exp(-a(15)/Vr^2));

% pure water vapor pressure
c = [-38.640844, 5.8948420, 59.876516, 26.654627, 10.637097];
Pcw = 220.85; % [bar]
Tcw = 647.29; % [K]
t = (T-Tcw)/Tcw;
P_water = Pcw*T/Tcw*(1+c(1)*(-t)^1.9+c(2)*t+c(3)*t^2+c(4)*t^3+c(5)*t^4);
y_CO2 = (P-P_water)/P;

log_coef = mu_CO2_L - log_phi_CO2 + log_gama_CO2;
m_CO2 = y_CO2*P/exp(log_coef);


function [vec,lmax] = SHMatrix2Vec(A,B)

% [vec,lmax] = SHMatrix2Vec(A,B)
%
% Converts the two coefficient matrices, A and B, to the real
% spherical harmonic coefficient vector. A contains coefficients
% by cos(m*phi) and B contains coefficients by sin(m*phi).
% Inverse of SHMatrix2Vec.

lmax = size(A,1)-1;
vec = SHCreateVec(lmax);

j=0;
for l=0:lmax
    j=j+1;
    vec(j)=A(l+1,1);
    for m=1:l
        j=j+1;
        vec(j)=A(l+1,m+1);
        j=j+1;
        vec(j)=B(l+1,m+1);
    end
end


function mappedX = hlle(X, no_dims, k, eig_impl)
%HLLE Runs the Hessian LLE algorithm
%
%   mappedX = hlle(X, no_dims, k, eig_impl)
%
% Runs the Hessian LLE algorithm on dataset X to reduce its dimensionality
% to no_dims. The variable k specifies the number of nearest negihbors that
% is used.
%
%

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

    if ~exist('no_dims', 'var')
        no_dims = 2;
    end
    if ~exist('k', 'var')
        k = 12;
    end
    if ~exist('eig_impl', 'var')
        eig_impl = 'Matlab';
    end

    % Compute nearest neighbors
    if ischar(k)
        warning('Adaptive neighborhood selection often leads to problems in Hessian LLE.');
    end
    disp('Finding nearest neighbors...');
    [D, nind] = find_nn(X, k);
    max_k = size(nind, 2);

    % Size of original data
    n = size(X, 1);

    % Extra term count for quadratic form
    dp = no_dims * (no_dims + 1) / 2;
    W = sparse([], [], [], dp * n, n, dp * n * max_k);

    % For all datapoints
    disp('Building Hessian estimator for neighboring points...');
    for i=1:n
        % Center datapoints by substracting their mean
        tmp_ind = nind(i,:);
        tmp_ind = tmp_ind(tmp_ind ~= 0);
        kt = length(tmp_ind);
        thisx = X(tmp_ind,:);
        thisx = (thisx - repmat(mean(thisx, 1), kt, 1))';

        % Compute local coordinates (using SVD)
        [U, D, Vpr] = svd(thisx);
        if size(Vpr, 2) < no_dims
            no_dims = size(Vpr, 2);
            dp = no_dims * (no_dims + 1) / 2;
            warning(['Target dimensionality reduced to ' num2str(no_dims) '...']);
        end
        V = Vpr(:,1:no_dims);
		% Basically, the above is applying PCA to the neighborhood of Xi. The PCA mapping that is found
		% (and that is contained in V) is an approximation for the tangent space at Xi.

        % Build Hessian estimator
        clear Yi; clear Pii;
        ct = 0;
        for mm=1:no_dims
            startp = V(:,mm);
            for nn=1:length(mm:no_dims)
                indles = mm:no_dims;
                Yi(:,ct+nn) = startp .* (V(:,indles(nn)));
            end
            ct = ct + length(mm:no_dims);
        end
        Yi = [repmat(1, kt, 1) V Yi];

        % Gram-Schmidt orthogonalization (works different from Matlab QR function)
        [Yt, Orig] = mgs(Yi);
        Pii = Yt(:,no_dims + 2:end)';

        % Double check weights sum to 1
        for j=1:dp
            if sum(Pii(j,:)) > 0.0001
                tpp = Pii(j,:) ./ sum(Pii(j,:));
            else
                tpp = Pii(j,:);
            end

            % Fill weight matrix
            W((i - 1) * dp + j, tmp_ind) = tpp;
        end
    end

    % The weight matrix W is now entirely filled, perform eigenanalysis of W
    disp('Computing HLLE embedding (eigenanalysis)...');

    % Make sparse matrix that is inproduct of W
    G = W' * W;
	G(isnan(G)) = 0;
    G = sparse(G);

    % Clear some memory
    clear X thisx W D nind U D Vpr;

    % Perform eigendecomposition
    tol = 0;
    if strcmp(eig_impl, 'JDQR')
        options.Disp = 0;
        options.LSolver = 'bicgstab';
        [mappedX, eigenvals] = jdqr(G, no_dims + 1, tol, options);
    else
        options.disp = 0;
        options.issym = 1;
        options.isreal = 1;
        [mappedX, eigenvals] = eigs(G, no_dims + 1, tol, options);
    end

    % Sort eigenvalues and eigenvectors
    [eigenvals, ind] = sort(diag(eigenvals), 'ascend');
    if size(mappedX, 2) < no_dims + 1, no_dims = size(mappedX, 2) - 1; end
    mappedX = mappedX(:,ind(2:no_dims + 1));

    % Extract nonzero coordinates
    mappedX = mappedX(:,1:no_dims)' * sqrt(n);
    mappedX = mappedX';


function rec = perform_bp_benchmark(P,W, slist, ntrial)

% perform_bp_benchmark - benchmark basis pursuit recovery
%
% rec = perform_bp_benchmark(P,W, slist, ntrial);
%
%   Try to solve many (ntrial) problems of the kind
%       x = argmin_a |a|_1    s.t. U*a=U*x0
%   where x_0 is an s-sparse signal.
%   U = P*W
%   The report the number of coordinate of x0 acuatly recovered in x.
%
%   rec(i) is the average ratio of recoverred dirac for sparsity slist(i).
%   rec=1 means perfect recovery and rec=0 means no recovery (bad).
%
%   Copyright (c) 2007 Gabriel Peyr?

if nargin<2
    slist = 10:18;
end
if nargin<3
    ntrial = 50;
end

% BP options
maxIters = 20; lambda = 0; OptTol = 1e-5;

U = P*W;
m = size(U,2);
rec = [];

% detection threshold
T = 0.1;

for is = 1:length(slist)
    progressbar(is,length(slist));
    % sparsity
    s = slist(is);
    e = 0;
    for k = 1:ntrial
        x = compute_rand_sparse(m,s);
        y = U*x;
        x0 = SolveBP(U, y, m, maxIters, lambda, OptTol);
        % compute the number of coordinates recovered
%        [tmp,I] = sort(x0);
%        x0 = x0 * 0; x0(I(end-s+1:end)) = 1;
%        e = e + sum(x0.*x)/s;
        e = e + norm( (W*x)-(W*x0) );
    end
    rec(end+1) = e/ntrial;
end


function y=PSNR_RGB(X,Y)

% Y= PSNR_RGB(X,Y)
% Computes the Peak Signal to Noise Ratio for two RGB images
% Class input : double [0,1] ,
% july ,25, 2012
% KHMOU Youssef

if size(X)~=size(Y)
    error('The images must have the same size');
end

%if ~isa(X,'double')
%   X=double(X)./255.00;
%end
%if  ~isa(Y,'double')
%    Y=double(Y)./255.00;
%end

% begin
d1=max(X(:));
d2=max(Y(:));
d=max(d1,d2);
sigma=mean2((X-Y).^2);

y=10*log((d.^2)./sigma);


function [xform, fiterr] = affineSolve(A,B);
% Solve the equation coordsA = xform * coordsB for the xform.
%
% Usage: [xform, fiterr] = affineSolve(coordsA,coordsB);
%
% Problem: You have coordinates of the same points in two
% coordinate spaces, A and B. You want to find a 4x4 affine
% transform matrix that optimally maps between these points.
%
% Sub-problem: Simply dividing coordsA / coordsB returns a
% 3x3 xform, not a 4x4 affine.
%
% Solution: use this. It uses singular value decomposition to
% find the rotation, scales (and skews, if the points are not
% really matching), and subtraction to find the translations,
% and builds a 4x4 affine xform which minimizes the error
% in the above equation.
%
% coordsA and coordsB should be 3xN coordinate matrices with the
% same number of columns. Each column of coordsA should reflect
% the (row, column, slice) of a point in coordinate space A (the
% NEW coordinate space being xformed to), and each column in coordsB
% should be the (row, column, slice) of the SAME POINT in coordinate
% space B (the OLD coordinate space being xformed from).
%
% The optional output argument fiterr is the root mean squared error
% between coordsA and coordsB given the best set of rotations and
% tanslations. It is a measure of the goodness-of-fit of the xform
% (since, if the points don't really correspond, not all equations
% can be perfectly solved).
%
% ras, 10/2005.
if nargin<2, help(mfilename); error('Not enough args.'); end

% size + number checks:
if ~isnumeric(A) | ~isnumeric(B), error('Need numeric args.'); end
if size(A,2)~=size(B,2)
    error('Coords should have same # of columns.');
end
if size(A,1)<3 | size(B,1)<3,
    error('Coords need to be specified in 3 dimensions.')
end

% just curious: does just doing / division work better?
n = size(A, 2);
xform = [A; ones(1, n)] / [B; ones(1, n)];

% % express points relative to center of all points:
% nPoints = size(A,2);
% centeredA = A - repmat(mean(A,2),[1 nPoints]);
% centeredB = B - repmat(mean(B,2),[1 nPoints]);
%
% % solve for rotation + scales (& skews, if points are bad):
% H = zeros(3,3);
% for i = 1:nPoints
%     H = H + (centeredB(:,i) * centeredA(:,i)');
% end
% [U S V] = svd(H);
% rot = V*(U');
%
% % solve for translation
% rotatedB = rot * B;
% trans = mean(A,2) - mean(rotatedB,2);
%
% % build 4x4 affine xform matrix
% xform = [rot trans; 0 0 0 1];
%
% % force skew to equal 0
% [trans rot scale skew] = affineDecompose(xform);
% skew = [0 0 0];
% xform = affineBuild(trans,rot,scale,skew);
%
% % return error of fit if requested
% if nargout > 1
% 	xB = xform * [B; ones(1, size(B, 2))];
% 	xB = xB(1:3,:);
% 	fiterr = mean(abs(A - xB).^2);
% end

return


% DEMVOWELSFGPLVM2 Model the vowels data with a 2-D FGPLVM using RBF kernel.

% FGPLVM

% Fix seeds
randn('seed', 1e5);
rand('seed', 1e5);

dataSetName = 'vowels';
experimentNo = 2;

% load data
[Y, lbls] = lvmLoadData(dataSetName);

% Set up model
options = fgplvmOptions('fitc');
options.numActive = 200;
latentDim = 2;
d = size(Y, 2);

model = fgplvmCreate(latentDim, d, Y, options);

% Optimise the model.
iters = 1000;
display = 1;

model = fgplvmOptimise(model, display, iters);

% Save the results.
modelWriteResult(model, dataSetName, experimentNo);

if exist('printDiagram') & printDiagram
  lvmPrintPlot(model, lbls, dataSetName, experimentNo);
end

% Load the results and display dynamically.
lvmResultsDynamic(dataSetName, experimentNo, 'vector')

errors = lvmNearestNeighbour(model, lbls);


function  omega = maxAngle(oR,varargin)
% get the maximum angle of the fundamental region
%
% Syntax
%   omega = maxAngle(oR)   % maximum angle in the orientation region
%   omega = maxAngle(oR,v) % maximum angle about axis v
%
% Description
% It is important that v is within the fundamental sector
%
% Input
%  oR - @orientationRegion
%  v  - @vector3d
%

if nargin>1 && isa(varargin{1},'vector3d')

  % ignore restrictions on the rotational axis
  %N = oR.N(oR.N.angle < pi<1e-3);
  N = oR.N;

  % project v to the fundamental sector
  dcs = properGroup(disjoint(oR.CS1,oR.CS2));
  if oR.antipodal, dcs = dcs.Laue; end
  h = varargin{1};
  h = reshape(project2FundamentalRegion(h,dcs),size(h));

  if isempty(N)
    omega = repmat(pi,size(h));
  else

    d = dot_outer(N.axis, normalize(h));
    dtan = repmat(-tan(N(:).angle./2),1,size(d,2));
    d(abs(d)<1e-2)=0;
    d = dtan .* d;

    %d = dot_outer(-tan(N.angle/2) .* N.axis, normalize(varargin{1}));

    d = 2*acot(d);
    d(d<0) = pi;

    omega = min(d,[],1);
    omega = reshape(omega,size(h));
    omega(omega<1e-4) = 0;
  end

elseif isempty(oR.V) || check_option(varargin,'complete')

  omega = pi;

else

  omega = max(angle(oR.V,'noSymmetry'));

end


function demoStarTrails()
%%DEMOSTARTRAILS This demonstrates how to determine the apparent locations
%                of stars in the sky at a particular location and time
%                using the Hipparcos catalog and a low-fidelity atmospheric
%                refraction model. The stars are plotted over a number of
%                times on the same plot such that their motion in the sky
%                becomes apparent. The planets, Sun and Moon are not
%                plotted.
%
%October 2014 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

%The location of the observer on the surface of the Earth in ellipsoidal
%coordinates.
obsLoc=[-pi/8;pi-pi/8+pi/8;0];

%Properties of the imaging sensor:
pixX=3264;%Pixels wide
pixY=2448;%Pixels high.
pixWidth=1.5e-6;%Width of a pixel in meters
f=4.12e-3;%Focal length, meters.
%The size of the sensor.
sensX=pixX*pixWidth;
sensY=pixY*pixWidth;
%The angular width of the field of view when viewing something at infinity.
radAz=2*atan(sensX/(2*f));%Width in azimuth (radians)
radEl=2*atan(sensY/(2*f));%Width in elevation (radians)

%The half-azimuth and half-elevation of the observer's field of view from
%the center of the field.
locFOVHalfWidth=[radAz/2;radEl/2];

%It is assumed that the observer's true orientation is a rotation 60
%degrees about the local ellipsoidal ENU x-axis-up followed by a rotation
%of 15 degrees about the rotated z-axis. So, we will now build the
%ENU2Local rotation matrix.
R1=axisAng2RotMat([1;0;0],60*pi/180);
R2=axisAng2RotMat([0;0;1],15*pi/180);
ENU2LocRotMat=R2*R1;

disp('Loading the star catalog.')
%Load stars with a magnitude of at most 6 (the visible region). The stars
%are formatted such that the data file can be directly passed to the
%changeEpoch function, which allows the data to be used at different times.
%The catalog is provided at epoch 1991.25.
dataFile=getHipparcosCat([],6,false,true);

%Change the epoch from 1995.25 to J2000.0
dataFile=changeEpoch(dataFile);

%Set the initial date of display. Minutes will be advanced to demonstrate
%the motion of the stars.
year=2014;
month=1;
day=1;
hour=0;
second=0;

disp('Plotting the stars at different times in the local coordinate')
disp('system and field of view of the observer')
figure(1)
hold on
for minute=0:5:20
    [Jul1,Jul2]=Cal2UTC(year,month,day,hour,minute,second);
    %This returns the apparent star locations in the local ENU coordinate
    %system, where zSpherENU is the location in spherical azimuth and
    %elevation in ENU coordinate and uObs is unit vectors in local ENU
    %coordinates.
    [zSpherENU,uStarsENU]=starCat2Obs(dataFile,Jul1,Jul2,obsLoc);

    %We have to determine which stars are in the observer's field of view.
    %First, we need to rotate the apparent locations into the observer's
    %local coordinate system
    uStarLocal=ENU2LocRotMat*uStarsENU;

    %Next, obtain the azimuth and elevation of the stars in the local
    %coordinate system.
    zStarLocal=Cart2Sphere(uStarLocal);
    %Discard the useless range component.
    zStarLocal=zStarLocal(2:3,:);

    %The thing in the middle of the field of view is at local azimuth and
    %elevation (0,0). We need to figure out which stars and celestial
    %bodies are in the true field of view and get rid of the others.
    sel=zStarLocal(1,:)<locFOVHalfWidth(1);
    sel=sel&(zStarLocal(1,:)>-locFOVHalfWidth(1));
    sel=sel&(zStarLocal(2,:)<locFOVHalfWidth(2));
    sel=sel&(zStarLocal(2,:)>-locFOVHalfWidth(2));

    uStarLocal=uStarLocal(:,sel);
    zStarLocal=zStarLocal(:,sel);

    %Plot the stars in terms of azimuth and elevation in degrees.
    scatter(zStarLocal(1,:)*(180/pi),zStarLocal(2,:)*(180/pi),'filled')
end
axis tight
h0=title('Stars Observed Every 5 Minutes for 20 Minutes');
h1=xlabel('Local Azimuth^\circ');
h2=ylabel('Local Elevation^\circ');
set(gca,'FontSize',14,'FontWeight','bold','FontName','Times')
set(h0,'FontSize',14,'FontWeight','bold','FontName','Times')
set(h1,'FontSize',14,'FontWeight','bold','FontName','Times')
set(h2,'FontSize',14,'FontWeight','bold','FontName','Times')

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 [xo]=groupthresh(xi,lambda,varargin)
%GROUPTHRESH   Group thresholding
%   Usage:  xo=groupthresh(xi,lambda);
%
%   `groupthresh(x,lambda)` performs group thresholding on *x*, with
%   threshold *lambda*.  *x* must be a two-dimensional array, the first
%   dimension labelling groups, and the second one labelling members. This
%   means that the groups are the row vectors of the input (the vectors
%   along the 2nd dimension).
%
%   Several types of grouping behaviour are available:
%
%   * `groupthresh(x,lambda,'group')` shrinks all coefficients within a given
%     group according to the value of the $l^2$ norm of the group in
%     comparison to the threshold *lambda*. This is the default.
%
%   * `groupthresh(x,lambda,'elite')` shrinks all coefficients within a
%     given group according to the value of the $l^1$ norm of the
%     group in comparison to the threshold value *lambda*.
%
%   `groupthresh(x,lambda,dim)` chooses groups along dimension
%   *dim*. The default value is $dim=2$.
%
%   `groupthresh` accepts all the flags of |thresh| to choose the
%   thresholding type within each group and the output type (full / sparse
%   matrix). Please see the help of |thresh| for the available
%   options. Default is to use soft thresholding and full matrix output.
%
%   See also:  thresh
%
%   Demos:  demo_audioshrink
%
%   References: Kowalski08sparsity kowalski2009mixed yu2008audio

%   AUTHOR : Kai Siedenburg, Bruno Torresani.
%   REFERENCE: OK

if nargin<2
  error('Too few input parameters.');k
end;

if (prod(size(lambda))~=1 || ~isnumeric(lambda))
  error('lambda must be a scalar.');
end;

% Define initial value for flags and key/value pairs.
definput.import={'thresh','groupthresh'};
definput.importdefaults={'soft'};
definput.keyvals.dim=2;

[flags,keyvals,dim]=ltfatarghelper({'dim'},definput,varargin);

% kv.dim (the time or frequency selector) is handled by assert_sigreshape_pre
[xi,L,NbMembers,NbGroups,dim,permutedsize,order]=assert_sigreshape_pre(xi,[],dim,'GROUPTHRESH');

if flags.do_sparse
  xo = sparse(size(xi));
else
  xo = zeros(size(xi));
end;

if flags.do_group

  groupnorm = sqrt(sum(abs(xi).^2));
  w = thresh(groupnorm, lambda, flags.iofun,flags.outclass)./groupnorm;

  % Clean w for NaN. NaN appears if the input has a group with norm
  % exactly 0.
  w(isnan(w)) = 0;

  xo = bsxfun(@times,xi,w);

end

if flags.do_elite
  for ii=1:NbGroups,
    y = sort(abs(xi(:,ii)),'descend');
    rhs = cumsum(y);
    rhs = rhs .* lambda ./ (1 + lambda * (1:NbMembers)');
    M_ii = find(diff(sign(y-rhs)));
    if (M_ii~=0)
      tau_ii = lambda * norm(y(1:M_ii),1)/(1+lambda*M_ii);
    else
      tau_ii = 0;
    end

    % FIXME: The following line does not work for sparse matrices.
    xo(:,ii) = thresh(xi(:,ii),tau_ii,flags.iofun,flags.outclass);
  end
end;

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


function [ dat ] = prep_filter( dat, varargin )
% prep_filter (Pre-processing procedure):
%
% Description:
%     This function filters the data within specified frequency band
%
% Example:
%    EEG.data=prep_filter(EEG.data, {'frequency', [7 13];'fs',100});
%
% Input:
%     dat       - EEG data structure
% Option:
%     frequency - Frequency range that you want to filter
%     fs        - Sampling frequency
% Return:
%     dat       - Spectrally filtered data
%
% Min-ho Lee
% mhlee@image.korea.ac.kr
%

if iscell(varargin{:})
    opt=opt_cellToStruct(varargin{:});
elseif isstruct(varargin{:}) % already structure(x-validation)
    opt=varargin{:}
end

switch isstruct(dat)
    case true %struct
        if ~isfield(dat, 'x') && ~isfield(dat, 'fs')
            warning('Parameter is missing: dat.x or dat.fs');
        end

        if ~isfield(opt,'fs')
            if isfield(dat, 'fs')
                opt.fs=dat.fs;
            else
                error('Parameter is missing: fs');
            end
        end
        tDat=dat.x;
        if ndims(tDat)==3   %smt
            [nD nT nC]=size(tDat);
            tDat=reshape(tDat, [nD*nT,nC]);
            band=opt.frequency;
            [b,a]= butter(5, band/(opt.fs/2),'bandpass');
            tDat(:,:)=filter(b, a, tDat(:,:));
            tDat=reshape(tDat, [nD, nT,nC]);
            dat.x=tDat;
        elseif ndims(tDat)==2  %cnt
            band=opt.frequency;
            [b,a]= butter(5, band/(opt.fs/2),'bandpass');
            tDat(:,:)=filter(b, a, tDat(:,:));
            dat.x=tDat;
        end

    case false
        % add if dat is not struct
        tDat=dat;
        if ndims(tDat)==3   %smt
            [nD nT nC]=size(tDat);
            tDat=reshape(tDat, [nD*nT,nC]);
            band=opt.frequency;
            [b,a]= butter(5, band/(opt.fs/2),'bandpass');
            tDat(:,:)=filter(b, a, tDat(:,:));
            tDat=reshape(tDat, [nD, nT,nC]);
            dat.x=tDat;
        elseif ndims(tDat)==2  %cnt
            band=opt.frequency;
            [b,a]= butter(5, band/(opt.fs/2),'bandpass');
            %% TODO: It' doesn't work when the sampling rate is 1000 Hz;
            tDat(:,:)=filter(b, a, tDat(:,:));
            fld='x';
            dat=tDat;
        end

        % History
        if isfield(dat,'stack')
            c = mfilename('fullpath');
            c = strsplit(c,'\');
            dat.stack{end+1}=c{end};
        end
end


% 1D Simple Bar
% clear memory
clear all

% E: modulus of elasticity
% A: area of cross section
% L: length of bar
E=30e6; A=1; L=[30 30 30];

% numberElements: number of elements
numberElements=3;
% numberNodes: number of nodes
numberNodes=4;
% generation of coordinates and connectivities
elementNodes=[1 2;2 3;3 4];
nodeCoordinates=[0 30 60 90];
% for structure:
   % displacements: displacement vector
   % force : force vector
   % stiffness: stiffness matrix
force=zeros(numberNodes,1);
stiffness=zeros(numberNodes,numberNodes);
% applied load at node 2
force(2)=3000.0;

% computation of the system stiffness matrix
for e=1:numberElements;
  % elementDof: element degrees of freedom (Dof)
  elementDof=elementNodes(e,:) ;
  k(e)=E*A/L(e) ;
  stiffness(elementDof,elementDof)=...
       stiffness(elementDof,elementDof)+k(e)*[1 -1;-1 1];
end
% boundary conditions and solution
% prescribed dofs
prescribedDof=[1;4];
% solution
GDof=numberNodes;
displacements=solution(GDof,prescribedDof,stiffness,force);
% output displacements/reactions
outputDisplacementsReactions(displacements,stiffness, ...
    numberNodes,prescribedDof,force)


function c = gamvbcost(x,logx,apost,bpost,aprior,bprior)
% function c = gamvbcost(x,logx,apost,bpost,aprior,bprior)
%
% Calculates c = -KL(q||p) = <log p(X)> - <log q(X)>
% where the expectation <.> is taken over q(X). This is used for
% calculating the lower bound of the marginal loglikelihood in VB
% models.
%
% Let X be a Gamma distributed variable:
% p(X) = G(aprior,bprior)
% q(X) = G(apost,bpost)
% <X> = x
% <log X> = logx

% Cost from prior
c = aprior*log(bprior) - gammaln(aprior) + (aprior-1)*logx - bprior*x;
% Cost from q-posterior
c = c + gamentropy(apost,bpost);


function dz = rocketDynamics(z,u)
% dz = rocketDynamics(z,u)
%
% The basic dynamics and drag coefficient data are from the paper:
%
%   "Drag-law Effects in the Goddard Problem"
%   P. Tsiotras, H. Kelley, H.Kelley    1991
%
% INPUTS:
%   z = [3,n] = [h; v; m] = state vector
%   u = [1,n] = [T] = control = thrust
%

h = z(1,:);   %Height
v = z(2,:);   %Velocity
m = z(3,:);   %Mass
T = u;        %Thrust

%%%% Density of air:
% altitude = [0, 1e4, 2e4, 3e4, 4e4];  %(m)  %height above the ground
% density = [1.23, 0.41, 0.089, 0.018, 0.004];   %(kg/m^3)  density of air
density = 1.474085291*(0.9998541833.^h);  %Data fit off of wolfram alpha

%%%% Drag coefficient, calculated from paper:
A1 = 0.0095;
A2 = 25;
A3 = 0.953;
A4 = 0.036;
speedOfSound = 280;  %(m/s)  %At 10 km altitude
mach = v/speedOfSound;
Cd = A1*atan(A2*(mach-A3))+A4;

%%%% Compute the drag:
Area = pi*3.66;  %(m^2) cross-sectional area (SpaceX F9 Falcon)
D = 0.5*Cd.*Area.*density.*v.^2;

%%%% Compute gravity from inverse-square law:
rEarth = 6.3674447e6;  %(m) radius of earth
mEarth = 5.9721986e24;  %(kg) mass of earth
G = 6.67e-11; %(Nm^2/kg^2) gravitational constant
g = G*mEarth./((h+rEarth).^2);

%%%% Complete the calculation:
c = 4500;  %(m/s) rocket exhaust velocity - chemical rocket
dh = v;  %vertical velocity
dv = (T-D)./m - g;   %vertical acceleration
dm = -T/c;   %mass rate

dz = [dh;dv;dm];

end


function [ bic ] = ml_gmm_bic(Data,Priors,Mu,Sigma,cov_type)
%ML_GMM_BIC Bayesian Information criterion
%
%  input ------------------------------------------------------------------
%
%   o Data:     D x N array representing N datapoints of D dimensions.
%
%   o Priors:   1 x K array representing the prior probabilities of the
%               K GMM components.
%   o Mu:       D x K array representing the centers of the K GMM components.
%
%   o Sigma:    D x D x K array representing the covariance matrices of the
%               K GMM components.
%
%   o cov_type: string, covariance type = 'full','diag' or 'iso'
%
%  output -----------------------------------------------------------------
%
%   o bic   : (1 x 1)
%
%

[D,N]       = size(Data);
K           = length(Priors);
num_param   = K-1 + K * D;

if strcmp(cov_type,'full') == true
    num_param = num_param + K * ( D * ( D - 1)/2 );
elseif strcmp(cov_type,'diag') == true
    num_param = num_param + K * D;
elseif strcmp(cov_type,'iso') == true
    num_param = num_param + K * 1;
else
   error(['no such covariance type: ' cov_type '  only full | diag | isot ']);
end

% compute the loglikelihood of the data given the model
% loglik: (N x 1)

loglik = ml_LogLikelihood_gmm(Data,Priors,Mu,Sigma);
bic    = - 2 * loglik + num_param * log(N);

end


function mse = get_mse(V_equal,W_equal,Weighted)
%compute mse or wmse

global Vn H Ns;

H_equal = W_equal' * H * V_equal;
E_matrix = H_equal * H_equal' - H_equal - H_equal' + eye(Ns) + Vn * W_equal'*W_equal;

%normal MSE
if nargin == 2
    mse = trace(E_matrix);
end

if nargin == 3
    mse = trace(Weighted * E_matrix);
end