%% Apparently it must be here!
% Written by Ali Akbar Eftekhari
% All rights reserved.
%
% Redistribution and use in source and binary forms, with or
% without modification, are permitted provided that the following
% conditions are met:
%
%     *   Redistributions of source code must retain the above copyright notice,
%         this list of conditions and the following disclaimer.
%     *   Redistributions in binary form must reproduce the above
%         copyright notice, this list of conditions and the following
%         disclaimer in the documentation and/or other materials provided
%         with the distribution.
%
% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
% THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
% PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
% CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
% EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
% PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
% OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
% LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
% NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
% SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

clc; clear; close all;
%% create a mesh and visualize it
% The first part of this demo shows you how to create a mesh and visualize
% it. The aim of the visualization is to introduce you to the mesh
% structure.
% Here, we are going to create and visualize a 1D uniform equidistant mesh
% and show the cell centers with a 'o' marker and the face of each cell
% with a '+' marker.
% We start by defining the length of the domain:
L = 1.0; % length of the domain
%%
% Then we define the number of cells in the domain:
%
Nx = 10; % number of cells in the domain
%%
% Now we call createMesh1D, one of the functions from a group of createMesh*
% functions:
%
m = createMesh1D(Nx, L);
%%
% This function generates a structure which keeps the domain and grid
% information, which will be used by almost every other function in the
% FVMtool.
%
% In general, 1D, 2D, 3D, 1D radial (axial symetry), and 2D cylindrical
% grids can be constructed. For more information, type
%%
%
%   help createMesh2D
%   help createMesh3D
%   help createMeshCylindrical1D
%   help createMeshCylindrical2D
%
%%
% Now, let's have a look at the structure of the created mesh:
%
disp(m);
%%
% You can easily get more information about this structure by typing
%%
%   help createMesh1D
%
%%
% As an example, we use the position of the cell centers to visualize the
% domain, the cells, and the interfaces between them:
%
figure(2);
plot(m.cellcenters.x, ones(size(m.cellcenters.x)), 'or', ...
     m.facecenters.x, ones(size(m.facecenters.x)), '-+b');
legend('cell centers', 'face centers');
title('Visualization of a 1D discretized domain');
%%
% The generated figure shows a 1D domain of length 1.0 [unit], discretized
% into 10 cells of the same size (dx = 1/10).
% I'm going to assume that you are familiar with the cell-centred finite
% volume method, discretization, and especially handling the boundary
% conditions using the ghost cells.
% You can also generate and visualize 2D and 3D meshes. To find some
% examples, type:
%
%   help createMesh2D;
%   help createMesh3D;
%
% You will end up with the following two simple examples:
%
Nx = 5;
Ny = 7;
Lx = 10;
Ly = 20;
m = createMesh2D(Nx, Ny, Lx, Ly);
[X, Y] = ndgrid(m.cellcenters.x, m.cellcenters.y);
[Xf,Yf]=ndgrid(m.facecenters.x, m.facecenters.y);
figure(3);
plot(X, Y, 'or', ...
     Xf, Yf, '-b', Xf', Yf', '-b');
%%
% that shows you a 2D grid, and
%
Nx = 2;
Lx = 1.0;
Ny = 3;
Ly = 2.0;
Nz = 4;
Lz = 3.0;
m = createMesh3D(Nx, Ny, Nz, Lx, Ly, Lz);
[X, Y, Z] = ndgrid(m.cellcenters.x, m.cellcenters.y, m.cellcenters.z);
[Xf, Yf, Zf] = ndgrid(m.facecenters.x, m.facecenters.y, m.facecenters.z);
figure(4);
plot3(X(:), Y(:), Z(:), 'or')
hold on;
plot3(Xf(:), Yf(:), Zf(:), '+b')
legend('cell centers', 'cell corners');
hold off;
%%
% that shows a 3D grid.
%% Boundary condition structure
% I had so many reasons to write this toy toolbox, of which, the most
% important one was to be able to implement different boundary conditions
% in the most convenient way! My final implementation makes the user able
% to define either a _periodic boundary condition_ or a _general boundary
% condition_ of the following form:
%%
%
% $$a (\nabla \phi .\mathbf{n}) + b \phi = c $$
%
%%
% In the above equation, $\phi$ is the unknown, and _a_, _b_, and _c_ are
% constants. In practice, this boundary condition equation will be discretized
% to the following system of algebraic equations:
%%
%
% $$M_{bc} \phi = {RHS}_{bc}$$
%
%%
% By adjusting the values of _a_ and _b_, onne can easily define
% one of the following well-known boundary conditions:
%%
%
% * Neumann (_a_ is nonzero; _b_ is 0)
% * Dirichlet (_a_ is zero; _b_ is nonzero)
% * Robin (_a_ and _b_ are both nonzero)
%
%%
% To clarify the above explanations, let us create a boundary condition
% structure for a 1D mesh.
%
Nx = 10; % number of cells in the domain
Lx = 1.0; % length of the domain
m = createMesh1D(Nx, Lx); % createMesh and createMesh are identical
BC = createBC(m); % creates a boundary condition structure
disp(BC); % display the BC structure
%%
% The BC structure has two substructures, i.e., _left_ that denotes the boundary
% at the left side of the domain (at x=0), and _right_ that denotes the
% boundary at the end of the domain (at x=Lx). Each of these substructures
% have three fields, i.e., _a_, _b_, and _c_. The default values are _a_=1,
% _b_=0, and _c_=0:
%
disp(BC.left); % show the values of the coefficients for the left boundary
%%
% There is one other field, i.e., _periodic_, which has a zero value. If
% you change it to one, then a periodic boundary condition will be created
% and the _a_, _b_, and _c_ values will be ignored.
% clearly, you can change the above boundary condition by assigning new
% values to the |BC.left| fields. For instance, you can define a Dirichlet
%  boundary (i.e., fixed value) with a value of 2.5 by typing
%
BC.left.a = 0;
BC.left.b = 1;
BC.left.c = 2.5;
%%
% You can define a periodic boundary condition simply by writing:
%
BC.left.periodic = 1;
%%
% For boundary condition structures created for 2D and 3D grids, we will
% have _left_, _right_, _bottom_, _top_, _back_, and _front_ boundaries
% and thus substructures. Let me show them to you in action:
%
m = createMesh2D(3,4, 1.0, 2.0);
BC = createBC(m);
disp(BC);
disp(BC.top);
%%
% Yes, that's right. _a_, _b_, and _c_ are vectors. It means that you can
% have different boundary conditions for
% different cell faces at each boundary. For instance, I can have a Neumann
% boundary condition for the first cell and a Dirichlet boundary condition
% for the last cell at the top boundary:
BC.top.a(1) = 1; BC.top.b(1) = 0; BC.top.c(1) = 0; % zero value Neumann
BC.top.a(end) =0; BC.top.b(end)=1; BC.top.c(end) = 0; % zero value Dirichlet
disp('top.  a     b     c'); % some fancy display!
disp('   ---------------');
disp([BC.top.a BC.top.b BC.top.c]);
%%
% The same procedure can be followed for a 3D grid. However, _a_, _b_, and
% _c_ values are 2D matrices for a 3D grid. I will discuss it in more
% details when we reach the practical examples.
% *Important note:* If you need to assign a boundary condition to the
% entire boundary, use (:) in your assignment. For instance, to define a
% Dirichlet boundary for the right boundary, you may write
BC.right.a(:)=0; BC.right.b(:)=1; BC.right.c(:)=0;
%% Solve a diffusion equation
% As the first example, we solve a steady-state diffusion equation of the
% following formform
%
% $$\nabla.\left(-D\nabla c\right)=0,$$
%
%%
% where _D_ is the diffusivity and _c_ is the concentration. Let me assume
% that we have a 1D domain, with Dirichlet boundary conditions at both
% boundaries, i.e.,
% at x=0, c=1; and at x=L, c=0.
% First of all, we need to define our domain, discretize it, and define the
% boundaries at the borders.
clc; clear; % clear the screen and memory
L = 0.01; % a 1 cm domain
Nx = 10; % number of cells
m = createMesh3D(Nx,Nx,Nx,L, L,L); % create the mesh
BC = createBC(m); % construct the BC structure (Neumann by default)
%%
% Now as you may remeber, we have to switch from Neumann to Dirichlet
% boundary conditions
%
BC.left.a(:) = 0; BC.left.b(:) = 1; BC.left.c(:) = 1; % Left boundary to Dirichlet
BC.right.a(:) = 0; BC.right.b(:) = 1; BC.left.c(:) = 1; % right boundary to Dirichlet
%%
% The next sep is to define the diffusivity coefficient. In this FVTool,
% the physical properties of the domain are defined for each cell, with the
% function createCellVariable
D = createCellVariable(m, 1e-5); % assign a constant value of 1e-5 to diffusivity value on each cell
%%
% However, the transfer coefficients must be known on the face of each cell.
% For this reason, we have a few averaging schemes implemented in the
% Utilities folder. For a 1D domain, we can use a harmonic mean scheme:
D_face = harmonicMean(D); % average diffusivity value on the cell faces
%%
% Now, we can convert the PDE to a algebraic system of linear equations,
% i.e.,
%%
%
% $$\nabla.\left(-D\nabla c\right) \approx Mc = 0$$
%
%%
% where M is the matrix of coefficient that is going to be calculated using
% this toolbox. The matrix of coefficient, _M_ has two parts. The diffusion
% equation and the boundary conditions. They are calculated by:
M_diff = diffusionTerm(D_face); % matrix of coefficients for diffusion term
[M_bc, RHS_bc] = boundaryCondition(BC); % matrix of coefficient and RHS vector for the boundary condition
%%
% A vector of right hand side values are always obtained during the
% discretization of the boundary conditions.
% Now that the PDE is discretized, we can solve it by a Matlab linear solver.
c = solvePDE(m, M_diff+M_bc, RHS_bc);
%%
% finally, the resut can be visualized:
visualizeCells(c);
%%
% Just to get excited a little bit, only change the mesh definition command
% from createMesh1D(Nx,L) to createMesh2D(Nx,Nx,L,L), run the code and see
% what happens. For even more excitement, change it to
% createMesh3D(Nx,Nx,Nx,L,L,L).
% This is usually the way we develop new mathematical models for a physical
% phenomenon. Write the equation, solve it in 1D, compare it to the
% analytical solution, then solve it numerically in 2D and 3D for more
% realistic cases with heterogeneous transfer coefficients and other
% nonidealities (and perhaps compare it to some experimental data)
%% Solve a convection-diffuison equation and compare it to analytical solution
% Here, I'm going to add a convection term to what we solved in the
% previous example. This tutorial is adopted from the fipy
% convection-diffusion example you can find at this address:
%%
% <http://www.ctcms.nist.gov/fipy/examples/convection/index.html>
%%
% The differential equation reads
%%
%
% $$\nabla.\left(\mathbf{u} \phi -D\nabla \phi \right)=0$$
%
%%
% Here, $\mathbf{u}$ is a velocity vector (face variable) and $D$ is the
% diffusion coefficient (again a face variable). Please see the PDF
% document for an explanation of cell and face variables. We use Dirichlet
% (constant value) boundary conditions on the left and right boundaries.
% It is zero at the left boundary and one at the right boundary. The
% analytical solution of this differential equation reads
%%
%
% $$c = \frac{1-exp(ux/D)}{1-exp(uL/D)}$$
%
%%
% We start the code as always with some cleaning up:
clc; clear;
%%
% Then we define the domain and mesh size:
L = 1;  % domain length
Nx = 25; % number of cells
meshstruct = createMesh1D(Nx, L);
x = meshstruct.cellcenters.x; % extract the cell center positions
%%
% The next step is to define the boundary condition:
BC = createBC(meshstruct); % all Neumann boundary condition structure
BC.left.a = 0; BC.left.b=1; % switch the left boundary to Dirichlet
BC.left.c=0; % value = 0 at the left boundary
BC.right.a = 0; BC.right.b=1; % switch the right boundary to Dirichlet
BC.right.c=1; % value = 1 at the right boundary
%%
% Now we define the transfer coefficients:
D_val = 1.0; % diffusion coefficient value
D = createCellVariable(meshstruct, D_val); % assign dif. coef. to all the cells
Dave = harmonicMean(D); % convert a cell variable to face variable
u = -10; % velocity value
u_face = createFaceVariable(meshstruct, u); % assign velocity value to cell faces
%%
% Now we discretize the differential equation into a system of linear
% algebraic equations:
%%
%
% $$(M_{conv}-M_{diff}+M_{bc})\phi={RHS}_{bc}$$
%
%%
% or if we use an upwind discretization scheme, we will obtain:
%%
%
% $$(M_{conv,uw}-M_{diff}+M_{bc})\phi={RHS}_{bc}$$
%
%%
%
Mconv =  convectionTerm(u_face); % convection term, central, second order
Mconvupwind = convectionUpwindTerm(u_face); % convection term, upwind, first order
Mdiff = diffusionTerm(Dave); % diffusion term
[Mbc, RHSbc] = boundaryCondition(BC); % boundary condition discretization
M = Mconv-Mdiff+Mbc; % matrix of coefficient for central scheme
Mupwind = Mconvupwind-Mdiff+Mbc; % matrix of coefficient for upwind scheme
RHS = RHSbc; % right hand side vector
c = solvePDE(meshstruct, M, RHS); % solve for the central scheme
c_upwind = solvePDE(meshstruct, Mupwind, RHS); % solve for the upwind scheme
c_analytical = (1-exp(u*x/D_val))/(1-exp(u*L/D_val)); % analytical solution
figure(5);
plot(x, c.value(2:Nx+1), x, c_upwind.value(2:Nx+1), '--',...
    x, c_analytical, '.');
