function b = cgbsl ( abd, lda, n, ml, mu, ipvt, b, job )

%*****************************************************************************80
%
%% CGBSL solves a complex band system factored by CGBCO or CGBFA.
%
%  Discussion:
%
%    CGBSL can solve A * X = B or hermitan ( A ) * X = B.
%
%    A division by zero will occur if the input factor contains a
%    zero on the diagonal.  Technically this indicates singularity
%    but it is often caused by improper arguments or improper
%    setting of LDA.  It will not occur if the subroutines are
%    called correctly and if CGBCO has set 0.0 < RCOND
%    or CGBFA has set INFO = 0.
%
%    To compute inverse ( A ) * C where C is a matrix with P columns:
%
%      call cgbco(abd,lda,n,ml,mu,ipvt,rcond,z)
%
%      if ( rcond is not too small ) then
%        do j = 1, p
%          call cgbsl(abd,lda,n,ml,mu,ipvt,c(1,j),0)
%        end do
%      end if
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    29 April 2007
%
%  Author:
%
%    MATLAB version by John Burkardt
%
%  Reference:
%
%    Jack Dongarra, Cleve Moler, Jim Bunch and Pete Stewart,
%    LINPACK User's Guide,
%    SIAM, (Society for Industrial and Applied Mathematics),
%    3600 University City Science Center,
%    Philadelphia, PA, 19104-2688.
%
%  Parameters:
%
%    Input, complex ABD(LDA,N), the output from CGBCO or CGBFA.
%
%    Input, integer LDA, the leading dimension of ABD.
%
%    Input, integer N, the order of the matrix.
%
%    Input, integer ML, the number of diagonals below the main diagonal.
%
%    Input, integer MU, the number of diagonals above the main diagonal.
%
%    Input, integer IPVT(N), the pivot vector from CGBCO or CGBFA.
%
%    Input, complex B(N), the right hand side.
%
%    Input, integer JOB.
%    0, to solve A*x = b,
%    nonzero, to solve hermitian(A)*x = b, where hermitian(A) is the
%    conjugate transpose.
%
%    Output, complex B(N), the solution.
%
  m = mu + ml + 1;

  if ( job == 0 )
%
%  JOB = 0, solve A * X = B.
%
%  First solve L * Y = B.
%
    if ( ml ~= 0 )

      for k = 1 : n - 1

        lm = min ( ml, n - k );
        l = ipvt(k);
        t = b(l);

        if ( l ~= k )
          b(l) = b(k);
          b(k) = t;
        end

        b(k+1:k+lm) = b(k+1:k+lm) + t * abd(m+1:m+lm,k);

      end

    end
%
%  Now solve U * X = Y.
%
    for k = n : -1 : 1
      b(k) = b(k) / abd(m,k);
      lm = min ( k, m ) - 1;
      la = m - lm;
      lb = k - lm;
      t = -b(k);
      b(lb:lb+lm-1) = b(lb:lb+lm-1) + t * transpose ( abd(la:la+lm-1,k) );
    end

  else
%
%  JOB = nonzero, solve hermitian(A) * X = B.
%
%  First solve hermitian ( U ) * Y = B.
%
    for k = 1 : n
      lm = min ( k, m ) - 1;
      la = m - lm;
      lb = k - lm;
      t = cdotc ( lm, abd(la,k), 1, b(lb), 1 );
      b(k) = ( b(k) - t ) / conj ( abd(m,k) );
    end
%
%  Now solve hermitian ( L ) * X = Y.
%
    if ( ml ~= 0 )

      for k = n-1 : -1 : 1

        lm = min ( ml, n - k );
        b(k) = b(k) + cdotc ( lm, abd(m+1,k), 1, b(k+1), 1 );
        l = ipvt(k);

        if ( l ~= k )
          t    = b(l);
          b(l) = b(k);
          b(k) = t;
        end

      end

    end

  end

  return
end


function [energyinband_array,sqi_array,hr_array] = ComputeMVM(ecg,ann_ecg,Fs,segment_size,normalize)

% [energyinband_array,sqi_array] = ComputeMVM(ecg,ann,Fs,segment_size,normalize)
%   OVERVIEW:   This function returns the QRS MVM measured in non overlapping
%   windows of size 'segment_size' in minutes. It also returns the
%   signal quality and mean heart rate for each window.
%
%   INPUT:      MANDATORY:
%               ecg             : a single row of ECG data in samples. Must be sampled at 1 KHz.
%
%               ann             : annotation for the ecg passed as input. The ann variable
%                                 is a struct with Q,R,S fiduical points in sample number as fields
%                                 for the struct. i.e. ann.R, ann.Q, ...
%
%               Fs              : sampling frequency for the ecg signal (Hz)
%
%               segment_size        : the length of non overlapping windows
%                                 being analyzed in minutes. Default size
%                                 is 5 minutes
%
%               normalize       : boolean value (0 or 1). When set to 1
%                               will normalize each analysis window by the median R
%                               amplitude. Default is 1.
%
%
%   OUTPUT:
%               energyinband_array     : the energy for QRS complex morphological variability measured in
%                                        the every 2-7 beats region for each analysis window.
%
%               sqi_array        : the signal quality index measured for each analysis window.
%
%               hr_array          : the median heart rate measured for analysis
%                                 window
%
%   REF:
%   The script is based on the algorithm given in the following paper,
%   Liu Y, Syed Z, Scirica BM, Morrow DA, Guttag JV, Stultz CM.
%   ECG morphological variability in beat space for risk stratification after acute coronary syndrome.
%   J Am Heart Assoc. 2014;3(3):e000981. Published 2014 Jun 24. doi:10.1161/JAHA.114.000981
%	REPO:
%       https://github.com/cliffordlab/PhysioNet-Cardiovascular-Signal-Toolbox
%   ORIGINAL SOURCE AND AUTHORS:
%       Written by Ismail Sadiq
%	COPYRIGHT (C) 2019
%   LICENSE:
%       This software is offered freely and without warranty under
%       the GNU (v3 or later) public license. See license file for
%       more information. The license may be found in
%       the Documents folder of the Physionet-Cardiovascular-Signal-Toolbox.
%

% divide into 5 minute segments
% initialize variables
segment_size_samples = Fs*60*segment_size; % size of analysis window in samples
Nsegments = floor(length(ecg)/segment_size_samples); % number of segments analyzed
energyinband_array = NaN(1, Nsegments); % array for storing the MVM energy measured in each analysis window
sqi_array = NaN(1, Nsegments); % array of storing the signal quality for each window analyzed
hr_array = NaN(1,Nsegments); % array for storing the heart rate estimate for each window analyzed
lengthflag = 0;

sqi_threshold = 0.7;

% convert ecg to row vector
if (size(ecg,1) > size(ecg,2))
   ecg = ecg';
end

if (Nsegments == 0)   % length of data < segment_size_samples, analyze available data
    Nsegments = 1; lengthflag = 1;
end

for segmentidx = 1:Nsegments

    try

        % isolate segment
        if(lengthflag)
            % if the length of signal is < segment_size in samples,
            % determine annotations till the end of the record.
            fiveminsegment = ecg(segmentidx*segment_size_samples+1-segment_size_samples:end);
            ann.R = ann_ecg.R(ann_ecg.R < length(ecg));
            ann.R = ann.R(ann.R > segmentidx*segment_size_samples+1-segment_size_samples) - (segmentidx-1)*segment_size_samples;% + 1;
            ann.Q = ann_ecg.Q(ann_ecg.Q < length(ecg));
            ann.Q = ann.Q(ann.Q > segmentidx*segment_size_samples+1-segment_size_samples) - (segmentidx-1)*segment_size_samples;% + 1;
            ann.S = ann_ecg.S(ann_ecg.S < length(ecg));
            ann.S = ann.S(ann.S > segmentidx*segment_size_samples+1-segment_size_samples) - (segmentidx-1)*segment_size_samples;% + 1;
            ann.QRSon = ann_ecg.QRSon(ann_ecg.QRSon < length(ecg));
            ann.QRSon = ann.QRSon(ann.QRSon > segmentidx*segment_size_samples+1-segment_size_samples) - (segmentidx-1)*segment_size_samples;
            ann.QRSoff = ann_ecg.QRSoff(ann_ecg.QRSoff < length(ecg));
            ann.QRSoff = ann.QRSoff(ann.QRSoff > segmentidx*segment_size_samples+1-segment_size_samples) - (segmentidx-1)*segment_size_samples;
            lengthflag = 0;
        else
            % get ecg signal and corresponding annotations in the current
            % window
            fiveminsegment = ecg(segmentidx*segment_size_samples+1-segment_size_samples:segmentidx*segment_size_samples);
            ann = ann_ecg;
            ann.R = ann_ecg.R(ann_ecg.R < segmentidx*segment_size_samples);
            ann.R = ann.R(ann.R > segmentidx*segment_size_samples+1-segment_size_samples) - (segmentidx-1)*segment_size_samples;% + 1;
            ann.Q = ann_ecg.Q(ann_ecg.Q < segmentidx*segment_size_samples);
            ann.Q = ann.Q(ann.Q > segmentidx*segment_size_samples+1-segment_size_samples) - (segmentidx-1)*segment_size_samples;% + 1;
            ann.S = ann_ecg.S(ann_ecg.S < segmentidx*segment_size_samples);
            ann.S = ann.S(ann.S > segmentidx*segment_size_samples+1-segment_size_samples) - (segmentidx-1)*segment_size_samples;% + 1;
            ann.QRSon = ann_ecg.QRSon(ann_ecg.QRSon < segmentidx*segment_size_samples);
            ann.QRSon = ann.QRSon(ann.QRSon > segmentidx*segment_size_samples+1-segment_size_samples) - (segmentidx-1)*segment_size_samples;
            ann.QRSoff = ann_ecg.QRSoff(ann_ecg.QRSoff < segmentidx*segment_size_samples);
            ann.QRSoff = ann.QRSoff(ann.QRSoff > segmentidx*segment_size_samples+1-segment_size_samples) - (segmentidx-1)*segment_size_samples;
            %figure(2); plot(fiveminsegment); hold on; % Check annotations after loading
            %scatter(ann.R(~isnan(ann.R)), fiveminsegment(ann.R(~isnan(ann.R)))); hold off;
        end

        ecg_mv = fiveminsegment; clear fiveminsegment;

        % remove any nan values from the annotations
        R = round(ann.R);
        sum(isnan(R)); R = R(~isnan(R)); sum(isnan(R));
        Q = round(ann.Q);
        sum(isnan(Q)); Q = Q(~isnan(Q)); sum(isnan(Q));
        S = round(ann.S);
        sum(isnan(S)); S = S(~isnan(S)); sum(isnan(S));

        % compute the median Q,S and R amplitude, used to determine
        % normalization factor
        median_Q_amp = nanmedian(ecg_mv(1,Q));
        median_R_amp = nanmedian(ecg_mv(1,R));
        median_S_amp = nanmedian(ecg_mv(1,S));

        % compute normalization factor
        if (median_S_amp <= median_Q_amp || isnan(median_Q_amp))
            norm_factor = median_R_amp - median_S_amp;
        else
            if (~isempty(median_Q_amp))
                norm_factor = median_R_amp - median_Q_amp;
            else
                % if unable to determine, set to 1
                disp('unable to compute norm factor, setting to 1')
                norm_factor = 1;
            end
        end

        % normalize signal
        if (normalize)
            ecg_mv = ecg_mv / norm_factor;
        end

        % generate second set of annotations
        refqrs = ann.R;
        % mean impute missing val
        ecg_mv(isnan(ecg_mv)) = nanmean(ecg_mv);
        testqrs = wqrsm(ecg_mv, Fs);
        thres = 0.1; margin = 0; windowlen = 60*segment_size;
        % Determine signal quality
        refqrs = refqrs./Fs; testqrs = testqrs./Fs; % convert to time (s)
        [current_win_sqi,Se,PPV,Nb] = run_sqi(refqrs,testqrs,thres,margin,windowlen,Fs);
        sqi_array(segmentidx) = current_win_sqi;
        if (current_win_sqi > sqi_threshold)   % may need to be set higher

            % perform morphological variability on QRS

            % determine qrs onset and qrs offset annotations over 5 minute segment
            qrson = ann.QRSon;
            qrsoff = ann.QRSoff;
            if (qrsoff(1) < qrson(1))
                startidx = find(qrsoff < qrson(1));
                qrsoff = qrsoff(startidx+1:end);
                qrson = qrson(1:length(qrsoff));
            else
                qrson = qrson(1:length(qrsoff));
            end

            % make sure sufficient annotations to estimate median S
            % amplitude
            if (-1*median_S_amp > median_R_amp && (length(S) > 200))
                alignmentpoint = 'S';
            else
                alignmentpoint = 'R';
            end

            % re-compute R peak
            radj = zeros(1,length(qrson)); % r-adjusted, used in ectopic beat removal
            % setting max value between qrson and qrsoff as r-pk
            for qrsonidx = 1:length(qrson)
                %[pks,locs] = findpeaks(ecg(qrson(qrsonidx):qrsoff(qrsonidx)));
                [pks,locs] = findpeaks(ecg_mv(qrson(qrsonidx):qrsoff(qrsonidx)));
                maxpk = max(pks); maxloc = locs((maxpk == pks));
                if (isempty(maxloc))
                    radj(qrsonidx) = qrson(qrsonidx);
                else
                    radj(qrsonidx) = qrson(qrsonidx)-1+maxloc(1);   % 1 in case of duplicates
                end
            end
            % compute alignemnt point
            % find optimal alignment points max r-pk or s-valley
            alignpoint = zeros(1,length(qrson)); % used in qrs complex isolation
            for qrsonidx = 1:length(qrson)

                if (strcmp(alignmentpoint, 'R'))
                    [pks,locs] = findpeaks((ecg_mv(qrson(qrsonidx):qrsoff(qrsonidx))));
                else
                    [pks,locs] = findpeaks(-1*(ecg_mv(qrson(qrsonidx):qrsoff(qrsonidx))));
                end

                if (isempty(pks))   % if no positive peaks define mid-point between qrson and qrsoff as alignment point
                    locs = round(mean(qrson(qrsonidx), qrsoff(qrsonidx)));
                    pks = ecg_mv(locs);
                    alignpoint(qrsonidx) = locs; continue;
                end
                maxpk = max(pks); maxloc = locs((maxpk == pks));
                alignpoint(qrsonidx) = qrson(qrsonidx)-1+maxloc(1);   % 1 in case of duplicates
            end

            % detect pvc beats
            signal = ecg_mv; th = 0.1;
            [qrs_time pvc_output] = detectpvc2(signal',Fs,th);
            qrs_time = qrs_time(logical(pvc_output));

            % only use 5 minute signals with complete annotations to maintane validity
            % of fourier analysis (may want to replace with interpolation techhnique)
            if (sum(isnan(qrson)) || sum(isnan(qrsoff)))
                disp(['annotations incomplete ' num2str(fileidx)]);
                continue;
            end

            % ectopic beat removal
            qrsonectopicfree = qrson(1);
            rectopicfree = radj(1);
            qrsoffectopicfree = qrsoff(1);
            alignpointsectopicfree = alignpoint(1);
            for ridx = 2:length(radj)-1   % start from idx 2 and goto length(r)-1
                if (ridx <= length(radj)-40)
                    %meanRR = mean(diff(radj(ridx-1:ridx+38)));   % compute mean over 40 beat segment
                    meanRR = median(diff(radj(ridx-1:ridx+38)));   % compute median over 40 beat segment
                else
                    %meanRR = mean(diff(radj(end-39:end)));   % compute mean over last 40 beat segment
                    meanRR = median(diff(radj(end-39:end)));   % compute median over last 40 beat segment
                end
                preRR = radj(ridx) - radj(ridx-1);    % pre-RR interval
                postRR = radj(ridx+1) - radj(ridx);   % post-RR interval
                % if the pre-RR and post-RR vary from the mean RR by more than
                % 20% consider as ectopic
                if ((((meanRR-preRR) > 0.2*meanRR) && ((postRR-meanRR) > 0.2*meanRR)) || (((meanRR-preRR) > 0.2*meanRR) && ((meanRR-postRR) > 0.2*meanRR))) % && ((meanRR-postRR) > 0.2*meanRR)))  % 326, 367
                    continue;
                else % include the QRS complex, may also want to include t-wave onset/offset/amplitude/QT-interval
                    qrsonectopicfree = [qrsonectopicfree qrson(ridx)];
                    rectopicfree = [rectopicfree radj(ridx)];
                    qrsoffectopicfree = [qrsoffectopicfree qrsoff(ridx)];
                    alignpointsectopicfree = [alignpointsectopicfree alignpoint(ridx)];
                end
            end

            % remove ectopic beats
            [C,ia,ib] = intersect(rectopicfree, qrs_time);
            rectopicfree(ia) = [];
            qrsonectopicfree(ia) = [];
            qrsoffectopicfree(ia) = [];
            alignpointsectopicfree(ia) = [];

            % calculate NTWDseries and TWDseries
            % Isolate complexes
            [Complexes] = IsolateQRSComplexes(ecg_mv, qrsoffectopicfree, alignpointsectopicfree, qrsonectopicfree);

            % NTWD and TWD
            [NTWDseries TWDseries] = SquaredDiffComputation_beatbybeat(Complexes);

            % compute spectrum
            nfft = 256; % standardize the length of the fft to 256
            pxx = pwelch(NTWDseries,[],[],nfft);
            %             pxx = pwelch(TWDseries,[],[],nfft);
            N = nfft;
            stepsize = 0.5/length(pxx);

            dl = 2; du = 7;
            indexeverytwobeats = round(N/dl, 0);    % corresponding to every 2 beats
            indexeverysevenbeats = floor(N/du); % corresponding to every 7 beats

            % compute energy in every 2-7 beats region
            energyinband = trapz((indexeverysevenbeats:indexeverytwobeats) * stepsize, pxx(indexeverysevenbeats:indexeverytwobeats));
            energyinband_array(segmentidx) = energyinband; disp(['MVM for current window is ' num2str(energyinband)]);

            % hr est
            hr_array(segmentidx) = 60/(nanmedian(diff(alignpointsectopicfree))/Fs);
        else
            disp('unclean segment')
        end

    catch
        disp('error in segment')
        energyinband_array(segmentidx) = -inf;
        sqi_array(segmentidx) = -inf;
        hr_array(segmentidx) = -inf;
    end

