% Check the configuration section to make the necessary adjustments to
% run this script.

% @
% Copyright (C) 2017 Jonas Ruesch
%
% 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 3 of the License, or
% (at your option) any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with this program.  If not, see <http://www.gnu.org/licenses/>.
% @

% Configuration
% ===================================================================================
tornadoDirectory = 'F:\svn\dev\matlab\tornado\T135_export'; % where is Tornado installed?
nameTornadoAircraftGeometry = 'ExperimentalCarrier'; % which Tornado model to use?
estimatedVelocity = 12; % 12; % the velocity at which aerodynamic coefficients are computed.
velocityDeviationTol = 0.5; % how much the found trimmed velocity can differ from the estimated velocity to accept the solution.
airDensity = 1.225;
elevatorFlapIndex = 1; % which is the elevator flap in the Tornado model?
rudderFlapIndex = 2; % which is the rudder flap in the Tornado model?
centerOfGravity = 0.092; % in meters from zero of Tornado aircraft model reference frame. Tornado x-axis extends aft (!) -> positive.
mass = 1.56; % aircraft mass in kg (overwrites the value set in the Tornado model).
InertiaTensor = [1 0 0; 0 1 0; 0 0 1]; % inertia tensor (TODO: compute a realistic one :)
% The alpha-sweep configuration
alphaStart = 4; %2;
alphaEnd = 6; % 6;
numAlphas = 5;
% The beta-sweep configuration
betaStart = -1;
betaEnd = 1;
numBetas = 5;
computeStaticMargin = 0; % [0/1]
% ===================================================================================

nameLongitudinalModel = 'ExperimentalCarrier_longitudinal';
nameLateralModel = 'ExperimentalCarrier_lateral';

disp(' ');

% Compute longitudinal and lateral LTIs for trimmed gliding.
[ltiLongitudinal, trimmedStateVariablesLongitudinal, ltiLateral, trimmedStateVariablesLateral, Cm_longitudinal, Cn_lateral] = computeLTIs(...
    tornadoDirectory, ... % absolute path to the Tornado root directory
    nameTornadoAircraftGeometry, ... % the name of the Tornado geometry file
    nameLongitudinalModel, ... % the name of the Simulink long. model.
    nameLateralModel,... % the name of the Simulink lateral model.
    alphaStart, alphaEnd, ... % range in degrees over which coefficients are computed for the longitudinal model
    numAlphas, ... % the number of alphas to sample
    betaStart, betaEnd, ... range in degrees over which coefficients are computed for the lateral model
    numBetas, ... the number of betas to sample
    estimatedVelocity, ... % velocity in m/s for which aerodyn. coeffs. are computed
    velocityDeviationTol, ... % factor [0 ... 1].
    airDensity, ... % kg/m^3 -> 1.225 at sea level
    elevatorFlapIndex, ...
    rudderFlapIndex, ...
    mass, ...
    centerOfGravity, ...
    InertiaTensor); % inertia tensor
trimmedAlphaRad = atan(trimmedStateVariablesLongitudinal(2)/trimmedStateVariablesLongitudinal(1)); % atan(w/u);
trimmedBetaRad = atan(trimmedStateVariablesLateral(1)/trimmedStateVariablesLongitudinal(1)); % atan(w/v);

% Compute static margin of the aircraft.
if(computeStaticMargin)
    disp('Computing static margin of longitudinal dynamics...');
    [ac, staticMarginMAC, staticMarginInMeters] = computeStaticMargin(nameTornadoAircraftGeometry, ...
        estimatedVelocity, airDensity, tornadoDirectory, centerOfGravity);
    disp(['Static margin as MAC factor  : ' num2str(staticMarginMAC)]);
    disp(['Static margin (sm)      (m)  : ' num2str(staticMarginInMeters)]);
    disp(['Aerodynamic center (ac) (m)  : ' num2str(ac)]);
    disp(['ac - sm = center of gravity  : ' num2str(ac - staticMarginInMeters)]);
end

% Longitudinal
% ========================================================================
% disp('The longitudinal LTI:');
% ltiLongitudinal

% sort complex numbers
polesLongitudinal = cplxpair(pole(ltiLongitudinal));  % sort first complex pairs (each with negative imag. part first, then pure real (ascending?)).

% Poles Map
figure();
h = subplot(2,2,1);
hold on;
plot(h, real(polesLongitudinal(1)), imag(polesLongitudinal(1)), 'rx');
plot(h, real(polesLongitudinal(2)), imag(polesLongitudinal(2)), 'bx');
plot(h, real(polesLongitudinal(3)), imag(polesLongitudinal(3)), 'ro');
plot(h, real(polesLongitudinal(4)), imag(polesLongitudinal(4)), 'bo');
hold off;
legend('phugoid mode 1', 'phugoid mode 2', 'short period mode 1', 'short period mode 2');
title('Longitudinal Poles Map');
xlabel('re');
ylabel('im');