legend('central', 'upwind', 'analytical');
%%
% As you see here, we obtain a more accurate result by using a central
% difference discretization scheme for the convection term compared to the
% first order upwind.
%% solve a transient diffusion equation
% This tutorial is adapted from the fipy 1D diffusion example
%%
% <http://www.ctcms.nist.gov/fipy/examples/diffusion/index.html FiPy diffusion tutorial>
% The transient diffusion equation reads
%%
%
% $$\alpha\frac{\partial c}{\partial t}+\nabla.\left(-D\nabla c\right)=0,$$
%
% where $c$ is the independent variable (concentration, temperature, etc)
% , $D$ is the diffusion coefficient, and $\alpha$ is a constant.
%%
% Once again, clean up:
clc; clear;
%%
% Define the domain and create a mesh structure
L = 50;  % domain length
Nx = 20; % number of cells
m = createMesh1D(Nx, L);
x = m.cellcenters.x; % cell centers position
%%
% Create the boundary condition structure:
BC = createBC(m); % all Neumann boundary condition structure
%%
% Switch the left and right boundaries to Dirichlet:
BC.left.a = 0; BC.left.b=1; BC.left.c=1; % left boundary
BC.right.a = 0; BC.right.b=1; BC.right.c=0; % right boundary
%%
% Define the transfer coefficients:
D_val = 1;
D = createCellVariable(m, D_val);
Dave = harmonicMean(D); % convert it to face variables
% Define alfa, the coefficient of the transient term:
alfa_val = 1;
alfa = createCellVariable(m, alfa_val);
%%
% Define the initial values:
c_init = 0;
c_old = createCellVariable(m, c_init, BC); % initial values
c = c_old; % assign the old value of the cells to the current values
%%
% Now define the time step and the final time:
dt = 0.1; % time step
final_t = 100;
%%
% Here, we first define the matrices of coefficients that will not change
% as we progress in time, viz. diffusion term and boundary condition:
Mdiff = diffusionTerm(Dave);
[Mbc, RHSbc] = boundaryCondition(BC);
%%
% The transitionTerm function gives a matrix of coefficient and a RHS
% vector. The matrix of coefficient does not change in each time step, but
% the RHS does (see the PDF documents). Therefore, we need to call the
% function inside the time loop.
% Start the loop here:
for t=dt:dt:final_t
    [M_trans, RHS_trans] = transientTerm(c_old, dt, alfa);
    M = M_trans-Mdiff+Mbc;
    RHS = RHS_trans+RHSbc;
    c = solvePDE(m,M, RHS);
    c_analytical = 1-erf(x/(2*sqrt(D_val*t)));
    c_old = c;
end
%%
% Now visualize the final results
figure(6)
plot(x, c.value(2:Nx+1), 'o', x, c_analytical);
xlabel('Length [m]'); ylabel('c');
legend('Numerical', 'Analytical');
%%
% you can visualize the results from each time step by moving the plot line
% inside the for loop.
%% convection equations; different discretization schemes
% If I want to highlight one special feature of this FVTool, I will point
% a finger on its various discretization schemes for a linear convection
% term, which includes central difference (second order), upwind (first
% order), and TVD scheme with various flux limiters.
%%
% Here, we are going to compare the performance of each scheme for solving
% two PDE's.
% First, a simple linear transient convection equation with an strange initial
% condition and later, we solve the well-known Burger's equation.
clc; clear;
% define a 1D domain and mesh
W = 1;
Nx = 500;
mesh1 = createMesh1D(Nx, W);
x = mesh1.cellcenters.x;
% define the boundaries
BC = createBC(mesh1); % all Neumann
BC.left.periodic=1;
BC.right.periodic =1;
% Initial values
phi_old = createCellVariable(mesh1, 0.0, BC);
phi_old.value(20:120) = 1;
phi_old.value(180:400)= sin(x(180:400)*10*pi());
% initial guess for phi
phi = phi_old;
phiuw_old=phi_old;
% initial values for upwind scheme
phiuw = phi;
% keep the initial values for visualization
phiinit=phi_old;
% velocity field
u = 0.3;
uf = createFaceVariable(mesh1, u);
% diffusion field
D = 1e-2;
Df = createFaceVariable(mesh1, D);
% transient term coefficient
alfa = createCellVariable(mesh1,1.0);
% upwind convection term
Mconvuw = convectionUpwindTerm1D(uf);
% define the BC term
[Mbc, RHSbc] = boundaryCondition(BC);
% choose a flux limiter
FL = fluxLimiter('Superbee');
% solver
dt = 0.001; % time step
final_t = W/u;
t = 0;
while t<final_t
    t = t+dt;
    % inner loop for TVD scheme
    for j = 1:5
        [Mt, RHSt] = transientTerm(phi_old, dt, alfa);
        [Mconv, RHSconv] = convectionTvdTerm1D(uf, phi, FL);
        M = Mconv+Mt+Mbc;
        RHS = RHSt+RHSbc+RHSconv;
        phi = solvePDE(mesh1, M, RHS);
    end
    [Mtuw, RHStuw] = transientTerm(phiuw_old, dt, alfa);
    Muw = Mconvuw+Mtuw+Mbc;
    RHSuw = RHStuw+RHSbc;
    phiuw = solvePDE(mesh1, Muw, RHSuw);
    phiuw_old = phiuw;
    phi_old = phi;
end
figure(7);plot(x, phiinit.value(2:Nx+1), x, phi.value(2:Nx+1), '-o', x, ...
        phiuw.value(2:Nx+1));

% %% method of lines: using Matlab's ODE solvers for adaptive time stepping
%
%
%
% %% solving a nonlinear PDE
%
% %% solving a system of linear PDE's: sequential and coupled methods
%
% %% solving a system of nonlinear PDE's: sequential and coupled olutions
%
% %% Real life cases: water-flooding in the production of oil
%
% %% and finally your examples?
%
%
%


%System model for geodetic navigation frame mechanization for both phi and psi parametrization.

%This script can be used for both quaternion and DCM attitude parametrizations.

%Position errors are represented as delta_r^n (position error with respect to
%earth defined in navigation frame) in meters rather than
%Delta-Llh (llh perturbation model see:sys_llh_phipsi) or Delta-Theta (Cen perturbation: to be added later)

%Delta_r^n, delta_llh and Delta_theta are assumed to be convertible to each
%other. That is why this system model can be used for any of these position
%mechanizations.

%There is no difference between the psi form of this script and the "sys_wander_psi". This is because
%theta_z does not appear anywhere in the error propagation model for the
%psi model. (Ccn*dv^c is assumed to be equal to dv^c in position error model
%and theta*g is independent of theta_z). Therefore, you can use
%sys_wander_psi instead of this script. (Also note that sys_wander_psi is a
%nonsingular implementation, whereas this script is singular at the pole as geoparam is called with arg 2)

%Returns F and Q of discrete time model. The IMU error model must be
%specified here. (Note that in the previous implementations I had seperated
%nav and imu models in different m-files. I no longer follow that approach)

%mode==1 --> PHI model
%mode==2 --> PSI model

%imutype is directly transferred to imu_err_defs_v000

function [STM Qd]=sys_metric_phipsi(Cen, h, Vn, att, acc, gyro, dt, mode, imutype, modelNo)

%%%Navigation error model
%1-3:Position error in meters
%4-6:vel errors
%7-9:Attitude errors in psi form

%N[da:dw]':Effect of IMU errors on nav states
if(nargin<10)
    modelNo=3; % random walk bias and scale factor
end
Anav=zeros(9); %pos, vel, attitude
Nnav=zeros(9,6);

[Fc, wen_n, wie_n, g]=geoparam_v001(2, Cen(:,3), h, Vn);

if (size(att,1)==1 || size(att,2)==1) %att is qbn
    Cbn=quat2dcm_v000(att);
else %att is dcm
    Cbn=att;
end

%%%Navigation System model
%%Common terms for both phi and psi
%position errors
Anav(1:3,1:3)=-skew(wen_n);
Anav(1:3,4:6)=eye(3);

%Velocity Errors
Anav(6,3)=2*g*Fc(1,1);  %effect of height on gravity (note that, drz is the negative of dh. Therefore, this is positive)
Anav(4:6,4:6)=-skew(wen_n+2*wie_n);
Anav(4:6,7:9)=skew(Cbn*acc);

Nnav(4:6,1:3)=Cbn; %%effect of accelerometer errors

%Attitude errors
Anav(7:9,7:9)=-skew(wen_n+ wie_n);
Nnav(7:9,4:6)=-Cbn; %Effect of gyroscope errors

%PHI model
if (mode==1)
    sL=-Cen(3,3);
    cL=Cen(3,1);    %Note that I assume wander=0 for Cen. (This is not a non-singular implementation)
    %%position errors
    Anav(1:3,1:2)=Anav(1:3,1:2)-skew(Vn)*[0 Fc(1,1);-Fc(2,2) 0; 0 Fc(3,1)];

    %%Velocity errors
    %Effect of 2*d_wie_n
    Anav(4:6,1)=Anav(4:6,1)+2*skew(Vn)*[wie_n(3);0;-wie_n(1)]*Fc(2,2);
    %Effect of d_w_en_n
    Anav(4:6,3:5)=Anav(4:6,3:5)+skew(Vn)*[Vn(2)*Fc(1,1)^2 0 Fc(1,1);-Vn(1)*Fc(2,2)^2 -Fc(2,2) 0;-Vn(2)*Fc(1,1)^2*sL/cL 0 Fc(3,1)];
    Anav(4:6,1)=Anav(4:6,1)+skew(Vn)*[0;0;-(Vn(2)*Fc(1,1)/cL/cL)]*Fc(2,2);

    %%Attitude errors
    %effect of dw_ie_n
    Anav(7:9,1)=Anav(7:9,1)+[wie_n(3);0;-wie_n(1)]*Fc(2,2);

    %effect of dw_en_n
    Anav(7:9,3:5)=Anav(7:9,3:5)+[-Vn(2)*Fc(1,1)^2 0 Fc(1,1);Vn(1)*Fc(2,2)^2 -Fc(2,2) 0;Vn(2)*Fc(1,1)^2*sL/cL 0 Fc(3,1)];
    Anav(9,1)=Anav(9,1)-(Vn(2)*Fc(1,1)/cL/cL)*Fc(2,2);
end

%PSI model
if (mode==2)
    %Velocity errors
    Anav(4:5,1:2)=Anav(4:5,1:2)+g*[-Fc(2,2) 0;0 -Fc(1,1)];    %effect of horizontal position error on gravity
end

%%%%Imu error model parameters
[Aimu_d, Qimu_d, Cimu, Rimu]=imu_err_model_v001(acc, gyro, dt, imutype, modelNo);

%%Convert continuous time Anav into discrete time
%I used van loan's method here to perform conversion so that I can obtain a
%correct Qnav_d. If you use a 1st order taylor series approximation, do not
%forget to correct the cross-correlation effects caused by arw/vrw. (In,
%the final implementation, it would be better to manually compute each
%element rather than using this method)
mx_a=dt*[-Anav,Nnav*Rimu*Nnav';zeros(9),Anav'];
mx_b = expm(mx_a);
Anav_d = mx_b(10:18,10:18)';
Qnav_d = Anav_d*mx_b(1:9,10:18);

%%Combine everything
nst_imu=size(Aimu_d);
STM=zeros(9+nst_imu);
Qd=zeros(9+nst_imu);

STM(1:9,1:9)=Anav_d;
STM(1:9,10:end)=Nnav*Cimu*dt;
STM(10:end,10:end)=Aimu_d;

Qd(1:9,1:9)=Qnav_d;
Qd(10:end,10:end)=Qimu_d;
Qd(1:9,10:end)=Nnav*Cimu*Qimu_d*dt/2;   %not necessary, but let's keep this
Qd(10:end,1:9)=Qd(1:9,10:end)';
end


function mean = normal_01_mean ( )

%*****************************************************************************80
%
%% NORMAL_01_MEAN returns the mean of the Normal 01 PDF.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 September 2004
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Output, real MEAN, the mean of the PDF.
%
  mean = 0.0;

  return
end


function [vertices, faces] = triangulatePolygonPair(poly1, poly2, varargin)
%TRIANGULATEPOLYGONPAIR Compute triangulation between a pair of 3D closed curves.
%
%   [V, F] = triangulatePolygonPair(POLY1, POLY2)
%
%   [V, F] = triangulatePolygonPair(..., 'recenter', FLAG)
%   Where FLAG is a boolean, specifies whether the second curve should be
%   translated to have the same centroid as the first curve. This can
%   improve mathcing of vertices. Default is true.
%
%
%   Example
%     % triangulate a surface patch between two ellipses
%     % create two sample curves
%     poly1 = ellipseToPolygon([50 50 40 20 0], 36);
%     poly2 = ellipseToPolygon([50 50 40 20 60], 36);
%     poly1 = poly1(1:end-1,:);
%     poly2 = poly2(1:end-1,:);
%     % transform to 3D polygons / curves
%     curve1 = [poly1 10*ones(size(poly1, 1), 1)];
%     curve2 = [poly2 20*ones(size(poly2, 1), 1)];
%     % draw as 3D curves
%     figure(1); clf; hold on;
%     drawPolygon3d(curve1, 'b'); drawPoint3d(curve1, 'bo');
%     drawPolygon3d(curve2, 'g'); drawPoint3d(curve2, 'go');
%     view(3); axis equal;
%     [vertices, faces] = triangulatePolygonPair(curve1, curve2);
%     % display the resulting mesh
%     figure(2); clf; hold on;
%     drawMesh(vertices, faces);
%     drawPolygon3d(curve1, 'color', 'b', 'linewidth', 2);
%     drawPolygon3d(curve2, 'color', 'g', 'linewidth', 2);
%     view(3); axis equal;
%
%   See also
%     meshes3D, triangulatePolygonPair3d, triangulateCurvePair,
%     meshSurfaceArea
%

% ------
% Author: David Legland
% E-mail: david.legland@inra.fr
% Created: 2017-05-18, using Matlab 9.1.0.441655 (R2016b)
% Copyright 2017-2022 INRA - Cepia Software Platform

%% Settings

recenterFlag = true;
while length(varargin) > 1
    pname = varargin{1};
    if strcmpi(pname, 'recenter')
        recenterFlag = varargin{2};
    else
        error('Unknown parameter name: %s', pname);
    end
    varargin(1:2) = [];
end

%% Memory allocation

% concatenate vertex coordinates for creating mesh
vertices = [poly1 ; poly2];

% number of vertices on each polygon
n1 = size(poly1, 1);
n2 = size(poly2, 1);

% allocate the array of facets (each edge of each polygon provides a facet)
nFaces = n1 + n2;
faces = zeros(nFaces, 3);