end

end


function [DEM] = spm_LAP(DEM)
% Laplacian model inversion (see also spm_LAPS)
% FORMAT DEM   = spm_LAP(DEM)
%
% DEM.M  - hierarchical model
% DEM.Y  - response variable, output or data
% DEM.U  - explanatory variables, inputs or prior expectation of causes
%__________________________________________________________________________
%
% generative model
%--------------------------------------------------------------------------
%   M(i).g  = v     =  g(x,v,P)   {inline function, string or m-file}
%   M(i).f  = dx/dt =  f(x,v,P)   {inline function, string or m-file}
%
%   M(i).ph = pi(v) = ph(x,v,h,M) {inline function, string or m-file}
%   M(i).pg = pi(x) = pg(x,v,g,M) {inline function, string or m-file}
%                                  (assumed to be linear in v and x)
%
%   pi(v,x) = vectors of log-precisions; (h,g) = precision parameters
%
%   M(i).pE = prior expectation of p model-parameters
%   M(i).pC = prior covariances of p model-parameters
%   M(i).hE = prior expectation of h log-precision (cause noise)
%   M(i).hC = prior covariances of h log-precision (cause noise)
%   M(i).gE = prior expectation of g log-precision (state noise)
%   M(i).gC = prior covariances of g log-precision (state noise)
%   M(i).xP = precision (states)
%   M(i).Q  = precision components (input noise)
%   M(i).R  = precision components (state noise)
%   M(i).V  = fixed precision (input noise)
%   M(i).W  = fixed precision (state noise)
%
%   M(i).P  = optional initial value for parameters (defaults to M(i).pE)
%
%   M(i).m  = number of inputs v(i + 1);
%   M(i).n  = number of states x(i);
%   M(i).l  = number of output v(i);
%
% conditional moments of model-states - q(u)
%--------------------------------------------------------------------------
%   qU.x    = Conditional expectation of hidden states
%   qU.v    = Conditional expectation of causal states
%   qU.w    = Conditional prediction error (states)
%   qU.z    = Conditional prediction error (causes)
%   qU.C    = Conditional covariance: cov(v)
%   qU.S    = Conditional covariance: cov(x)
%
% conditional moments of model-parameters - q(p)
%--------------------------------------------------------------------------
%   qP.P    = Conditional expectation
%   qP.C    = Conditional covariance
%
% conditional moments of hyper-parameters (log-transformed) - q(h)
%--------------------------------------------------------------------------
%   qH.h    = Conditional expectation (cause noise)
%   qH.g    = Conditional expectation (state noise)
%   qH.C    = Conditional covariance
%
% F         = log-evidence = log-marginal likelihood = negative free-energy
%
%__________________________________________________________________________
% Accelerated methods: To accelerate computations one can specify the
% nature of the model equations using:
%
% M(1).E.linear = 0: full        - evaluates 1st and 2nd derivatives
% M(1).E.linear = 1: linear      - equations are linear in x and v
% M(1).E.linear = 2: bilinear    - equations are linear in x, v & x*v
% M(1).E.linear = 3: nonlinear   - equations are linear in x, v, x*v, & x*x
% M(1).E.linear = 4: full linear - evaluates 1st derivatives (for GF)
%
% similarly, for evaluating precisions:
%
% M(1).E.method.h = 0,1  switch for precision parameters (hidden causes)
% M(1).E.method.g = 0,1  switch for precision parameters (hidden states)
% M(1).E.method.v = 0,1  switch for precision (hidden causes)
% M(1).E.method.x = 0,1  switch for precision (hidden states)
%__________________________________________________________________________
%
% spm_LAP implements a variational scheme under the Laplace
% approximation to the conditional joint density q on states u, parameters
% p and hyperparameters (h,g) of an analytic nonlinear hierarchical dynamic
% model, with additive Gaussian innovations.
%
%            q(u,p,h,g) = max <L(t)>q
%
% L is the ln p(y,u,p,h,g|M) under the model M. The conditional covariances
% obtain analytically from the curvature of L with respect to the unknowns.
%__________________________________________________________________________
% Copyright (C) 2010-2013 Wellcome Trust Centre for Neuroimaging

% Karl Friston
% $Id: spm_LAP.m 6508 2015-07-25 15:23:25Z karl $

% find or create a DEM figure
%--------------------------------------------------------------------------
try
    DEM.M(1).nograph;
catch
    DEM.M(1).nograph = 0;
end
if ~DEM.M(1).nograph
    Fdem = spm_figure('GetWin','DEM');
end

% check model, data and priors
%==========================================================================
[M,Y,U] = spm_DEM_set(DEM);

% set regularisation
%--------------------------------------------------------------------------
try
    dt = DEM.M(1).E.v;
catch
    dt = 0;
    DEM.M(1).E.v = dt;
end

% number of iterations
%--------------------------------------------------------------------------
try, nD = M(1).E.nD; catch, nD = 1;  end
try, nN = M(1).E.nN; catch, nN = 8;  end

% ensure integration scheme evaluates gradients at each time-step
%--------------------------------------------------------------------------
M(1).E.linear = 4;

% assume precisions are a function of, and only of, hyperparameters
%--------------------------------------------------------------------------
try
    method = M(1).E.method;
catch
    method.h = 1;
    method.g = 1;
    method.x = 0;
    method.v = 0;
end
try method.h; catch, method.h = 0; end
try method.g; catch, method.g = 0; end
try method.x; catch, method.x = 0; end
try method.v; catch, method.v = 0; end

% assume precisions are a function of, and only of, hyperparameters
%--------------------------------------------------------------------------
try
    form = M(1).E.form;
catch
    form = 'Gaussian';
end

% checks for Laplace models (precision functions; ph and pg)
%--------------------------------------------------------------------------
for i  = 1:length(M)
    try
        feval(M(i).ph,M(i).x,M(i + 1).v,M(i).hE,M(i)); method.v = 1;
    catch
        M(i).ph = inline('spm_LAP_ph(x,v,h,M)','x','v','h','M');
    end
    try
        feval(M(i).pg,M(i).x,M(i + 1).v,M(i).gE,M(i)); method.x = 1;
    catch
        M(i).pg = inline('spm_LAP_pg(x,v,h,M)','x','v','h','M');
    end
end

M(1).E.method = method;

% order parameters (d = n = 1 for static models) and checks
%==========================================================================
d   = M(1).E.d + 1;                          % embedding order of q(v)
n   = M(1).E.n + 1;                          % embedding order of q(x)

% number of states and parameters
%--------------------------------------------------------------------------
ns  = size(Y,2);                             % number of samples
nl  = size(M,2);                             % number of levels
nv  = sum(spm_vec(M.m));                     % number of v (casual states)
nx  = sum(spm_vec(M.n));                     % number of x (hidden states)
ny  = M(1).l;                                % number of y (inputs)
nc  = M(end).l;                              % number of c (prior causes)
nu  = nv*d + nx*n;                           % number of generalised states
ne  = nv*n + nx*n + ny*n;                    % number of generalised errors

% precision (R) of generalised errors and null matrices for concatenation
%==========================================================================
s     = M(1).E.s;
Rh    = spm_DEM_R(n,s,form);
Rg    = spm_DEM_R(n,s,form);

if ~nx, Rg = sparse(0,0); end

W     = sparse(nx*n,nx*n);
V     = sparse((ny + nv)*n,(ny + nv)*n);

% fixed priors on states (u)
%--------------------------------------------------------------------------
Px    = kron(spm_DEM_R(n,2),spm_cat(spm_diag({M(1:end).xP})));
Pv    = kron(spm_DEM_R(d,2),spm_cat(spm_diag({M(2:end).vP})));
Pu    = spm_cat(spm_diag({Px Pv}));