% Step response
% Remember: for a trimmed glide, LTI state variables are all 0. Thus, the plot
% shows the resulting deviation of state variables from the trimmed state.
% Also note: with the current model (very little air resistance -> very
% good gliding ratio, the non-linear matlab model shows that it is sensible
% that w (the velocity in body-z) is becoming negative (moving 'upwards' in
% body-z).
h = subplot(2,2,2);
elevatorDeflectionStep = 2*pi/180;
stepOpt = stepDataOptions('StepAmplitude', elevatorDeflectionStep); % step response for elevatorDeflectionStep radian elevator deflection
stepplot(h, ltiLongitudinal, stepOpt);
title('Longitudinal Step Response');
hAllAxes = findobj(gcf,'type','axes');
hLeg = findobj(hAllAxes,'tag','legend');
hAxes = setdiff(hAllAxes,hLeg); % All axes which are not
for k=1:length(hAxes)
   if ~isempty(strfind(hAxes(k).YLabel.String, '(1)'))
       hAxes(k).Title.String = ['State change for an elevator step of ' num2str(elevatorDeflectionStep*180/pi) ' degrees'];
       hAxes(k).YLabel.String = 'u (m/s)'; % velocity in body x-axis
   elseif ~isempty(strfind(hAxes(k).YLabel.String, '(2)'))
       hAxes(k).YLabel.String = 'w (m/s)'; % velocity in body z-axis
   elseif ~isempty(strfind(hAxes(k).YLabel.String, '(3)'))
       hAxes(k).YLabel.String = 'q (rad/s)'; % rotation velocity around y-axis
   elseif ~isempty(strfind(hAxes(k).YLabel.String, '(4)'))
       hAxes(k).YLabel.String = 'theta (rad)'; % angle body-x vs earth-x
   end
end

% Impulse response
h = subplot(2,2,3);
impulseplot(h, ltiLongitudinal);
title('Longitudinal Impulse Response');
hAllAxes = findobj(gcf,'type','axes');
hLeg = findobj(hAllAxes,'tag','legend');
hAxes = setdiff(hAllAxes,hLeg); % All axes which are not
for k=1:length(hAxes)
   if ~isempty(strfind(hAxes(k).YLabel.String, '(1)'))
       hAxes(k).Title.String = 'State change for an elevator dirac impulse';
       hAxes(k).YLabel.String = 'u (m/s)'; % velocity in body x-axis
   elseif ~isempty(strfind(hAxes(k).YLabel.String, '(2)'))
       hAxes(k).YLabel.String = 'w (m/s)'; % velocity in body z-axis
   elseif ~isempty(strfind(hAxes(k).YLabel.String, '(3)'))
       hAxes(k).YLabel.String = 'q (rad/s)'; % rotation velocity around y-axis
   elseif ~isempty(strfind(hAxes(k).YLabel.String, '(4)'))
       hAxes(k).YLabel.String = 'theta (rad)'; % angle body-x vs earth-x
   end
end

% Plot pitch moment curve
h = subplot(2,2,4);
hold on;
plot(h, [alphaStart:(alphaEnd-alphaStart)/(numAlphas-1):alphaEnd], Cm_longitudinal, 'r-');
plot(h, trimmedAlphaRad*180/pi, 0, 'bx');
hold off;
legend('Cm', ['trimmed alpha = ' num2str(trimmedAlphaRad*180/pi) ' (deg)']);
title('Pitch moment');
xlabel('alpha');
ylabel('Cm');

% Compute damping, frequency, period from poles. See also p83 in Caughey.

% Short period poles
polesShortPeriod = polesLongitudinal(3:4); % choose last ones from sorted complex numbers
polesShortPeriodReal = real(polesShortPeriod);
polesShortPeriodImag = imag(polesShortPeriod);

Damping_sp = sqrt(1./(1+(polesShortPeriodImag./polesShortPeriodReal).^2)); % Damping ratio
Frequency_sp = -polesShortPeriodReal./Damping_sp; % Undampted, natural frequency per second
Period_sp = 2*pi./(Frequency_sp.*sqrt(1-Damping_sp.^2)); % seconds
Cycles_half_sp = ((log(2))/(2*pi))*sqrt(1-Damping_sp.^2)./Damping_sp; % Number of cycles to damp to half the amplitude

% Output result for longitudinal trimmed state
disp('Short Period Mode Properties (longitudinal): ');
disp('                                                 Pole 1      Pole 2');
disp('==============================================================================');
disp(sprintf(['Damping ratio                                 : ' num2str(Damping_sp(:)')]));
disp(sprintf(['Undampted, natural frequency             (1/s): ' num2str(Frequency_sp(:)')]));
disp(sprintf(['Period                                     (s): ' num2str(Period_sp(:)')]));
disp(sprintf(['Num. cycles to damp to half the amplitude     : ' num2str(Cycles_half_sp(:)')]));
disp(' ');

% Phugoid poles
polesPhugoid = polesLongitudinal(1:2); % choose first ones from sorted complex numbers
polesPhugoidReal = real(polesPhugoid);
polesPhugoidImag = imag(polesPhugoid);

Damping_ph = sqrt(1./(1+(polesPhugoidImag./polesPhugoidReal).^2)); % Damping ratio
Frequency_ph = -polesPhugoidReal./Damping_ph; % Undampted, natural frequency per second
Period_ph = 2*pi./(Frequency_ph.*sqrt(1-Damping_ph.^2)); % seconds
Cycles_half_ph = ((log(2))/(2*pi))*sqrt(1-Damping_ph.^2)./Damping_ph; % Number of cycles to damp to half the amplitude

% Output result for longitudinal trimmed state
disp('Phugoid Mode Properties (longitudinal): ');
disp('                                                 Pole 1      Pole 2');
disp('==============================================================================');
disp(sprintf(['Damping ratio                                 : ' num2str(Damping_ph(:)')]));
disp(sprintf(['Undampted, natural frequency             (1/s): ' num2str(Frequency_ph(:)')]));
disp(sprintf(['Period                                     (s): ' num2str(Period_ph(:)')]));
disp(sprintf(['Num. cycles to damp to half the amplitude     : ' num2str(Cycles_half_ph(:)')]));
disp(' ');

% Lateral
% ========================================================================

%disp('The lateral LTI:');
%ltiLateral

% sort complex numbers
polesLateral = cplxpair(pole(ltiLateral)); % sort first complex pairs (each with negative imag. part first, then pure real (ascending?)).

% Poles Map
figure();
h = subplot(2,2,1);
hold on;
plot(h, real(polesLateral(1)), imag(polesLateral(1)), 'rx');
plot(h, real(polesLateral(2)), imag(polesLateral(2)), 'bx');
plot(h, real(polesLateral(3)), imag(polesLateral(3)), 'ro');
plot(h, real(polesLateral(4)), imag(polesLateral(4)), 'bo');
hold off;
legend('dutch roll mode 1', 'dutch roll mode 2', 'rolling mode', 'spiral mode');
title('Lateral Poles Map');
xlabel('re');
ylabel('im');

% Step response
% Remember: for a trimmed glide, LTI state variables are all 0. Thus, the plot
% shows the resulting deviation of state variables from the trimmed state.
h = subplot(2,2,2);
rudderDeflectionStep = 2*pi/180;
stepOpt = stepDataOptions('StepAmplitude', rudderDeflectionStep); % step response for elevatorDeflectionStep radian elevator deflection
stepplot(h, ltiLateral, stepOpt);
title('Lateral Step Response');
hAllAxes = findobj(gcf,'type','axes');
hLeg = findobj(hAllAxes,'tag','legend');
hAxes = setdiff(hAllAxes,hLeg); % All axes which are not
for k=1:length(hAxes)
   if ~isempty(strfind(hAxes(k).YLabel.String, '(1)'))
       hAxes(k).Title.String = ['State change for a rudder step of ' num2str(rudderDeflectionStep*180/pi) ' degrees'];
       hAxes(k).YLabel.String = 'v (m/s)'; % velocity in body y-axis
   elseif ~isempty(strfind(hAxes(k).YLabel.String, '(2)'))
       hAxes(k).YLabel.String = 'p (rad/s)'; % rotation velocity around x-axis
   elseif ~isempty(strfind(hAxes(k).YLabel.String, '(3)'))
       hAxes(k).YLabel.String = 'phi (rad)'; % roll angle (euler phi)
   elseif ~isempty(strfind(hAxes(k).YLabel.String, '(4)'))
       hAxes(k).YLabel.String = 'r (rad/s)'; % rotation velocity around z-axis
   end
end

% Impulse response
h = subplot(2,2,3);
impulseplot(h, ltiLateral);
title('Lateral Impulse Response');
hAllAxes = findobj(gcf,'type','axes');
hLeg = findobj(hAllAxes,'tag','legend');
hAxes = setdiff(hAllAxes,hLeg); % All axes which are not
for k=1:length(hAxes)
   if ~isempty(strfind(hAxes(k).YLabel.String, '(1)'))
       hAxes(k).Title.String = 'State change for a rudder dirac impulse';
      hAxes(k).YLabel.String = 'v (m/s)'; % velocity in body y-axis
   elseif ~isempty(strfind(hAxes(k).YLabel.String, '(2)'))
       hAxes(k).YLabel.String = 'p (rad/s)'; % rotation velocity around x-axis
   elseif ~isempty(strfind(hAxes(k).YLabel.String, '(3)'))
       hAxes(k).YLabel.String = 'phi (rad)'; % roll angle (euler phi)
   elseif ~isempty(strfind(hAxes(k).YLabel.String, '(4)'))
       hAxes(k).YLabel.String = 'r (rad/s)'; % rotation velocity around z-axis
   end
end

% Plot yaw moment curve
h = subplot(2,2,4);
hold on;
plot(h, [betaStart:(betaEnd-betaStart)/(numBetas-1):betaEnd], Cn_lateral, 'r-');
plot(h, trimmedBetaRad*180/pi, 0, 'bx');
hold off;
legend('Cn', ['trimmed beta = ' num2str(trimmedBetaRad*180/pi) ' (deg)']);
title('Yaw moment');
xlabel('beta');
ylabel('Cn');

% Compute damping, frequency, period from poles. See also p83 in Caughey.

% Roll pole
poleRoll = polesLateral(3);
poleRollReal = real(poleRoll);
poleRollImag = imag(poleRoll);

Damping_roll = sqrt(1./(1+(poleRollImag./poleRollReal).^2)); % Damping ratio
Frequency_roll = -poleRollReal./Damping_roll; % Undampted, natural frequency per second
Period_roll = 2*pi./(Frequency_roll.*sqrt(1-Damping_roll.^2)); % seconds
Cycles_half_roll = ((log(2))/(2*pi))*sqrt(1-Damping_roll.^2)./Damping_roll; % Number of cycles to damp to half the amplitude

disp('Roll Mode Properties (lateral): ');
disp('                                                 Pole 1');
disp('==============================================================================');
disp(sprintf(['Damping ratio                                 : ' num2str(Damping_roll(:)')]));
disp(sprintf(['Undampted, natural frequency             (1/s): ' num2str(Frequency_roll(:)')]));
disp(sprintf(['Period                                     (s): ' num2str(Period_roll(:)')]));
disp(sprintf(['Num. cycles to damp to half the amplitude     : ' num2str(Cycles_half_roll(:)')]));
disp(' ');

% Spiral pole
poleSpiral = polesLateral(4);
poleSpiralReal = real(poleSpiral);
poleSpiralImag = imag(poleSpiral);

Damping_spiral = sqrt(1./(1+(poleSpiralImag./poleSpiralReal).^2)); % Damping ratio
Frequency_spiral = -poleSpiralReal./Damping_spiral; % Undampted, natural frequency per second
Period_spiral = 2*pi./(Frequency_spiral.*sqrt(1-Damping_spiral.^2)); % seconds
Cycles_half_spiral = ((log(2))/(2*pi))*sqrt(1-Damping_spiral.^2)./Damping_spiral; % Number of cycles to damp to half the amplitude

disp('Spiral Mode Properties (lateral): ');
disp('                                                 Pole 1');
disp('==============================================================================');
disp(sprintf(['Damping ratio                                 : ' num2str(Damping_spiral(:)')]));
disp(sprintf(['Undampted, natural frequency             (1/s): ' num2str(Frequency_spiral(:)')]));
disp(sprintf(['Period                                     (s): ' num2str(Period_spiral(:)')]));
disp(sprintf(['Num. cycles to damp to half the amplitude     : ' num2str(Cycles_half_spiral(:)')]));
disp(' ');

% Dutch Roll poles
polesDutchRoll = polesLateral(1:2); % choose first ones from sorted complex numbers
polesDutchRollReal = real(polesDutchRoll);
polesDutchRollImag = imag(polesDutchRoll);

Damping_dutch = sqrt(1./(1+(polesDutchRollImag./polesDutchRollReal).^2)); % Damping ratio
Frequency_dutch = -polesDutchRollReal./Damping_dutch; % Undampted, natural frequency per second
Period_dutch = 2*pi./(Frequency_dutch.*sqrt(1-Damping_dutch.^2)); % seconds
Cycles_half_dutch = ((log(2))/(2*pi))*sqrt(1-Damping_dutch.^2)./Damping_dutch; % Number of cycles to damp to half the amplitude

% Output result for longitudinal trimmed state
disp('Dutch Roll Mode Properties (lateral): ');
disp('                                                 Pole 1      Pole 2');
disp('==============================================================================');
disp(sprintf(['Damping ratio                                 : ' num2str(Damping_dutch(:)')]));
disp(sprintf(['Undampted, natural frequency             (1/s): ' num2str(Frequency_dutch(:)')]));
disp(sprintf(['Period                                     (s): ' num2str(Period_dutch(:)')]));
disp(sprintf(['Num. cycles to damp to half the amplitude     : ' num2str(Cycles_half_dutch(:)')]));
disp(' ');

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

% Observability
disp(['Number of unobservable states (longitudinal)  : ' num2str(length(ltiLongitudinal.a) - rank(obsv(ltiLongitudinal)))]);
disp(['Number of unobservable states (lateral)       : ' num2str(length(ltiLateral.a) - rank(obsv(ltiLateral)))]);
disp(' ');

% Controllability
disp(['Number of uncontrollable states (longitudinal): ' num2str(length(ltiLongitudinal.a) - rank(ctrb(ltiLongitudinal)))]);
disp(['Number of uncontrollable states (lateral)     : ' num2str(length(ltiLateral.a) - rank(ctrb(ltiLateral)))]);
disp(' ');

% TODO
% ========================================================================

% 1) Run this whole thing for different center of gravity settings and plot
% Cm curve and poles.


function [xsol,objp,info,msg,times,x,y,z,s,w] = lipsol(A,b,c,lb,ub,opts)
% LIPSOL        - Main program for LIPSOL

% Yin Zhang, January, 1995
% Department of Mathematics and Statistics
% University of Maryland  Baltimore County
% Modified J.Currie AUT May 2013

global probData

if(nargin < 6)
    opts = lipsolset;
    opts.maxiter = 100;
    opts.maxtime = 1000;
    opts.tol = 1e-5;
end
if(nargin < 5), ub = []; end
if(nargin < 4), lb = []; end
if(nargin < 3), error('You must supply at least 3 arguments to lipsol(A,b,c)'); end

times = zeros(5,1);
t0 = tic;

%Problem Data Structure to avoid so many Global Variables
probData = struct('NNZA',0,'NNZL',0,'XLNZ',[],'PERM',[],'INVP',[],'LNZ',[],'LOOP_LEVEL',[],...
                  'XSUPER',[],'XLINDX',[],'LINDX',[],'SNODE',[],'SPLIT',[],'TMPSIZ',[],...
                  'LNZ0',[],'b_orig',b,'c_orig',c,'data_changed',0,'isFeasible',1,...
                  'Lbounds_non0',[],'Ubounds_exist',0,'nub',0,'Fixed_exist',0,...
                  'ifix',[],'infx',[],'xfix',[],'Zrcols_exist',0,'izrcol',[],...
                  'inzcol',[],'xzrcol',[],'Sgtons_exist',0,'isolved',[],...
                  'insolved',[],'xsolved',[],'nt',0,'message',[],...
                  'col_scaled',0,'colscl',[],'Dense_cols_exist',0,'Mdense',[],...
                  'idense',[],'ispars',[],'Hist',[],'Sherman_OK',[],'backed',[]);
%Initial Time
opts.t0 = tic;

%Save loop_level into problem structure
probData.LOOP_LEVEL = opts.loop_level;

%Pretty Printing
if(opts.verb), fprintf('----------------------------------------------\n'); end

%Preprocess the Input Data
[A,b,c,lb,ub] = ls_preprocess(A,b,c,lb,ub,opts);
times(1) = toc(t0);

%Check if preproccesor indicates infeasible
if(~probData.isFeasible)
    xsol = []; objp = []; info = [-1 0 0]; msg = 'Presolve Indicated Infeasible'; times = [];
    x = []; y = []; z = []; s = []; w = [];
    return;
end

%Scale the Input Data if Required
[A,b,c,ub] = scaling(A,b,c,ub,opts);
%Determine density of the problem
checkDense(A,opts);
times(2) = toc(t0) - times(1);

%Solve the Problem
[x,y,z,s,w,info] = ls_miip(A,b,c,ub,opts);
times(3) = toc(t0) - times(2);

%Display Solve Status
if(opts.verb && ~isempty(probData.message)), fprintf('\n  Status: %s\n',probData.message); end

%Post Process the Solution
[xsol, objp, times ] = ls_postprocess(x,y,lb,opts.verb,times,t0);
msg = probData.message;

%Pretty Printing
if(opts.verb), fprintf('----------------------------------------------\n'); end

function [A,b,c,ub] = scaling(A,b,c,ub,opts)
% SCALING     - Scale matrix A. Usage: [A,b,c,ubounds] = scaling(A,b,c,ubounds)
% Yin Zhang, January, 1995
% Department of Mathematics and Statistics
% University of Maryland  Baltimore County

global probData

[m, n] = size(A);
badscl = 1.e-4;

absnzs = abs(nonzeros(A));
thescl = min(absnzs)/max(absnzs);
clear absnzs

if (thescl < badscl)
    if(opts.verb), fprintf('Scaling ...\n'); end

    % ----- scaling vectors ------
    AA = abs(A);
    colscl = full(sqrt(max(AA)'));
    rowscl = full(sqrt(max(AA,[],2)));
    clear AA;

    % ----- column scaling -----
    if (probData.Ubounds_exist), ub = ub.*colscl; end
    colscl = reciprocal(colscl);
    A = A*sparse(1:n,1:n,colscl);
    c = c.*colscl;
    probData.col_scaled = 1;

    % ----- row scaling -----
    rowscl = reciprocal(rowscl);
    A = sparse(1:m,1:m,rowscl)*A;
    b = b.*rowscl;
    bnrm = norm(b);
    if (bnrm > 0)
      q = median([1 norm(c)/bnrm 1.e+8]);
      if (q > 10), A = q*A; b = q*b; end
    end
    probData.data_changed = 1;
    probData.colscl = colscl;
end

function checkDense(A,opts) %#ok<INUSD>
% CHECKDENSE - Determine and locate dense columns of matrix A.
% Yin Zhang, January, 1995
% Department of Mathematics and Statistics
% University of Maryland  Baltimore County

%PROBLEM IN PCG - SKIPPING DENSE COLUMN MANAGEMENT [J.C. 19/5/13]

% global probData
%
% [m, n] = size(A);
% probData.ispars = 1:n;
% nzratio = 1;
% if (m >  500), nzratio = 0.20; end;
% if (m > 1000), nzratio = 0.10; end;
% if (m > 5000), nzratio = 0.05; end;
%
% if (nzratio < 1)
%    checking = sum(spones(A))/m <= nzratio;
%    if any(checking == 0)
%       probData.Dense_cols_exist = 1;
%       probData.idense = find(sparse(1-checking));   % Dense  column indices
%       probData.ispars = find(checking);             % Sparse column indices
%       if(opts.verb), fprintf('Dense columns (nnz/m > %g): %i\n',nzratio,length(probData.idense)); end
%    else
%       if(opts.verb), fprintf('No Significant Dense Columns\n'); end
%    end
%    clear checking
% else
%     if(opts.verb), fprintf('Problem is Small Enough to Ignore Dense Columns\n'); end
% end

function Y = reciprocal(X)
% RECIPROCAL  - Invert the nonzero entries of a matrix elementwise.
%             Y = RECIPROCAL(X) has the same sparsity pattern as X
%	      (except possibly for underflow).

% Yin Zhang, January, 1995
% Department of Mathematics and Statistics
% University of Maryland  Baltimore County

if issparse(X)
   [m, n]  = size(X);
   [i,j,Y] = find(X);
   Y = sparse(i,j,1./Y,m,n);
else
   Y = 1./X;
end
ceiling = 1.e+16; Y = min(ceiling,Y);


function [funOut, indexStart, problemDom, coeffs, totalDiffOrders] = ...
    toFirstOrder(funIn, rhs, domain, numArgs, cellArg)
%TOFIRSTORDER   Convert higher order anonymous functions to first order systems.
%   Calling sequence:
%      [FUNOUT, INDEXSTART, PROBLEMDOM, COEFFS, TOTALDIFFORDERS] = ...
%          TOFIRSTORDER(FUNIN, RHS, DOMAIN)
%   where the inputs are:
%      FUNIN:  An anonymous function which describes an ODE, usually including
%              higher order derivatives, e.g. @(x,u) diff(u, 2) + sin(u).
%              Usually, the anonymous function comes from the OP field of a
%              CHEBOP.
%      RHS:    The right hand side of the differential equation being solved
%              with a CHEBOP, that is, the right argument of a CHEBOP
%              backslash.
%      DOMAIN: The domain of the problem we're trying to solve.
%   and the outputs are:
%      FUNOUT:     An anonymous function that is the first order reformulation
%                  of FUNIN, which can be passed to the MATLAB solvers.
%      INDEXSTART: A vector that denotes at which index we should start
%                  indexing each variable from so that they're in the correct
%                  order for the MATLAB ODE solvers.
%      PROBLEMDOM: The domain on which the problem can be specified, which may
%                  include breakpoints originally not included in DOMAIN.
%      COEFFS:     A cell array of the coefficients the multiply the highest
%                  order derivatives in problem. For example, for the problem
%                  @(x,u) 5*diff(u, 2) + u, we will have COEFFS{1} = 5.
%      TOTALDIFFORDERS:
%                  A vector that contains the maximum diffOrder applying to
%                  each variable in the problem.
%
%   In addition, one can pass the following arguments to the function, which are
%   only required if working in CHEBMATRIX syntax, for example,
%   @(x,u)[diff(u{1}) + u{2};...]:
%       NVARS:   The number of unknown variables in the ODE.
%       CELLARG: Specify as TRUE if FUNIN is specified in CHEBMATRIX syntax.

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

% Independent variable on the domain
t = chebfun(@(t) t, domain);

% See if we need to determine the number of variables involved, or if it was
% passed as an argument.
if ( nargin < 4)
    % We always have at least one argument to FUNIN, even in the scalar case. In
    % the system case, the first argument to funIn must be the independent time
    % variable. So the number of treeVar arguments needed is one less:
    numArgs = max(1, nargin(funIn) - 1);
    % If working in CHEBMATRIX syntax, we need all possible input variables to
    % be passed in as arguments:
    cellArg = false;
end
args = cell(numArgs, 1);
argsVec = zeros(1, numArgs);

% Populate the args cell
for argCount = 1:numArgs
    argsVec(argCount) = 1;
    args{argCount} = treeVar(argsVec, domain);
    % Reset the index vector
    argsVec = 0*argsVec;
end

% Evaluate FUNIN with the TREEVAR arguments. Need different calling sequences
% depending on whether we're working with CHEBMATRIX syntax and cells or not:
if ( cellArg )
    fevalResult = funIn(t, args);
else
    % If FUNIN only takes one argument, we just give it the TREEVAR argument.
    % Otherwise, the first input will be the independent variable on the domain:
    if ( nargin(funIn) == 1 )
        fevalResult = funIn(args{:});
    else
        fevalResult = funIn(t, args{:});
    end
end

% If we got passed the problem as N\0, i.e. a RHS that did not match the
% dimensions, we need to repmat it so that the RHS variable has the correct
% dimensions below:
if ( isnumeric(rhs) && length(rhs) == 1 )
    rhs = repmat(rhs, size(fevalResult));
end

% Ensure RHS is a CHEBMATRIX
if ( ~isa(rhs, 'chebmatrix') )
    rhs = chebmatrix(rhs, domain);
end

% Initialize cells to store the infix forms of the expressions, the coefficients
% multiplying the highest order derivatives and any variables that appear in the
% anonymous functions:
systemInfix = cell(length(fevalResult), 1);
coeffs = systemInfix;
varArrays = systemInfix;

% We want to return all potential breakpoints caused by piecewise coefficients
% in the problem. So loop through fevalResult, and take the union of the
% breakpoints:
problemDom = fevalResult(1).domain;
for resCounter = 2:length(fevalResult)
    problemDom = union(problemDom, fevalResult(resCounter).domain);
end

% Ensure we also include potential breakpoints from the RHS:
problemDom = union(problemDom, rhs.domain);

% First look at all diffOrders to ensure we start with the correct indices.
% INDEXSTART denotes at which index we should start indexing each variable from.
% E.g., in the coupled system
%   [v'' + w; v + w'']
% we will have v = u(1), v' = u(2), w = u(3), w' = u(4), so INDEXSTART = [1, 3],
% since v and its derivatives starts getting counted at 1, and w and its
% derivatives start getting counted at 3.
%
% The vector INDEXSTARTDER is similar, but here, we also assign the highest
% order derivative of each variable it's own index. This is so that later on, we
% can correctly evaluate the coefficient multiplying the highest order
% derivative in problem. Thus, in the example above, we'd have
%   v = u(1), v' = u(2), v'' = u(3), w = u(4), w' = u(5), w'' = u(6)
% Here, INDEXSTARTDER = [1 4].

% First, we need to find out the maximum diffOrder of each variable appearing in
% the problem. We loop through the each component of the evaluation tree:
totalDiffOrders = zeros(1, numArgs);
for wCounter = 1:length(fevalResult)
    totalDiffOrders = max(totalDiffOrders, ...
        fevalResult(wCounter).tree.diffOrder);
end

% We always start indexing the first variable and its derivative(s) at 1. The
% index of the other variables depend on the cumulative diffOrders of the
% variables with lower indices.
indexStart = [ 1, cumsum(totalDiffOrders(1:end-1)) + 1 ];
% To get the indices of the derivatives as well, we shift all the previous
% indices right by 1, in cumulative fashion:
indexStartDer = indexStart + (0:numArgs-1);

% COEFFARG will be used to evaluate the functions that gives us information
% about the coefficients multiplying the highest order derivative in each
% equation. The vector has to be equally long to the total number of derivatives
% appearing in the problem; we'll then change one of the entries to 1 at a time
% to get the coefficient information.
coeffArg = zeros(1, indexStartDer(end) + totalDiffOrders(end));

% Go through each componenent from the result of evaluating FUNIN,
% and change it to infix format.
for wCounter = 1:length(fevalResult)

    % The current result we're looking at.
    res = fevalResult(wCounter);

    % Current diffOrders
    diffOrders = res.tree.diffOrder;

    % Ensure that we never have the highest derivatives of more than one
    % variable appearing in a single equation:
    if ( sum(totalDiffOrders == diffOrders) > 1 )
        error('CHEBFUN:TREEVAR:toFirstOrder:diffOrders', ...
            ['The highest order derivative of more than one variable ' ...
            'appears to be\npresent in the same equation. ' ...
            'Unable to convert to first order format.'])
    end

    % Expand the tree, so that PLUS rather than TIMES is sitting at the top of
    % it.
    expTree = treeVar.expandTree(res.tree, totalDiffOrders);

    % Split the tree into derivative part and non-derivative part.
    [newTree, derTree] = treeVar.splitTree(expTree, totalDiffOrders);

    % If newTree is empty, we only have a derivative part in the expression,
    % e.g. diff(u) = 0. We must replace it with a 0, as otherwise, we can't
    % evaluate the resulting odeFun in the ODE solvers.
    if ( isempty(newTree) )
        newTree = 0;
    end

    % Find what argument corresponds to the highest derivative one in the
    % current expression we're looking at. This will also be the order in which
    % we store the outputs from converting individual expressions to first order
    % form -- if the input is of the form @(x,u,v) [diff(v) - u; diff(u) - v],
    % the equations have to be sorted so that they'll be correctly converted.
    maxDerLoc = find(expTree.diffOrder == totalDiffOrders);

    % Convert the derivative part to infix form.
    % Indicate that we are converting a coeffFun
    isCoeffFun = true;
    [infixDer, varArrayDer] = ...
        treeVar.tree2infix(derTree, maxDerLoc, indexStartDer, isCoeffFun);

    % Convert the infix form of the expression that gives us the coefficient
    % multiplying the highest order derivative appearing in the expression to an
    % anonymous function we can evaluate:
    coeffFun = treeVar.toAnon(infixDer, varArrayDer);

    % Reset coeffArg for next evaluation:
    coeffArg = 0*coeffArg;

    % Replace one of the 0s in coeffFun with 1 so that we can evaluate COEFFFUN:
    if ( maxDerLoc == numArgs )
        % The last variable in the system currently appears in the highest order
        % derivate.
        coeffArg(end) = 1;
    else
        % The variable with index maxDerLoc+1 is the next variable we need to
        % start numbering at. So subtract 1 for the index of the highest
        % derivative we're currently interested in.
        coeffArg(indexStartDer(maxDerLoc+1) - 1) = 1;
    end

    % Evaluate the COEFFFUN to the coefficient!
    coeffs{maxDerLoc} = coeffFun(t, coeffArg);

    % Now work with the remaining syntax tree of the current expression of
    % interest. We need to negate the syntax tree as we're moving it to the
    % right-hand side. But if it already starts with a unary minus, we can
    % simply remove it rather than doing a double negation:
    % [TODO: Remove double UMINUS]
    newTree = struct('method', 'minus', 'numArgs', 2, ...
        'left', rhs{wCounter}, 'right', newTree);
    % Convert current expression to infix form:
    isCoeffFun = false;
    [infix, varArray] = ...
        treeVar.tree2infix(newTree, maxDerLoc, indexStart, isCoeffFun);
    % Store the infix form and the variables that appeared in the anonymous
    % function.
    systemInfix{maxDerLoc} = infix;
    varArrays{maxDerLoc} = varArray;
end

% Convert all the infix expressions, coefficients and variables stored to an
% anonymous function we can evaluate and use as the RHS of our ODE system:
funOut = treeVar.toRHS(systemInfix, varArrays, coeffs, ...
    indexStart, totalDiffOrders);

end


% LIONSIMBA example script
% Different heat exchange coefficients: this script provides the example simulation shown in the
% paper.

%   This file is part of the LIONSIMBA Toolbox
%
%	Official web-site: 	http://sisdin.unipv.it/labsisdin/lionsimba.php
% 	Official GitHUB: 	https://github.com/lionsimbatoolbox/LIONSIMBA
%
%   LIONSIMBA: A Matlab framework based on a finite volume model suitable for Li-ion battery design, simulation, and control
%   Copyright (C) 2016-2018 :Marcello Torchio, Lalo Magni, Davide Raimondo,
%                            University of Pavia, 27100, Pavia, Italy
%                            Bhushan Gopaluni, Univ. of British Columbia,
%                            Vancouver, BC V6T 1Z3, Canada
%                            Richard D. Braatz,
%                            Massachusetts Institute of Technology,
%                            Cambridge, Massachusetts 02142, USA
%
%   Main code contributors to LIONSIMBA 2.0:
%                           Ian Campbell, Krishnakumar Gopalakrishnan,
%                           Imperial college London, London, UK
%
%   LIONSIMBA is a free Matlab-based software distributed with an MIT
%   license.

% Clear the workspace
clear

% Define the integration times.
t0 = 0;
tf = 10^4;
% Define the parameters structure. By default the hcell parameter is set to
% 1 [W / (m^2 K)]
param{1} = Parameters_init;

% Change the hcell parameter and set it to 0.01 [W / (m^2 K)]
param{1}.hcell = 0.01;

% Start the simulation. Note that the final integration time is 10^4 and
% LIONSIMBA will stop automatically when reached the Cutoff Voltage of
% 2.5V. Note that no Jacobian among one simulation and the other. This is
% due to the fact that the model structure changes for each of the
% simulation (because hcell changes) and consequently also the Jacobian
% matrix changes and cannot be used for all the scenarios.

out1 = startSimulation(t0,tf,[],-30,param);

% Change the hcell parameter and set it to 1 [W / (m^2 K)]
param{1}.hcell = 1;

% Run the simulation
out2 = startSimulation(t0,tf,[],-30,param);

% Change the hcell parameter and set it to 100 [W / (m^2 K)]
param{1}.hcell = 100;

% Run the simulation
out3 = startSimulation(t0,tf,[],-30,param);

%% Plot the results

figure(1)
plot(out1.time{1},out1.Temperature{1}(:,end),'LineWidth',6)
hold on
plot(out2.time{1},out2.Temperature{1}(:,end),'--','LineWidth',6)
plot(out3.time{1},out3.Temperature{1}(:,end),'-.','LineWidth',6)
xlabel('Time [s]')
ylabel('Temperature [K]')
grid on
box on

figure(2)
plot(out1.time{1},out1.Voltage{1},'LineWidth',6)
hold on
plot(out2.time{1},out2.Voltage{1},'--','LineWidth',6)
plot(out3.time{1},out3.Voltage{1},'-.','LineWidth',6)
xlabel('Time [s]')
ylabel('Voltage [V]')
grid on
box on


function [Lmk,Obs] = reparametrizeLmk(Rob,Sen,Lmk,Obs,Opt)

%REPARAMETRIZELMK Reparametrize landmark.
%   [LMK,OBS] = REPARAMETRIZELMK(ROB,SEN,LMK,OBS,OPT) return the
%   reparametrized landmark and Obs structure.
%   The landmark is reparametrized only in the case of an inverse depth
%   point. A linearity test of cartesian point given inverse depth
%   point is performed.

%   Copyright 2009 David Marquez @ LAAS-CNRS.

global Map

switch Lmk.type

    case 'idpPnt'
        % we will convert from inverse-depth to euclidean.

        % Test for linearity:
        Ld = idpLinearTest(Rob,Sen,Lmk);

        if Ld < Opt.correct.linTestIdp

            % ranges
            ir = Lmk.state.r;  % idp
            er = ir(1:3);      % euclidean
            m  = Map.used;     % map

            % point coordinates
            idp     = Map.x(ir);   % idp
            [p,P_i] = idp2euc(idp);  % euclidean

            % map updates
            Map.x(er) = p;     % mean

            Map.P(er,m) = P_i * Map.P(ir,m); % co- and cross-variances
            Map.P(m,er) = Map.P(m,ir) * P_i';

            Map.used(Lmk.state.r(4:6)) = false; % used positions

            % Lmk and Obs updates
            Lmk.state.r = er;    % new range
            Lmk.type  = 'eucPnt'; % new type
            Obs.ltype = 'eucPnt'; % new type
        end

    case 'ahmPnt'
        % we will convert from anchored homogeneous to euclidean.

        % Test for linearity:
        Ld = ahmLinearTest(Rob,Sen,Lmk);

        if Ld < Opt.correct.linTestIdp

            % ranges
            ar  = Lmk.state.r;  % ahmPnt
            er  = ar(1:3);      % euclidean
            m   = Map.used;     % map

            % point coordinates
            ahm     = Map.x(ar);   % idp
            [p,P_a] = ahm2euc(ahm);  % euclidean

            % map updates
            Map.x(er)   = p;     % mean

            Map.P(er,m) = P_a * Map.P(ar,m); % co- and cross-variances
            Map.P(m,er) = Map.P(m,ar) * P_a';

            Map.used(Lmk.state.r(4:7)) = false; % used positions

            % Lmk and Obs updates
            Lmk.state.r = er;    % new range
            Lmk.type  = 'eucPnt'; % new type
            Obs.ltype = 'eucPnt'; % new type
        end

    case {'eucPnt'}
        % do nothing

    case {'hmgPnt','ahmPnt','plkLin','aplLin','idpLin','ahmLin','hmgLin'}
        % do nothing, by now <- probably add here something to do
        % Points should go to euclidean
        % Lines should go to some minimal representation (polar? 'plrLin')

    % case 'myLmk'
        % edit this 'myLmk' name to put your own landmark type
        % do something

    otherwise
        error('??? Unknown landmark type ''%s''.',Lmk.type)

end

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

%   # START GPL LICENSE

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

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

%   # END GPL LICENSE


%%***************************************************************************
%% steplength: compute xstep such that  X + xstep*dX >= 0.
%%
%% [xstep] = steplength(blk,X,dX,Xchol,invXchol);
%%
%% SDPT3: version 3.1
%% Copyright (c) 1997 by
%% K.C. Toh, M.J. Todd, R.H. Tutuncu
%% Last Modified: 16 Sep 2004
%%***************************************************************************

    function [xstep,invXchol] = steplength(blk,X,dX,Xchol,invXchol);

%%
    for p = 1:size(blk,1)
       pblk = blk(p,:);
       numblk = length(pblk{2});
       pblksize = sum(pblk{2});
       if (any(isnan(dX{p})) | any(isinf(dX{p}))); xstep = 0; break; end;
       if strcmp(pblk{1},'s')
          if (max(pblk{2}) >= 200)
             use_lanczos = 1;
          else
             use_lanczos = 0;
          end
          if (use_lanczos)
             tol = 1e-3;
             maxit = max(min(pblksize,30),round(sqrt(pblksize)));
             [lam,delta,res] = lanczosfun(Xchol{p},-dX{p},maxit,tol);
             %%
             %% Note: lam <= actual largest eigenvalue <= lam + delta.
             %%
	     d = lam+delta;
          else
             if isempty(invXchol{p});
                invXchol{p} = inv(Xchol{p});
             end
	     tmp = Prod2(pblk,dX{p},invXchol{p},0);
             M = Prod2(pblk,invXchol{p}',tmp,1);
             if (exist('mexblkeig')==3)
                d = mexblkeig(pblk,-M);
             else
                d = blkeig(pblk,-M);
             end
          end
          tmp = max(d) + 1e-15*max(abs(d));
          if (tmp > 0);
             xstep(p) = 1/max(tmp);
          else
             xstep(p) = 1e12;
          end
       elseif strcmp(pblk{1},'q')
          aa = qops(pblk,dX{p},dX{p},2);
          bb = qops(pblk,dX{p},X{p},2);
          cc = qops(pblk,X{p},X{p},2);
          dd = bb.*bb - aa.*cc;
          tmp = min(aa,bb);
          idx = find(dd > 0 & tmp < 0);
          steptmp = 1e12*ones(numblk,1);
          if ~isempty(idx)
             steptmp(idx) = -(bb(idx)+sqrt(dd(idx)))./aa(idx);
          end
          idx = find(abs(aa) < eps & bb < 0);
          if ~isempty(idx)
             steptmp(idx) = -cc(idx)./(2*bb(idx));
          end
          %%
          %% also need first component to be non-negative
          %%
          ss = 1 + [0, cumsum(pblk{2})];
          ss = ss(1:length(pblk{2}));
          dX0 = dX{p}(ss);
          X0 = X{p}(ss);
          idx = find(dX0 < 0 & X0 > 0);
          if ~isempty(idx)
             steptmp(idx) = min(steptmp(idx),-X0(idx)./dX0(idx));
          end
          xstep(p) = min(steptmp);
       elseif strcmp(pblk{1},'l')
          idx = find(dX{p} < 0);
          if ~isempty(idx)
             xstep(p) = min(-X{p}(idx)./dX{p}(idx));
          else
             xstep(p) = 1e12;
          end
       elseif strcmp(pblk{1},'u')
          xstep(p) = 1e12;
       end
    end
    xstep = min(xstep);
%%***************************************************************************
%%***************************************************************************
%% lanczos: find the largest eigenvalue of
%%          invXchol'*dX*invXchol via the lanczos iteration.
%%
%% [lam,delta] = lanczosfun(Xchol,dX,maxit,tol,v)
%%
%% lam:  an estimate of the largest eigenvalue.
%% lam2: an estimate of the second largest eigenvalue.
%% res:  residual norm of the largest eigen-pair.
%% res2: residual norm of the second largest eigen-pair.
%%***************************************************************************

   function [lam,delta,res] = lanczosfun(Xchol,dX,maxit,tol,v)

   if (norm(dX,'fro') < 1e-13)
      lam = 0; delta = 0; res = 0;
      return;
   end
   n = length(dX);
   if (nargin < 5);
      state = randn('state');
      randn('state',0);
      v = randn(n,1);
      randn('state',state);
   end
   if (nargin < 4); tol = 1e-3; end
   if (nargin < 3); maxit = 30; end
   V = zeros(n,maxit+1); H = zeros(maxit+1,maxit);
   v = v/norm(v);
   V(:,1) = v;
   if issparse(Xchol); Xcholtransp = Xchol'; end
%%
%% lanczos iteration.
%%
   for k = 1:maxit
      if issparse(Xchol)
         w = dX*mextriangsp(Xcholtransp,v,1);
         w = mextriangsp(Xchol,w,2);
      else
         w = dX*mextriang(Xchol,v,1);
         w = mextriang(Xchol,w,2);
      end
      wold = w;
      if (k > 1);
         w = w - H(k,k-1)*V(:,k-1);
      end;
      alp = w'*V(:,k);
      w   = w - alp*V(:,k);
      H(k,k) = alp;
      %%
      %% one step of iterative refinement if necessary.
      %%
      if (norm(w) <= 0.8*norm(wold));
         s = (w'*V(:,1:k))';
         w = w - V(:,1:k)*s;
         H(1:k,k) = H(1:k,k) + s;
      end;
      nrm = norm(w);
      v = w/nrm;
      V(:,k+1) = v;
      H(k+1,k) = nrm;  H(k,k+1) = nrm;
      %%
      %% compute ritz pairs and test for convergence
      %%
      if (rem(k,5) == 0) | (k == maxit);
         Hk = H(1:k,1:k); Hk = 0.5*(Hk+Hk');
         [Y,D] = eig(Hk);
         eigH  = real(diag(D));
         [dummy,idx] = sort(eigH);
         res_est = abs(H(k+1,k)*Y(k,idx(k)));
         if (res_est <= 0.1*tol) | (k == maxit);
            lam = eigH(idx(k));
            lam2 = eigH(idx(k-1));
            z  = V(:,1:k)*Y(:,idx(k));
            z2 = V(:,1:k)*Y(:,idx(k-1));
            if issparse(Xchol)
               tmp = dX*mextriangsp(Xcholtransp,z,1);
               res = norm(mextriangsp(Xchol,tmp,2) -lam*z);
               tmp = dX*mextriangsp(Xcholtransp,z2,1);
               res2 = norm(mextriangsp(Xchol,tmp,2) -lam*z2);
            else
               tmp = dX*mextriang(Xchol,z,1);
               res = norm(mextriang(Xchol,tmp,2) -lam*z);
               tmp = dX*mextriang(Xchol,z2,1);
               res2 = norm(mextriang(Xchol,tmp,2) -lam*z2);
            end
            tmp = lam-lam2 -res2;
            if (tmp > 0); beta = tmp; else; beta = eps; end;
            delta = min(res,res^2/beta);
            if (delta <= tol); break; end;
         end
      end
   end
%%***************************************************************************


% THIS SOFTWARE AND ANY ACCOMPANYING DOCUMENTATION IS RELEASED "AS IS."  THE U.S. GOVERNMENT MAKES NO WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, CONCERNING THIS SOFTWARE AND ANY ACCOMPANYING DOCUMENTATION, INCLUDING, WITHOUT LIMITATION, ANY WARRANTIES OF MERCHANTABILITY OR FITNESS FOR A PARTICULAR PURPOSE.  IN NO EVENT WILL THE U.S. GOVERNMENT BE LIABLE FOR ANY DAMAGES, INCLUDING ANY LOST PROFITS, LOST SAVINGS OR OTHER INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE, OR INABILITY TO USE, THIS SOFTWARE OR ANY ACCOMPANYING DOCUMENTATION, EVEN IF INFORMED IN ADVANCE OF THE POSSIBILITY OF SUCH DAMAGES.
%
% integrate_poisspdf.m
% integrates a poisson pdf with respect to lambda from a to b
%
% 022300 tdr created
% 030400 tdr replaced global variable approach to getting x and n to binopdf.
% 030600 tdr added adjustments to a and b to help quad8 converge quickly.
% 			quad8 has trouble with long stretches of near-zero values
% 			so we try to bound the interval to avoid these regions
% 031600 tdr created integrate_poisspdf.m from integrate_binopdf.m
% 012901 tdr changed quad8 to quadl per MatLab 6 recommendation.  Turned off warnings
%   for quadl call because it thinks intervals starting at zero or ending at one
%   involve singularities and generates a warning, although accuracy is okay.
% 031501 tdr changed thres2 from 0.001 to 1.

function y = integrate_poisspdf(x,a,b)

if nargin ~= 3,
    error('Requires three input arguments (x,a,b)');
end

TRACE = 0;
lambda_hat = x;
thres = 1e-6*poisspdf_0ok(x,x); % pdf values less than this are assumed to be negligible.
thres2 = 1; % resolution of adjustments to a and b.
tiny_a = (poisspdf_0ok(x,a) < thres);
tiny_b = (poisspdf_0ok(x,b) < thres);

if tiny_a & tiny_b & (a > lambda_hat | b < lambda_hat),
	%both a and b give results near zero, with no non-zero values between them
   y = 0;
   return
end;

% defaults
new_a = a;
new_b = b;

if tiny_a,
	% a needs to be increased, but not past b or lambda_hat
	ll = a; ul = min(b, lambda_hat);
   while (abs(ll - ul) > thres2),
   	test = (ll + ul)/2;
      if poisspdf_0ok(x,test) < thres,
      	ll = test;
      else
      	ul = test;
      end;
   end;
	new_a = ll;
end;

if tiny_b,
	% b needs to be decreased, but not past a or lambda_hat
	ll = max(a, lambda_hat); ul = b;
   while (abs(ll - ul) > thres2),
   	test = (ll + ul)/2;
      if poisspdf_0ok(x,test) < thres,
      	ul = test;
      else
      	ll = test;
      end;
   end;
	new_b = ul;
end;

% call quad with increased a
%   quadl thinks interval starting at zero or ending at one might involve
%   a singularity and produces a warning, despite being accurate.
warning off;
y=quadl('poisspdf_of_lambda',new_a,new_b,[],TRACE,x);
warning on;


function VoigtSpectrGUI
%this is a GUI program i wrote about simulating absorption lineshape and
%spectrum from HITRAN output data
% this program simulate the spectrum using the HITRAN data and
% Calculating the Voigt porfile According to the paper:
%   Applied spectro. V58, 468(2004);
%   Phi= A*K(x,y)
% The lineshape depends on Guassian (vD) width and Lorentzian width(vL).
% Absorption depends on number density and absorption length, I/I0 = Exp(-aNL);
% Notice the Half Width at Half Maxium for the vD and vL inputs;

    fh = figure('Visible','off','Name','Spectrum Simulation',...
             'position',[50,50, 850, 600]);

    hax = axes('Units','pixels','Position',[60,60,500,500]);

    %construct the components
    hp = uipanel('Units','pixels','ShadowColor',[0.1,1,0.01],...
              'Position',[628 160 200 430]);

    hpb1 = uicontrol('Style','pushbutton','String','Open HITRFile',...
          'Foregroundcolor',[1 0.1 0.2],'FontWeight','bold',...
           'Position',[650 540, 110, 30],'Callback',{@fileopen_Callback});

    het1 = uicontrol('Style','edit','String','file name',...
           'Foregroundcolor',[0.2 0.4 0.1],'FontWeight','bold',...
           'Position',[650,510,110,30],'Callback',{@edittext_Callback});

    hst1 = uicontrol('Style','text', 'String','HWHM[vD vL]/cm-1',...
            'Foregroundcolor',[0.2 0.3 0.3],'FontWeight','bold',...
             'Backgroundcolor',[0.7 0.98 0.98],...
             'Position', [650 470 160 15]);

    het2 = uicontrol('Style','edit', 'String','0.003',...
            'Foregroundcolor',[0.30 0.1 1],'FontWeight','bold',...
             'Backgroundcolor',[0.97842 0.98 1],...
             'Position', [650 445 75 20]);

    het3 = uicontrol('Style','edit', 'String','0.006',...
            'Foregroundcolor',[0.30 0.1 1],'FontWeight','bold',...
             'Backgroundcolor',[1 0.99 1],...
             'Position', [735 445 75 20]);

    hst2 = uicontrol('Style','text', 'String','NumDensity/Lightpath/cm',...
            'Foregroundcolor',[0.2 0.3 0.3],'FontWeight','bold',...
             'Backgroundcolor',[0.7 0.98 1],...
             'Position', [650 410 160 15]);

    het4 = uicontrol('Style','edit', 'String','1E+10',...
            'Foregroundcolor',[0.30 0.1 1],'FontWeight','bold',...
             'Backgroundcolor',[0.97842 0.98 1],...
             'Position', [650 385 75 20]);

    het5 = uicontrol('Style','edit', 'String','1E+5',...
            'Foregroundcolor',[0.30 0.1 1],'FontWeight','bold',...
             'Backgroundcolor',[1 0.99 1],...
             'Position', [735 385 75 20]);

    hpb2 = uicontrol('Style','pushbutton','String','Simu-LineShape',...
        'Foregroundcolor',[1 0.1 0.2],'FontWeight','bold',...
           'Position',[650 340, 110, 30],'Callback',{@SimuLS_Callback});

    het6 = uicontrol('Style','edit', 'String','VoigtWidth',...
        'Foregroundcolor',[0.30 0.1 1],'FontWeight','bold',...
         'Backgroundcolor',[0.4 0.9 0.7],...
         'Position', [735 315 75 20]);

    hpb3 = uicontrol('Style','pushbutton','String','Simu-Absorption',...
        'Foregroundcolor',[1 0.1 0.2],'FontWeight','bold',...
           'Position',[650 280, 110, 30],'Callback',{@SimuAp_Callback});

    hpb4 = uicontrol('Style','pushbutton','String','Open ExpSpec',...
        'Foregroundcolor',[1 0.1 0.2],'FontWeight','bold',...
           'Position',[650 240, 110, 30],'Callback',{@ExpSpectr_Callback});

    hst4 = uicontrol('Style','text', 'String','Display Options',...
          'Backgroundcolor',[0.678402 0.98 1],...
            'Foregroundcolor',[0.2 0.3 0.3],'FontWeight','bold',...
             'Position',[650 210 130 15]);

    hpm = uicontrol('Style','popupmenu',...
                'String',{'LineShape','Absorption','CombinedPlot'},...
                'Foregroundcolor',[0.99 0.01 0.01],'FontWeight','bold',...
                'Value',1,'Position',[690 170 110 30],'Callback',{@popup_Callback});

    hp2 = uipanel('Units','pixels','ShadowColor',[0.1 0.1 1],...
               'Position',[628 40 200 100]);

    het7 = uicontrol('Style','edit','String','FileName.dat',...
             'Foregroundcolor',[0.2 0.4 0.1],'FontWeight','bold',...
             'Position',[640,105,90,25]);

    hcb1 = uicontrol('Style','checkbox','String','Line Shape',...
                'Position',[655 75 85 25],'Foregroundcolor',[0.2 0.3 0.9]);
    hcb2 = uicontrol('Style','checkbox','String','Absorption',...
                'Position',[655 50 85 25],'Foregroundcolor',[0.2 0.3 0.9]);

    hpb5 = uicontrol('Style','pushbutton','String','Save',...
              'Foregroundcolor',[0.6 0.2 0.3],'FontWeight','bold',...
              'Position',[750 66, 60, 50],'Callback',{@save_Callback});

      % to align the controls
    align([hp,het1,het6,hpb1,hpb2,hpb3,hpb4,hst1,hst2,hst4,hpm,hp2],'Center','None');

    % Initialize the GUI.
    % Change units to normalized so components resize automatically.
    set([hp,het1,het2,het3,het4,het5,het6,het7,hpb1,hpb2,hax,hst1,hst2,...
       hst4,hpm,hpb3,hpb4,hp2,hcb1,hcb2,hpb5],'Units','normalized');

    %make figure visible.
    set(fh, 'Visible','on');
    set(fh, 'NumberTitle','off')%% supress the NumverTitle

    % define some variables
    format long e ; %% define numerical format
    stp=0.0002; %% calculation(convolution) stepsize
    Dd = 0.5; %% define the edge extended beyond the spectrum region
    Nd = 1E+10; %% Number density
    L  = 1E+5; %% Optical path length cm

    gL = 0.001; %% in cm -1, pressure broadening cofffient at 50 torr.
    gD = 0.003; %% 0.0032 doppler broadening in cm-1
    SgmvTot= zeros(1,10);  %% initiate the arrays
    expx = zeros(1,10);
    expy = zeros(1,10);
    A = zeros(1,10);
    dtfl = zeros(1,10);
    grd = zeros(1,10); %% default grid

    %%file open callback
    function fileopen_Callback(source,eventdata)
        % **********uiputfile for open the save file dialog*********

        [flnm, flpth] = uigetfile({'*.out','All Files' },...
                         'Select HITRAN outputfile','fl.out');
        %specify application data, so it can be used by other objects;
        % setappdata(h,'name',value)
        setappdata(hpb1,'fname',flnm);

        set(het1, 'String',flnm);
        % set the edit text box value.

     end

  % read in the HITRAN data and simulate Line Shape spectrum
    function SimuLS_Callback(source, eventdata)%callback is just a regular func

        format long e ;
        % get the value of specified data
        % value = getappdata(h,name)
        % values = getappdata(h)
        flnm = getappdata(hpb1,'fname');

        fid = fopen(flnm);
        C = textscan(fid,...
        '%d%f%f%u%f%f%f%f %d%d%d%d%d %d%d%d%d %d%d%d%d %d%d%d%d%d%d%d%d%d%d%d%d%d ');
        fclose(fid);  %% returned C is a cell

        N = length(C{2}); %% Number of lines/ Cell length
        %% define a grid
        rangeL= -Dd + min(C{2});  %% spectrum region: First line minus 1 cm-1
        rangeH = Dd + max(C{2}); %% Last line position + 1 cm-1
        grd=[rangeL:stp:rangeH];  %% define the grid

        v = grd;
        gD = str2num(get(het2, 'string'));
        gL = str2num(get(het3, 'string'));
        gV = 0.5346*gL + sqrt(0.2166*gL^2 + gD^2); %% Voigt profile half width
        x = gL/gV;
        SgmvTot = 0;

        for i=1:N  %% calculate the line shape for each peak

            v0(i) = C{2}(i);
            S(i) = C{3}(i);
            y = abs(v-v0(i))/gV;
            Sgmv0(i) = S(i)/(2*gV*(1.065 + 0.447*x + 0.058*x^2));
            Sgmv = Sgmv0(i)*((1-x)*exp(-0.693.*y.^2) + (x./(1+y.^2)) + ...
              0.016*(1-x)*x*(exp(-0.0841.*y.^2.25)-1./(1 + 0.021.*y.^2.25)));

         SgmvTot = SgmvTot + Sgmv;

        end

        plot(grd,SgmvTot,'-b.');
        xlabel('WaveLength in cm-1');
        ylabel('Effective absorption Cross Section in cm2');

        set(hpm, 'Value',1)% change the popup menu settings;

        %% display the Voigt width;
        strg = sprintf('gV= %1.4f',gV);
        set(het6,'string',strg);
        set(het6,'Backgroundcolor',[0.9 0.9 0.9])

        %%figure, plot(v,A);
        %%ylabel('Absorption')
    end

    function SimuAp_Callback(source, eventdata)
        %% calculate absorption

        Nd = str2num(get(het4, 'string')); %% Number density in molecule/cm3
        L = str2num(get(het5, 'string')); %% effective length path in cm
        A = 1 - exp(-SgmvTot.*Nd*L); %% I/I0 = Exp(-aNL);

        plot(grd,A,'-m');
        xlabel('WaveLength in cm-1');
        set(hpm, 'Value',2)% change the popup menu settings;

    end

    %slice X callback
    function ExpSpectr_Callback(source, eventdata)

        format long e ;
        [flnm, flpth] = uigetfile({'*.dat','All Files' },'Select Exp. file','ExpSpectr.dat');
        fid = fopen(flnm, 'r');
        a = fscanf(fid, '%f %f', [2 inf]);    % It has two rows now.
        a = a';
        fclose(fid)
        expx = a(:,1);
        expy = a(:,2);
        hold on;
        plot3 = plot(expx,expy,'-.b');
        ylabel('Absorption')

        hold off;

    end

   %popup menu callback
    function popup_Callback(source,eventdata)
        str = get(source, 'String');
        val = get(hpm, 'Value'); %use either function handle 'hpm' or 'source';
        % the string of popup menu is a cell array {}, cell{num} returns num th
        % element of the cell.
        switch val
        case 1  %'showImage'
             %disp(str{val});
                hold off;
                plot(grd,SgmvTot,'-b.');
                xlabel('WaveLength in cm-1')
                ylabel('Effective absorption Cross Section in cm2')

        case 2
                hold off;
                plot(grd,A,'-m');

        case 3 %combined plot
                hold on;
                plot(expx,expy,'-.b');
                ylabel('Absorption')

                hold off;

        end
    end

    function save_Callback(source, eventdata)

        if get(hcb1,'Value')> 0.9 % it should be 1.0
            dtfl = [grd;SgmvTot]; %% create a data matrix for data saving
            fid = fopen(get(het7, 'string'), 'wt');
            fprintf(fid, '%6.4f %e\n', dtfl); %% set the data format
            fclose(fid);

        end

        if get(hcb2,'Value')> 0.9 % it should be 1.0

           dtfl = [grd;A]; %% create a data matrix for data saving
           fid = fopen(get(het7, 'string'), 'wt');
           fprintf(fid, '%6.4f %5.6f\n', dtfl); %% set the data format
           fclose(fid);
        end

        if get(hcb1,'Value')> 0.9 && get(hcb2,'Value')> 0.9 % it should be 1.0

            dtfl = [grd;SgmvTot;A]; %% create a data matrix for data saving

            fid = fopen(get(het7, 'string'), 'wt');
            fprintf(fid, '%6.4f %e %5.6f\n', dtfl); %% set the data format
            fclose(fid);

        end
    end

%%% read in the HITRAN file to obtain line positions and intensities.
end


function jac = autoJac(fun,x0,varargin)
%AUTOJAC  Returns a Automatically Differentiated (AD) Jacobian
%
%   jac = autoJac(fun,x0) uses the user supplied Matlab function to
%   generate the Jacobian using automatic differentiation. The user
%   function must be a Matlab function and must not call external code or
%   class / toolbox functions. If your function breaks any of these
%   conditions consider using mklJac instead.
%
%   The underlying AD algorithm is adiff by William McIlhagga and its
%   documentation pdf is provided in the Differentiation folder. See the
%   BSD license below the code.

%   Copyright (C) 2011 Jonathan Currie (I2C2)

if(~isa(fun,'function_handle'))
    error('Fun should be a function handle!');
end

[~,jac] = adiffget(fun(adiff(x0),varargin{:}));

end

% Copyright (c) 2010, William McIlhagga
% 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 OWNER 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.


% std_stat() - compute statistics for ERP/spectral traces or ERSP/ITC images
%              of a component or channel cluster in a STUDY.
% Usage:
%          >> std_stat( data, 'key', 'val', ...)
% Inputs:
%  data  -  [cell array] mean data for each subject group and/or data
%           condition. For example, to compute mean ERPs statistics from a
%           STUDY for epochs of 800 frames in two conditions from three
%           groups of 12 subjects:
%
%           >> data = { [800x12] [800x12] [800x12];... % 3 groups, cond 1
%                       [800x12] [800x12] [800x12] };  % 3 groups, cond 2
%           >> pcond = std_stat(data, 'condstats', 'on');
%
%           By default, parametric statistics are computed across subjects
%           in the three groups. See below and >> help statcond
%           for more information about the statistical computations.
%
% Statistics options:
%  'groupstats'  - ['on'|'off'] Compute (or not) statistics across groups.
%                  {default: 'off'}
%  'condstats'   - ['on'|'off'] Compute (or not) statistics across groups.
%                  {default: 'off'}
%  'statistics'  - ['param'|'perm'] Type of statistics to use: 'param' for
%                  parametric; 'perm' for permutations {default: 'param'}
%  'naccu'       - [integer] Number of surrogate averges fo accumulate when
%                  computing permutation-based statistics. For example, to
%                  test p<0.01 use naccu>=200; for p<0.001, use naccu>=2000.
%                  If a non-NaN 'threshold' is set (see below) and 'naccu'
%                  is too low, it will be automatically increased. This
%                  keyword available only from the command line {default:500}
%  'threshold'   - [NaN|p-value] threshold for computing p-value. In this
%                  function, it is only used to compute naccu above. NaN
%                  means that no threshold has been set.
%  'mcorrect'    - ['none'|'fdr'] apply correcting for multiple comparisons.
%
% Outputs:
%  pcond        - [cell] condition pvalues or mask (0 or 1). One element per group.
%  pgroup       - [cell] group pvalues or mask (0 or 1). One element per condition.
%  pinter       - [cell] three elements, condition pvalues (group pooled),
%                 group pvalues (condition pooled) and interaction pvalues.
%
% Author: Arnaud Delorme, CERCO, CNRS, 2006-
%
% See also: statcond()

%  'statmode'    - ['subjects'|'trials'] standard statistics are
%                  'subjects' where the statistics is performed accross
%                  the mean ERSP (or ITC) of single subjects. For 'trials'
%                  statistics, the single-trial data epochs of all subjects
%                  are pooled together. This requires that they were
%                  saved on disk using option 'savetrials', 'on' at the time
%                  of computation. Note that these single-trial data
%                  may use several GB of disk space and that computation
%                  of 'trials' statistics requires a lot of RAM.

function [pcond, pgroup, pinter] = std_stat(data, varargin)

pgroup = {};
pcond  = {};
pinter = {};
if nargin < 1
    help std_stat;
    return;
end;

% decode inputs
% -------------
opt = {};
if nargin > 1
    if isstruct(varargin{1})
        g = struct(varargin{2:end});
        if isfield(g, 'condstats'),  varargin{1} = rmfield(varargin{1}, 'condstats'); end;
        if isfield(g, 'groupstats'), varargin{1} = rmfield(varargin{1}, 'groupstats'); end;
        tmpparams      = fieldnames(varargin{1}); tmpparams = tmpparams';
        tmpparams(2,:) = struct2cell(varargin{1});
        varargin       = { tmpparams{:} varargin{2:end} };
    end;
end;
opt = finputcheck( varargin, { 'threshold'   'real'    []               NaN;
                               'mcorrect'    'string'  { 'none' 'fdr' } 'none';
                               'naccu'       'integer' []               [];
                               'groupstats'  'string'  { 'on' 'off' }   'off';
                               'paired'      'cell'    { 'on' 'off' }   { 'on' 'on' };
                               'condstats'   'string'  { 'on' 'off' }   'off';
                               'statistics'  'string'  { 'param' 'perm' 'bootstrap' }       'param' }, ...
                               'std_stat', 'ignore');

if isstr(opt), error(opt); end;
if ~isnan(opt.threshold(1)) && isempty(opt.naccu), opt.naccu = 1/opt.threshold(end)*2; end;
if any(any(cellfun('size', data, 2)==1)), opt.groupstats = 'off'; opt.condstats = 'off'; end;
if strcmpi(opt.mcorrect, 'fdr'), opt.naccu = opt.naccu*20; end;
if isempty(opt.naccu), opt.naccu = 2000; end;
if strcmpi(opt.statistics, 'param') && ~isreal(data{1})
    fprintf('*** Cannot use parametric statistics for single-trial ITC significance ***\n');
    return;
end;
nc = size(data,1);
ng = size(data,2);

% compute significance mask
% --------------------------
if strcmpi(opt.condstats, 'on') && nc > 1
    for g = 1:ng
        [F df pval] = statcond(data(:,g), 'mode', opt.statistics, 'naccu', opt.naccu, 'paired', opt.paired{1});
        pcond{g} = squeeze(pval);
    end;
else
    pcond = {};
end;
if strcmpi(opt.groupstats, 'on') && ng > 1
    for c = 1:nc
        [F df pval] = statcond(data(c,:), 'mode', opt.statistics, 'naccu', opt.naccu, 'paired', opt.paired{2});
        pgroup{c} = squeeze(pval);
    end;
else
    pgroup = {};
end;
if ( strcmpi(opt.groupstats, 'on') && strcmpi(opt.condstats, 'on') ) & ng > 1 & nc > 1
    opt.paired = sort(opt.paired); % put 'off' first if present
    [F df pval] = statcond(data, 'mode', opt.statistics, 'naccu', opt.naccu, 'paired', opt.paired{1});
    for index = 1:length(pval)
        pinter{index} = squeeze(pval{index});
    end;
else
    pinter = {};
end;

if ~isempty(opt.groupstats) || ~isempty(opt.condstats)
    if strcmpi(opt.mcorrect, 'fdr'),
        disp('Applying FDR correction for multiple comparisons');
        for ind = 1:length(pcond),  pcond{ind} = fdr( pcond{ind} ); end;
        for ind = 1:length(pgroup), pgroup{ind} = fdr( pgroup{ind} ); end;
        if ~isempty(pinter),
            pinter{1} = fdr(pinter{1});
            pinter{2} = fdr(pinter{2});
            pinter{3} = fdr(pinter{3});
        end;
    end;
    if ~isnan(opt.threshold)
        for ind = 1:length(pcond),  pcond{ind}  = applythreshold(pcond{ind},  opt.threshold); end;
        for ind = 1:length(pgroup), pgroup{ind} = applythreshold(pgroup{ind}, opt.threshold); end;
        for ind = 1:length(pinter), pinter{ind} = applythreshold(pinter{ind}, opt.threshold); end;
    end;
end;

function newdata = applythreshold(data, threshold)
    threshold = sort(threshold);
    newdata = zeros(size(data));
    for index = 1:length(threshold)
        inds = data < threshold(index);
        data(inds)    = 1;
        newdata(inds) = length(threshold)-index+1;
    end;


2*((x - x2)*((x - x1)^2 + (y - y1)^2)*(-r12^2 + (r11 -
    ((x - x1)^2 + (y - y1)^2)^(1/2))^2 + (z - z1)^2)*(-r22^2 + (y -
    y2)^2 + (r21 - ((x - x2)^2 + (z - z2)^2)^(1/2))^2) + (x -
    x2)*((x - x1)^2 + (y - y1)^2)*(-r12^2 + (r11 -
    ((x - x1)^2 + (y - y1)^2)^(1/2))^2 + (z - z1)^2)*(-r21 +
    ((x - x2)^2 + (z - z2)^2)^(1/2))*((x - x2)^2 + (z -
    z2)^2)^(1/2) + (x - x1)*(-r11 +
    ((x - x1)^2 + (y - y1)^2)^(1/2))*((x - x1)^2 + (y -
    y1)^2)^(1/2)*(-r22^2 + (y - y2)^2 + (r21 -
    ((x - x2)^2 + (z - z2)^2)^(1/2))^2)*((x - x2)^2 + (z -
    z2)^2) + (x - x1)*(-r12^2 + (r11 - ((x - x1)^2 + (y - y1)^2)^(1/2))^2 +
    (z - z1)^2)*(-r22^2 + (y - y2)^2 + (r21 -
    ((x - x2)^2 + (z - z2)^2)^(1/2))^(2))*((x - x2)^2 + (z - z2)^2));

2*(((x - x1)^2 + (y - y1)^2)*(y -
    y2)*(-r12^2 + (r11 - ((x - x1)^2 + (y - y1)^2)^(1/2))^2 +
    (z - z1)^2) + (-r11 + ((x - x1)^2 + (y - y1)^2)^(1/2))*
    ((x - x1)^2 + (y - y1)^2)^(1/2)*(y - y1)*(-r22^2 +
    (y - y2)^2 + (r21 - ((x - x2)^2 + (z - z2)^2)^(1/2))^2) +
    (y - y1)*(-r12^2 + (r11 - ((x - x1)^2 + (y - y1)^2)^(1/2))^2 +
    (z - z1)^2)*(-r22^2 + (y - y2)^2 + (r21 -
    ((x - x2)^2 + (z - z2)^2)^(1/2))^2))*((x - x2)^2 + (z - z2)^2);

2*((x - x1)^2 + (y - y1)^2)*((z - z1)*(-r22^2 + (y - y2)^2 +
    (r21 - ((x - x2)^2 + (z - z2)^2)^(1/2))^2)*((x - x2)^2 +
    (z - z2)^2) + (-r12^2 + (r11 - ((x - x1)^2 + (y - y1)^2)^(1/2))^2 +
    (z - z1)^2)*(-r22^2 + (y - y2)^2 + (r21 - ((x - x2)^2 + (z - z2)^2)^(1/2))^2)*
    (z - z2) + (-r12^2 + (r11 - ((x - x1)^2 + (y - y1)^2)^(1/2))^2+
    (z - z1)^2)*(-r21 + ((x - x2)^2 + (z - z2)^2)^(1/2))*
    ((x - x2)^2 + (z - z2)^2)^(1/2)*(z - z2));


function [g1, g2] = simXsimKernDiagGradient(simKern1, simKern2, t, covDiag)

% SIMXSIMKERNDIAGGRADIENT Gradient for the diagonal between two SIM kernels.
% FORMAT
% DESC computes cross gradient of parameters in the diagonal of a cross
% kernel between two sim kernels for the multiple output kernel.
% ARG simKern1 : the kernel structure associated with the first SIM
% kernel.
% ARG simKern2 : the kernel structure associated with the second SIM
% kernel.
% ARG t : inputs for which kernel is to be computed.
% ARG covDiag : gradient of the objective function with respect to
% the elements of the cross kernel matrix.
% RETURN g1 : gradient of the parameters of the first kernel, for
% ordering see simKernExtractParam.
% RETURN g2 : gradient of the parameters of the second kernel, for
% ordering see simKernExtractParam.
%
% SEEALSO : simXsimKernDiagCompute.m, simKernParamInit, simKernExtractParam
%
% COPYRIGHT : Mauricio A. Alvarez, 2010

% KERN

if size(t, 2) > 1
  error('Input can only have one column');
end

if simKern1.inverseWidth ~= simKern2.inverseWidth
  error('Kernels cannot be cross combined if they have different inverse widths.')
end
% The normalisation in the SIM kernel arises from the use of the normalised
% version of the RBF kernel. Hence, both have to be normalised or not.
if ~isfield(simKern1, 'isNormalised')
    isSim1Normalised = false;
else
    isSim1Normalised = simKern1.isNormalised;
end
if ~isfield(simKern2, 'isNormalised')
    isSim2Normalised = false;
else
    isSim2Normalised = simKern2.isNormalised;
end
if isSim1Normalised ~= isSim2Normalised
    error('Both SIM kernels have to be either normalised or not.');
end

sigma = sqrt(2/simKern1.inverseWidth);

if (~simKern1.isStationary || ~ simKern2.isStationary)
    [h1, dh1_dD1, dh1_dD2, dh1_dsigma] = ...
        simXsimComputeDiagH(t, simKern1.decay, simKern2.decay, simKern1.delay, simKern2.delay, sigma);
else
    [h1, dh1_dD1, dh1_dD2, dh1_dsigma] = ...
        simXsimComputeDiagHStat(t, simKern1.decay, simKern2.decay, simKern1.delay, simKern2.delay, sigma);
end

% Avoid making the expensive call twice unless really necessary
if ((simKern1.decay == simKern2.decay) && (simKern1.delay == simKern2.delay)),
  h2 = h1;
  dh2_dD2 = dh1_dD1;
  dh2_dD1 = dh1_dD2;
  dh2_dsigma = dh1_dsigma;
elseif (~simKern1.isStationary) || (~simKern2.isStationary)
  [h2, dh2_dD2, dh2_dD1, dh2_dsigma] = ...
      simXsimComputeDiagH(t, simKern2.decay, simKern1.decay, simKern2.delay, simKern1.delay, sigma);
else
    [h2, dh2_dD2, dh2_dD1, dh2_dsigma] = ...
      simXsimComputeDiagHStat(t, simKern2.decay, simKern1.decay, simKern2.delay, simKern1.delay, sigma);
end

dK_dsigma = dh1_dsigma + dh2_dsigma;

if isfield(simKern1, 'isVarS') && (simKern1.isVarS)
    K = 0.5 * (h1 + h2);
    if ~isSim1Normalised
        K = sqrt(pi) * K;
        dk_dD1 = (sum(covDiag.*dh1_dD1) + sum(covDiag.*dh2_dD1))*0.5*sqrt(pi)*sigma;
        dk_dD2 = (sum(covDiag.*dh1_dD2) + sum(covDiag.*dh2_dD2))*0.5*sqrt(pi)*sigma;
        dk_dsigma = sum(covDiag.*(dK_dsigma*0.5*sqrt(pi)*sigma + K));
    else
        dk_dD1 = (sum(covDiag.*dh1_dD1) + sum(covDiag.*dh2_dD1))*0.5;
        dk_dD2 = (sum(covDiag.*dh1_dD2) + sum(covDiag.*dh2_dD2))*0.5;
        dk_dsigma = 0.5 * sum(covDiag.*dK_dsigma);
    end
    dk_dinvWidth = -0.5*sqrt(2)/(simKern1.inverseWidth* ...
        sqrt(simKern1.inverseWidth))*dk_dsigma;
    % only pass the gradient with respect to the inverse width to one
    % of the gradient vectors ... otherwise it is counted twice.
    g1 = real([dk_dD1 dk_dinvWidth]);
    g2 = real([dk_dD2 0]);
else
    if isfield(simKern1, 'isNegativeS') && (simKern1.isNegativeS == true)
        C1 = simKern1.sensitivity;
        C2 = simKern2.sensitivity;
    else
        C1 = sqrt(simKern1.variance);
        C2 = sqrt(simKern2.variance);
    end
    K = 0.5 * (h1 + h2);
    var2 = C1*C2;
    if ~isSim1Normalised
        K = sqrt(pi) * K;
        dk_dD1 = (sum(covDiag.*dh1_dD1) + sum(covDiag.*dh2_dD1))*0.5*sqrt(pi)*sigma*var2;
        dk_dD2 = (sum(covDiag.*dh1_dD2) + sum(covDiag.*dh2_dD2))*0.5*sqrt(pi)*sigma*var2;
        dk_dsigma = sum(covDiag.*(dK_dsigma*0.5*sqrt(pi)*sigma + K))*var2;
        dk_dC1 = sigma * C2 * sum(covDiag.*K);
        dk_dC2 = sigma * C1 * sum(covDiag.*K);
    else
        dk_dD1 = (sum(covDiag.*dh1_dD1) + sum(covDiag.*dh2_dD1))*0.5*var2;
        dk_dD2 = (sum(covDiag.*dh1_dD2) + sum(covDiag.*dh2_dD2))*0.5*var2;
        dk_dsigma = 0.5 * var2 * sum(covDiag.*dK_dsigma);
        dk_dC1 = C2 * sum(covDiag.*K);
        dk_dC2 = C1 * sum(covDiag.*K);
    end
    if isfield(simKern1, 'isNegativeS') && simKern1.isNegativeS
        dk_dSim1Variance = dk_dC1;
    else
        dk_dSim1Variance = dk_dC1*0.5/C1;
    end
    if isfield(simKern2, 'isNegativeS') && simKern2.isNegativeS
        dk_dSim2Variance = dk_dC2;
    else
        dk_dSim2Variance = dk_dC2*0.5/C2;
    end
    dk_dinvWidth = -0.5*sqrt(2)/(simKern1.inverseWidth* ...
        sqrt(simKern1.inverseWidth))*dk_dsigma;
    % only pass the gradient with respect to the inverse width to one
    % of the gradient vectors ... otherwise it is counted twice.
    g1 = real([dk_dD1 dk_dinvWidth dk_dSim1Variance]);
    g2 = real([dk_dD2 0 dk_dSim2Variance]);
end


function p06_story ( )

%*****************************************************************************80
%
%% P06_STORY prints the "story" for problem p06.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    01 August 2011
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Carl DeBoor, John Rice,
%    Least-squares cubic spline approximation II - variable knots.
%    Technical Report CSD TR 21,
%    Purdue University, Lafayette, Indiana, 1968.
%
%    Carl DeBoor,
%    A Practical Guide to Splines,
%    Springer, 2001,
%    ISBN: 0387953663,
%    LC: QA1.A647.v27.
%
%  Parameters:
%
%    None
%
  fprintf ( 1, '\n' );
  fprintf ( 1, '  The data is due to deBoor and Rice.\n' );
  fprintf ( 1, '  The data represents a temperature dependent property of titanium.\n' );
  fprintf ( 1, '  The data has been used extensively as an example in spline\n' );
  fprintf ( 1, '  approximation with variably-spaced knots.\n' );
  fprintf ( 1, '  DeBoor considers two sets of knots:\n' );
  fprintf ( 1, '  (595,675,755,835,915,995,1075)\n' );
  fprintf ( 1, '  and\n' );
  fprintf ( 1, '  (595,725,850,910,975,1040,1075).\n' );

  return
end


function Obs = projectLmk(Rob,Sen,Lmk,Obs,Opt)

% PROJECTLMK  Project landmark estimate into sensor's measurement space.
%   Obs = PROJECTLMK(Rob,Sen,Lmk,Obs) projects the landmark Lmk into sensor
%   Sen mounted on robot Rob, and updates the information of the
%   observation structure Obs. The observation model is determined from
%   Sen.type and Lmk.type. It is an error if no model exists for the chosen
%   Sen-Lmk pair.
%
%   The updated fields in Obs are:
%       .sid        % sensor ID
%       .lid        % landmark ID
%       .ltype      % landmark type
%       .vis        % flag: true if landmark is visible
%       .meas.R     % measurement noise cov. matrix
%       .exp.e      % expectation's mean
%       .exp.E      % expectation's covariances matrix
%       .exp.um     % expectation's uncertainty measure
%       .Jac.E_r    % Jacobian wrt robot frame
%       .Jac.E_s    % Jacobian wrt sensor frame
%       .Jac.E_l    % Jacobian wrt landmark state
%
%   See also OBSERVEKNOWNLMKS.

%   Copyright 2008-2009 Joan Sola @ LAAS-CNRS.

global Map

% PREVIOUS TASKS
% get landmark range and mean
lr = Lmk.state.r ;        % lmk range in Map
switch Map.type
    case 'ekf'
        l  = Map.x(lr);   % lmk mean
    case 'graph'
        l = Lmk.state.x;  % lmk mean
    otherwise
        error('??? Unknown Map type ''%s''.',Map.type)
end

% PROJECTION FUNCTION
% explore all sensor and landmark types
switch Sen.type

    case {'pinHole'} % camera pinHole

        switch Lmk.type

            case {'eucPnt'} % euclidean point

                % Point3D --> pixel -(value and Jacobians)-
                [e, depth, E_rf, E_sf, E_k, E_d, E_l] = ...
                    projEucPntIntoPinHoleOnRob( ...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    Sen.par.d, ...
                    l) ;

                vis = isVisible(e,depth,Sen.par.imSize);

            case {'idpPnt'} % inverse depth point

                % IDP --> pixel -(value and Jacobians)-
                [e, depth, E_rf, E_sf, E_k, E_d, E_l] = ...
                    projIdpPntIntoPinHoleOnRob( ...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    Sen.par.d, ...
                    l) ;

                vis = isVisible(e,depth,Sen.par.imSize);

            case {'hmgPnt'} % euclidean point

                % Point3D --> pixel -(value and Jacobians)-
                [e, depth, E_rf, E_sf, E_k, E_d, E_l] = ...
                    projHmgPntIntoPinHoleOnRob( ...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    Sen.par.d, ...
                    l) ;

                vis = isVisible(e,depth,Sen.par.imSize);

            case {'ahmPnt'}

                % Point3D --> pixel -(value and Jacobians)-
                [e, depth, E_rf, E_sf, E_k, E_d, E_l] = ...
                    projAhmPntIntoPinHoleOnRob( ...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    Sen.par.d, ...
                    l);

                vis = isVisible(e,depth,Sen.par.imSize);

            case {'fhmPnt'}

                % Point3D --> pixel -(value and Jacobians)-
                [e, depth, E_rf, E_sf, E_k, E_d, E_l] = ...
                    projFhmPntIntoPinHoleOnRob( ...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    Sen.par.d, ...
                    l);

                vis = isVisible(e,depth,Sen.par.imSize);

            case {'plkLin'}

                % Plucker line --> homogeneous line (value and Jacs)
                [e, v, E_rf, E_sf, E_k, E_l] = ...
                    projPlkLinIntoPinHoleOnRob( ...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    l); % expectation e is a homogeneous line

                % normalize wrt director vector e(1:2):
                ine12 = 1/norm(e(1:2));
                e     = e    * ine12;
                E_rf  = E_rf * ine12;
                E_sf  = E_sf * ine12;
                % E_k   = E_k  * ine12;
                E_l   = E_l  * ine12;

                % 3d Segment from Plucker line and abscissas
                [si,SI_l] = pluckerSegment(l,[Lmk.par.endp.t]);

                % projected segment
                [s, d, S_rf, S_sf, S_k, S_si] = projSegLinIntoPinHoleOnRob(...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    si);

                % segment visibility
                [s,vis] = visibleSegment(s,d,Sen.par.imSize);

            case 'aplLin'

                % Anchored Plucker line --> homogeneous line (value and Jacs)
                [e, v, E_rf, E_sf, E_k, E_l] = ...
                    projAplLinIntoPinHoleOnRob( ...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    l); % expectation e is a homogeneous line

                % normalize wrt director vector e(1:2):
                ine12 = 1/norm(e(1:2));
                e     = e    * ine12;
                E_rf  = E_rf * ine12;
                E_sf  = E_sf * ine12;
                % E_k   = E_k  * ine12;
                E_l   = E_l  * ine12;

                % 3d Segment from Plucker line and abscissas
                [si,SI_l] = aPluckerSegment(l,[Lmk.par.endp.t]);

                % projected segment
                [s, d, S_rf, S_sf, S_k, S_si] = projSegLinIntoPinHoleOnRob(...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    si);

                % segment visibility
                [s,vis] = visibleSegment(s,d,Sen.par.imSize);

            case 'idpLin'

                % IDP line --> homogeneous line (value and Jacs)
                [s, v, S_rf, S_sf, S_k, S_l] = ...
                    projIdpLinIntoPinHoleOnRob( ...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    l); % expectation s is a 2d segment
                [e, E_s] = seg2hmgLin(s); % expectation e is a homogeneous line
                E_rf = E_s*S_rf;
                E_sf = E_s*S_sf;
                E_l  = E_s*S_l;

                % normalize wrt director vector e(1:2):
                ine12 = 1/norm(e(1:2));
                e     = e    * ine12;
                E_rf  = E_rf * ine12;
                E_sf  = E_sf * ine12;
                % E_k   = E_k  * ine12;
                E_l   = E_l  * ine12;

                % 3d Segment from Plucker line and abscissas
                [si,SI_l] = idpLinSegment(l,[Lmk.par.endp.t]);

                % projected segment
                [s, d, S_rf, S_sf, S_k, S_si] = projSegLinIntoPinHoleOnRob(...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    si);

                % segment visibility
                [s,vis] = visibleSegment(s,d,Sen.par.imSize,0,Opt.obs.lines.minLength);

            case 'hmgLin'

                % HMG line --> homogeneous line (value and Jacs)
                [s, v, S_rf, S_sf, S_k, S_l] = ...
                    projHmgLinIntoPinHoleOnRob( ...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    l); % expectation s is a 2d segment
                [e, E_s] = seg2hmgLin(s); % expectation e is a homogeneous line
                E_rf = E_s*S_rf;
                E_sf = E_s*S_sf;
                E_l  = E_s*S_l;

                % normalize wrt director vector e(1:2):
                ine12 = 1/norm(e(1:2));
                e     = e    * ine12;
                E_rf  = E_rf * ine12;
                E_sf  = E_sf * ine12;
                % E_k   = E_k  * ine12;
                E_l   = E_l  * ine12;

                % 3d Segment from Plucker line and abscissas
                [si,SI_l] = hmgLinSegment(l,[Lmk.par.endp.t]);

                % projected segment
                [s, d, S_rf, S_sf, S_k, S_si] = projSegLinIntoPinHoleOnRob(...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    si);

                % segment visibility
                [s,vis] = visibleSegment(s,d,Sen.par.imSize,0,Opt.obs.lines.minLength);

            case 'ahmLin'

                % AHM line --> homogeneous line (value and Jacs)
                [s, v, S_rf, S_sf, S_k, S_l] = ...
                    projAhmLinIntoPinHoleOnRob( ...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    l); % expectation s is a 2d segment
                [e, E_s] = seg2hmgLin(s); % expectation e is a homogeneous line
                E_rf = E_s*S_rf;
                E_sf = E_s*S_sf;
                E_l  = E_s*S_l;

                % normalize wrt director vector e(1:2):
                ine12 = 1/norm(e(1:2));
                e     = e    * ine12;
                E_rf  = E_rf * ine12;
                E_sf  = E_sf * ine12;
                % E_k   = E_k  * ine12;
                E_l   = E_l  * ine12;

                % 3d Segment from Plucker line and abscissas
                [si,SI_l] = ahmLinSegment(l,[Lmk.par.endp.t]);

                % projected segment
                [s, d, S_rf, S_sf, S_k, S_si] = projSegLinIntoPinHoleOnRob(...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    si);

                % segment visibility
                [s,vis] = visibleSegment(s,d,Sen.par.imSize,0,Opt.obs.lines.minLength);

            otherwise % unknown landmark type for pin hole sensor
                error('??? Unknown landmark type ''%s'' for sensor ''%s''.', Lmk.type, Sen.type);

        end

    case 'pinHoleDepth'  % Pin hole sensor with depth information
        switch Lmk.type
            case 'eucPnt'

                % Point3D --> pixel+depth -(value and Jacobians)-
                [e, E_rf, E_sf, E_k, E_d, E_l] = ...
                    projEucPntIntoPhdOnRob( ...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    Sen.par.d, ...
                    l) ;

                vis = isVisible(e(1:2,:),e(3,:),Sen.par.imSize);

            otherwise
                error('??? Unknown landmark type ''%s'' for sensor ''%s''.',Lmk.type,Sen.type);
        end

    case {'omniCam'} % Omnidirectional camera

        switch Lmk.type

            case {'eucPnt'} % euclidean point

                % Point3D --> pixel -(value and Jacobians)-
                [e, depth, E_rf, E_sf, E_k, E_d, E_l] = ...
                    projEucPntIntoOmniCamOnRob( ...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    Sen.par.d, ...
                    l) ;

                vis = isVisible(e,depth,Sen.par.imSize);

            case {'ahmPnt'}

                % Point3D --> pixel -(value and Jacobians)-
                [e, depth, E_rf, E_sf, E_k, E_d, E_l] = ...
                    projAhmPntIntoOmniCamOnRob( ...
                    Rob.frame, ...
                    Sen.frame, ...
                    Sen.par.k, ...
                    Sen.par.d, ...
                    l);

                vis = isVisible(e,depth,Sen.par.imSize);

            otherwise % unknown landmark type for pin hole sensor
                error('??? Unknown landmark type ''%s'' for sensor ''%s''.',Lmk.type,Sen.type);

        end

    otherwise % unknown Sensor type
        error('??? Unknown sensor type ''%s''.',Sen.type);

end % sensor type

% COVARIANCES
if strcmp(Map.type,'ekf')

    % Rob-Sen-Lmk range and Jacobian
    if Sen.frameInMap
        rslr  = [Rob.frame.r ; Sen.frame.r ; lr]; % range of robot, sensor, and landmark
        E_rsl = [E_rf E_sf E_l];
    else
        rslr  = [Rob.frame.r ; lr];               % range of robot and landmark
        E_rsl = [E_rf E_l];
    end

    % Expectation covariances matrix
    E = E_rsl*Map.P(rslr,rslr)*E_rsl' ;

else
    E = [];
end

% Other parameters
switch Lmk.type(4:6)
    case 'Lin'
        % for lines, project endpoints with covariances:

        % compute endpoints
        Obs.par.endp(1).e = s(1:2);
        Obs.par.endp(2).e = s(3:4);

        if Map.type == 'ekf'
            % Rob-Sen-Lmk Jacobian of projected segment
            if Sen.frameInMap
                S_rsl = [S_rf S_sf S_si*SI_l];
            else
                S_rsl = [S_rf S_si*SI_l];
            end
            % compute covariances
            S = S_rsl*Map.P(rslr,rslr)*S_rsl'; % segment covariance
            Obs.par.endp(1).E = S(1:2,1:2);
            Obs.par.endp(2).E = S(3:4,3:4);
        end

end

% UPDATE OBS STRUCTURE
Obs.sid     = Sen.id ;
Obs.lid     = Lmk.id ;
Obs.ltype   = Lmk.type ;
Obs.vis     = vis ;
Obs.exp.e   = e ;
Obs.exp.E   = E ;
Obs.exp.um  = det(E);  % uncertainty measure proportional to ellipsoid area
Obs.Jac.E_r = E_rf;
Obs.Jac.E_s = E_sf;
Obs.Jac.E_l = E_l;

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

%   # START GPL LICENSE

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

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

%   # END GPL LICENSE