% Translate the second polygon such that the centroids of the bounding
% boxes coincide. This is expected to improve the matching of the two
% curves.
if recenterFlag
    box1 = boundingBox3d(poly1);
    box2 = boundingBox3d(poly2);
    center1 = (box1(2:2:end) + box1(1:2:end-1)) / 2;
    center2 = (box2(2:2:end) + box2(1:2:end-1)) / 2;
    vecTrans = center1 - center2;
    trans = createTranslation3d(vecTrans);
    poly2 = transformPoint3d(poly2, trans);
end

%% Init iteration

% find the pair of points with smallest distance.
% This will be the current diagonal.
[dists, inds] = minDistancePoints(poly1, poly2);
[dummy, ind1] = min(dists); %#ok<ASGLU>
ind2 = inds(ind1);

% consider two consecutive vertices on each polygon
currentIndex1 = ind1;
currentIndex2 = ind2;

%% Main iteration
% For each diagonal, consider the two possible facets (one for each 'next'
% vertex on each polygon), each create current facet according to the
% closest one.
% Then update current diagonal for next iteration.

for iFace = 1:nFaces
    nextIndex1 = mod(currentIndex1, n1) + 1;
    nextIndex2 = mod(currentIndex2, n2) + 1;

    % compute lengths of diagonals
    dist1 = distancePoints(poly1(currentIndex1, :), poly2(nextIndex2,:));
    dist2 = distancePoints(poly1(nextIndex1, :), poly2(currentIndex2,:));

    if dist1 < dist2
        % keep current vertex of curve1, use next vertex on curve2
        face = [currentIndex1 currentIndex2+n1 nextIndex2+n1];
        currentIndex2 = nextIndex2;
    else
        % keep current vertex of curve2, use next vertex on curve1
        face = [currentIndex1 currentIndex2+n1 nextIndex1];
        currentIndex1 = nextIndex1;
    end

    % create the facet
    faces(iFace, :) = face;
end


function [ x, c, m ] = dvandprg ( n, alpha, b, x, c, m )

%*****************************************************************************80
%
%% DVANDPRG solves a Vandermonde system A' * x = f progressively.
%
%  Discussion:
%
%    This function receives the solution to the system of equations A' * x = f
%    where A is a Vandermonde matrix for alpha(0) through alpha(n-1),
%    and new values alpha(n) and f(n).  It updates the solution.
%
%    To solve a system of Nbig equations, this function may be called repeatedly,
%    with N = 1, 2, ..., Nbig.  Each time, a solution to the current subsystem
%    is returned.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    17 April 2014
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Ake Bjorck, Victor Pereyra,
%    Solution of Vandermonde Systems of Equations,
%    Mathematics of Computation,
%    Volume 24, Number 112, October 1970, pages 893-903.
%
%  Parameters:
%
%    Input, integer N, the new order of the matrix, which is 1 larger
%    than on the previous call.  For the first call, N must be 1.
%
%    Input, real ALPHA(N), the parameters that define the matrix.
%    The values should be distinct.  The value ALPHA(N) has just been
%    added to the system.
%
%    Input, real B(N), the right hand side of the linear system.
%
%    Input, real X(N-1), the previous solution of the linear system.
%
%    Input, real C(N-1), real M(N-1), factorization data from the previous call.
%
%    Output, real X(N), the updated solution to the linear system.
%
%    Output, real C(N), M(N), updated factorization data.
%
  c(n) = b(n);
  for j = n - 1 : -1 : 1
    c(j) = ( c(j+1) - c(j) ) / ( alpha(n) - alpha(j) );
  end

  if ( n == 1 )
    m(n) = 1.0;
  else
    m(n) = 0.0;
  end
  cn = c(1);
  x(n) = c(1);

  for j = n - 1 : -1 : 1
    m(j+1) = m(j+1) - alpha(n-1) * m(j);
    x(n-j) = x(n-j) + m(j+1) * cn;
  end

  return
end


function [ triangle_num, triangle_node, triangle_neighbor ] = r8tris2 ( ...
  node_num, node_xy )

%*****************************************************************************80
%
%% R8TRIS2 constructs a Delaunay triangulation of 2D vertices.
%
%  Discussion:
%
%    The routine constructs the Delaunay triangulation of a set of 2D vertices
%    using an incremental approach and diagonal edge swaps.  Vertices are
%    first sorted in lexicographically increasing (X,Y) order, and
%    then are inserted one at a time from outside the convex hull.
%
%  Modified:
%
%    07 February 2005
%
%  Author:
%
%    Original FORTRAN77 version by Barry Joe,
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Barry Joe,
%    GEOMPACK - a software package for the generation of meshes
%    using geometric algorithms,
%    Advances in Engineering Software,
%    Volume 13, pages 325-331, 1991.
%
%  Parameters:
%
%    Input, integer NODE_NUM, the number of nodes.
%
%    Input, real NODE_XY(2,NODE_NUM), the coordinates of the nodes.
%
%    Output, integer TRIANGLE_NUM, the number of triangles in the triangulation;
%    TRIANGLE_NUM is equal to 2*NODE_NUM - BOUNDARY_NUM - 2, where BOUNDARY_NUM
%    is the number of boundary vertices.
%
%    Output, integer TRIANGLE_NODE(3,TRIANGLE_NUM), the nodes that make up each triangle.
%    The elements are indices of P.  The vertices of the triangles are
%    in counter clockwise order.
%
%    Output, integer TRIANGLE_NEIGHBOR(3,TRIANGLE_NUM), the triangle neighbor list.
%    Positive elements are indices of TIL; negative elements are used for links
%    of a counter clockwise linked list of boundary edges; LINK = -(3*I + J-1)
%    where I, J = triangle, edge index; TRIANGLE_NEIGHBOR(J,I) refers to
%    the neighbor along edge from vertex J to J+1 (mod 3).
%
  triangle_num = 0;
  triangle_node = [];
  triangle_neighbor = [];

  tol = 100.0 * eps;
%
%  Sort the vertices by increasing (x,y).
%
  indx = r82vec_sort_heap_index_a ( node_num, node_xy );

  node_xy = r82vec_permute ( node_num, node_xy, indx );
%
%  Make sure that the data points are "reasonably" distinct.
%
  m1 = 1;

  for i = 2 : node_num

    m = m1;
    m1 = i;

    k = 0;

    for j = 1 : 2

      cmax = max ( abs ( node_xy(j,m) ), abs ( node_xy(j,m1) ) );

      if ( tol * ( cmax + 1.0 ) < abs ( node_xy(j,m) - node_xy(j,m1) ) )
        k = j;
        break
      end

    end

    if ( k == 0 )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'R8TRIS2 - Fatal error!\n' );
      fprintf ( 1, '  Fails for point number I = %d\n', i );
      fprintf ( 1, '  M = %d\n', m );
      fprintf ( 1, '  M1 = %d\n', m1 );
      fprintf ( 1, '  X,Y(M)  = %f  %f\n', node_xy(1,m), node_xy(2,m) );
      fprintf ( 1, '  X,Y(M1) = %f  %f\n', node_xy(1,m1), node_xy(2,m1) );
      error ( 'R8TRIS2 - Fatal error!' )
      return
    end

  end
%
%  Starting from points M1 and M2, search for a third point M that
%  makes a "healthy" triangle (M1,M2,M)
%
  m1 = 1;
  m2 = 2;
  j = 3;

  while ( 1 )

    if ( node_num < j )
      fprintf ( 1, '\n' );
      fprintf ( 1, 'R8TRIS2 - Fatal error!\n' );
      error ( 'R8TRIS2 - Fatal error!' )
      return
    end

    m = j;

    lr = lrline ( node_xy(1,m), node_xy(2,m), node_xy(1,m1), node_xy(2,m1), ...
      node_xy(1,m2), node_xy(2,m2), 0.0 );

    if ( lr ~= 0 )
      break
    end

    j = j + 1;

  end
%
%  Set up the triangle information for (M1,M2,M), and for any other
%  triangles you created because points were collinear with M1, M2.
%
  triangle_num = j - 2;

  if ( lr == -1 )

    triangle_node(1,1) = m1;
    triangle_node(2,1) = m2;
    triangle_node(3,1) = m;
    triangle_neighbor(3,1) = -3;

    for i = 2 : triangle_num

      m1 = m2;
      m2 = i+1;
      triangle_node(1,i) = m1;
      triangle_node(2,i) = m2;
      triangle_node(3,i) = m;
      triangle_neighbor(1,i-1) = -3 * i;
      triangle_neighbor(2,i-1) = i;
      triangle_neighbor(3,i) = i - 1;

    end

    triangle_neighbor(1,triangle_num) = -3 * triangle_num - 1;
    triangle_neighbor(2,triangle_num) = -5;
    ledg = 2;
    ltri = triangle_num;

  else

    triangle_node(1,1) = m2;
    triangle_node(2,1) = m1;
    triangle_node(3,1) = m;
    triangle_neighbor(1,1) = -4;

    for i = 2 : triangle_num
      m1 = m2;
      m2 = i+1;
      triangle_node(1,i) = m2;
      triangle_node(2,i) = m1;
      triangle_node(3,i) = m;
      triangle_neighbor(3,i-1) = i;
      triangle_neighbor(1,i) = -3 * i - 3;
      triangle_neighbor(2,i) = i - 1;
    end

    triangle_neighbor(3,triangle_num) = -3 * triangle_num;
    triangle_neighbor(2,1) = -3 * triangle_num - 2;
    ledg = 2;
    ltri = 1;

  end
%
%  Insert the vertices one at a time from outside the convex hull,
%  determine visible boundary edges, and apply diagonal edge swaps until
%  Delaunay triangulation of vertices (so far) is obtained.
%
  top = 0;

  for i = j+1 : node_num

    m = i;
    m1 = triangle_node(ledg,ltri);

    if ( ledg <= 2 )
      m2 = triangle_node(ledg+1,ltri);
    else
      m2 = triangle_node(1,ltri);
    end

    lr = lrline ( node_xy(1,m), node_xy(2,m), node_xy(1,m1), node_xy(2,m1), ...
      node_xy(1,m2), node_xy(2,m2), 0.0 );

    if ( 0 < lr )
      rtri = ltri;
      redg = ledg;
      ltri = 0;
    else
      l = -triangle_neighbor(ledg,ltri);
      rtri = floor ( l / 3 );
      redg = mod(l,3) + 1;
    end

    [ ltri, ledg, rtri, redg ] = vbedg ( node_xy(1,m), node_xy(2,m), node_num, node_xy, ...
      triangle_num, triangle_node, triangle_neighbor, ltri, ledg, rtri, redg );

    n = triangle_num + 1;
    l = -triangle_neighbor(ledg,ltri);

    while ( 1 )

      t = floor ( l / 3 );
      e = mod ( l, 3 ) + 1;
      l = -triangle_neighbor(e,t);
      m2 = triangle_node(e,t);

      if ( e <= 2 )
        m1 = triangle_node(e+1,t);
      else
        m1 = triangle_node(1,t);
      end

      triangle_num = triangle_num + 1;
      triangle_neighbor(e,t) = triangle_num;
      triangle_node(1,triangle_num) = m1;
      triangle_node(2,triangle_num) = m2;
      triangle_node(3,triangle_num) = m;
      triangle_neighbor(1,triangle_num) = t;
      triangle_neighbor(2,triangle_num) = triangle_num - 1;
      triangle_neighbor(3,triangle_num) = triangle_num + 1;
      top = top + 1;

      if ( node_num < top )
        fprintf ( 1, '\n' );
        fprintf ( 1, 'R8TRIS2 - Fatal error!\n' );
        fprintf ( 1, '  Stack overflow.\n' );
        error ( 'R8TRIS2 - Fatal error!' )
      end

      work(top) = triangle_num;

      if ( t == rtri & e == redg )
        break
      end

    end

    triangle_neighbor(ledg,ltri) = -3 * n - 1;
    triangle_neighbor(2,n) = -3 * triangle_num - 2;
    triangle_neighbor(3,triangle_num) = -l;
    ltri = n;
    ledg = 2;

    [ top, ltri, ledg, triangle_node, triangle_neighbor ] = swapec ( ...
      m, top, ltri, ledg, node_num, node_xy, triangle_num, triangle_node, ...
      triangle_neighbor, work );

  end
%
%  Now account for the sorting that we did.
%
  for i = 1 : 3
    for j = 1 : triangle_num
      triangle_node(i,j) = indx ( triangle_node(i,j) );
    end
  end

  indx = perm_inverse ( node_num, indx );

  node_xy = r82vec_permute ( node_num, node_xy, indx );

  return
end


switch input_sig
    case 1          % 1: synthetic data
        if load_sig
            load matlab
        else
        A_0=randn(n1,data.r)*randn(data.r,n2);  % true low-rank matrix A_0
%         A_max=max(abs(A_0(:)));
        A_max=sqrt(max(n1,n2));
        B_0=((rand(n1,n2)<.5)-.5)*2*A_max;
        B_0(randperm(n1*n2,n1*n2-data.s))=0;    % true sparse matrix B_0
%         X_0=A_0+B_0; Z=X_0;
        Z=A_0+B_0;
%         Z=randn(size(Z));
        end

    case 2          % 2: airport video
        addpath('./video_airport')
        nFrame=200;
        iFrame=1000;
        pathname=['airport',num2str(iFrame),'.bmp'];
        Z1=imread(pathname);
        Z1=double(uint8(round(sum(Z1,3)/3)))/255;
        Z=zeros(size(Z1,1),size(Z1,2),nFrame);
        Z(:,:,1)=Z1;
        for j=1:nFrame-1
            pathname=['airport',num2str(iFrame+j),'.bmp'];
            Z1=imread(pathname);
            Z1=double(uint8(round(sum(Z1,3)/3)))/255;
            Z(:,:,j+1)=Z1;
        end

        [z1,z2,z3]=size(Z);
        n1=z1*z2; n2=z3;
        Z=reshape(Z,n1,n2);

        clear nFrame iFrame pathname Z1

    case 3          % 3: lobby video
        addpath('./video_lobby')
        nFrame=400;
        iFrame=1900;
        pathname=['SwitchLight',num2str(iFrame),'.bmp'];
        Z1=imread(pathname);
        Z1=double(uint8(round(sum(Z1,3)/3)))/255;
        Z=zeros(size(Z1,1),size(Z1,2),nFrame);
        Z(:,:,1)=Z1;
        for j=1:nFrame-1
            pathname=['SwitchLight',num2str(iFrame+j),'.bmp'];
            Z1=imread(pathname);
            Z1=double(uint8(round(sum(Z1,3)/3)))/255;
            Z(:,:,j+1)=Z1;
        end

        [z1,z2,z3]=size(Z);
        n1=z1*z2; n2=z3;
        Z=reshape(Z,n1,n2);

        clear nFrame iFrame pathname Z1