% hyperpriors
%--------------------------------------------------------------------------
ph.h  = spm_vec({M.hE M.gE});                % prior expectation of h,g
ph.c  = spm_cat(spm_diag({M.hC M.gC}));      % prior covariances of h,g
Ph    = spm_inv(ph.c);                       % prior precision of h,g

qh.h  = {M.hE};                              % conditional expectation h
qh.g  = {M.gE};                              % conditional expectation g
nh    = length(spm_vec(qh.h));               % number of hyperparameters h
ng    = length(spm_vec(qh.g));               % number of hyperparameters g
nb    = nh + ng;                             % number of hyperparameters

% priors on parameters (in reduced parameter space)
%==========================================================================
pp.c  = cell(nl,nl);
qp.p  = cell(nl,1);
for i = 1:(nl - 1)

    % eigenvector reduction: p <- pE + qp.u*qp.p
    %----------------------------------------------------------------------
    qp.u{i}   = spm_svd(M(i).pC,0);          % basis for parameters
    M(i).p    = size(qp.u{i},2);             % number of qp.p
    qp.p{i}   = sparse(M(i).p,1);            % initial deviates
    pp.c{i,i} = qp.u{i}'*M(i).pC*qp.u{i};    % prior covariance

end
Up    = spm_cat(spm_diag(qp.u));

% priors on parameters
%--------------------------------------------------------------------------
pp.p  = spm_vec(M.pE);
pp.c  = spm_cat(pp.c);
Pp    = spm_inv(pp.c);

% initialise conditional density q(p)
%--------------------------------------------------------------------------
for i = 1:(nl - 1)
    try
        qp.p{i} = qp.p{i} + qp.u{i}'*(spm_vec(M(i).P) - spm_vec(M(i).pE));
    end
end
np    = size(Up,2);

% initialise cell arrays for D-Step; e{i + 1} = (d/dt)^i[e] = e[i]
%==========================================================================
qu.x      = cell(n,1);
qu.v      = cell(n,1);
qu.y      = cell(n,1);
qu.u      = cell(n,1);
[qu.x{:}] = deal(sparse(nx,1));
[qu.v{:}] = deal(sparse(nv,1));
[qu.y{:}] = deal(sparse(ny,1));
[qu.u{:}] = deal(sparse(nc,1));

% initialise cell arrays for hierarchical structure of x[0] and v[0]
%--------------------------------------------------------------------------
x         = {M(1:end - 1).x};
v         = {M(1 + 1:end).v};
qu.x{1}   = spm_vec(x);
qu.v{1}   = spm_vec(v);

% derivatives for Jacobian of D-step
%--------------------------------------------------------------------------
Dx     = kron(spm_speye(n,n,1),spm_speye(nx,nx));
Dv     = kron(spm_speye(d,d,1),spm_speye(nv,nv));
Dy     = kron(spm_speye(n,n,1),spm_speye(ny,ny));
Dc     = kron(spm_speye(d,d,1),spm_speye(nc,nc));
Du     = spm_cat(spm_diag({Dx,Dv}));
Ib     = spm_speye(np + nb,np + nb);
dbdt   = sparse(np + nb,1);

% gradients of generalised weighted errors
%--------------------------------------------------------------------------
dedh   = sparse(nh,ne);
dedg   = sparse(ng,ne);
dedv   = sparse(nv,ne);
dedx   = sparse(nx,ne);
dedhh  = sparse(nh,nh);
dedgg  = sparse(ng,ng);

% curvatures of Gibb's energy w.r.t. hyperparameters
%--------------------------------------------------------------------------
dHdh   = sparse(nh,1);
dHdg   = sparse(ng,1);
dHdp   = sparse(np,1);
dHdu   = sparse(nu,1);

% preclude unnecessary iterations and set switches
%--------------------------------------------------------------------------
if ~np && ~nh && ~ng, nN = 1; end
mnx    = nx*~~method.x;
mnv    = nv*~~method.v;

% preclude very precise states from entering free-energy/action
%--------------------------------------------------------------------------
p      = spm_LAP_eval(M,qu,qh);
ih     = p.h < 8;
ig     = p.g < 8;
ie     = kron(ones(n,1),ih);
ix     = kron(ones(n,1),ig);
iv     = kron(ones(d,1),ih((1:nv) + ny));
je     = find([ie; ix]); ix(1:nx) = 1;
ju     = find([ix; iv]);

% and other useful indices
%--------------------------------------------------------------------------
ix     = (1:nx);
ih     = (1:nb);
iv     = (1:nv) + nx*n;
ip     = (1:np) + nu;

% number of iterations for convergence
%--------------------------------------------------------------------------
convergence = -4;