end

% add noise
% noise=.0;
% noise=.05*std(data);
if ~load_sig && noise~=0
    Z=Z+noise*randn(size(Z));                       % Gaussian noise
end


classdef CEC2010_F14 < PROBLEM
% <single> <real> <constrained>
% CEC'2010 constrained optimization benchmark problem

%------------------------------- Reference --------------------------------
% R. Mallipeddi and P. N. Suganthan, Problem definitions and evaluation
% criteria for the CEC 2010 competition on constrained real-parameter
% optimization, Nanyang Technological University, Singapore, 2010.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    properties
        O;  % Optimal decision vector
    end
    methods
        %% Default settings of the problem
        function Setting(obj)
            CallStack = dbstack('-completenames');
            load(fullfile(fileparts(CallStack(1).file),'CEC2010.mat'),'Data');
            obj.O = Data{14};
            obj.M = 1;
            if isempty(obj.D); obj.D = 10; end
            obj.D = min(obj.D,length(obj.O));
            obj.lower    = zeros(1,obj.D) - 1000;
            obj.upper    = zeros(1,obj.D) + 1000;
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,PopDec)
            Z = 1 + PopDec - repmat(obj.O(1:size(PopDec,2)),size(PopDec,1),1);
            PopObj = sum(100*(Z(:,1:end-1).^2-Z(:,2:end)).^2+(Z(:,1:end-1)-1).^2,2);
        end
        %% Calculate constraint violations
        function PopCon = CalCon(obj,PopDec)
            Z = PopDec - repmat(obj.O(1:size(PopDec,2)),size(PopDec,1),1);
            PopCon(:,1) = sum((-Z.*cos(sqrt(abs(Z)))),2) - size(Z,2);
            PopCon(:,2) = sum((Z.*cos(sqrt(abs(Z)))),2) - size(Z,2);
            PopCon(:,3) = sum((Z.*sin(sqrt(abs(Z)))),2) - 10*size(Z,2);
        end
    end
end


function [K, sK] = heatXrbfhKernCompute(heatKern, rbfhKern, x1, x2)

% HEATXRBFHKERNCOMPUTE Cross kernel between a HEAT and a RBF kernels.
% FORMAT
% DESC computes cross kernel terms between a HEAT kernel and a RBF kernel
% for the multiple output kernel.
% ARG heatKern : the kernel structure associated with the HEAT kernel.
% ARG rbfKern : the kernel structure associated with the RBFH kernel.
% ARG x1 : inputs for which kernel is to be computed. First column represent
% the time points, while the second column represents the spatial points.
% Entries with Inf indicate missing values.
% RETURN K : block of values from kernel matrix.
% RETURN sK : unscaled kernel matrix
%
% FORMAT
% DESC computes cross kernel terms between a HEAT kernel and a RBF kernel
% for the multiple output kernel.
% ARG heatKern : the kernel structure associated with the HEAT kernel.
% ARG rbfKern : the kernel structure associated with the RBFH kernel.
% ARG x1 : row inputs for which kernel is to be computed. First column
% corresponds to time points and the second column corresponds to spatial
% points. Entries with Inf indicate missing values.
% ARG x2 : column inputs for which kernel is to be computed. First column
% corresponds to time points and the second column corresponds to spatial
% points. Entries with Inf indicate missing values.
% RETURN k : block of values from kernel matrix.
% RETURN sK : unscaled kernel matrix
%
% SEEALSO : multiKernParamInit, multiKernCompute, heatKernParamInit
%
% COPYRIGHT : Mauricio A. Alvarez, 2010

% KERN

if nargin < 4
    x2 = x1;
end
if size(x1, 2) ~= 2 || size(x2, 2) ~= 2
    error('Input can only have two columns');
end
if (heatKern.inverseWidthTime ~= rbfhKern.inverseWidthTime) || ...
        (heatKern.inverseWidthSpace ~= rbfhKern.inverseWidthSpace)
    error('Kernels cannot be cross combined if they have different inverse widths.')
end

% Split the domain into time domain and spatial domain and account for
% missing values. If there are no missing values the computation of the
% kernel is a pointwise prodruct, otherwise it is a kronecker product.
t1 = x1(x1(:,1)~=Inf,1);
t2 = x2(x2(:,1)~=Inf,1);
s1 = x1(x1(:,2)~=Inf,2);
s2 = x2(x2(:,2)~=Inf,2);
if (length(t1) == length(s1)) && (length(t2) == length(s2))
    ut1 = unique(t1);
    ut2 = unique(t2);
    us1 = unique(s1);
    us2 = unique(s2);
    if (length(ut1)*length(us1) == length(t1)) && ...
            (length(ut2)*length(us2) == length(t2))
        t1 = ut1; s1 = us1; t2 = ut2; s2 = us2;
        isPointwise = false;
        K = zeros(length(t1)*length(s1), length(t2)*length(s2));
    else
        isPointwise = true;
        K = zeros(length(t1), length(t2));
    end
else
    isPointwise = false;
    K = zeros(length(t1)*length(s1), length(t2)*length(s2));
end

% Although this is done in heatKernExpandParam.m, we do it here again as a
% precaution.

heatKern.sim.inverseWidth = heatKern.inverseWidthTime;
rbfhKern.rbf.inverseWidth = rbfhKern.inverseWidthTime;

sigmax = sqrt(2/heatKern.inverseWidthSpace);
lengthX = heatKern.lengthX;
nterms = heatKern.nTerms;
decay = heatKern.decay;
diff = heatKern.diffusion;

% Precompute some terms
w = ((1:nterms)*(pi/lengthX))';
gamma = sqrt(-1)*w;
beta = decay + diff*(w.^2);
cK = 2/lengthX;

if heatKern.includeIC
   error('Not implemented yet')
else
    if isPointwise
        for i=1:nterms
            heatKern.sim.decay = beta(i);
            Kt = simXrbfKernCompute(heatKern.sim, rbfhKern.rbf, t1, t2);
            Ks = srbfhKernCompute(sigmax, lengthX, s1, s2, w, gamma, i);
            K = K + Kt.*Ks;
        end
    else
        for i=1:nterms
            heatKern.sim.decay = beta(i);
            Kt = simXrbfKernCompute(heatKern.sim, rbfhKern.rbf, t1, t2);
            Ks = srbfhKernCompute(sigmax, lengthX, s1, s2, w, gamma, i);
            K = K + kron(Kt,Ks);
        end
    end
    sK = cK*K;
    K = heatKern.sensitivity*sK;
end


function [U,S,V] = spm_svd(X,U)
% Computationally efficient SVD (that can handle sparse arguments)
% FORMAT [U,S,V] = spm_svd(X,u)
% X    - (m x n) matrix
% u    - threshold (1 > u > 0) for normalized eigenvalues (default = 1e-6)
%      - a value of zero induces u = 64*eps
%
% U    - {m x p} singular vectors
% V    - {m x p} singular variates
% S    - {p x p} singular values
%__________________________________________________________________________
% Copyright (C) 1994-2011 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_svd.m 6110 2014-07-21 09:36:13Z karl $

% default thresholds - preclude singular vectors with small singular values
%--------------------------------------------------------------------------
if nargin < 2, U = 1e-6; end
if U >= 1; U = U - 1e-6; end
if U <= 0; U = 64*eps;   end

% deal with sparse matrices
%--------------------------------------------------------------------------
[M,N] = size(X);
p     = find(any(X,2));
q     = find(any(X,1));
X     = X(p,q);

% SVD
%--------------------------------------------------------------------------
[i, j, s] = find(X);
[m, n]    = size(X);
if any(i - j)

    % off-leading diagonal elements - full SVD
    %----------------------------------------------------------------------
    X     = full(X);
    if m > n

        [v, S, v] = svd(X'*X,0);
        S         = sparse(S);
        s         = diag(S);
        j         = find(s*length(s)/sum(s) > U);
        v         = v(:,j);
        u         = spm_en(X*v);
        S         = sqrt(S(j,j));

    elseif m < n

        [u, S, u] = svd(X*X',0);
        S         = sparse(S);
        s         = diag(S);
        j         = find(s*length(s)/sum(s) > U);
        u         = u(:,j);
        v         = spm_en(X'*u);
        S         = sqrt(S(j,j));

    else

        [u, S, v] = svd(X,0);
        S         = sparse(S);
        s         = diag(S).^2;
        j         = find(s*length(s)/sum(s) > U);
        v         = v(:,j);
        u         = u(:,j);
        S         = S(j,j);
    end

else
    S             = sparse(1:n,1:n,s,m,n);
    u             = speye(m,n);
    v             = speye(m,n);
    [i, j]        = sort(-s);
    S             = S(j,j);
    v             = v(:,j);
    u             = u(:,j);
    s             = diag(S).^2;
    j             = find(s*length(s)/sum(s) > U);
    v             = v(:,j);
    u             = u(:,j);
    S             = S(j,j);

end

% replace in full matrices
%--------------------------------------------------------------------------
j      = length(j);
U      = sparse(M,j);
V      = sparse(N,j);
if j
    U(p,:) = u;
    V(q,:) = v;
end


y       = load('data/gas.dat')';
time    = 1960:1/4:1986+3/4;
fprintf(1, '\n');

%% Analysis based on Gaussian model %%
stsm        = ssm_stsm('trend', 'dummy', 4);
[stsm logL] = estimate(y, stsm, 0.01, [], 'fmin', 'bfgs', 'disp', 'off');
fprintf(1, '[Gaussian STSM]\n');
fprintf(1, 'loglikelihood:      %g\n', logL);
fprintf(1, 'Irregular variance: %g\n', stsm.param(1));
fprintf(1, 'Trend variance:     %g\n', stsm.param(2));
fprintf(1, 'Level variance:     %g\n', stsm.param(3));
fprintf(1, 'Seasonal variance:  %g\n\n', stsm.param(4));

[alpha irr] = fastsmo(y, stsm);
ycom        = signal(alpha, stsm);
seas        = ycom(2, :);
figure('Name', 'Gaussian model analysis');
subplot(2, 1, 1), plot(time, seas), title('Seasonal'), xlim([1959 1988]), ylim([-0.9 0.8]);
subplot(2, 1, 2), plot(time, irr), title('Gaussian Irregular'), xlim([1959 1988]), ylim([-0.4 0.4]);
if ispc, set(gcf, 'WindowStyle', 'docked'); end
drawnow;

%% Analysis based on t-model %%
stsmt           = [ssm_t ssm_llt ssm_seasonal('dummy', 4)];
randn('state', [3765023265; 2369472656]);
[stsmt logLt]   = estimate(y, stsmt, [stsm.param(1) 4 stsm.param(2:end)], alpha, 'fmin', 'bfgs', 'disp', 'off');
[alphat irrt]   = fastsmo(y, stsmt);
ycomt           = signal(alphat, stsmt);
lvlt            = ycomt(1, :);
seast           = ycomt(2, :);

fprintf(1, '[t-dist error STSM]\n');
fprintf(1, 'loglikelihood:      %g\n', logLt);
fprintf(1, 't-dist variance:    %g\n', stsmt.param(1));
fprintf(1, 't-dist df:          %g\n', stsmt.param(2));
fprintf(1, 'Trend variance:     %g\n', stsmt.param(3));
fprintf(1, 'Level variance:     %g\n', stsmt.param(4));
fprintf(1, 'Seasonal variance:  %g\n', stsmt.param(5));

figure('Name', 'Component comparison between Gaussian and t-dist. models');
subplot(2, 1, 1), plot(time, seas, 'r', 'DisplayName', 'Gaussian'), hold all, plot(time, seast, 'b', 'DisplayName', 't-distribution'), hold off, title('Seasonal component'), xlim([1959 1988]), ylim([-0.9 0.8]);
subplot(2, 1, 2), plot(time, irr, 'r', 'DisplayName', 'Gaussian'), hold all, plot(time, irrt, 'b', 'DisplayName', 't-distribution'), hold off, title('Irregular component'), xlim([1959 1988]), ylim([-0.4 0.4]);
if ispc, set(gcf, 'WindowStyle', 'docked'); end

fprintf(1, '\n');


% Simulating 2*2 MIMO-LDPC Base-Band Systems
% Copyright (C2010-2015) Yang XIAO, Beijing Jiaotong University, Aug.10, 2010, E-Mail: yxiao@bjtu.edu.cn.
% This program can simulate 2*2 MIMO-LDPC base-band systems.
% The MIMO system's design can refer my following book.
% [1] Yang Xiao, MIMO Multiple Antenna Wireless Communication Systems, Press of Posts and Telecommunications, Beijing, 2009.
% Different from the MIMO scheme of IEEE 802.16e, our MIMO-LDPC system has no space-time coding,
% while it achieved good BER performance.
%---------------------------
clear;
p=87;   %p is the prime number of rank of sub-matrices of parity check matrix;
k2=3;   % k is the row weight of parity check matrix;
j2=6;   % j is the column weight of parity check matrix;
M=k2*p; % M is the number of row of parity check matrix;
N=j2*p;  % N is the number of column of parity check matrix;

NT1=1;
E0=eye(p);
EZ=zeros(p);
a=3; b=5;
R=0.5; % coding rate
frame_num =200;
Npf=2*N*frame_num
dlta=1/Npf;

EZ=zeros(p);
E0=eye(p);
% The design of parity check matrix of QC LDPC without girth_4, see my book.
E11=E0;                   E12=circshift(E0,a*NT1);   E13= circshift(E0,a^2*NT1);   E14= circshift(E0,a^3*NT1);   E15= circshift(E0,a^4*NT1);    E16= circshift(E0,a^5*NT1);
E21=EZ;                   E22=E0;                    E23=circshift(E0,a^2*b*NT1);  E24=circshift(E0,a^3*b*NT1);  E25= circshift(E0,a^4*b*NT1);  E26= circshift(E0,a^5*b*NT1);
E31=circshift(E0,b^2*NT1);E32=EZ;                    E33=E0;                       E34=circshift(E0,b^2*a^3*NT1);E35= circshift(E0,b^2*a^4*NT1);E36= circshift(E0,b^2*a^5*NT1);

h1=[E11 E12 E13 E14 E15 E16;
    E21 E22 E23 E24 E25 E26;
    E31 E32 E33 E34 E35 E36];
 A(:,1:M)=h1(:,1:M);
 B(:,1:M)=h1(:,M+1:N);
%

d=mod(inv_GF2(A)*B,2);
E00=eye(M);
% The generator matrix of QC LDPC, see my book.
G=[d' E00];
H=sparse(h1);

N1=0;
N2=16;
% Flat fading MIMO Channel
h11=0.9;
h12=0.3;
h21=0.4;
h22=0.73;

SNRindB1=N1:1:N2;
for i=1:length(SNRindB1)
error_count1= 0;
error_count2= 0;
    for f_n = 1:frame_num
    SNR=(10^(SNRindB1(i)/10));
  	sigma = 1/sqrt(2*R*SNR);
    x1 = (sign(randn(1,size(G,1)))+1)/2; % random information bits
    x2 = (sign(randn(1,size(G,1)))+1)/2; % random information bits
 y1 = mod(x1*G,2);                   % LDPC Encoding for signal_1 from atenna 1 of the transmitter
 y2 = mod(x2*G,2);                   % LDPC Encoding for signal_2 from atenna 2 of the transmitter
    z1=2*y1-1;                       % BPSK modulation
    z2=2*y2-1;                         % BPSK modulation
    z11=h11*z1+h12*z2+sigma*randn(1,size(G,2));    % AWGN transmission
    z22=h21*z1+h22*z2+sigma*randn(1,size(G,2));
HC=[h11 h12;h21 h22];   % Channel Matrix
HC1=HC^(-1);
u=HC1*[z11;z22];        % ZF spatial decoding
u1=u(1,:);              % recieved signal_1 from atenna 1 of the receiver
u2=u(2,:);              % recieved signal_2 from atenna 1 of the receiver
    f11=1./(1+exp(-2*u1/sigma^2));        % likelihoods
    f01=1-f11;
    [z1_hat, success, k] = ldpc_decode(f01,f11,H);   %  LDPC decoding for signal_1
    x1_hat = z1_hat(size(G,2)+1-size(G,1):size(G,2));
    x1_hat1 = x1_hat';
    f12=1./(1+exp(-2*u2/sigma^2));        % likelihoods
    f02=1-f12;
    [z2_hat, success, k] = ldpc_decode(f02,f12,H);   %  LDPC decoding for signal_2
    x2_hat = z2_hat(size(G,2)+1-size(G,1):size(G,2));
    x2_hat1 = x2_hat';
    error_count2= sum(xor(x1,x1_hat1))+sum(xor(x2,x2_hat1));    % bit error count
    error_count1 = error_count1 + error_count2;
    end
    BER(i)= error_count1/Npf
    if BER(i)<.1*dlta
    BER(i)=.08*dlta;
    end
end
figure(1)
semilogy(SNRindB1,BER);
xlabel('SNR(dB)');
ylabel('BER');
title('2*2 MIMO-LDPC Simulation');
axis([N1 N2 dlta 1]);
%diary off


function [A, B] = gsp_filterbank_bounds(G,W,param)
%GSP_FILTERBANK_BOUNDS Compute approximated frame bounds for a filterbank
%   Usage: [A, B] = gsp_filterbank_bounds(G,W);
%          [A, B] = gsp_filterbank_bounds(G,W,param);
%          [A, B] = gsp_filterbank_bounds([xmin, xmax],W);
%          [A, B] = gsp_filterbank_bounds([xmin, xmax],W,param);
%
%   Input parameters
%       G   : Graph structure or interval to compute the bound
%       W   : Filterbank (cell array of inline function)
%       param: optional parameter
%   Output parameters
%       A   : Filterbank lower bound
%       B   : Filterbank Upper bound
%
%   *param* is a Matlab structure containing the following fields:
%
%   * *param.N* : Number of points for the line search default (default 999)
%   * *param.use_eigenvalues* : Use eigenvalues if possible (default 1). To
%     be used, the eigenvalues have to be computed first using
%     |gsp_compute_fourier_basis|.
%

% Author: Nathanael Perraudin
% Date : 26 March 2014

if nargin < 3
    param = struct;
end

if ~isfield(param,'N'), param.N = 999; end
if ~isfield(param,'use_eigenvalues'),  param.use_eigenvalues = 1; end

if iscell(G)
    NG = numel(G);
    A = cell(NG,1);
    B = cell(NG,1);
    for ii = 1:NG
        [A{ii}, B{ii}] = gsp_filterbank_bounds(G{ii},W{ii},param);
    end
    return
end

if isstruct(G)
    if ~isfield(G,'lmax')
        G = gsp_estimate_lmax(G);
    warning(['GSP_FILTERBANK_BOUNDS: To be more efficient you should run: ',...
        'G = gsp_estimate_lmax(G); before using this proximal operator.']);
    end
    xmax = G.lmax;
    xmin = 0;
else
    xmin = G(1);
    xmax = G(2);
end

if param.use_eigenvalues && isstruct(G) && isfield(G,'e')
    lambda = G.e;
else
    lambda = linspace(xmin,xmax , param.N);
end

Nf = numel(W);

sum_filters = sum(abs(gsp_filter_evaluate(W,lambda).^2),2);
% for ii=1:Nf
%     sum_filters = sum_filters + (W{ii}(lambda)).^2;
% end

A = min(sum_filters);
B = max(sum_filters);

end


%INTLAB interval hessian toolbox
%
%Hessian constructors
%  hessianinit  - Initialization of dependent variables
%  horzcat      - Horizontal concatenation          [ , ]
%  vertcat      - Vertical concatenation            [ ; ]
%  subsasgn     - Subscripted assignment A(i,:) = 1
%  subsref      - Subscripted reference r = A(3,4)
%  hessian      - Double to gradient
%
%Display of hessians and interval hessians (rigorous)
%  display      - Command window display of hessian
%  disp         - Display function for pop-up windows in debugger
%  realimag     - Real and Imaginary part separately
%  infsup       - Display infimum and supremum
%  midrad       - Display midpoint and radius
%  disp_        - Display in "_" notation
%
%Hessian arithmetic operations
%  plus         - Plus                              +
%  uplus        - Unary plus                        +
%  minus        - Minus                             -
%  uminus       - Unary minus                       -
%  mtimes       - Matrix multiply                   *
%  times        - Elementwise multiply              .*
%  mrdivide     - Slash or right division           /
%  mldivide     - Backslash or left division        \
%  rdivide      - Elementwise right division        ./
%  ldivide      - Elementwise left division         .\
%  mpower       - Matrix power                      ^
%  power        - Elementwise power                 .^
%  intersect    - Intersection
%
%Other hessian operations
%  abs          - Absolute value
%  inf          - Infimum
%  sup          - Supremum
%  mid          - Midpoint
%  rad          - Radius
%  diam         - Diameter
%  real         - Real part
%  imag         - Imaginary part
%  trace        - Trace
%  sum          - Sum
%  prod         - Product
%  ctranspose   - Complex conjugate transpose       '
%  transpose    - Transpose                         .'
%
%Utility routines
%  isnan        - True for Not a Number
%  isreal       - Hessian is real
%  isintval     - Hessian is intval
%  isfinite     - Interval is finite
%  isinf        - Interval is infinite
%  isempty      - Hessian is empty in Matlab sense, i.e. []
%  emptyintersect - detect empty intersections
%  issparse     - Hessian has sparse structure
%  find         - find indices of nonzero elements
%  all          - Determine if all array elements are nonzero
%  any          - Determine if any array elements are nonzero
%  logical      - Convert hessian values to logical
%  nnz          - number of nonzero elements
%  end          - determine last index
%
%Structural operations
%  full         - Convert to full hessian
%  sparse       - Convert to sparse hessian
%  band         - Extract band
%  diag         - Extract diagonal
%  tril         - Extract lower triangular
%  triu         - Extract upper triangular
%  bandwidth    - Bandwidth
%  length       - Length
%  size         - Size
%  dim          - Dimension of square matrix
%  reshape      - Reshape
%  repmat       - Duplicate arrays
%
%Hessian trigonometric functions (rigorous, real and complex)
%  sin          - Sine
%  cos          - Cosine
%  tan          - Tangent
%  cot          - Cotangent
%  sec          - Secant
%  csc          - Cosecant
%  asin         - Inverse sine
%  acos         - Inverse cosine
%  atan         - Inverse tangent
%  acot         - Inverse cotangent
%  asec         - Inverse secant
%  acsc         - Inverse cosecant
%  sinh         - Hyperbolic sine
%  cosh         - Hyperbolic cosine
%  tanh         - Hyperbolic tangent
%  coth         - Hyperbolic cotangent
%  asinh        - Inverse hyperbolic sine
%  acosh        - Inverse hyperbolic cosine
%  atanh        - Inverse hyperbolic tangent
%  acoth        - Inverse hyperbolic cotangent
%
%Hessian exponential functions (rigorous, real and complex)
%  exp          - Exponential
%  log          - Natural logarithm
%  log10        - Logarithm to base 10
%  sqr          - Square
%  sqrt         - Square root
%
%Hessian comparison of ".x" part
%  eq           - Equal                             ==
%  ne           - Not equal                         ~=
%  gt           - Greater than                      >
%  ge           - Greater than or equal             >=
%  lt           - Less than                         <
%  le           - Less than or equal                <=
%
%Verification routines and auxiliary
%  typeadj      - Type adjustment
%  typeof       - Type for type adjustment
%
%Initialization of INTLAB hessian package and system variables
%  hessianinit  - Initialization and definition of INTLAB switches, also
%                   initialization of dependent variables
%
%Demonstration, samples
%  demohessian - Some examples for using INTLAB hessian package
%
%
%The hessian package uses forward mode of automatic differentiation. For
%an introduction to forward differentation, see
%  L.B. Rall: Automatic Differentiation: Techniques and Applications,
%    Lecture Notes in Computer Science 120, Springer, 1981.
%

% written  04/04/04     S.M. Rump  INTLAB Version 5
%
% Copyright (c) Siegfried M. Rump, head of the Institute for Reliable Computing,
%               Hamburg University of Technology


function sp=mtimes(sp1,sp2)
% sympoly/mtimes: Matrix multiplication of sympoly objects or scalars
% usage: sp=sp1*sp2;
%
% arguments:
%  sp,sp1,sp2   - sympoly objects or numeric scalars or a numeric array

% are they of compatible sizes?
s1 = size(sp1);
s2 = size(sp2);
if (length(s1)>2) || (length(s2)>2)
  error 'Matrix multiplication is only defined for vectors and simple arrays.'
end

if (s1(2) == s2(1)) || (numel(sp1)==1) || (numel(sp2)==1)
  % they are compatible

  if (numel(sp1) == 1) && (numel(sp2) == 1)
    % both are scalars. Just use .*
    sp = sp1.*sp2;

  elseif (numel(sp1) == 1) && (numel(sp2) > 1)
    % sp1 is a scalar, but not sp2
    sp = sympoly(sp2);
    for i = 1:numel(sp2)
      sp(i) = sp1.*sp2(i);
    end

  elseif (numel(sp1) > 1) && (numel(sp2) == 1)
    % sp2 is a scalar, but not sp1
    sp = sympoly(sp1);
    for i = 1:numel(sp1)
      sp(i) = sp1(i).*sp2;
    end

  elseif (numel(sp1) > 1) && (numel(sp2) > 1)
    % both are arrays.
    sp = sympoly(zeros(s1(1),s2(2)));

    for i = 1:s1(1)
      for j = 1:s2(2)
        for k = 1:s1(2)
          sp(i,j) = sp(i,j) + sp1(i,k).*sp2(k,j);
        end
      end
    end

  end

else
  error 'sp1 and sp2 are of incompatible sizes for .* operation.'

end


function [E,J]=SynthMeasWatsonSHCylSingleRadTortGPD(x, protocol, fibredir, roots)
% Substrate: Impermeable cylinders with one radius in a homogeneous background.
% Orientation distribution: Watson's distribution with SH approximation
% Pulse sequence: Any
% Signal approximation: Gaussian phase distribution.
% Notes: This version estimates the hindered diffusivity from the free diffusivity
% and packing density using Szafer et al's tortuosity model for randomly
% packed cylinders.
%
% [E,J]=SynthMeasWatsonSHCylSingleRadTortGPD(x, protocol, fibredir, roots)
% returns the measurements E according to the model and the Jacobian J of the
% measurements with respect to the parameters.  The Jacobian does not
% include derivates with respect to the fibre direction.
%
% x is the list of model parameters in SI units:
% x(1) is the volume fraction of the intracellular space.
% x(2) is the free diffusivity of the material inside and outside the cylinders.
% x(3) is the radius of the cylinders.
% x(4) is the concentration parameter of the Watson's distribution.
%
% protocol is the object containing the acquisition protocol.
%
% fibredir is a unit vector along the symmetry axis of the Watson's
% distribution.  It must be in Cartesian coordinates [x y z]' with size [3 1].
%
% roots contains solutions to the Bessel function equation from function
% BesselJ_RootsCyl.
%
% author: Gary Hui Zhang (gary.zhang@ucl.ac.uk)
%

f=x(1);
dPar=x(2);
% This version is for cylinders with regular packing.
%dPerp = dPar/((1 + f^(3/2))^2);
% This one is for randomly packed cylinders
dPerp = dPar*(1-f);
R=[x(3)];
kappa=x(4);

x_full = [f dPar dPerp R kappa];

% Call the model with no isotropic component to get the anisotropic component.
if(nargout == 1)
    E=SynthMeasWatsonSHCylSingleRadGPD(x_full, protocol, fibredir, roots);
else
    [E,J_full]=SynthMeasWatsonSHCylSingleRadGPD(x_full, protocol, fibredir, roots);
    J(:,1) = J_full(:,1) - J_full(:,3)*dPar;
    J(:,2) = J_full(:,2) + J_full(:,3)*(1-f);
    J(:,3) = J_full(:,4);
    J(:,4) = J_full(:,5);
end


function value = dmach ( job )

%*****************************************************************************80
%
%% DMACH computes machine parameters of floating point arithmetic.
%
%  Discussion:
%
%    This routine is for testing only.  It is not required by LINPACK.
%
%    If there is trouble with the automatic computation of these quantities,
%    they can be set by direct assignment statements.
%    We assume the computer has
%
%      B = base of arithmetic
%      T = number of base B digits
%      L = smallest possible exponent
%      u = largest possible exponent
%
%    then
%
%      EPS = B**(1-T)
%      TINY = 100.0D+00 *B**(-L+T)
%      HUGE = 0.01D+00 *B**(U-T)
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    08 June 2005
%
%  Author:
%
%    MATLAB version by John Burkardt.
%
%  Reference:
%
%    Jack Dongarra, Cleve Moler, Jim Bunch and Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, 1979.
%
%    Charles Lawson, Richard Hanson, David Kincaid, Fred Krogh,
%    Basic Linear Algebra Subprograms for Fortran Usage,
%    Algorithm 539,
%    ACM Transactions on Mathematical Software,
%    Volume 5, Number 3, September 1979, pages 308-323.
%
%  Parameters:
%
%    Input, integer JOB:
%    1: requests EPSILON;
%    2: requests TINY;
%    3: requests HUGE.
%
%    Output, real VALUE, the requested value.
%
  s = 1.0;

  while ( 1 )

    tiny = s;
    s = s / 2.0;

    if ( s * 1.0 == 0.0 | s == 0.0 )
      break
    end

  end

  tiny = ( tiny / eps ) * 100.0;
  huge = 1.0 / tiny;

  if ( job == 1 )
    value = eps;
  elseif ( job == 2 )
    value = tiny;
  elseif ( job == 3 )
    value = huge;
  else
    xerbla ( 'DMACH', 1 );
  end

  return
end


function W_down = get_W_down(W)
N = size(W, 2);
W_down = zeros(2, 2 * N^2);
for u2 = 0 : 1
    for y1 = 1 : N
        for y2 = 1 : N
            for u1 = 0 : 1
%                 P = 0.5 * W(mod(u1 + u2, 2) + 1, y1) * W(u2 + 1, y2);
%                 if P == 0
%                     W_down(u2 + 1, 2 * N * (y1 - 1) + 2 * (y2 - 1) + u1 + 1) = realmin;
%                 else
%                     W_down(u2 + 1, 2 * N * (y1 - 1) + 2 * (y2 - 1) + u1 + 1) = P;
%                 end
                W_down(u2 + 1, 2 * N * (y1 - 1) + 2 * (y2 - 1) + u1 + 1) = 0.5 * W(mod(u1 + u2, 2) + 1, y1) * W(u2 + 1, y2);
            end
        end
    end
end
end


function [transprob, termprob] = remove_hhmm_end_state(A)
% REMOVE_END_STATE Infer transition and termination probabilities from automaton with an end state
% [transprob, termprob] = remove_end_state(A)
%
% A(i,k,j) = Pr( i->j | Qps=k), where i in 1:Q, j in 1:(Q+1), and Q+1 is the end state
% This implements the equation in footnote 3 of my NIPS 01 paper,
% transprob(i,k,j) = \tilde{A}_k(i,j)
% termprob(k,j) = \tau_k(j)
%
% For the top level, the k index is missing.

Q = size(A,1);
toplevel = (ndims(A)==2);
if toplevel
  Qk = 1;
  A = reshape(A, [Q 1 Q+1]);
else
  Qk = size(A, 2);
end

transprob = A(:, :, 1:Q);
term = A(:,:,Q+1)'; % term(k,j) = P(Qj -> end | k)
termprob = term;
%termprob = zeros(Qk, Q, 2);
%termprob(:,:,2) = term;
%termprob(:,:,1) = 1-term;

for k=1:Qk
  for i=1:Q
    for j=1:Q
      denom = (1-termprob(k,i));
      denom = denom + (denom==0)*eps;
      transprob(i,k,j) = transprob(i,k,j) / denom;
    end
  end
end

if toplevel
  termprob = squeeze(termprob);
  transprob = squeeze(transprob);
end


function dXdt = dynamics_SRB(t,Xt,Ut,Xd,U_ext,p)

%% parameters
mass = p.mass;
J = p.J;       % inertia tensor in body frame {B}
g = 9.81;

%% decompose
% X = [pc dpc vR wb pf]'
pc = reshape(Xt(1:3),[3,1]);
dpc = reshape(Xt(4:6),[3,1]);
R = reshape(Xt(7:15),[3,3]);
wb = reshape(Xt(16:18),[3,1]);
pf34 = reshape(Xt(19:30),[3,4]);
pfd34 = reshape(Xd(19:30),[3,4]);

% r
r34 = pf34 - repmat(pc,[1,4]);

% GRF
f34 = reshape(Ut,[3,4]);

%% dynamics
ddpc = 1/mass * (sum(f34,2) + U_ext) + [0;0;-g];
dR = R * hatMap(wb);

tau_s = zeros(3,1);       % body torque expressed in {S}
for ii = 1:4
    tau_s = tau_s + hatMap(r34(:,ii)) * f34(:,ii);
end
tau_ext = hatMap(R * p.p_ext) * U_ext;
tau_tot = sum(tau_s,2) + tau_ext;
dwb = J \ (R' * tau_tot - hatMap(wb) * J * wb);

dpf = p.Kp_sw * (pfd34(:) - pf34(:));

dXdt = [dpc;ddpc;dR(:);dwb;dpf];

end


function cut = knapsack_create_cover_cut(a,b,x,alg,gubs)
% Derive cover inequalities cut*[1;x]>=0 for a*x <= b

if (all(a>=0) && sum(a)<=b) || (all(a<=0) && (b>=0))
    % quick exit sum x <= large or sum x >= neg
    cut = [];
    return
end

if nargin < 3
    x = ones(length(a),1);
    alg = 'crowder';
    gubs = [];
elseif nargin < 4
    alg = 'crowder';
    gubs = [];
elseif nargin < 5
    gubs = spalloc(1,length(a),0);
end

if ~all(a)
    nz = find(a);
    a_ = a(nz);
    cut_ = knapsack_create_cover_cut(a_,b,x(nz),alg,gubs(nz));
    if ~isempty(cut_)
        cut = spalloc(1,length(a)+1,0);
        cut(1) = cut_(1);
        cut(1+nz) = cut_(2:end);
    else
        cut = [];
    end
    return
elseif any(a<0)
    % Negative values in a*x <= b
    % models -a*x >= -b i.e. typically something like sum stuff >= bound
    %        -a*(1-y) >= -b
    %        -a*y <= b-sum(a)
    neg = find(a < 0);
    a_ = a;
    a_(neg) = -a(neg);
    x_ = x;
    x_(neg) = 1-x(neg);
    cut = knapsack_create_cover_cut(a_,b-sum(a(neg)),x_,alg,gubs);
    % we know have a cut, cut(1) + cut(2:end)*y >=0
    %                     cut(1) + cut(2:end)(1-x) >= 0
    %                     cut(1) + sum(cut(2:end)) + (-cut(2))*x >= 0
    if ~isempty(cut)
        cut(1) = cut(1)+sum(cut(1 + neg));
        cut(1 + neg) = -cut(1 + neg);
    end
    return
end

switch alg
    case {'crowder',''}
        % Heuristics from H. Crowder, E. Johnson, M. Padberg
        % Solving large-scale 0–1 linear programming programs
        [val,loc] = sort((1-x)./a(:),'ascend');
    case 'gu'
        [val,loc] = sort((x),'descend');
    case 'gu-reverse'
        [val,loc] = sort((x),'ascend');

    otherwise
        error('Unsupport cover cut separation')
end
% Initial cover
C = min(find(cumsum(a(loc))>b));

% Apply Balas lifting
q = knapsack_cover_lift_balas(a,loc(1:C));
% qL = knapsack_cover_lift_letchford(a,b,loc(1:C));
% if q~=qL
%    'HEJ'
% end
% Return row where row*[1;x] hopefully is violated
cut = spalloc(1,length(a)+1,0);
cut(1) = C-1;
cut(2:end)=-q;


function J = NLMF2Dtree(I, Options)
% This function NLMF2Dtree performs Non-Local Means noise filtering of
% 2D grey/color image. This function constructs a KD-tree of the whole
% image to find patches which have the closest intensity difference,
% instead of searching only in the local neighboorhood like in the
% NLMF function.
% Warning, despite the usage of a kd-tree, this function will take more
% then a hour for one image.
%
% Function:
%
%   J = NLMF2Dtree(I, Options);
%
% inputs,
%   I : 2D grey/color or 3D image data, of type Single or Double
%           in range [0..1]
%   Options : Struct with options
%
% outputs,
%   J : The NL-means filtered image or image volume
%
% options,
%   Options.kernelratio : Radius of local Patch (default 3)
%   Options.filterstrength : Strength of the NLMF filtering (default 0.05)
%   Options.number : The number of patches which have the closest distance
%                      to a local patch used for filtering (default 9).
%
% First Compile c-code!!!!, with :
%   mex image2vectors_double.c -v
%
% Note: This function uses the "Matlab Statistics Toolbox"
%

% Process inputs
defaultoptions=struct('kernelratio',3,'filterstrength',0.05,'number',9);
if(~exist('Options','var')), Options=defaultoptions;
else
    tags = fieldnames(defaultoptions);
    for i=1:length(tags), if(~isfield(Options,tags{i})), Options.(tags{i})=defaultoptions.(tags{i}); end, end
    if(length(tags)~=length(fieldnames(Options))),
        warning('NLMF2Dtree:unknownoption','unknown options found');
    end
end

% Detect if this is a color or grey scale image
switch(size(I,3))
    case 1, rgb=false;
    case 3, rgb=true;
    otherwise,error('NLMF2Dtree:inputs','This is not a 2D image');
end

% Pad the image, to allow simple extraction of local patches at the image
% boundary
Ipad = padarray(I,[Options.kernelratio Options.kernelratio],'symmetric');

% Get the local patches
V=image2vectors_double(double(Ipad),Options.kernelratio,2);

% Create a KDtree with all local patches
ns = createns(V','nsmethod','kdtree');

% Find the patches closest by in intensity in relation to the local patch
% itself
[VM,D] = knnsearch(ns,V','k',Options.number+1);

% Calculate the weight of the patches used for a pixel
W=exp(-(D.^2)*Options.filterstrength);
% The first column is the local patch itself, set the weight to maximum
% found in the other patches
W(:,1)=max(W(:,2:end),[],2);
% Make the sum of weights which will be used for one patch, equal to one.
W=W./repmat((sum(W,2)+eps),[1 size(W,2)]);

% Add the weighted intensities, of the center pixels of the in intensity
% closest by patches
if(rgb)
    Ir=I(:,:,1);
    Ig=I(:,:,2);
    Ib=I(:,:,3);
    Jr=sum(Ir(VM).*W,2);
    Jg=sum(Ig(VM).*W,2);
    Jb=sum(Ib(VM).*W,2);
    Jr=reshape(Jr,[size(I,1) size(I,2)]);
    Jg=reshape(Jg,[size(I,1) size(I,2)]);
    Jb=reshape(Jb,[size(I,1) size(I,2)]);
    J(:,:,1)=Jr;
    J(:,:,2)=Jg;
    J(:,:,3)=Jb;
else
    J=sum(I(VM).*W,2);
    J=reshape(J,size(I));
end


function [ x, y, z, w ] = ld1454 ( )

%*****************************************************************************80
%
%% LD1454 computes the 1454 point Lebedev angular grid.
%
%  Modified:
%
%    14 September 2010
%
%  Author:
%
%    Dmitri Laikov
%
%  Reference:
%
%    Vyacheslav Lebedev, Dmitri Laikov,
%    A quadrature formula for the sphere of the 131st
%    algebraic order of accuracy,
%    Russian Academy of Sciences Doklady Mathematics,
%    Volume 59, Number 3, 1999, pages 477-481.
%
%  Parameters:
%
%    Output, real X(N), Y(N), Z(N), W(N), the coordinates
%    and weights of the points.
%
  n = 0;
  x = zeros(1454,1);
  y = zeros(1454,1);
  z = zeros(1454,1);
  w = zeros(1454,1);
  a = 0.0;
  b = 0.0;
  v = 0.7777160743261247E-04;
  [ n, x, y, z, w ] = gen_oh ( 1, n, a, b, v, x, y, z, w );
  v = 0.7557646413004701E-03;
  [ n, x, y, z, w ] = gen_oh ( 3, n, a, b, v, x, y, z, w );
  a = 0.3229290663413854E-01;
  v = 0.2841633806090617E-03;
  [ n, x, y, z, w ] = gen_oh ( 4, n, a, b, v, x, y, z, w );
  a = 0.8036733271462222E-01;
  v = 0.4374419127053555E-03;
  [ n, x, y, z, w ] = gen_oh ( 4, n, a, b, v, x, y, z, w );
  a = 0.1354289960531653;
  v = 0.5417174740872172E-03;
  [ n, x, y, z, w ] = gen_oh ( 4, n, a, b, v, x, y, z, w );
  a = 0.1938963861114426;
  v = 0.6148000891358593E-03;
  [ n, x, y, z, w ] = gen_oh ( 4, n, a, b, v, x, y, z, w );
  a = 0.2537343715011275;
  v = 0.6664394485800705E-03;
  [ n, x, y, z, w ] = gen_oh ( 4, n, a, b, v, x, y, z, w );
  a = 0.3135251434752570;
  v = 0.7025039356923220E-03;
  [ n, x, y, z, w ] = gen_oh ( 4, n, a, b, v, x, y, z, w );
  a = 0.3721558339375338;
  v = 0.7268511789249627E-03;
  [ n, x, y, z, w ] = gen_oh ( 4, n, a, b, v, x, y, z, w );
  a = 0.4286809575195696;
  v = 0.7422637534208629E-03;
  [ n, x, y, z, w ] = gen_oh ( 4, n, a, b, v, x, y, z, w );
  a = 0.4822510128282994;
  v = 0.7509545035841214E-03;
  [ n, x, y, z, w ] = gen_oh ( 4, n, a, b, v, x, y, z, w );
  a = 0.5320679333566263;
  v = 0.7548535057718401E-03;
  [ n, x, y, z, w ] = gen_oh ( 4, n, a, b, v, x, y, z, w );
  a = 0.6172998195394274;
  v = 0.7554088969774001E-03;
  [ n, x, y, z, w ] = gen_oh ( 4, n, a, b, v, x, y, z, w );
  a = 0.6510679849127481;
  v = 0.7553147174442808E-03;
  [ n, x, y, z, w ] = gen_oh ( 4, n, a, b, v, x, y, z, w );
  a = 0.6777315251687360;
  v = 0.7564767653292297E-03;
  [ n, x, y, z, w ] = gen_oh ( 4, n, a, b, v, x, y, z, w );
  a = 0.6963109410648741;
  v = 0.7587991808518730E-03;
  [ n, x, y, z, w ] = gen_oh ( 4, n, a, b, v, x, y, z, w );
  a = 0.7058935009831749;
  v = 0.7608261832033027E-03;
  [ n, x, y, z, w ] = gen_oh ( 4, n, a, b, v, x, y, z, w );
  a = 0.9955546194091857;
  v = 0.4021680447874916E-03;
  [ n, x, y, z, w ] = gen_oh ( 5, n, a, b, v, x, y, z, w );
  a = 0.9734115901794209;
  v = 0.5804871793945964E-03;
  [ n, x, y, z, w ] = gen_oh ( 5, n, a, b, v, x, y, z, w );
  a = 0.9275693732388626;
  v = 0.6792151955945159E-03;
  [ n, x, y, z, w ] = gen_oh ( 5, n, a, b, v, x, y, z, w );
  a = 0.8568022422795103;
  v = 0.7336741211286294E-03;
  [ n, x, y, z, w ] = gen_oh ( 5, n, a, b, v, x, y, z, w );
  a = 0.7623495553719372;
  v = 0.7581866300989608E-03;
  [ n, x, y, z, w ] = gen_oh ( 5, n, a, b, v, x, y, z, w );
  a = 0.5707522908892223;
  b = 0.4387028039889501;
  v = 0.7538257859800743E-03;
  [ n, x, y, z, w ] = gen_oh ( 6, n, a, b, v, x, y, z, w );
  a = 0.5196463388403083;
  b = 0.3858908414762617;
  v = 0.7483517247053123E-03;
  [ n, x, y, z, w ] = gen_oh ( 6, n, a, b, v, x, y, z, w );
  a = 0.4646337531215351;
  b = 0.3301937372343854;
  v = 0.7371763661112059E-03;
  [ n, x, y, z, w ] = gen_oh ( 6, n, a, b, v, x, y, z, w );
  a = 0.4063901697557691;
  b = 0.2725423573563777;
  v = 0.7183448895756934E-03;
  [ n, x, y, z, w ] = gen_oh ( 6, n, a, b, v, x, y, z, w );
  a = 0.3456329466643087;
  b = 0.2139510237495250;
  v = 0.6895815529822191E-03;
  [ n, x, y, z, w ] = gen_oh ( 6, n, a, b, v, x, y, z, w );
  a = 0.2831395121050332;
  b = 0.1555922309786647;
  v = 0.6480105801792886E-03;
  [ n, x, y, z, w ] = gen_oh ( 6, n, a, b, v, x, y, z, w );
  a = 0.2197682022925330;
  b = 0.9892878979686097E-01;
  v = 0.5897558896594636E-03;
  [ n, x, y, z, w ] = gen_oh ( 6, n, a, b, v, x, y, z, w );
  a = 0.1564696098650355;
  b = 0.4598642910675510E-01;
  v = 0.5095708849247346E-03;
  [ n, x, y, z, w ] = gen_oh ( 6, n, a, b, v, x, y, z, w );
  a = 0.6027356673721295;
  b = 0.3376625140173426;
  v = 0.7536906428909755E-03;
  [ n, x, y, z, w ] = gen_oh ( 6, n, a, b, v, x, y, z, w );
  a = 0.5496032320255096;
  b = 0.2822301309727988;
  v = 0.7472505965575118E-03;
  [ n, x, y, z, w ] = gen_oh ( 6, n, a, b, v, x, y, z, w );
  a = 0.4921707755234567;
  b = 0.2248632342592540;
  v = 0.7343017132279698E-03;
  [ n, x, y, z, w ] = gen_oh ( 6, n, a, b, v, x, y, z, w );
  a = 0.4309422998598483;
  b = 0.1666224723456479;
  v = 0.7130871582177445E-03;
  [ n, x, y, z, w ] = gen_oh ( 6, n, a, b, v, x, y, z, w );
  a = 0.3664108182313672;
  b = 0.1086964901822169;
  v = 0.6817022032112776E-03;
  [ n, x, y, z, w ] = gen_oh ( 6, n, a, b, v, x, y, z, w );
  a = 0.2990189057758436;
  b = 0.5251989784120085E-01;
  v = 0.6380941145604121E-03;
  [ n, x, y, z, w ] = gen_oh ( 6, n, a, b, v, x, y, z, w );
  a = 0.6268724013144998;
  b = 0.2297523657550023;
  v = 0.7550381377920310E-03;
  [ n, x, y, z, w ] = gen_oh ( 6, n, a, b, v, x, y, z, w );
  a = 0.5707324144834607;
  b = 0.1723080607093800;
  v = 0.7478646640144802E-03;
  [ n, x, y, z, w ] = gen_oh ( 6, n, a, b, v, x, y, z, w );
  a = 0.5096360901960365;
  b = 0.1140238465390513;
  v = 0.7335918720601220E-03;
  [ n, x, y, z, w ] = gen_oh ( 6, n, a, b, v, x, y, z, w );
  a = 0.4438729938312456;
  b = 0.5611522095882537E-01;
  v = 0.7110120527658118E-03;
  [ n, x, y, z, w ] = gen_oh ( 6, n, a, b, v, x, y, z, w );
  a = 0.6419978471082389;
  b = 0.1164174423140873;
  v = 0.7571363978689501E-03;
  [ n, x, y, z, w ] = gen_oh ( 6, n, a, b, v, x, y, z, w );
  a = 0.5817218061802611;
  b = 0.5797589531445219E-01;
  v = 0.7489908329079234E-03;
  [ n, x, y, z, w ] = gen_oh ( 6, n, a, b, v, x, y, z, w );

  return
end


function [beta,r,J,Sigma,mse,errorparam,robustw] = nlinmultifit(x_cell, y_cell, mdl_cell, beta0, options)
%NLINMULTIFIT Nonlinear least-squares regression of multiple data sets
%
%	A wrapper function for NLINFIT which allows simulatenous fitting of
%	multiple data sets with shared fitting parameters. See example below.
%
%	Unlike different solutions (using fminsearch or similar functions)
%	this approach enables simple estimation of model predictions and
%	their confidence intervals, as well as confidence intervals on the
%	fitted parameters (using the built-in NLPREDCI and NLPARCI functions).
%
%	KNOWN ISSUE:
%		In this implementation, different data sets are weighted according
%		to their relative lengths. Special care should be taken when those
%		lengths are considerably different from one another to avoid
%		biased results towards one data set in particular.
%
%	INPUT:
% 		x_cell,y_cell: Cell arrays containing the x,y vectors of the fitted
%					   data sets.
% 		mdl_cell: Cell array containing model functions for each data set.
% 		beta0: Vector containing initial guess of the fitted parameters.
% 		options: Structure containing control parameters for NLINFIT (see
%				 help file on NLINFIT for more details).
%
%	OUTPUT:
%		beta,r,J,Sigma,mse,errorparam,robustw: Direct output from NLINFIT.
%
%	EXAMPLE:
% 		% Generate X vectors for both data sets
% 		x1 = 0:0.1:10;
% 		x2 = 0:0.2:10;
%
% 		% Generate Y data with some noise
% 		y1 = cos(2*pi*0.5*x1).*exp(-x1/5) + 0.1*randn(size(x1));
% 		y2 = 0.5 + 2*exp(-(x2/5)) + 0.1*randn(size(x2));
%
% 		% Define fitting functions and parameters, with identical
%		% exponential decay for both data sets
% 		mdl1 = @(beta,x) cos(2*pi*beta(1)*x).*exp(-x/beta(2));
% 		mdl2 = @(beta,x) beta(4) + beta(3)*exp(-(x/beta(2)));
%
% 		% Prepare input for NLINMULTIFIT and perform fitting
% 		x_cell = {x1, x2};
% 		y_cell = {y1, y2};
% 		mdl_cell = {mdl1, mdl2};
% 		beta0 = [1, 1, 1, 1];
% 		[beta,r,J,Sigma,mse,errorparam,robustw] = ...
%					nlinmultifit(x_cell, y_cell, mdl_cell, beta0);
%
% 		% Calculate model predictions and confidence intervals
% 		[ypred1,delta1] = nlpredci(mdl1,x1,beta,r,'covar',Sigma);
% 		[ypred2,delta2] = nlpredci(mdl2,x2,beta,r,'covar',Sigma);
%
% 		% Calculate parameter confidence intervals
% 		ci = nlparci(beta,r,'Jacobian',J);
%
% 		% Plot results
% 		figure;
% 		hold all;
% 		box on;
% 		scatter(x1,y1);
% 		scatter(x2,y2);
% 		plot(x1,ypred1,'Color','blue');
% 		plot(x1,ypred1+delta1','Color','blue','LineStyle',':');
% 		plot(x1,ypred1-delta1','Color','blue','LineStyle',':');
% 		plot(x2,ypred2,'Color',[0 0.5 0]);
% 		plot(x2,ypred2+delta2','Color',[0 0.5 0],'LineStyle',':');
% 		plot(x2,ypred2-delta2','Color',[0 0.5 0],'LineStyle',':');
%
%	AUTHOR:
%		Chen Avinadav
%		mygiga (at) gmail
%

	num_curves = length(x_cell);
	if length(y_cell) ~= num_curves || length(mdl_cell) ~= num_curves
		error('Invalid input to NLINMULTIFIT');
	end

	x_vec = [];
	y_vec = [];
	mdl_vec = '@(beta,x) [';
	mdl_ind1 = 1;
	mdl_ind2 = 0;
	for ii = 1:num_curves
		if length(x_cell{ii}) ~= length(y_cell{ii})
			error('Invalid input to NLINMULTIFIT');
		end
		if size(x_cell{ii},2) == 1
			x_cell{ii} = x_cell{ii}';
		end
		if size(y_cell{ii},2) == 1
			y_cell{ii} = y_cell{ii}';
		end
		x_vec = [x_vec, x_cell{ii}];
		y_vec = [y_vec, y_cell{ii}];
		mdl_ind2 = mdl_ind2 + length(x_cell{ii});
		mdl_vec = [mdl_vec, sprintf('mdl_cell{%d}(beta,x(%d:%d)), ', ii, mdl_ind1, mdl_ind2)];
		mdl_ind1 = mdl_ind1 + length(x_cell{ii});
	end
	mdl_vec = [mdl_vec(1:end-2), '];'];
	mdl_vec = eval(mdl_vec);

	if nargin == 4
		[beta,r,J,Sigma,mse,errorparam,robustw] = nlinfit(x_vec, y_vec, mdl_vec, beta0);
	else
		[beta,r,J,Sigma,mse,errorparam,robustw] = nlinfit(x_vec, y_vec, mdl_vec, beta0, options);
	end
end


function x = lprec1(c, d, h, g, extmod)
% LPREC1   One-level Laplacian pyramid reconstruction
%
%	x = lprec1(c, d, h, g)
%
% Input:
%   c:      coarse signal at half size
%   d:      detail signal at full size
%   h, g:   two biorthogonal 1-D lowpass filters
%   extmod: [optional] extension mode (default is 'per')
%
% Output:
%   x:      reconstructed signal
%
% Note:     This uses a new reconstruction method by Do and Vetterli,
%           "Framming pyramids", IEEE Trans. on Sig Proc., Sep. 2003.
%
% See also:	LPDEC1

if ~exist('extmod', 'var')
    extmod = 'per';
end

nd = ndims(c);

% First, filter and downsample the detail image
r = d;
for dim = 1:nd
    r = filtdn(r, h, dim, extmod, 0);
end

% Then subtract the result from the coarse signal
p = c - r;

% Even size filter needs to be adjusted to obtain perfect reconstruction
adjust = mod(length(g) + 1, 2);

% Then upsample and filter
for dim = 1:nd
    p = upfilt(p, g, dim, extmod, adjust);
end

% Final combination
x = p + d;


x = linspace(-20,2,100);
w = exp(-0.5*x.^2 -0.5*log(2*pi));
f = exp(-normcdfln(x));
g = 1./normcdf(x);
%g = exp((log(1+exp(0.88+x))./1.5).^2);
%g = exp(0.5*log(2*pi) +0.5*x.^2 + log(x));
plot(x, f.*w, x, g.*w)
legend('exp(-normcdfln)','1/normcdf');

if 0
  true = -27.3843074988;
  [abs(log(normcdf(-7))-true) abs(normcdfln(-7)-true)]
end

if 0
  %matnet
  %imports('c:/Documents and Settings/minka/Depots/Infer/Core/bin/Debug/Core.dll')
  h = g;
  for i = 1:length(x)
    h(i) = 1./cl.MMath.NormalCdf(x(i));
  end
  %h = 0.5*(-x + sqrt(x.*x + 8/pi));
  plot(x, f.*w, x, g.*w, x, h.*w)
  legend('exp(-normcdfln)','1/normcdf', '1/normcdf2');

  plot(x, f.*w - h.*w)
end

% evalf(subs(t=1e-4,subs(x=2*(1-t)/t,erfc(x)*exp(x*x))),100);

if 0
% test approximations
a = exp(sqrt(2/pi));
b = 1/log(2/pi*a);
g = log(a-1 + exp(x.*exp(1./(x.^2 + b))));
g = log(a-1 + exp(x));
plot(x, f, x, g)
plot(x, log(exp(f)+1-a)./x)
plot(x, 1./log(log(exp(f)+1-a)./x))
end

% read `/u/tpminka/src/maple/gauss_cdf`;
% f := 1/sqrt(2*Pi)*exp(-1/2*x^2)/gauss_cdf(-x);
% g := exp(sqrt(2/Pi))-1+exp(x);
% plot(f, x=0..50);
% h := log(exp(f*sqrt(Pi/2)-1)+1);
% h := log(exp(f - sqrt(2/Pi))+1);
% h := 1/log(log(exp(f) +1-exp(sqrt(2/Pi)))/x);
% asympt(h,x);


function [beta_gibbs,sigma_gibbs,beta_mean,sigma_mean,lambda_posterior]=panel4gibbs(N,n,h,T,k,q,Yi,Xi,s0,omegab,v0,It,Bu,pick,pickf)

% compute first  preliminary elements
% compute sbar
sbar=h+s0;
% compute the inverse of omegab
invomegab=diag(1./diag(omegab));
% initiate the Gibbs sampler
% initiate the counting of iterations
count=1;
pickcount=1;
% initiate the record matrices
beta_gibbs=zeros(q,It-Bu,N);
sigma_gibbs=zeros(n^2,It-Bu,N);
beta_mean=zeros(q,It-Bu);
sigma_mean=zeros(n^2,It-Bu);
lambda_posterior=zeros((It-Bu),1);

% step 1: compute initial values
% initial value for beta (use OLS values)
for ii=1:N
beta(:,ii)=bear.vec((Xi(:,:,ii)'*Xi(:,:,ii))\(Xi(:,:,ii)'*Yi(:,:,ii)));
end
% initial value for b
b=(1/N)*sum(beta,2);
% initial value for lambda1
lambda1=0.01;
sigmab=lambda1*omegab;
% initial value for sigma (use OLS values)
for ii=1:N
eps=Yi(:,:,ii)-Xi(:,:,ii)*reshape(beta(:,ii),k,n);
sigma(:,:,ii)=(1/(T-k-1))*eps'*eps;
end

hbar = bear.parfor_progressbar(It-Bu,'Progress of Panel BVAR Gibbs Sampler');  %create the progress bar

% run the Gibbs sampler
while count<=It

% step 2: obtain b
% first compute betam, the mean value of the betas over all units
betam=(1/N)*sum(beta,2);
% draw b from a multivariate normal N(betam,(1/N)*sigmab))
b=betam+chol(bear.nspd((1/N)*sigmab),'lower')*mvnrnd(zeros(q,1),eye(q))';

% step 3: obtain sigmab
% compute first vbar
for ii=1:N
temp(1,ii)=(beta(:,ii)-b)'*invomegab*(beta(:,ii)-b);
end
vbar=v0+sum(temp,2);
% compute lambda1
lambda1=bear.igrandn(sbar/2,vbar/2);
% recover sigmab
sigmab=lambda1*omegab;

% step 4: draw the series of betas
% first obtain the inverse of sigmab
invsigmab=diag(1./diag(sigmab));
% then loop over units
for ii=1:N
% take the choleski factor of sigma of unit ii, inverse it, and obtain from it the inverse of the original sigma
C=bear.trns(chol(bear.nspd(sigma(:,:,ii)),'Lower'));
invC=C\speye(n);
invsigma=invC*invC';
% obtain omegabar
invomegabar=kron(invsigma,Xi(:,:,ii)'*Xi(:,:,ii))+invsigmab;
% invert
C=bear.trns(chol(bear.nspd(invomegabar),'Lower'));
invC=C\speye(q);
omegabar=invC*invC';
% obtain betabar
betabar=omegabar*(kron(invsigma,Xi(:,:,ii)')*bear.vec(Yi(:,:,ii))+invsigmab*b);
% draw beta
beta(:,ii)=betabar+chol(bear.nspd(omegabar),'lower')*mvnrnd(zeros(q,1),eye(q))';
end

% step 5: draw the series of sigmas
% loop over units
for ii=1:N
% compute Stilde
Stilde=(Yi(:,:,ii)-Xi(:,:,ii)*reshape(beta(:,ii),k,n))'*(Yi(:,:,ii)-Xi(:,:,ii)*reshape(beta(:,ii),k,n));
% draw sigma
sigma(:,:,ii)=bear.iwdraw(Stilde,T);
end

   % record phase
   % if the burn-in sample phase is not yet over
   if count<=Bu
   % simply add 1 to the iteration count
   count=count+1;
   % on the other hand, if the burn-in sample phase is over
   elseif count>Bu
   % adding one iteration to the count will depend on wether post-burn selection applies
      % if there is no post burn selection
      if pick==0
      % record the draw
         % loop over units
         for ii=1:N
         beta_gibbs(:,count-Bu,ii)=beta(:,ii);
         sigma_gibbs(:,count-Bu,ii)=bear.vec(sigma(:,:,ii));
         end
      % and add one to the count
      count=count+1;
      % if there is post burn selection, only one draw over 'fpick' draws will be retained
      elseif pick==1
         % if the iteration does not correspond to fpick, don't record the results, don't increase the regular count, but do increase pickcount by 1
         if pickcount~=pickf
         pickcount=pickcount+1;
         % on the other hand, if the iteration does correspond to fpick
         elseif pickcount==pickf
         % do record the results
            % loop over units
            beta_mean(:,count-Bu)=b;
            sigma_mean(:,count-Bu)=bear.vec(mean(sigma,3));
            lambda_posterior(count-Bu)=lambda1;
            for ii=1:N
            beta_gibbs(:,count-Bu,ii)=beta(:,ii);
            sigma_gibbs(:,count-Bu,ii)=bear.vec(sigma(:,:,ii));
            end
         % then increase the regular count by 1 and re-initialise pickcount
         count=count+1;
         pickcount=1;
         end
      end
   end

   hbar.iterate(1);   % update progress by one iteration

end

close(hbar);   %close progress bar


function K = covADD(cov, hyp, x, z, i)

% Additive covariance function using a 1d base covariance function
% cov(x^p,x^q;hyp) with individual hyperparameters hyp.
%
% k(x^p,x^q) = \sum_{r \in R} sf_r \sum_{|I|=r}
%                 \prod_{i \in I} cov(x^p_i,x^q_i;hyp_i)
%
% hyp = [ hyp_1
%         hyp_2
%          ...
%         hyp_D
%         log(sf_R(1))
%          ...
%         log(sf_R(end)) ]
%
% where hyp_d are the parameters of the 1d covariance function which are shared
% over the different values of R(1) to R(end).
%
% Please see the paper Additive Gaussian Processes by Duvenaud, Nickisch and
% Rasmussen, NIPS, 2011 for details.
%
% Copyright (c) by Carl Edward Rasmussen and Hannes Nickisch, 2010-09-10.
%
% See also COVFUNCTIONS.M.

R = cov{1};
nh = eval(feval(cov{2}));           % number of hypers per individual covariance
nr = numel(R);                      % number of different degrees of interaction
if nargin<3                                  % report number of hyper parameters
  K = ['D*', int2str(nh), '+', int2str(nr)];
  return
end
if nargin<4, z = []; end                                   % make sure, z exists
xeqz = isempty(z); dg = strcmp(z,'diag');                       % determine mode

[n,D] = size(x);                                                % dimensionality
sf2 = exp( 2*hyp(D*nh+(1:nr)) );        % signal variances of individual degrees

Kd = Kdim(cov{2},hyp,x,z);                % evaluate dimensionwise covariances K
if nargin<5                                                        % covariances
  EE = elsympol(Kd,max(R));               % Rth elementary symmetric polynomials
  K = 0; for ii=1:nr, K = K + sf2(ii)*EE(:,:,R(ii)+1); end    % sf2 weighted sum
else                                                               % derivatives
  if i <= D*nh                       % individual covariance function parameters
    j = fix(1+(i-1)/nh);              % j is the dimension of the hyperparameter
    if dg, zj='diag'; else if xeqz, zj=[]; else zj=z(:,j); end, end
    dKj = feval(cov{2},hyp(nh*(j-1)+(1:nh)),x(:,j),zj,i-(j-1)*nh);  % other dK=0
    % the final derivative is a sum of multilinear terms, so if only one term
    % depends on the hyperparameter under consideration, we can factorise it
    % out and compute the sum with one degree less
    E = elsympol(Kd(:,:,[1:j-1,j+1:D]),max(R)-1);  %  R-1th elementary sym polyn
    K = 0; for ii=1:nr, K = K + sf2(ii)*E(:,:,R(ii)); end     % sf2 weighted sum
    K = dKj.*K;
  elseif i <= D*nh+nr
    EE = elsympol(Kd,max(R));             % Rth elementary symmetric polynomials
    j = i-D*nh;
    K = 2*sf2(j)*EE(:,:,R(j)+1);                  % rest of the sf2 weighted sum
  else
    error('Unknown hyperparameter')
  end
end

% evaluate dimensionwise covariances K
function K = Kdim(cov,hyp,x,z)
  [n,D] = size(x);                                              % dimensionality
  nh = eval(feval(cov));            % number of hypers per individual covariance
  if nargin<4, z = []; end                                 % make sure, z exists
  xeqz = numel(z)==0; dg = strcmp(z,'diag') && numel(z)>0;      % determine mode

  if dg                                                        % allocate memory
    K = zeros(n,1,D);
  else
    if xeqz, K = zeros(n,n,D); else K = zeros(n,size(z,1),D); end
  end

  for d=1:D
    hyp_d = hyp(nh*(d-1)+(1:nh));                 % hyperparamter of dimension d
    if dg
      K(:,:,d) = feval(cov,hyp_d,x(:,d),'diag');
    else
      if xeqz
        K(:,:,d) = feval(cov,hyp_d,x(:,d));
      else
        K(:,:,d) = feval(cov,hyp_d,x(:,d),z(:,d));
      end
    end
  end


function pp=lpcra2pp(ra)
%LPCAR2PP LPC: Convert ar filter autocorrelation to power spectrum polynomial in cos(w) PP=(RA)
% pp is a polynomial such that |polyval(ra,e^jw)| = polyval(pp,cos(w))

%      Copyright (C) Mike Brookes 1998
%      Version: $Id: lpcra2pp.m 713 2011-10-16 14:45:43Z dmb $
%
%   VOICEBOX is a MATLAB toolbox for speech processing.
%   Home page: http://www.ee.ic.ac.uk/hp/staff/dmb/voicebox/voicebox.html
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   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 can obtain a copy of the GNU General Public License from
%   http://www.gnu.org/copyleft/gpl.html or by writing to
%   Free Software Foundation, Inc.,675 Mass Ave, Cambridge, MA 02139, USA.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

persistent tp;
[nf,p1]=size(ra);
% we check here if p is the same as before and reuse the tp matrix
if size(tp,1)~=p1
   p=p1-1;
   % chebyshev polynomials up to order p
   tp=zeros(p1,p1);
   tp(1,p1)=2;
   tp(2,p)=2;
   for i=3:p1
      tp(i,p+2-i:p)=2*tp(i-1,p+3-i:p1)-tp(i-2,p+2-i:p);
      tp(i,p1)=-tp(i-2,p1);
   end
   tp(1,p1)=1;
end
pp=ra*tp;


function dimacs = computedimacs(b,c,A,xin,y,s,K);
% COMPUTEDIMACS
%
% min <C,X> s.t     AX = b, X > 0
% max b'y   s.t S-C+A'y =0, S > 0

% If no primal exist, fake till later
if isempty(xin)
    x = c*0;
else
    x = xin;
end

if isempty(s)
    s = c-A'*y;
end

xres = inf;
sres = inf;

% Not officially defined in DIMACS
if K.f>0
    sres = -min(norm(s(1:K.f),inf));
end

% Errors in linear cone
if K.l>0
    xres = min(x(1+K.f:K.f+K.l));
    sres = min(s(1+K.f:K.f+K.l));
end

% Errors in quadratic cone
if K.q(1)>0
    top = K.f+K.l;
    for i = 1:length(K.q)
        X = x(1+top:top+K.q(i));
        S = s(1+top:top+K.q(i));
        xres = min(xres,X(1)-norm(X(2:end)));
        sres = min(sres,S(1)-norm(S(2:end)));
        top = top + K.q(i);
    end
end

% Errors in semidefinite cone
if K.s(1)>0
    top = K.f+K.l+K.q+K.r;
    for i = 1:length(K.s)
        X = reshape(x(1+top:top+K.s(i)^2),K.s(i),K.s(i));
        S = reshape(s(1+top:top+K.s(i)^2),K.s(i),K.s(i));
        xres = min(xres,min(eig(full(X))));
        sres = min(sres,min(eig(full(S))));
        top = top + K.s(i)^2;
    end
end

err1 = norm(b-A*x)/(1 + norm(b,inf));
err2 = max(0,-xres)/(1 + norm(b,inf));
err3 = conenorm(s-(c-A'*y),K)/(1+norm(c,inf));
%err3 = norm(s-(c-A'*y))/(1+norm(c,inf)); % Used by some solvers
err4 = max(0,-sres)/(1+max(abs(c)));
err5 = (c'*x-b'*y)/(1+abs(c'*x)+abs(b'*y));
err6 = x'*(c-A'*y)/(1+abs(c'*x)+abs(b'*y));

% No primal was computed
if isempty(xin)
    err1 = nan;
    err2 = nan;
    err5 = nan;
    err6 = nan;
end
dimacs = [err1 err2 err3 err4 err5 err6];

function t = conenorm(s,K)

% Implementation of the norm described on
% http://plato.asu.edu/dimacs/node3.html

t = 0;

if K.f + K.l>0
    t = t + norm(s(1:K.f+K.l));
end

top = 1+K.f+K.l;
if K.q(1)>0
    for i = 1:length(K.q)
        t = t + norm(s(top:top+K.q(i)-1));
        top  = top + K.q(i);
    end
end

if K.s(1)>0
    for i = 1:length(K.s)
        S = reshape(s(top:top+K.s(i)^2-1),K.s(i),K.s(i));
        t = t + norm(S,'fro');
        top  = top + K.s(i)^2;
    end
end


function bc = specific_bc(xbd,ybd)
%solution3_bc   Reference problem 1.3  boundary condition
%   bc = specific_bc(xbd,ybd);
%   input
%          xbd          x boundary coordinate vector
%          ybd          y boundary coordinate vector
%   IFISS function: DJS; 28 February 2005.
% Copyright (c) 2005 D.J. Silvester, H.C. Elman, A. Ramage
bc=2*(1+ybd)./((ybd+1).*(ybd+1) + (xbd+3).*(xbd+3));
return


function [nmps2] = mGal2nmps2(mGal)
% Convert acceleration from milligalileos to nanometers per second squared
% Chad A. Greene 2012
nmps2 = mGal*1e+4;
end