% Iterate Laplace scheme
%==========================================================================
F      = -Inf;
for iN = 1:nN

    % get time and clear persistent variables in evaluation routines
    %----------------------------------------------------------------------
    tic; clear spm_DEM_eval

    % [re-]set states & their derivatives
    %----------------------------------------------------------------------
    try, qu = Q(1).u; end

    % D-Step: (nD D-Steps for each sample)
    %======================================================================
    for is = 1:ns

        % D-Step: until convergence for static systems
        %==================================================================
        for iD = 1:nD

            % sampling time
            %--------------------------------------------------------------
            ts = is + (iD - 1)/nD;

            % derivatives of responses and inputs
            %--------------------------------------------------------------
            try
                qu.y(1:n) = spm_DEM_embed(Y,n,ts,1,M(1).delays);
                qu.u(1:d) = spm_DEM_embed(U,d,ts);
            catch
                qu.y(1:n) = spm_DEM_embed(Y,n,ts);
                qu.u(1:d) = spm_DEM_embed(U,d,ts);
            end

            % evaluate functions and derivatives
            %==============================================================

            % prediction errors (E) and precision vectors (p)
            %--------------------------------------------------------------
            [E,dE] = spm_DEM_eval(M,qu,qp);
            [p,dp] = spm_LAP_eval(M,qu,qh);

            % gradients of log(det(iS)) dDd...
            %==============================================================

            % get precision matrices
            %--------------------------------------------------------------
            iSh   = diag(exp(p.h));
            iSg   = diag(exp(p.g));
            iS    = blkdiag(kron(Rh,iSh),kron(Rg,iSg));

            % gradients of trace(diag(p)) = sum(p); p = precision vector
            %--------------------------------------------------------------
            dpdx  = n*sum(spm_cat({dp.h.dx; dp.g.dx}));
            dpdv  = n*sum(spm_cat({dp.h.dv; dp.g.dv}));
            dpdh  = n*sum(dp.h.dh);
            dpdg  = n*sum(dp.g.dg);
            dpdx  = kron(sparse(1,1,1,1,n),dpdx);
            dpdv  = kron(sparse(1,1,1,1,d),dpdv);
            dDdu  = [dpdx dpdv]';
            dDdh  = [dpdh dpdg]';

            % gradients precision-weighted generalised error dSd..
            %==============================================================

            % gradients w.r.t. hyperparameters
            %--------------------------------------------------------------
            for i = 1:nh
                diS       = diag(dp.h.dh(:,i).*exp(p.h));
                diSdh{i}  = blkdiag(kron(Rh,diS),W);
                dedh(i,:) = E'*diSdh{i};
            end
            for i = 1:ng
                diS       = diag(dp.g.dg(:,i).*exp(p.g));
                diSdg{i}  = blkdiag(V,kron(Rg,diS));
                dedg(i,:) = E'*diSdg{i};
            end

            % gradients w.r.t. hidden states
            %--------------------------------------------------------------
            for i = 1:mnx
                diV       = diag(dp.h.dx(:,i).*exp(p.h));
                diW       = diag(dp.g.dx(:,i).*exp(p.g));
                diSdx{i}  = blkdiag(kron(Rh,diV),kron(Rg,diW));
                dedx(i,:) = E'*diSdx{i};
            end

            % gradients w.r.t. causal states
            %--------------------------------------------------------------
            for i = 1:mnv
                diV       = diag(dp.h.dv(:,i).*exp(p.h));
                diW       = diag(dp.g.dv(:,i).*exp(p.g));
                diSdv{i}  = blkdiag(kron(Rh,diV),kron(Rg,diW));
                dedv(i,:) = E'*diSdv{i};
            end

            dSdx  = kron(sparse(1,1,1,n,1),dedx);
            dSdv  = kron(sparse(1,1,1,d,1),dedv);
            dSdu  = [dSdx; dSdv];
            dEdh  = [dedh; dedg];
            dEdp  = dE.dp'*iS;
            dEdu  = dE.du'*iS;

            % curvatures w.r.t. hyperparameters
            %--------------------------------------------------------------
            for i = 1:nh
                for j = i:nh
                    diS        = diag(dp.h.dh(:,i).*dp.h.dh(:,j).*exp(p.h));
                    diS        = blkdiag(kron(Rh,diS),W);
                    dedhh(i,j) = E'*diS*E;
                    dedhh(j,i) = dedhh(i,j);
                end
            end
            for i = 1:ng
                for j = i:ng
                    diS        = diag(dp.g.dg(:,i).*dp.g.dg(:,j).*exp(p.g));
                    diS        = blkdiag(V,kron(Rg,diS));
                    dedgg(i,j) = E'*diS*E;
                    dedgg(j,i) = dedgg(i,j);
                end
            end

            % combined curvature
            %--------------------------------------------------------------
            dSdhh = spm_cat({dedhh  [] ;
                             [] dedgg});

            % errors (from prior expectations) (NB pp.p = 0)
            %--------------------------------------------------------------
            Eu    = spm_vec(qu.x(1:n),qu.v(1:d));
            Ep    = spm_vec(qp.p);
            Eh    = spm_vec(qh.h,qh.g) - ph.h;

            % first-order derivatives of Gibb's Energy
            %==============================================================
            dLdu  = dEdu*E + dSdu*E/2 - dDdu/2 + Pu*Eu;
            dLdh  = dEdh*E/2          - dDdh/2 + Ph*Eh;
            dLdp  = dEdp*E                     + Pp*Ep;

            % and second-order derivatives of Gibb's Energy
            %--------------------------------------------------------------
            dLduu = dEdu*dE.du + Pu;
            dLdpp = dEdp*dE.dp + Pp;
            dLdhh = dSdhh/2    + Ph;
            dLduy = dEdu*dE.dy;
            dLduc = dEdu*dE.dc;
            dLdup = dEdu*dE.dp;
            dLdhp = dEdh*dE.dp;
            dLdpu = dLdup';
            dLdph = dLdhp';

            % precision and covariances for entropy
            %--------------------------------------------------------------
            dLdaa = spm_cat({dLduu dLdup  ;
                             dLdpu dLdpp});
            dLdbb = spm_cat({dLdpp dLdph  ;
                             dLdhp dLdhh});

            Cup   = spm_inv(dLdaa);
            Chh   = spm_inv(dLdhh);

            % first-order derivatives of Entropy term
            %==============================================================

            % log-precision
            %--------------------------------------------------------------
            for i = 1:nh
                Luub    = dE.du'*diSdh{i}*dE.du;
                Lpub    = dE.dp'*diSdh{i}*dE.du;
                Lppb    = dE.dp'*diSdh{i}*dE.dp;
                diCdh   = spm_cat({Luub Lpub';
                                   Lpub Lppb});
                dHdh(i) = spm_trace(diCdh,Cup)/2;
            end
            for i = 1:ng
                Luub    = dE.du'*diSdg{i}*dE.du;
                Lpub    = dE.dp'*diSdg{i}*dE.du;
                Lppb    = dE.dp'*diSdg{i}*dE.dp;
                diCdg   = spm_cat({Luub Lpub';
                                   Lpub Lppb});
                dHdg(i) = spm_trace(diCdg,Cup)/2;
            end

            % parameters
            %--------------------------------------------------------------
            for i = 1:np
                Luup    = dE.dup{i}'*dEdu';
                Lpup    = dEdp*dE.dup{i};
                Luup    = Luup + Luup';
                diCdp   = spm_cat({Luup Lpup';
                                   Lpup [] });
                dHdp(i) = spm_trace(diCdp,Cup)/2;
            end

            % hidden states and causes (disabled for stability)
            %--------------------------------------------------------------
            for i = 1:(nu - nu)
                Lppu    = dE.dpu{i}'*dEdp';
                Lupu    = dEdu*dE.dpu{i};
                Lppu    = Lppu + Lppu';
                diCdu   = spm_cat({[]    Lupu;
                                   Lupu' Lppu});
                dHdu(i) = spm_trace(diCdu,Cup)/2;
            end

            % and concatenate
            %--------------------------------------------------------------
            dHdb  = [dHdh; dHdg];
            dHdb  = [dHdp; dHdb];
            dLdb  = [dLdp; dLdh];

            % save conditional moments (and prediction error) at Q{t}
            %==============================================================
            if iD == 1 || ns == 1

                % save means
                %----------------------------------------------------------
                Q(is).e   = E;
                Q(is).E   = diag(diag(iS))*E;
                Q(is).u   = qu;
                Q(is).p   = qp;
                Q(is).h   = qh;

                % and conditional covariances
                %----------------------------------------------------------
                Q(is).u.s = Cup(ix,ix);
                Q(is).u.c = Cup(iv,iv);
                Q(is).p.c = Cup(ip,ip);
                Q(is).h.c = Chh(ih,ih);

                % Free-energy (components)
                %----------------------------------------------------------
                Fc(is,1)  = - E(je)'*iS(je,je)*E(je)/2;
                Fc(is,2)  = - Eu(ju)'*Pu(ju,ju)*Eu(ju)/2;
                Fc(is,3)  = - n*ny*log(2*pi)/2;
                Fc(is,4)  = spm_logdet(iS(je,je))/2;
                Fc(is,5)  = spm_logdet(Pu(ju,ju)*Cup(ju,ju))/2;

                % Free-action (states and parameters)
                %----------------------------------------------------------
                AC(is)    = sum(Fc(is,:))    ...
                          - Ep'*Pp*Ep/2      ...
                          - Eh'*Ph*Eh/2      ...
                          + spm_logdet(Pp)/2 ...
                          + spm_logdet(Ph)/2 ...
                          - spm_logdet(dLdbb)/2;

            end

            % update conditional moments
            %==============================================================

            % prior precision of fluctuations on [hyper] parameters
            %--------------------------------------------------------------
            Kb    = ns*Ib;

            % accumulate curvatures of [hyper] parameters
            %--------------------------------------------------------------
            try
                dLdBB = dLdBB*(1 - 1/ns) + dLdbb/ns;
            catch
                dLdBB = dLdbb + Ib*32;
            end

            % whiten gradient (and curvatures) with regularised precision
            %--------------------------------------------------------------
            Cb    = spm_inv(dLdBB + diag(diag(dLdBB))*exp(dt));
            dLdb  = Cb*dLdb;
            dHdb  = Cb*dHdb;

            % assemble conditional means
            %--------------------------------------------------------------
            q.y  = qu.y(1:n);
            q.x  = qu.x(1:n);
            q.v  = qu.v(1:d);
            q.c  = qu.u(1:d);
            q.p  = qp.p;
            q.h  = qh.h;
            q.g  = qh.g;
            q.d  = dbdt;

            % flow
            %--------------------------------------------------------------
            f.y  =  Dy*spm_vec(q.y)                  ;
            f.u  =  Du*spm_vec(q.x,q.v) - dLdu - dHdu;
            f.c  =  Dc*spm_vec(q.c)                  ;
            f.b  =     spm_vec(q.d)                  ;
            f.d  = -Kb*spm_vec(q.d)     - dLdb - dHdb;

            % and Jacobian
            %--------------------------------------------------------------
            dfdq = {Dy     []        []     []    [] ;
                   -dLduy  Du-dLduu -dLduc  []    [] ;
                    []     []        Dc     []    [] ;
                    []     []        []     []    Ib ;
                    []     []        []    -Ib   -Kb};

            dfdq = spm_cat(dfdq);

            % get eigenvalues of Jacobian (on hidden states)
            %--------------------------------------------------------------
            try DEM.options.eigenvalues;
                DEM.E(:,is) = eig(full(Du - dLduu));
            end

            % update conditional modes of states
            %==============================================================
            dq   = spm_dx(dfdq,spm_vec(f),1/nD);
            q    = spm_unvec(spm_vec(q) + dq,q);

            % unpack conditional means
            %--------------------------------------------------------------
            qu.x(1:n) = q.x;
            qu.v(1:d) = q.v;
            qp.p      = q.p;
            qh.h      = q.h;
            qh.g      = q.g;
            dbdt      = q.d;

        end % D-Step

    end % sequence (ns)

    % Bayesian parameter averaging
    %======================================================================

    % Conditional moments of time-averaged parameters
    %----------------------------------------------------------------------
    Ep    = 0;
    Qp    = 0;
    for i = 1:ns
        P   = spm_inv(Q(i).p.c);
        Ep  = Ep  + P*spm_vec(Q(i).p.p);
        Qp  = Qp + P;
    end
    Ep    = spm_inv(Qp)*Ep;
    Cp    = spm_inv(Qp + (1 - ns)*Pp);

    % conditional moments of hyper-parameters
    %----------------------------------------------------------------------
    Eh    = 0;
    Qh    = 0;
    for i = 1:ns
        P   = spm_inv(Q(i).h.c);
        Eh  = Eh  + P*spm_vec(Q(i).h.h,Q(i).h.g);
        Qh  = Qh + P;
    end
    Eh    = spm_inv(Qh)*Eh - ph.h;
    Ch    = spm_inv(Qh + (1 - ns)*Ph);

    % Free-action of states plus free-energy of parameters
    %======================================================================
    FT    = sum(Fc,2);          %   instantaneous Free energy (of states)
    FC(1) = sum(Fc(:,1));       % - E'*iS*E/2;
    FC(2) = sum(Fc(:,2));       % - Eu'*Pu*Eu/2;
    FC(3) = sum(Fc(:,3));       % - n*ny*log(2*pi)/2;
    FC(4) = sum(Fc(:,4));       %   spm_logdet(iS)/2;
    FC(5) = sum(Fc(:,5));       %   spm_logdet(Pu*Cu)/2;
    FC(6) = -Ep'*Pp*Ep/2;
    FC(7) = -Eh'*Ph*Eh/2;
    FC(8) = spm_logdet(Pp*Cp)/2;
    FC(9) = spm_logdet(Ph*Ch)/2;

    CC(iN,:) = FC;
    S(iN)    = sum(AC);
    Fe       = sum(FC);

    % if F is decreasing, revert [hyper] parameters and slow down
    %----------------------------------------------------------------------
    if Fe < F(iN) && iN > 4

        % save free-energy
        %------------------------------------------------------------------
        F(iN + 1) = F(iN);

        % load current MAP estimates
        %------------------------------------------------------------------
        qp = PQ.qp;
        qh = PQ.qh;

        % decrease update time
        %------------------------------------------------------------------
        dt = max(dt + 2,2);

        % convergence
        %------------------------------------------------------------------
        if dt > 6; convergence = 1; end

    else

        % convergence
        %------------------------------------------------------------------
        if Fe - F(iN) < 1, convergence = convergence + 1; end

        % save free-energy
        %------------------------------------------------------------------
        F(iN)     = Fe;
        F(iN + 1) = Fe;

        % save current MAP estimates
        %------------------------------------------------------------------
        PQ.qp = qp;
        PQ.qh = qh;

        % increase update time
        %------------------------------------------------------------------
        dt    = max(dt - 1,-8);

    end

    % Convergence
    %======================================================================
    if convergence > 0; break, end

    % otherwise save conditional moments (for each time point)
    %======================================================================
    for t = 1:length(Q)

        % states and predictions
        %------------------------------------------------------------------
        v     = spm_unvec(Q(t).u.v{1},v);
        x     = spm_unvec(Q(t).u.x{1},x);
        z     = spm_unvec(Q(t).e(1:(ny + nv)),{M.v});
        Z     = spm_unvec(Q(t).E(1:(ny + nv)),{M.v});
        w     = spm_unvec(Q(t).e((1:nx) + (ny + nv)*n),{M.x});
        X     = spm_unvec(Q(t).E((1:nx) + (ny + nv)*n),{M.x});
        for i = 1:(nl - 1)
            if M(i).m, qU.v{i + 1}(:,t) = spm_vec(v{i});  end
            if M(i).n, qU.x{i}(:,t)     = spm_vec(x{i});  end
            if M(i).n, qU.w{i}(:,t)     = spm_vec(w{i});  end
            if M(i).l, qU.z{i}(:,t)     = spm_vec(z{i});  end
            if M(i).n, qU.W{i}(:,t)     = spm_vec(X{i});  end
            if M(i).l, qU.Z{i}(:,t)     = spm_vec(Z{i});  end
        end
        if    M(nl).l, qU.z{nl}(:,t)    = spm_vec(z{nl}); end
        if    M(nl).l, qU.Z{nl}(:,t)    = spm_vec(Z{nl}); end

        qU.v{1}(:,t)  = spm_vec(Q(t).u.y{1}) - spm_vec(z{1});

        % and conditional covariances
        %------------------------------------------------------------------
        qU.S{t} = Q(t).u.s;
        qU.C{t} = Q(t).u.c;

        % parameters
        %------------------------------------------------------------------
        qP.p{t} = spm_vec(Q(t).p.p);
        qP.c{t} = Q(t).p.c;

        % hyperparameters
        %------------------------------------------------------------------
        qH.p{t} = spm_vec(Q(t).h.h,Q(t).h.g);
        qH.c{t} = Q(t).h.c;

    end

    % graphics (states)
    %----------------------------------------------------------------------
    if exist('Fdem','var')
        spm_figure('Select', Fdem)
        spm_DEM_qU(qU)

        % graphics (parameters and log-precisions)
        %------------------------------------------------------------------
        if np && nb
            subplot(2*nl,2,4*nl - 2)
            plot(1:ns,full(spm_cat(qP.p)))
            set(gca,'XLim',[1 ns])
            title('parameters (modes)','FontSize',16)

            subplot(2*nl,2,4*nl)
            plot(1:ns,full(spm_cat(qH.p)))
            set(gca,'XLim',[1 ns])
            title('log-precision','FontSize',16)

        elseif nb
            subplot(nl,2,2*nl)
            plot(1:ns,full(spm_cat(qH.p)))
            set(gca,'XLim',[1 ns])
            title('log-precision','FontSize',16)

        elseif np
            subplot(nl,2,2*nl)
            plot(1:ns,full(spm_cat(qP.p)))
            set(gca,'XLim',[1 ns])
            title('parameters (modes)','FontSize',16)

        end
        drawnow
    end

    % report (EM-Steps)
    %----------------------------------------------------------------------
    try
        dF = F(iN) - F(iN - 1);
    catch
        dF = 0;
    end
    str{1} = sprintf('LAP: %i (%i)', iN,iD);
    if iN == 1
        str{2} = sprintf('  F0:%.4e', full(F(iN)));
    else
        str{2} = sprintf('F-F0:%.4e', full(F(iN) - F(1)));
    end
    str{3} = sprintf('dF:%.2e',      full(dF));
    str{4} = sprintf('(%.2e sec)',   full(toc));
    fprintf('%-16s%-20s%-14s%-16s\n',str{:})

end
fprintf('%-19sF:%.4e\n', 'LAP: Converged', F(end));
if exist('Fdem','var')
    spm_figure('Focus', Fdem)
end

% Place Bayesian parameter averages in output arguments
%==========================================================================

% Conditional moments of time-averaged parameters
%--------------------------------------------------------------------------
Qp = 0;
Ep = 0;
for i = 1:ns

    % weight in proportion to precisions
    %----------------------------------------------------------------------
    P  = spm_inv(qP.c{i});
    Ep = Ep + P*qP.p{i};
    Qp = Qp + P;

end
Ep     = spm_inv(Qp)*Ep;
Cp     = spm_inv(Qp + (1 - ns)*Pp);
qP.P   = spm_unvec(Up*Ep + pp.p,{M.pE});
qP.C   = Up*Cp*Up';
qP.V   = spm_unvec(diag(qP.C),{M.pE});
qP.U   = Up;

% conditional moments of hyper-parameters
%--------------------------------------------------------------------------
Qh = 0;
Eh = 0;
for i = 1:ns

    % weight in proportion to precisions
    %----------------------------------------------------------------------
    P  = spm_inv(qH.c{i});
    Eh = Eh + P*qH.p{i};
    Qh = Qh + P;

end
Eh     = spm_inv(Qh)*Eh;
Ch     = spm_inv(Qh + (1 - ns)*Ph);
P      = spm_unvec(Eh,{qh.h,qh.g});
qH.h   = P{1};
qH.g   = P{2};
qH.C   = Ch;
P      = spm_unvec(diag(qH.C),P);
qH.V   = P{1};
qH.W   = P{2};

% assign output variables
%--------------------------------------------------------------------------
DEM.M  = M;                   % model
DEM.U  = U;                   % causes

DEM.qU = qU;                  % conditional moments of model-states
DEM.qP = qP;                  % conditional moments of model-parameters
DEM.qH = qH;                  % conditional moments of hyper-parameters

DEM.F  = F(1:iN);             % [-ve] Free-energy
DEM.S  = S(1:iN);             % [-ve] Free-action
DEM.FC = FC;                  % Free-energy components
DEM.CC = CC;                  % over iterations
DEM.FT = FT;                  % over time


function [Csig,Cbak,Covl]= composite(cleanFile, enhancedFile);
% ----------------------------------------------------------------------
%          Composite Objective Speech Quality Measure
%
%   This function implements the composite objective measure proposed in
%   [1].
%
%   Usage:  [sig,bak,ovl]=composite(cleanFile.wav, enhancedFile.wav)
%
%         cleanFile.wav - clean input file in .wav format
%         enhancedFile  - enhanced output file in .wav format
%         sig           - predicted rating [1-5] of speech distortion
%         bak           - predicted rating [1-5] of noise distortion
%         ovl           - predicted rating [1-5] of overall quality
%
%       In addition to the above ratings (sig, bak, & ovl) it returns
%       the individual values of the LLR, SNRseg, WSS and PESQ measures.
%
%  Example call:  [sig,bak,ovl] =composite('sp04.wav','enhanced.wav')
%
%
%  References:
%
%     [1]   Hu, Y. and Loizou, P. (2006). Evaluation of objective measures
%           for speech enhancement. Proc. Interspeech, Pittsburg, PA.
%
%   Authors: Yi Hu and Philipos C. Loizou
%   (the LLR, SNRseg and WSS measures were based on Bryan Pellom and John
%     Hansen's implementations)
%
% Copyright (c) 2006 by Philipos C. Loizou
% $Revision: 0.0 $  $Date: 10/09/2006 $

% ----------------------------------------------------------------------

if nargin~=2
    fprintf('USAGE: [sig,bak,ovl]=composite(cleanFile.wav, enhancedFile.wav)\n');
    fprintf('For more help, type: help composite\n\n');
    return;
end

alpha= 0.95;

[data1, Srate1, Nbits1]= wavread(cleanFile);
[data2, Srate2, Nbits2]= wavread(enhancedFile);
if ( Srate1~= Srate2) | ( Nbits1~= Nbits2)
    error( 'The two files do not match!\n');
end

len= min( length( data1), length( data2));
data1= data1( 1: len)+eps;
data2= data2( 1: len)+eps;

% -- compute the WSS measure ---
%
wss_dist_vec= wss( data1, data2,Srate1);
wss_dist_vec= sort( wss_dist_vec);
wss_dist= mean( wss_dist_vec( 1: round( length( wss_dist_vec)*alpha)));

% --- compute the LLR measure ---------
%
LLR_dist= llr( data1, data2,Srate1);
LLRs= sort(LLR_dist);
LLR_len= round( length(LLR_dist)* alpha);
llr_mean= mean( LLRs( 1: LLR_len));

% --- compute the SNRseg ----------------
%
[snr_dist, segsnr_dist]= snr( data1, data2,Srate1);
snr_mean= snr_dist;
segSNR= mean( segsnr_dist);

% -- compute the pesq ----
%
% if     Srate1==8000,    mode='nb';
% elseif Srate1 == 16000, mode='wb';
% else,
%      error ('Sampling freq in PESQ needs to be 8 kHz or 16 kHz');
% end

 [pesq_mos_scores]= pesq(cleanFile, enhancedFile);

 if length(pesq_mos_scores)==2
     pesq_mos=pesq_mos_scores(1); % take the raw PESQ value instead of the
                                  % MOS-mapped value (this composite
                                  % measure was only validated with the raw
                                  % PESQ value)
 else
     pesq_mos=pesq_mos_scores;
 end

% --- now compute the composite measures ------------------
%
Csig = 3.093 - 1.029*llr_mean + 0.603*pesq_mos-0.009*wss_dist;
  Csig = max(1,Csig);  Csig=min(5, Csig); % limit values to [1, 5]
Cbak = 1.634 + 0.478 *pesq_mos - 0.007*wss_dist + 0.063*segSNR;
  Cbak = max(1, Cbak); Cbak=min(5,Cbak); % limit values to [1, 5]
Covl = 1.594 + 0.805*pesq_mos - 0.512*llr_mean - 0.007*wss_dist;
  Covl = max(1, Covl); Covl=min(5, Covl); % limit values to [1, 5]

fprintf('\n LLR=%f   SNRseg=%f   WSS=%f   PESQ=%f\n',llr_mean,segSNR,wss_dist,pesq_mos);

return; %=================================================================

function distortion = wss(clean_speech, processed_speech,sample_rate)

% ----------------------------------------------------------------------
% Check the length of the clean and processed speech.  Must be the same.
% ----------------------------------------------------------------------

clean_length      = length(clean_speech);
processed_length  = length(processed_speech);

if (clean_length ~= processed_length)
  disp('Error: Files  musthave same length.');
  return
end

% ----------------------------------------------------------------------
% Global Variables
% ----------------------------------------------------------------------

winlength   = round(30*sample_rate/1000); %240;		   % window length in samples
skiprate    = floor(winlength/4);		   % window skip in samples
max_freq    = sample_rate/2;	   % maximum bandwidth
num_crit    = 25;		   % number of critical bands

USE_FFT_SPECTRUM = 1;		   % defaults to 10th order LP spectrum
n_fft       = 2^nextpow2(2*winlength);
n_fftby2    = n_fft/2;		   % FFT size/2
Kmax        = 20;		   % value suggested by Klatt, pg 1280
Klocmax     = 1;		   % value suggested by Klatt, pg 1280

% ----------------------------------------------------------------------
% Critical Band Filter Definitions (Center Frequency and Bandwidths in Hz)
% ----------------------------------------------------------------------

cent_freq(1)  = 50.0000;   bandwidth(1)  = 70.0000;
cent_freq(2)  = 120.000;   bandwidth(2)  = 70.0000;
cent_freq(3)  = 190.000;   bandwidth(3)  = 70.0000;
cent_freq(4)  = 260.000;   bandwidth(4)  = 70.0000;
cent_freq(5)  = 330.000;   bandwidth(5)  = 70.0000;
cent_freq(6)  = 400.000;   bandwidth(6)  = 70.0000;
cent_freq(7)  = 470.000;   bandwidth(7)  = 70.0000;
cent_freq(8)  = 540.000;   bandwidth(8)  = 77.3724;
cent_freq(9)  = 617.372;   bandwidth(9)  = 86.0056;
cent_freq(10) = 703.378;   bandwidth(10) = 95.3398;
cent_freq(11) = 798.717;   bandwidth(11) = 105.411;
cent_freq(12) = 904.128;   bandwidth(12) = 116.256;
cent_freq(13) = 1020.38;   bandwidth(13) = 127.914;
cent_freq(14) = 1148.30;   bandwidth(14) = 140.423;
cent_freq(15) = 1288.72;   bandwidth(15) = 153.823;
cent_freq(16) = 1442.54;   bandwidth(16) = 168.154;
cent_freq(17) = 1610.70;   bandwidth(17) = 183.457;
cent_freq(18) = 1794.16;   bandwidth(18) = 199.776;
cent_freq(19) = 1993.93;   bandwidth(19) = 217.153;
cent_freq(20) = 2211.08;   bandwidth(20) = 235.631;
cent_freq(21) = 2446.71;   bandwidth(21) = 255.255;
cent_freq(22) = 2701.97;   bandwidth(22) = 276.072;
cent_freq(23) = 2978.04;   bandwidth(23) = 298.126;
cent_freq(24) = 3276.17;   bandwidth(24) = 321.465;
cent_freq(25) = 3597.63;   bandwidth(25) = 346.136;

bw_min      = bandwidth (1);	   % minimum critical bandwidth

% ----------------------------------------------------------------------
% Set up the critical band filters.  Note here that Gaussianly shaped
% filters are used.  Also, the sum of the filter weights are equivalent
% for each critical band filter.  Filter less than -30 dB and set to
% zero.
% ----------------------------------------------------------------------

min_factor = exp (-30.0 / (2.0 * 2.303));       % -30 dB point of filter

for i = 1:num_crit
  f0 = (cent_freq (i) / max_freq) * (n_fftby2);
  all_f0(i) = floor(f0);
  bw = (bandwidth (i) / max_freq) * (n_fftby2);
  norm_factor = log(bw_min) - log(bandwidth(i));
  j = 0:1:n_fftby2-1;
  crit_filter(i,:) = exp (-11 *(((j - floor(f0)) ./bw).^2) + norm_factor);
  crit_filter(i,:) = crit_filter(i,:).*(crit_filter(i,:) > min_factor);
end

% ----------------------------------------------------------------------
% For each frame of input speech, calculate the Weighted Spectral
% Slope Measure
% ----------------------------------------------------------------------

num_frames = clean_length/skiprate-(winlength/skiprate); % number of frames
start      = 1;					% starting sample
window     = 0.5*(1 - cos(2*pi*(1:winlength)'/(winlength+1)));

for frame_count = 1:num_frames

   % ----------------------------------------------------------
   % (1) Get the Frames for the test and reference speech.
   %     Multiply by Hanning Window.
   % ----------------------------------------------------------

   clean_frame = clean_speech(start:start+winlength-1);
   processed_frame = processed_speech(start:start+winlength-1);
   clean_frame = clean_frame.*window;
   processed_frame = processed_frame.*window;

   % ----------------------------------------------------------
   % (2) Compute the Power Spectrum of Clean and Processed
   % ----------------------------------------------------------

    if (USE_FFT_SPECTRUM)
       clean_spec     = (abs(fft(clean_frame,n_fft)).^2);
       processed_spec = (abs(fft(processed_frame,n_fft)).^2);
    else
       a_vec = zeros(1,n_fft);
       a_vec(1:11) = lpc(clean_frame,10);
       clean_spec     = 1.0/(abs(fft(a_vec,n_fft)).^2)';

       a_vec = zeros(1,n_fft);
       a_vec(1:11) = lpc(processed_frame,10);
       processed_spec = 1.0/(abs(fft(a_vec,n_fft)).^2)';
    end

   % ----------------------------------------------------------
   % (3) Compute Filterbank Output Energies (in dB scale)
   % ----------------------------------------------------------

   for i = 1:num_crit
      clean_energy(i) = sum(clean_spec(1:n_fftby2) ...
		            .*crit_filter(i,:)');
      processed_energy(i) = sum(processed_spec(1:n_fftby2) ...
			        .*crit_filter(i,:)');
   end
   clean_energy = 10*log10(max(clean_energy,1E-10));
   processed_energy = 10*log10(max(processed_energy,1E-10));

   % ----------------------------------------------------------
   % (4) Compute Spectral Slope (dB[i+1]-dB[i])
   % ----------------------------------------------------------

   clean_slope     = clean_energy(2:num_crit) - ...
		     clean_energy(1:num_crit-1);
   processed_slope = processed_energy(2:num_crit) - ...
		     processed_energy(1:num_crit-1);

   % ----------------------------------------------------------
   % (5) Find the nearest peak locations in the spectra to
   %     each critical band.  If the slope is negative, we
   %     search to the left.  If positive, we search to the
   %     right.
   % ----------------------------------------------------------

   for i = 1:num_crit-1

       % find the peaks in the clean speech signal

       if (clean_slope(i)>0) 		% search to the right
	  n = i;
          while ((n<num_crit) & (clean_slope(n) > 0))
	     n = n+1;
 	  end
	  clean_loc_peak(i) = clean_energy(n-1);
       else				% search to the left
          n = i;
	  while ((n>0) & (clean_slope(n) <= 0))
	     n = n-1;
 	  end
	  clean_loc_peak(i) = clean_energy(n+1);
       end

       % find the peaks in the processed speech signal

       if (processed_slope(i)>0) 	% search to the right
	  n = i;
          while ((n<num_crit) & (processed_slope(n) > 0))
	     n = n+1;
	  end
	  processed_loc_peak(i) = processed_energy(n-1);
       else				% search to the left
          n = i;
	  while ((n>0) & (processed_slope(n) <= 0))
	     n = n-1;
 	  end
	  processed_loc_peak(i) = processed_energy(n+1);
       end

   end

   % ----------------------------------------------------------
   %  (6) Compute the WSS Measure for this frame.  This
   %      includes determination of the weighting function.
   % ----------------------------------------------------------

   dBMax_clean       = max(clean_energy);
   dBMax_processed   = max(processed_energy);

   % The weights are calculated by averaging individual
   % weighting factors from the clean and processed frame.
   % These weights W_clean and W_processed should range
   % from 0 to 1 and place more emphasis on spectral
   % peaks and less emphasis on slope differences in spectral
   % valleys.  This procedure is described on page 1280 of
   % Klatt's 1982 ICASSP paper.

   Wmax_clean        = Kmax ./ (Kmax + dBMax_clean - ...
		 	    clean_energy(1:num_crit-1));
   Wlocmax_clean     = Klocmax ./ ( Klocmax + clean_loc_peak - ...
				clean_energy(1:num_crit-1));
   W_clean           = Wmax_clean .* Wlocmax_clean;

   Wmax_processed    = Kmax ./ (Kmax + dBMax_processed - ...
			        processed_energy(1:num_crit-1));
   Wlocmax_processed = Klocmax ./ ( Klocmax + processed_loc_peak - ...
			            processed_energy(1:num_crit-1));
   W_processed       = Wmax_processed .* Wlocmax_processed;

   W = (W_clean + W_processed)./2.0;

   distortion(frame_count) = sum(W.*(clean_slope(1:num_crit-1) - ...
		       processed_slope(1:num_crit-1)).^2);

   % this normalization is not part of Klatt's paper, but helps
   % to normalize the measure.  Here we scale the measure by the
   % sum of the weights.

   distortion(frame_count) = distortion(frame_count)/sum(W);

   start = start + skiprate;

end

%-----------------------------------------------
function distortion = llr(clean_speech, processed_speech,sample_rate)

% ----------------------------------------------------------------------
% Check the length of the clean and processed speech.  Must be the same.
% ----------------------------------------------------------------------

clean_length      = length(clean_speech);
processed_length  = length(processed_speech);

if (clean_length ~= processed_length)
  disp('Error: Both Speech Files must be same length.');
  return
end

% ----------------------------------------------------------------------
% Global Variables
% ----------------------------------------------------------------------

winlength   = round(30*sample_rate/1000); %  window length in samples
skiprate    = floor(winlength/4);		   % window skip in samples
if sample_rate<10000
   P           = 10;		   % LPC Analysis Order
else
    P=16;     % this could vary depending on sampling frequency.
end

% ----------------------------------------------------------------------
% For each frame of input speech, calculate the Log Likelihood Ratio
% ----------------------------------------------------------------------

num_frames = clean_length/skiprate-(winlength/skiprate); % number of frames
start      = 1;					% starting sample
window     = 0.5*(1 - cos(2*pi*(1:winlength)'/(winlength+1)));

for frame_count = 1:num_frames

   % ----------------------------------------------------------
   % (1) Get the Frames for the test and reference speech.
   %     Multiply by Hanning Window.
   % ----------------------------------------------------------

   clean_frame = clean_speech(start:start+winlength-1);
   processed_frame = processed_speech(start:start+winlength-1);
   clean_frame = clean_frame.*window;
   processed_frame = processed_frame.*window;

   % ----------------------------------------------------------
   % (2) Get the autocorrelation lags and LPC parameters used
   %     to compute the LLR measure.
   % ----------------------------------------------------------

   [R_clean, Ref_clean, A_clean] = ...
      lpcoeff(clean_frame, P);
   [R_processed, Ref_processed, A_processed] = ...
      lpcoeff(processed_frame, P);

   % ----------------------------------------------------------
   % (3) Compute the LLR measure
   % ----------------------------------------------------------

   numerator   = A_processed*toeplitz(R_clean)*A_processed';
   denominator = A_clean*toeplitz(R_clean)*A_clean';
   distortion(frame_count) = log(numerator/denominator);
   start = start + skiprate;

end

%---------------------------------------------
function [acorr, refcoeff, lpparams] = lpcoeff(speech_frame, model_order)

   % ----------------------------------------------------------
   % (1) Compute Autocorrelation Lags
   % ----------------------------------------------------------

   winlength = max(size(speech_frame));
   for k=1:model_order+1
      R(k) = sum(speech_frame(1:winlength-k+1) ...
		     .*speech_frame(k:winlength));
   end

   % ----------------------------------------------------------
   % (2) Levinson-Durbin
   % ----------------------------------------------------------

   a = ones(1,model_order);
   E(1)=R(1);
   for i=1:model_order
      a_past(1:i-1) = a(1:i-1);
      sum_term = sum(a_past(1:i-1).*R(i:-1:2));
      rcoeff(i)=(R(i+1) - sum_term) / E(i);
      a(i)=rcoeff(i);
      a(1:i-1) = a_past(1:i-1) - rcoeff(i).*a_past(i-1:-1:1);
      E(i+1)=(1-rcoeff(i)*rcoeff(i))*E(i);
   end

   acorr    = R;
   refcoeff = rcoeff;
   lpparams = [1 -a];

   % ----------------------------------------------------------------------

function [overall_snr, segmental_snr] = snr(clean_speech, processed_speech,sample_rate)

% ----------------------------------------------------------------------
% Check the length of the clean and processed speech.  Must be the same.
% ----------------------------------------------------------------------

clean_length      = length(clean_speech);
processed_length  = length(processed_speech);

if (clean_length ~= processed_length)
  disp('Error: Both Speech Files must be same length.');
  return
end

% ----------------------------------------------------------------------
% Scale both clean speech and processed speech to have same dynamic
% range.  Also remove DC component from each signal
% ----------------------------------------------------------------------

%clean_speech     = clean_speech     - mean(clean_speech);
%processed_speech = processed_speech - mean(processed_speech);

%processed_speech = processed_speech.*(max(abs(clean_speech))/ max(abs(processed_speech)));

overall_snr = 10* log10( sum(clean_speech.^2)/sum((clean_speech-processed_speech).^2));

% ----------------------------------------------------------------------
% Global Variables
% ----------------------------------------------------------------------

winlength   = round(30*sample_rate/1000); %240;		   % window length in samples
skiprate    = floor(winlength/4);		   % window skip in samples
MIN_SNR     = -10;		   % minimum SNR in dB
MAX_SNR     =  35;		   % maximum SNR in dB

% ----------------------------------------------------------------------
% For each frame of input speech, calculate the Segmental SNR
% ----------------------------------------------------------------------

num_frames = clean_length/skiprate-(winlength/skiprate); % number of frames
start      = 1;					% starting sample
window     = 0.5*(1 - cos(2*pi*(1:winlength)'/(winlength+1)));

for frame_count = 1: num_frames

   % ----------------------------------------------------------
   % (1) Get the Frames for the test and reference speech.
   %     Multiply by Hanning Window.
   % ----------------------------------------------------------

   clean_frame = clean_speech(start:start+winlength-1);
   processed_frame = processed_speech(start:start+winlength-1);
   clean_frame = clean_frame.*window;
   processed_frame = processed_frame.*window;

   % ----------------------------------------------------------
   % (2) Compute the Segmental SNR
   % ----------------------------------------------------------

   signal_energy = sum(clean_frame.^2);
   noise_energy  = sum((clean_frame-processed_frame).^2);
   segmental_snr(frame_count) = 10*log10(signal_energy/(noise_energy+eps)+eps);
   segmental_snr(frame_count) = max(segmental_snr(frame_count),MIN_SNR);
   segmental_snr(frame_count) = min(segmental_snr(frame_count),MAX_SNR);

   start = start + skiprate;

end


% NMFLAB for Signal Processing written by A. Cichocki and R. Zdunek
% in cooperation with other members of Laboratory for Advanced Brain Signal
% Processing, BSI, RIKEN, Saitama, JAPAN

function [A,X,Distance_output]=nmf_conv(Y,r,Index_norm, A_true, S, mc_on, NoAlts,restart_mc_on,AL,Y_true,type_alg_A, type_alg_X, max_restart, no_conv)
%
% Non-negative Matrix Factorization (NMF) with convolutive algorithms
%
% [A,X]=nmf_IP(Y, r, Index_norm, A_true, S, mc_on, NoAlts, restart_mc_on, AL, Y_true, type_alg_A, type_alg_X, ...
%       max_restart, no_iter, alpha0, tau)
%       produces mixing matrix A of dimension [m by r],
%       and source matrix X of dimension [r by T], for the linear mixing model: AX = Y,
%       where Y is an observation matrix [m by T].
% Note: > m: number of sensors,
%       > r: number of sources,
%       > T: number of samples,
%
% INPUTS:
%       > Index_norm:    vector of 13 binary entries indicating which number
%                        divergence measures are turned on in the View Options,
%
%       > A_true:        true mixing matrix (only for synthetic data),
%       > S:             true source matrix (only for synthetic data),
%       > mc_on:         1 - Monte Carlo analysis enabled, 0 - otherwise,
%       > NoAlts:        number of alternating steps (only for Monte Carlo
%                        analysis and the option "Fixed Alternatings" is selected)
%       > restart_mc_on: 1 - restarts in Monte Carlo analysis are enabled,
%                        0 - otherwise,
%       > AL:            mixing matrix estimaed from the preceeding layer,
%       > Y_true:        the first layer mixed signals (mixtures),
%       > type_alg_A:    indicates the selected algorithm for computation
%                        of the mixing matrix,
%       > type_alg_X:    indicates the selected algorithm for computation of the sources,
%       > max_restart:   number of restarts,
%       > no_iter:       number of inner iterations,
%       > alpha0:        initial magnitude in the exponential model for regularization parameter,
%       > tau:           damping factor in the exponential model for regularization parameter,
%
% OUTPUTS:
%       > A:               estimated mixing matrix,
%       > X:               estimated source matrix,
%       > Distance_output: structures of different divergences measured between "Y" and estimated "AX" versus iterations,
%
% #########################################################################
A = [];
X = [];
if (nargin < 14) | isempty(no_conv) | (no_conv < 0) | max(size(no_conv) > 1)
   disp('Incorrect number of convolutive components');
   return
end
if (nargin < 13) | isempty(max_restart)  | (max_restart < 0) | max(size(max_restart) > 1)
   disp('Number of restarts must be given correctly');
   return
end
if (nargin < 12) | isempty(type_alg_X) | (type_alg_X < 1) | max(size(type_alg_X) > 1)
   disp('Incorrect algorithm for X');
   return
end
if (nargin < 11) | isempty(type_alg_A) | (type_alg_A < 1) | max(size(type_alg_A) > 1)
   disp('Incorrect algorithm for A');
   return
end
if (nargin < 10) | isempty(Y_true)
   disp('The first layer mixed signals are unknown');
   Y_true = zeros(size(Y_true));
end
if (nargin < 9) | isempty(AL)
   disp('Mixing matrix from the preceeding layer unknown');
   AL = eye(size(Y,1));
end
if (nargin < 8) | isempty(restart_mc_on) | max(size(restart_mc_on) > 1)
   disp('Index od restarts in MC analysis unknown');
   restart_mc_on = 0;
end
if (nargin < 7) | isempty(NoAlts) | max(size(NoAlts) > 1)
   disp('Adjustable number of alternatings');
   NoAlts = [];
end
if (nargin < 6) | isempty(mc_on) | max(size(mc_on) > 1)
   disp('No Monte Carlo Analysis');
   mc_on = 0;
end
if (nargin < 5) | isempty(S)
   disp('X_true not given');
end
if (nargin < 4) | isempty(A_true)
   disp('A_true not given');
   index_fixed_A = 1;
else
   index_fixed_A = 0;
end
if (nargin < 3) | isempty(Index_norm)
   '"Index_norm" must be specified'
   return
end
if (nargin < 2) | isempty(r)
   'Rank of factorization must be given'
   return
end

if isempty(Y) | isnan(Y)
   error('No data');
   return
end
% test for negative values in Y
if min(min(Y)) < 0
    disp('Some matrix entries are changed from negative to small positive');
    Y(Y< 0) = eps;
end
if min(sum(Y,2)) == 0
    disp('Not all entries in a row can be zero');
    return
end

[m,T]=size(Y);
niter_selected = 1000;     % maximum number of iterations for the selected sample (can be adjusted)
niter_sample = 30; % maximum number of iterations for each random sample
epsil_normA = 1E-14; % tolerance for alternating

if mc_on & ~restart_mc_on
    max_restart = 0;
end
if ~isempty(NoAlts)
    niter_selected = NoAlts;
end

if (type_alg_A == 3) & (size(A_true,1) ~= size(Y,1))
   disp('Multilayer technique cannot be used with A fixed');
   Distance_output = [];
   return
end

% Declaration for A and X
A=zeros(m,r,no_conv+1);
Ap = A;
X=zeros(r,T);
Ainit = A;
Xinit = X;
Z = zeros(m,T);
KL_outer_temp = 0;
Z_outer = 0;
nr = 0; restart_on = 0; norm_A = 10; nr_best = -1;
m_sx = 1:m; r_sx = 1:r; T_sx = 1:T;
s_dist = 0;
delta = .1;

while (nr <= max_restart)

   % Initialize random A and X
       if ~nr & (~mc_on | restart_mc_on)
          Ainit_1(m_sx',r_sx) = abs(repmat(.1*sin(2*pi*.1*m_sx'),1,r) + repmat(.1*cos(2*pi*.1*r_sx),m,1) + repmat(cos(2*pi*.471*m_sx'),1,r) + repmat(sin(2*pi*.471*r_sx),m,1));
          Ainit_1 = Ainit_1/max(max(Ainit_1));
          Ainit = repmat(Ainit_1,[1 1 no_conv+1]);

          Xinit(r_sx',T_sx) = abs(repmat(.1*sin(2*pi*.1*r_sx'),1,T) + repmat(.1*cos(2*pi*.1*T_sx),r,1) + repmat(cos(2*pi*.471*r_sx'),1,T) + repmat(sin(2*pi*.471*T_sx),r,1));
          Xinit = Xinit/max(max(Xinit));
       else
          Ainit=rand(m,r,no_conv+1);
          Xinit=rand(r,T);
       end

        % Normalization of initial guess
       Ainit = Ainit.*repmat((1./sum(Ainit,1)),[m,1,1]);

        if (nr == max_restart)&(max_restart > 0)
           A = A_best;
           X = X_best;
        else
           A = Ainit;
           X = Xinit;
        end % initial guess assignment

    Yx = zeros(m,T);
    n = 0; k = 0;

while ((k <= niter_sample)&(nr < max_restart)) | ((k <= niter_selected)&(nr == max_restart)&(norm_A > epsil_normA)& isempty(NoAlts)) | ((k <= niter_selected)&(nr == max_restart)& (NoAlts > 0))

k = k + 1;

l = mod(k-1,no_conv+1)+1;
%Xs = (circshift(X',l-1))';
Xs = X(:, (no_conv-l+2):(T-l+1));
Ys = Y(:, (no_conv-l+2):(T-l+1));

        switch type_alg_A

           case 1 % ALS

              Ap = A;
              Xpinv = Xs'*pinv(Xs*Xs');
              A(:,:,l) = max(1E6*eps, Ys*Xpinv);
              A = A.*repmat((1./sum(A,1)),[m,1,1]);

           case 2 % EMML

              Ap = A;
              A(:,:,l) = A(:,:,l).*((Y./(Yz + eps))*Xs')./repmat(sum(Xs,2)',m,1);
              A = A.*repmat((1./sum(A,1)),[m,1,1]);

           case 3 % Fixed A

              Ap = A;
              niter_selected = 1000;     % maximum number of iterations for the selected sample (can be adjusted)
              A = A_true + eps;
              A = A.*repmat((1./sum(A,1)),[m,1,1]);

        end % type_alg_A

        switch type_alg_X

            case 1 % ALS

               Xs = max(1E6*eps,pinv(squeeze(A(:,:,l))'*squeeze(A(:,:,l)))*squeeze(A(:,:,l))'*Ys);

            case 2 % EMML

              Zy = (circshift((Y./(Yz + eps))',-l+1))';
               if l > 1
                  Zy(:,(T-l+2):T) = zeros(m,l-1);
               end
              X = X.*(A(:,:,l)'*Zy)./repmat(sum(A(:,:,l),1)',1,T);

            case 3 % Fixed X

              X = S + eps;
              niter_selected = 1000;     % maximum number of iterations for the selected sample (can be adjusted)

        end % type_alg_X

      %  X = (circshift(Xs',-l+1))';

                if (nr == max_restart)&(mod(k,50)==0)& (~mc_on | restart_mc_on)
                   norm_A = sqrt(sum(sum(sum((A - Ap).^2))));
                   fprintf(1, 'Restart %d,  %d-th alternating step\n',nr_best+1,k);
                end

                if sum(Index_norm)
                   if (nr == max_restart) & (((k < 50) & (mod(k,5)==0)) | ((k>49) & ((mod(k,50)==0))))

                       s_dist = s_dist + 1;
                       k_select(s_dist) = k;
                       Z = A*X + eps;
                       Z = diag(1./sqrt(var(Z')))*Z;
                       Y = Y + eps;

                        dist_Fro(s_dist) = norm(Y - Z,'fro');
                        dist_KL(s_dist) = sum(sum(Y.*log(Y./Z + eps) - Y + Z));
                        dist_KL2(s_dist) = sum(sum(Z.*log(Z./Y + eps) + Y - Z));
                        dist_Pearson(s_dist) = sum(sum( ((Y - Z).^2)./Z ));
                        dist_Hellinger(s_dist) = sum(sum( (sqrt(Z) - sqrt(Y)).^2 ));
                        dist_JS_rel(s_dist) = sum(sum(2*Y.*log(2*Y./(Y + Z) + eps) + Z - Y));
                        dist_JS_rel2(s_dist) = sum(sum(2*Z.*log(2*Z./(Y + Z) + eps) - Z + Y));
                        Zy = Y + Z;
                        dist_JS(s_dist) = sum(sum(Y.*log(2*Y./Zy + eps) + Z.*log(2*Z./Zy + eps) ));
                        dist_AG_rel(s_dist) = sum(sum(Zy.*log(.5*Zy./Y + eps) + Y - Z));
                        dist_AG(s_dist) = sum(sum(.5*Zy.*log(.5*Zy./sqrt(Y.*Z) + eps)));
                        dist_J(s_dist) = sum(sum( .5*(Y - Z).*log(Y./Z + eps) ));
                        dist_Chi(s_dist) = sum(sum( ((Y + Z).*(Y - Z).^2)./(Y.*Z) ));
                        dist_Tria(s_dist) = sum(sum( ((Y - Z).^2)./(Y + Z) ));

                    end % if multiple
                end % if sum

end % while (k)

% Outer KL divergence
if ndims(A) > 2
   Yz = zeros(m,T);
   for t = 1:size(A,3)
       Ytmp = squeeze(A(:,:,t))*X;
       Yz = Yz + Ytmp;
   end
   Z = AL*Yz;
else
    Z = AL*A*X;
end
Z_outer = norm(Z,'fro') + eps;
KL_outer = sum(sum(Y_true.*log((Y_true + eps)./(Z + eps)) - Y_true + Z))/Z_outer;

           if (nr == 0) | (KL_outer < KL_outer_temp)
              A_best = A; X_best = X; KL_outer_temp = KL_outer; nr_best = nr;
           end % multi-conditions

   nr = nr + 1;

   if nr <=max_restart
      fprintf(1, 'Restart %d, Kullback-Leibler divergence = %e\n',	nr, KL_outer);
   end

end % while (restarts)

% One-Variance scaling
X(X <= 0) = eps;

if ndims(A) > 2
   A =  sum(A,3);
end

Distance_output = cell(length(s_dist),1);
Distance_output(1) = {[]};
Distance_output(2) = {[]};
Distance_output(3) = {[]};
Distance_output(4) = {[]};
Distance_output(5) = {[]};
Distance_output(6) = {[]};
Distance_output(7) = {[]};
Distance_output(8) = {[]};
Distance_output(9) = {[]};
Distance_output(10) = {[]};
Distance_output(11) = {[]};
Distance_output(12) = {[]};
Distance_output(13) = {[]};
Distance_output(14) = {[]};

if sum(Index_norm)
   Distance_output(1) = {k_select};
   Distance_output(2) = {dist_Fro};
   Distance_output(3) = {dist_KL};
   Distance_output(4) = {dist_KL2};
   Distance_output(5) = {dist_Pearson};
   Distance_output(6) = {dist_Hellinger};
   Distance_output(7) = {dist_JS_rel};
   Distance_output(8) = {dist_JS_rel2};
   Distance_output(9) = {dist_JS};
   Distance_output(10) = {dist_AG_rel};
   Distance_output(11) = {dist_AG};
   Distance_output(12) = {dist_J};
   Distance_output(13) = {dist_Chi};
   Distance_output(14) = {dist_Tria};
end


function [SE_MR,SE_RZF,SE_MMMSE,SE_ZF,SE_SMMSE] = functionComputeSE_DL_hardening(H,Hhat,C,R,tau_c,tau_p,nbrOfRealizations,M,K,L,p,rho)
%Compute DL SE for different transmit precoding schemes using Theorem 4.6.
%
%INPUT:
%H                 = M x nbrOfRealizations x K x L x L matrix with the
%                    exact channel realizations
%Hhat              = M x nbrOfRealizations x K x L x L matrix with the
%                    channel estimates
%C                 = M x M x K x L x L matrix with estimation error
%                    correlation matrices
%R                 = M x M x K x L x L matrix with spatial correlation
%                    matrices
%tau_c             = Length of coherence block
%tau_p             = Length of pilot sequences
%nbrOfRealizations = Number of channel realizations
%M                 = Number of antennas per BS
%K                 = Number of UEs per cell
%L                 = Number of BSs and cells
%p                 = Uplink transmit power per UE (same for everyone)
%rho               = Downlink transmit power per UE (same for everyone)
%
%OUTPUT:
%SE_MR    = K x L x length(tau_c) matrix where element (k,l,:) is the
%           downlink SE of UE k in cell l achieved with MR precoding for
%           different lengths of the coherence block
%SE_RZF   = Same as SE_MR but with RZF precoding
%SE_MMMSE = Same as SE_MR but with M-MMSE precoding
%SE_ZF    = Same as SE_MR but with ZF precoding
%SE_SMMSE = Same as SE_MR but with S-MMSE precoding
%
%
%This Matlab function was developed to generate simulation results to:
%
%Emil Bjornson, Jakob Hoydis and Luca Sanguinetti (2017),
%"Massive MIMO Networks: Spectral, Energy, and Hardware Efficiency",
%Foundations and Trends in Signal Processing: Vol. 11, No. 3-4,
%pp. 154-655. DOI: 10.1561/2000000093.
%
%For further information, visit: https://www.massivemimobook.com
%
%This is version 1.01 (Last edited: 2020-05-15)
%
%License: This code is licensed under the GPLv2 license. If you in any way
%use this code for research that results in publications, please cite our
%monograph as described above.

%Store identity matrices of different sizes
eyeK = eye(K);
eyeM = eye(M);

if nargout > 2

    %Compute sum of all estimation error correlation matrices at every BS
    C_totM = reshape(p*sum(sum(C,3),4),[M M L]);

end

%Compute sum of intra-cell estimation error correlation matrices at every BS
if nargout>4

    CR_totS = zeros(M,M,L);

    for j = 1:L
        CR_totS(:,:,j) = p*(sum(C(:,:,:,j,j),3)+sum(sum(R(:,:,:,[1:j-1 j+1:end],j),3),4));
    end

end

%Compute the prelog factor assuming only downlink transmission
prelogFactor = (tau_c-tau_p)./(tau_c);

%Prepare to store simulation results for signal gains
signal_MR = zeros(K,L);

if nargout > 1
    signal_RZF = zeros(K,L);
end

if nargout > 2
    signal_MMMSE = zeros(K,L);
end

if nargout > 3
    signal_ZF = zeros(K,L);
end

if nargout > 4
    signal_SMMSE = zeros(K,L);
end

%Prepare to store simulation results for sum interference powers
interf_MR = zeros(K,L);

if nargout > 1
    interf_RZF = zeros(K,L);
end

if nargout > 2
    interf_MMMSE = zeros(K,L);
end

if nargout > 3
    interf_ZF = zeros(K,L);
end

if nargout > 4
    interf_SMMSE = zeros(K,L);
end

%% Go through all channel realizations
for n = 1:nbrOfRealizations

    %Go through all cells
    for j = 1:L

        %Extract channel realizations from all UEs to BS j
        Hallj = reshape(H(:,n,:,:,j),[M K*L]);

        %Extract channel realizations from all UEs to BS j
        Hhatallj = reshape(Hhat(:,n,:,:,j),[M K*L]);

        %Compute MR combining in (4.11)
        V_MR = Hhatallj(:,K*(j-1)+1:K*j);

        if nargout > 1 %Compute RZF combining in (4.9)
            V_RZF = (p*V_MR)/(p*(V_MR'*V_MR)+eyeK);
        end

        if nargout > 2 %Compute M-MMSE combining in (4.7)
            V_MMMSE = (p*(Hhatallj*Hhatallj')+C_totM(:,:,j)+eyeM)\(p*V_MR);
        end

        if nargout > 3 %Compute ZF combining in (4.10), with the small regularization term 1e-12 for numerical stability
            V_ZF = V_MR/(V_MR'*V_MR+1e-12*eyeK);
        end

        if nargout > 4 %Compute S-MMSE combining in (4.8)
            V_SMMSE = (p*(V_MR*V_MR')+CR_totS(:,:,j)+eyeM)\(p*V_MR);
        end

        %Go through all UEs in cell j
        for k = 1:K

            if norm(V_MR(:,k))>0

                %%MR precoding in (4.37) computed based on combining vector
                w = V_MR(:,k)/norm(V_MR(:,k));

                %Compute realizations of the terms inside the expectations
                %of the signal and interference terms of (4.26)
                signal_MR(k,j) = signal_MR(k,j) + (w'*H(:,n,k,j,j))/nbrOfRealizations;
                interf_MR = interf_MR + rho*reshape(abs(w'*Hallj).^2,[K L])/nbrOfRealizations;

                %%RZF precoding
                if nargout > 1

                    %RZF precoding in (4.37) computed based on combining vector
                    w = V_RZF(:,k)/norm(V_RZF(:,k));

                    %Compute realizations of the terms inside the expectations
                    %of the signal and interference terms of (4.26)
                    signal_RZF(k,j) = signal_RZF(k,j) + (w'*H(:,n,k,j,j))/nbrOfRealizations;
                    interf_RZF = interf_RZF + rho*reshape(abs(w'*Hallj).^2,[K L])/nbrOfRealizations;

                end

                %%M-MMSE precoding
                if nargout > 2

                    %M-MMSE precoding in (4.37) computed based on combining vector
                    w = V_MMMSE(:,k)/norm(V_MMMSE(:,k));

                    %Compute realizations of the terms inside the expectations
                    %of the signal and interference terms of (4.26)
                    signal_MMMSE(k,j) = signal_MMMSE(k,j) + (w'*H(:,n,k,j,j))/nbrOfRealizations;
                    interf_MMMSE = interf_MMMSE + rho*reshape(abs(w'*Hallj).^2,[K L])/nbrOfRealizations;

                end

                %%ZF precoding
                if nargout > 3

                    %ZF precoding in (4.37) computed based on combining vector
                    w = V_ZF(:,k)/norm(V_ZF(:,k));

                    %Compute realizations of the terms inside the expectations
                    %of the signal and interference terms of (4.26)
                    signal_ZF(k,j) = signal_ZF(k,j) + (w'*H(:,n,k,j,j))/nbrOfRealizations;
                    interf_ZF = interf_ZF + rho*reshape(abs(w'*Hallj).^2,[K L])/nbrOfRealizations;

                end

                %%S-MMSE precoding
                if nargout > 4

                    %S-MMSE precoding in (4.37) computed based on combining vector
                    w = V_SMMSE(:,k)/norm(V_SMMSE(:,k));

                    %Compute realizations of the terms inside the expectations
                    %of the signal and interference terms of (4.26)
                    signal_SMMSE(k,j) = signal_SMMSE(k,j) + (w'*H(:,n,k,j,j))/nbrOfRealizations;
                    interf_SMMSE = interf_SMMSE + rho*reshape(abs(w'*Hallj).^2,[K L])/nbrOfRealizations;

                end

            end

        end

    end

end

%% Prepare to compute the SEs with different pilot reuse factors
SE_MR = zeros(K,L,length(tau_c));

if nargout > 1
    SE_RZF = zeros(K,L,length(tau_c));
end

if nargout > 2
    SE_MMMSE = zeros(K,L,length(tau_c));
end

if nargout > 3
    SE_ZF = zeros(K,L,length(tau_c));
end

if nargout > 4
    SE_SMMSE = zeros(K,L,length(tau_c));
end

%Go through all lengths of the pilot reuse factors
for n = 1:length(tau_c)

    %Compute SEs according to Theorem 4.6
    SE_MR(:,:,n) = prelogFactor(n)*real(log2(1+(rho*abs(signal_MR).^2) ./ (interf_MR - rho*abs(signal_MR).^2 + 1)));

    if nargout > 1
        SE_RZF(:,:,n) = prelogFactor(n)*real(log2(1+(rho*abs(signal_RZF).^2) ./ (interf_RZF - rho*abs(signal_RZF).^2 + 1)));
    end

    if nargout > 2
        SE_MMMSE(:,:,n) = prelogFactor(n)*real(log2(1+(rho*abs(signal_MMMSE).^2) ./ (interf_MMMSE - rho*abs(signal_MMMSE).^2 +1)));
    end

    if nargout > 3
        SE_ZF(:,:,n) = prelogFactor(n)*real(log2(1+(rho*abs(signal_ZF).^2) ./ (interf_ZF - rho*abs(signal_ZF).^2 + 1)));
    end

    if nargout > 4
        SE_SMMSE(:,:,n) = prelogFactor(n)*real(log2(1+(rho*abs(signal_SMMSE).^2) ./ (interf_SMMSE - rho*abs(signal_SMMSE).^2 + 1)));
    end

end


function [metaboliteList, stoichCoeffList, revFlag] = parseRxnFormula(formula)
% Parses reaction formula into a list of metabolites and a
% list of `S` coefficients
%
% USAGE:
%
%    [metaboliteList, stoichCoeffList, revFlag] = parseRxnFormula(formula)
%
% INPUT:
%    formula:           Reaction formula, may contain symbols '+', '->', '<=>' in
%                       addition to stoichiometric coefficients and metabolite names
%                       examples:
%                       '0.01 cdpdag-SC[m] + 0.01 pg-SC[m]  -> 0.01 clpn-SC[m] + cmp[m] + h[m]' (irreversible reaction)
%                       'cit[c] + icit[x]  <=> cit[x] + icit[c] ' (reversible reaction)
%                       If no stoichiometric coefficient is provided, it is assumed
%                       to be = 1.
%                       Reaction formula should be a string, not a cell array
%
% OUTPUTS:
%    metaboliteList:     Cell array with metabolite names
%    stoichCoeffList:    List of `S` coefficients
%    revFlag:            Indicates whether the reaction is reversible (true) or not (false)
%
% EXAMPLE:
%
%    formula = '0.01 cdpdag-SC[m] + 0.01 pg-SC[m]  -> 0.01 clpn-SC[m] + cmp[m] + h[m]'
%
%    [metaboliteList, stoichCoeffList, revFlag] = parseRxnFormula(formula)
%
%    %metaboliteList = 'cdpdag-SC[m]'    'pg-SC[m]'    'clpn-SC[m]'    'cmp[m]'    'h[m]'
%    %stoichCoeffList = -0.01 -0.01 0.01 1 1
%    %revFlag = false
%
% .. Authors:
%       - Markus Herrgard 6/1/07
%       - Richard Que 1/25/10 Modified to handle '-->' and '<==>' as arrows
%         as well as reactionsformatted as '[compartment] : A --> C'.
%       - IT May 2012 Modified to handle '=>'

tokens = splitString(formula);

stoichCoeffList = [];
metaboliteList = {};
revFlag = true;

% Marks the start of a new stoichiometry + metabolite block
newMetFlag = true;
% Designates products vs reactants
productFlag = false;
compartment = '';
for i = 1:length(tokens)
    t = tokens{i};
    if strcmp(t(1),'[')
        %set compartment
        compartment = t;
    elseif strcmp(t,':')
        %Do nothing
    elseif strcmp(t,'+')
        % Do nothing
        newMetFlag = true;
    elseif strcmp(t,'->') || strcmp(t,'-->') || strcmp(t,'=>')
        % Irreversible
        revFlag = false;
        productFlag = true;
        newMetFlag = true;
    elseif strcmp(t,'<=>') || strcmp(t,'<==>')
        % Reversible
        revFlag = true;
        productFlag = true;
        newMetFlag = true;
    else
        sCoeff = str2double(t);
        if (~isnan(sCoeff))
            % Stoich coefficient
            if ~productFlag
                sCoeff = -sCoeff;
            end
            stoichCoeffList(end+1) = sCoeff;
            newMetFlag = false;
        else
            % Metabolite name
            metaboliteList{end+1} = strcat(t,compartment);
            if newMetFlag
                if ~productFlag
                    stoichCoeffList(end+1) = -1;
                else
                    stoichCoeffList(end+1) = 1;
                end
                newMetFlag = true;
            end
        end
    end
end


function res = adjD(y)

res = zeros(size(y,1),size(y,2));

%y1 = ones(imsize)*y(1)/sqrt(prod(imsize));
%yx = (reshape(y(2:prod(imsize)+1), imsize(1), imsize(2)));
%yy = (reshape(y(prod(imsize)+2:end), imsize(1), imsize(2)));

res = adjDx(y(:,:,1)) + adjDy(y(:,:,2));

return;

function res = adjDy(x)
res = x(:,[1,1:end-1]) - x;
res(:,1) = -x(:,1);
res(:,end) = x(:,end-1);

function res = adjDx(x)
res = x([1,1:end-1],:) - x;
res(1,:) = -x(1,:);
res(end,:) = x(end-1,:);


function z_hat = estimate_states(uu, time, dt, p_drone, p_physics)
% ESTIMATE_STATES: Estimate the Drone states using gyros, accels, pressure
% sensors, and GPS.
% Inputs:
%   uu: input measurements
%   p_drone: drone parameters
%
% Outputs:
%   pnhat    - estimated North position,
%   pehat    - estimated East position,
%   hhat     - estimated altitude,
%   Vahat    - estimated airspeed,
%   alphahat - estimated angle of attack
%   betahat  - estimated sideslip angle
%   phihat   - estimated roll angle,
%   thetahat - estimated pitch angel,
%   chihat   - estimated course,
%   phat     - estimated roll rate,
%   qhat     - estimated pitch rate,
%   rhat     - estimated yaw rate,
%   Vghat    - estimated ground speed,
%   wnhat    - estimate of North wind,
%   wehat    - estimate of East wind
%   psihat   - estimate of heading angle
%

% Rename inputs
y_gyro_x      = uu(1);
y_gyro_y      = uu(2);
y_gyro_z      = uu(3);
y_accel_x     = uu(4);
y_accel_y     = uu(5);
y_accel_z     = uu(6);
y_static_pres = uu(7);
y_diff_pres   = uu(8);

y_gps_n       = uu(9);
y_gps_e       = uu(10);
y_gps_h       = uu(11);
y_gps_Vg      = uu(12);
y_gps_course  = uu(13);

% Persistent variables
persistent  kf1;
%persistent  kf2;
persistent x;
persistent kf_P;

% Initilize estimated states at t=0
if time == 0
    kf1.x_prev = [p_drone.phi_T, p_drone.theta_T]';
    kf1.P_prev = zeros(2,2);
    Vg_T = p_drone.Va_T;
    chi_T = p_drone.psi_T;
    %kf2.x_prev = [P.pn_T; P.pe_T; Vg_T; chi_T; P.wn; P.we; P.psi_T];
    %kf2.P_prev = zeros(7,7);
    phi_hat     = p_drone.phi_T;
    theta_hat   = p_drone.theta_T;
    x = [0; 0];
    kf_P = [0, 0;
        0  , 0];
end

% Not estimating these states
alphahat = 0;
betahat  = 0;
bxhat    = 0;
byhat    = 0;
bzhat    = 0;

pnhat=0;
pehat=0;
hhat=0;
Vahat=0;
alphahat=0;
betahat=0;
phihat=0;
thetahat=0;
chihat=0;
phat=0;
qhat=0;
rhat=0;
Vghat=0;
wnhat=0;
wehat=0;
psihat=0;
bxhat=0;
byhat=0;
bzhat=0;

%% Estimate states by low pass filtering

phat     = lp_filter( y_gyro_x, 0, p_drone.a_gyro, dt);
qhat     = lp_filter( y_gyro_y, 0, p_drone.a_gyro, dt);
rhat     = lp_filter( y_gyro_z, 0, p_drone.a_gyro, dt);

hhat     = (p_drone.pres0 - lp_filter( y_static_pres, 0, p_drone.a_static_pres, dt))/...
    (p_physics.rho*p_physics.gravity);
Vahat    = sqrt(2/p_physics.rho*lp_filter(y_diff_pres, 0, p_drone.a_diff_pres, dt));

%thetahat = asin(lp_filter(y_accel_x, 0, P.a_accel, P.dt)/p_physics.gravity);
%phihat   = atan(lp_filter(y_accel_y, 0, P.a_accel, P.dt)/lp_filter(y_accel_z, 0, P.a_accel, P.dt));

pnhat    = lp_filter(y_gps_n, 0, p_drone.a_gps_pos, dt);
pehat    = lp_filter(y_gps_e, 0, p_drone.a_gps_pos, dt);
chihat   = lp_filter(y_gps_course, 0, p_drone.a_gps_speed, dt);
Vghat    = lp_filter(y_gps_Vg, 0, p_drone.a_gps_speed, dt);

%% Estimated states by kalman filter

% Reconstruct theta and phi measured
% theta_m  = asin((y_accel_x)/p_physics.gravity);
% phi_m    = atan(y_accel_y/y_accel_z);
% last_measure1 = [ t       phi_m;           % not really t, but time of sensor measurement
%                   t       theta_m];        % not really t, ...
%
% input_z_hat = [phihat; thetahat; phat; qhat; rhat; Vahat];
% [F1, G1, H1, u1, Q1, R1] = ekf_phi_theta( input_z_hat, P);
% [kf1] = ext_kalman_filter(F1, G1, H1, u1, Q1, R1, t, last_measure1, kf1, P);

% Extract x to output variables
% phihat = kf1.x(1);
% thetahat = kf1.x(2);
x_prev = x;
phi_hat = x(1);
theta_hat = x(2);
kf_P_prev = kf_P;

z = [y_accel_x ; y_accel_y; y_accel_z];

% Compute f, F, h and H

cp = cos(phi_hat);
sp = sin(phi_hat);
ct = cos(theta_hat);
st = sin(theta_hat);
tt = tan(theta_hat);

p = y_gyro_x;
q = y_gyro_y;
r = y_gyro_z;
Va = Vahat;
g = p_physics.gravity;

f = [  p + q*sp*tt + r*cp*tt; ...
    q*cp - r*sp];

F = [  q*cp*tt - r*sp*tt,   (q*sp-r*cp)/(ct^2); ...
    - q*sp - r*cp,          0                ];

h = [ q*Va*st + g*st; ...
    r*Va*ct - p*Va*st - g*ct*sp; ...
    -q*Va*ct - g*ct*cp];

H = [  0,                 q*Va*ct + p_physics.gravity*ct;...
    - p_physics.gravity*cp*ct,   - r*Va*st - p*Va*ct + p_physics.gravity*sp*st;...
    p_physics.gravity*sp*ct,   (q*Va+p_physics.gravity*cp)*st               ];

%% Compute prediction (a priori)

x = x_prev+dt*f;    % a priori state estimate
kf_P = kf_P_prev + (F*kf_P_prev + kf_P_prev*F' + p_drone.Q);    % a priori estimate covariance

%% Compute update (a posteriori)

if(norm(z-p_physics.gravity) < 0.5)
    z_tilde = z - H*x;              % innovation (a priori residual)
    S = p_drone.R + H*kf_P*(H');    % innovation covariance
    K = kf_P*(H')/S;                % optimal kalman gain
    x = x + K*z_tilde;              % a posteriori state estimate
    kf_P = (eye(2) - K*H)*kf_P;     % a posteriori estimate covariance
end

z_res = z - H*x;                    % a posteriori residual

phihat      = x(1);
thetahat    = x(2);

%% Output data

z_hat = [
    pnhat;...
    pehat;...
    hhat;...
    0
    0
    0
    Vahat;...
    alphahat;...
    betahat;...
    phihat;...
    thetahat;...
    chihat;...
    phat;...
    qhat;...
    rhat;...
    Vghat;...
    wnhat;...
    wehat;...
    psihat;...
    bxhat;...
    byhat;...
    bzhat
    ];

end