text:
function g = svargplvmLogLikeGradients(model)

% SVARGPLVMLOGLIKEGRADIENTS Compute the gradients for the variational shared GPLVM.
% FORMAT
% DESC returns the gradients of the log likelihood with respect to the
% parameters of the GP-LVM model and with respect to the latent
% positions of the GP-LVM model.
% ARG model : the FGPLVM structure containing the parameters and
% the latent positions.
% RETURN g : the gradients of the latent positions (or the back
% constraint's parameters) and the parameters of the GP-LVM model.
%
% FORMAT
% DESC returns the gradients of the log likelihood with respect to the
% parameters of the GP-LVM model and with respect to the latent
% positions of the GP-LVM model in seperate matrices.
% ARG model : the FGPLVM structure containing the parameters and
% the latent positions.
% RETURN gX : the gradients of the latent positions (or the back
% constraint's parameters).
% RETURN gParam : gradients of the parameters of the GP-LVM model.
%
% COPYRIGHT : Andreas C. Damianou, 2011

% SEEALSO : svargplvmLogLikelihood, svargplvmCreate, modelLogLikeGradients

% VARGPLVM

try
    pool_open = matlabpool('size')>0;
catch e
    pool_open = 0;
end

if pool_open && (isfield(model,'parallel') && model.parallel)
    g=gPar(model);
else
    % Functions g1 and g2 should be equivalent for the static case, but
    % g1 might (?) be faster.
    if ~isfield(model, 'dynamics') || isempty(model.dynamics)
        g=g1(model);
        %g = g2(model);
    else
        if isfield(model.dynamics, 'seq') & ~isempty(model.dynamics.seq)
            g = gDyn(model); % Slower but memory efficient.
        else
            %g = gDyn(model); % Slower but memory efficient.
            g = gDynFast(model); % Faster, but memory consuming
        end
    end
end

%g = g1(model);
%g = g2(model);
end

%%%%%!!!!!! NOTE:
% g1 and g2 should be equivalent for the non-dynamics case. gPar is the
% equivalent of g1 for parallel computations. gDyn and gDynFast are
% suitable when there are dynamics, with the first focusing on memory
% efficiency and the second on speed.

function g = gPar(model)

% THE FOLLOWING WORKS ONLY WHEN THERE ARE NO DYNAMICS... %___ TODO TEMP
% Shared params
if isfield(model, 'dynamics') && isempty(model.dynamics) % TEMP
    error('The gradients for the dynamics case are not implemented correctly yet!'); % TEMP
else % ....
    gVarmeansKL = - model.vardist.means(:)';
    gVarcovsKL = 0.5 - 0.5*model.vardist.covars(:)';
end
model = svargplvmPropagateField(model, 'onlyLikelihood', 1);

%fprintf('# Derivs for KL is (should be zero): ');%%%TEMP

% Private params
% g = [[sum_m(gVar_only_likelihood)+gVar_onlyKL] g_1 g_2 ...]
% where sum_m is the sum over all models and g_m is the gradients for the
% non-shared parameters for model m
gShared = [gVarmeansKL gVarcovsKL];
modelTemp=model.comp;
parfor i=1:model.numModels
    gAll{i} = vargplvmLogLikeGradients(modelTemp{i});
end
g=[];
for i=1:model.numModels
    g_i = gAll{i};
    % Now add the derivatives for the shared parameters.
    if isfield(model, 'dynamics') & ~isempty(model.dynamics) % TEMP (doesn't work correctly for dynamics)
        gShared = gShared + g_i(1:model.dynamics.nParams); % TEMP (doesn't work correctly for dynamics)
        g_i = g_i((model.dynamics.nParams+1):end); % TEMP (doesn't work correctly for dynamics)
    else % else it's only the vardist. of the KL
        gShared = gShared + g_i(1:model.vardist.nParams);
        g_i = g_i((model.vardist.nParams+1):end);
    end
    g = [g g_i];
end
g = [gShared g];

end

function g = g1(model)

% THE FOLLOWING WORKS ONLY WHEN THERE ARE NO DYNAMICS... %___ TODO TEMP
% Shared params
if isfield(model, 'dynamics') && isempty(model.dynamics) % TEMP !!!! TODO (~isempty?)
    error('The gradients for the dynamics case are not implemented correctly yet!'); % TEMP
else % ....
    gVarmeansKL = - model.vardist.means(:)';
    gVarcovsKL = 0.5 - 0.5*model.vardist.covars(:)';
end
model = svargplvmPropagateField(model, 'onlyLikelihood', 1);

%fprintf('# Derivs for KL is (should be zero): ');%%%TEMP

% Private params
% g = [[sum_m(gVar_only_likelihood)+gVar_onlyKL] g_1 g_2 ...]
% where sum_m is the sum over all models and g_m is the gradients for the
% non-shared parameters for model m
g = [];
gShared = [gVarmeansKL gVarcovsKL];

for i=1:model.numModels
    g_i = vargplvmLogLikeGradients(model.comp{i});
    % Now add the derivatives for the shared parameters.
    if isfield(model, 'dynamics') & ~isempty(model.dynamics) % !! (doesn't work correctly for dynamics)
        gShared = gShared + g_i(1:model.dynamics.nParams); % !! (doesn't work correctly for dynamics)
        g_i = g_i((model.dynamics.nParams+1):end); % TEMP (doesn't work correctly for dynamics)
    else % else it's only the vardist. of the KL
        gShared = gShared + g_i(1:model.vardist.nParams);
        g_i = g_i((model.vardist.nParams+1):end);
    end
    g = [g g_i];
end
g = [gShared g];
end
% TEMP = vargplvmLogLikeGradients(model.comp{1}); %%%TEMP
% sum(sum(abs(gShared - TEMP(1:160)))) %%%TEMP
% g = [g 0*g_i]; %%%%%%%%%%% TEMP
%g = [-vargplvmLogLikeGradients(model.comp{1}) 0*g_i];

% NOT TESTED FOR THE DYNAMICS CASE
function g = g2(model)
g_1 = vargplvmLogLikeGradients(model.comp{1});
gShared = g_1(1:model.vardist.nParams);
g_1 = g_1(model.vardist.nParams+1:end);
model = svargplvmPropagateField(model, 'onlyLikelihood', 1, true);
g=[];
for i=2:model.numModels
    g_i = vargplvmLogLikeGradients(model.comp{i});
    % Now add the derivatives for the shared parameters.
    if isfield(model, 'dynamics') & ~isempty(model.dynamics)
        gShared = gShared + g_i(1:model.dynamics.nParams);
        g_i = g_i((model.dynamics.nParams+1):end);
    else % else it's only the vardist. of the KL
        gShared = gShared + g_i(1:model.vardist.nParams);
        g_i = g_i((model.vardist.nParams+1):end);
    end
    g = [g g_i];
end
g = [gShared g_1 g];
end

function g = gDynFast(modelAll)

gPrivAll = [];
gSharedCoeff = 0;
dynModel = modelAll.dynamics;
gSharedVar = zeros(1,dynModel.vardist.nParams);

for m=1:modelAll.numModels
    % This is similar to vargplvmLogLikeGradients but for every model

    model = modelAll.comp{m}; % current model

    % Include calculations for the KL term only once, for the first model.
    if m == 1
        includeKL = 1;
    else
        includeKL = 0;
    end

    % Likelihood terms (coefficients)
    [gK_uu, gPsi0, gPsi1, gPsi2, g_Lambda, gBeta, tmpV] = vargpCovGrads(model);

    if isfield(model, 'learnInducing')
        learnInducing = model.learnInducing;
    else
        learnInducing = true;
    end

    % Get (in three steps because the formula has three terms) the gradients of
    % the likelihood part w.r.t the data kernel parameters, variational means
    % and covariances (original ones). From the field model.vardist, only
    % vardist.means and vardist.covars and vardist.lantentDimension are used.
    [gKern1, gVarmeans1, gVarcovs1, gInd1] = kernVardistPsi1Gradient(model.kern, model.vardist, model.X_u, gPsi1', learnInducing);
    [gKern2, gVarmeans2, gVarcovs2, gInd2] = kernVardistPsi2Gradient(model.kern, model.vardist, model.X_u, gPsi2, learnInducing);
    [gKern0, gVarmeans0, gVarcovs0] = kernVardistPsi0Gradient(model.kern, model.vardist, gPsi0);
    gKern3 = kernGradient(model.kern, model.X_u, gK_uu);

    % At this point, gKern gVarmeansLik and gVarcovsLik have the derivatives for the
    % likelihood part. Sum all of them to obtain the final result.
    gKern = gKern0 + gKern1 + gKern2 + gKern3;
    gVarmeansLik = gVarmeans0 + gVarmeans1 + gVarmeans2;

    if strcmp(model.kern.type, 'rbfardjit')
        % different derivatives for the variance, which is super-numerically stable for
        % this particular kernel
        if model.learnSigmaf == 1
            gKern(1) = 0.5*model.d*( - model.k+ sum(sum(model.invLat.*model.invLat))/model.beta - model.beta*(model.Psi0-model.TrC)  )...
                + 0.5*tmpV;

            if ~isstruct(model.kern.transforms(1))
                fhandle = str2func([model.kern.transform(1) 'Transform']);
                gKern(1) = gKern(1).*fhandle(model.kern.variance, 'gradfact');
            else
                fhandle = str2func([model.kern.transforms(1).type 'Transform']);
                if ~isfield(model.kern.transforms(1), 'transformsettings')
                    gKern(1) = gKern(1).*fhandle(model.kern.variance, 'gradfact');
                else
                    gKern(1) = gKern(1).*fhandle(model.kern.variance, 'gradfact', model.kern.transforms(1).transformsettings);
                end
            end
        else
            gKern(1) = 0;
        end
    end

    %%% Compute Gradients with respect to X_u %%%
    gKX = kernGradX(model.kern, model.X_u, model.X_u);

    % The 2 accounts for the fact that covGrad is symmetric
    gKX = gKX*2;
    dgKX = kernDiagGradX(model.kern, model.X_u);
    for i = 1:model.k
        gKX(i, :, i) = dgKX(i, :);
    end

    if learnInducing
        % Allocate space for gX_u
        gX_u = zeros(model.k, model.q);
        % Compute portion associated with gK_u
        for i = 1:model.k
            for j = 1:model.q
                gX_u(i, j) = gKX(:, j, i)'*gK_uu(:, i);
            end
        end

        gInd = gInd1 + gInd2 + gX_u(:)';
    end

    % If the inducing points are fixed (tied to the latent points) then
    % X_u=K_t*dynamics.vardist.means and the derivatives w.r.t theta_t must be
    % amended with the appropriate partial derivatives. gInd must be passed,
    % in that case, as an argument to the function which calculates the
    % derivatives for the reparametrized quantities.
    if isfield(model, 'fixInducing') & model.fixInducing
        if learnInducing
            gIndRep = gInd;
        end
    else
        gIndRep=[];
    end

    gVarcovsLik = gVarcovs0 + gVarcovs1 + gVarcovs2;

    %----------------   This replaces vargpTimeDynamicsPriorReparamGrads

    % gVarmeansLik and gVarcovsLik are serialized into 1x(NxQ) vectors.
    % Convert them to NxQ matrices, i.e. fold them column-wise.
    gVarmeansLik = reshape(gVarmeansLik,dynModel.N,dynModel.q);
    gVarcovsLik = reshape(gVarcovsLik,dynModel.N,dynModel.q);

    if ~isempty(gIndRep)
        gIndRep = reshape(gIndRep, dynModel.N, dynModel.q); %%%%%
    end

    % The second term in the parenthesis, corresponds to the KL term. The
    % multiplier will set it to zero, if we need only the derivatives
    % corresponding only to the likelihood part.
    gVarmeans = dynModel.Kt * (gVarmeansLik - includeKL* dynModel.vardist.means);

    gVarcovs = zeros(dynModel.N, dynModel.q); % memory preallocation

    % The following quantities (in the loop) are needed to be calculated in
    % a single loop wrt q,
    if m==1
        for q=1:dynModel.q
            LambdaH_q = dynModel.vardist.covars(:,q).^0.5;
            Bt_q = eye(dynModel.N) + LambdaH_q*LambdaH_q'.*dynModel.Kt;

            % Invert Bt_q
            Lbt_q = jitChol(Bt_q)';
            G1 = Lbt_q \ diag(LambdaH_q);
            G = G1*dynModel.Kt;

            % Find Sq
            Sq{q} = dynModel.Kt - G'*G; %%%%%
            % Find the coefficient for the grad. wrt theta_t (params of Kt)
            G1T=G1';
            Bhat=G1T*G1;
            BhatKt=G1T*G;

            trGradKL{q} = -0.5*(BhatKt*Bhat + dynModel.vardist.means(:,q) * dynModel.vardist.means(:,q)');
            IBK = eye(dynModel.N) - BhatKt;
        end
    else
        for q=1:dynModel.q
            trGradKL{q} = 0;
        end
    end

    sumTrGradKL = 0;
    for q=1:dynModel.q
        % If includeKL is set to 0, then the KL part will be multiplied
        % with 0, thus being ignored.
        gVarcovs(:,q) = - (Sq{q} .* Sq{q}) * (gVarcovsLik(:,q) + includeKL * 0.5*dynModel.vardist.covars(:,q));

        diagVarcovs = repmat(gVarcovsLik(:,q)', dynModel.N,1);
        trGradKL{q} = trGradKL{q} + IBK .*  diagVarcovs * IBK';
        trGradKL{q} = trGradKL{q} + dynModel.vardist.means(:,q) * gVarmeansLik(:,q)';

        % In case gInd is empty then the inducing points are not reparametrized
        % (are not fixed to the variational means) we need not amend further the
        % derivative w.r.t theta_t, otherwise we have to do that.
        if ~isempty(gIndRep)
            trGradKL{q} = trGradKL{q} + dynModel.vardist.means(:,q) * gIndRep(:,q)';
        end

        sumTrGradKL = sumTrGradKL + trGradKL{q};
    end
    gSharedCoeff = gSharedCoeff + sumTrGradKL;

    % Serialize (unfold column-wise) gVarmeans and gVarcovs from NxQ matrices
    % to 1x(NxQ) vectors
    gVarcovs = gVarcovs(:)';
    gVarmeans = gVarmeans(:)';

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

    % Variational variances are positive: Now that the final covariances
    % are obtained we amend with the partial derivatives due to the
    % exponential transformation to ensure positiveness.
    gVarcovs = (gVarcovs(:).*model.dynamics.vardist.covars(:))';

    if isfield(model, 'fixInducing') && model.fixInducing && learnInducing
        % If there are dynamics the derivative must further be amended with the
        % partial deriv. due to the mean reparametrization.
        if isfield(model, 'dynamics') && ~isempty(model.dynamics)
            gInd = reshape(gInd,model.k,model.q);
            %gInd = gInd' * model.dynamics.Kt;
            gInd =  model.dynamics.Kt * gInd;
            gInd = gInd(:)';
        end
        %gVarmeans(model.inducingIndices, :) = gVarmeans(model.inducingIndices,
        %:) + gInd; % This should work AFTER reshaping the matrices...but here
        %we use all the indices anyway.
        gVarmeans = gVarmeans + gInd;
        gInd = []; % Inducing points are not free variables anymore, they dont have derivatives on their own.
    end

    gVar = [gVarmeans gVarcovs];

    if isfield(model.vardist,'paramGroups')
        gVar = gVar*model.vardist.paramGroups;
    end

    % In case we are in the phase where the vardistr. is initialised (see above
    % for the variance of the kernel), beta is kept fixed. For backwards
    % compatibility this can be controlled either with the learnBeta field or
    % with the initVardist field. The later overrides the first.
    if isfield(model, 'learnBeta') && model.learnBeta
        gBetaFinal = gBeta;
    else
        gBetaFinal = 0*gBeta;
    end
    if isfield(model, 'initVardist')
        if model.initVardist == 1
            gBetaFinal = 0*gBeta;
        else
            gBetaFinal = gBeta;
        end
    end

    gSharedVar = gSharedVar + gVar;

    if ~learnInducing
        gInd = [];
    end

    % At this point, gDynKern will be [] if there are no dynamics.
    gPrivAll = [gPrivAll gInd gKern gBetaFinal];
end

gDynKern = kernGradient(dynModel.kern, dynModel.t, gSharedCoeff);

%%%% ...... was commented...
if isfield(model, 'dynamics') && ~isempty(model.dynamics)
    if strcmp(model.dynamics.kern.comp{1}.type,'rbf') || strcmp(model.dynamics.kern.comp{1}.type,'matern32') || strcmp(model.dynamics.kern.comp{1}.type,'rbfperiodic') || strcmp(model.dynamics.kern.comp{1}.type,'rbfperiodic2')
        if ~isfield(model.dynamics, 'learnVariance') || ~model.dynamics.learnVariance
            gDynKern(2) = 0;
        end
    end

    %___NEW: assume that the second rbf/matern etc kernel is last in the
    %compound kernel
    %if numel(model.dynamics.kern.comp) > 3
    if isfield(model.dynamics, 'learnSecondVariance') && ~model.dynamics.learnSecondVariance   %%%%% NEW
        gDynKern(end) = 0;
    end
    %end
    %___
end

%---- NEW 2012: This is to fix selectively some of the kernel's parameters
if ~isfield(model.dynamics, 'learnVariance') || ~model.dynamics.learnVariance
    % The field model.dynamics.fixedVariance must have the
    % indexes of the gradient vector that correspond to
    % variances of kernels of the matern class and we wish to
    % not learn them.
    if isfield(model.dynamics, 'fixedKernVariance') && ~isempty(model.dynamics.fixedKernVariance)
        gDynKern(model.dynamics.fixedKernVariance) = 0;
    end
end
%----

g = [gSharedVar gDynKern gPrivAll];

end

% The bound is written as:
% ll = KL + ll1 + ll2 + ...
% For the derivatives, it's a bit trickier, since the functions we have
% from vargplvm calculate the derivatives for ll+KL in a mixed way (because
% the derivatives w.r.t mu_bar and lambda exist ALSO in the likelihood
% parts, due to reparametrising mu and S with Kt. So, we split as follows:
% [gVarAll gDynKern gPriv1 gPriv2 ... ]
% TODO: for varcovs, perform (Sq.*Sq)*[large sum]
%   and for varmeans, perform Kt * [large sum
% TODO: add switch so that one model can be dynamical, another can be
%   static
function g = gDyn(modelAll)

gPrivAll = [];
gSharedCoeff = 0;
dynModel = modelAll.dynamics;
gSharedVar = zeros(1,dynModel.vardist.nParams);

if isfield(dynModel, 'seq') & ~isempty(dynModel.seq)
    seqStart=1;
    seq = dynModel.seq;
    for i=1:length(dynModel.seq)
        seqEnd = seq(i);
        sumTrGradKL{i} = zeros(seqEnd-seqStart+1, seqEnd-seqStart+1);
        seqStart = seqEnd+1;
    end
end

for m=1:modelAll.numModels
    model = modelAll.comp{m}; % current model

    % Include calculations for the KL term only once, for the first model.
    if m == 1
        includeKL = 1;
    else
        includeKL = 0;
    end

    % Likelihood terms (coefficients)
    [gK_uu, gPsi0, gPsi1, gPsi2, g_Lambda, gBeta, tmpV] = vargpCovGrads(model);

    % Get (in three steps because the formula has three terms) the gradients of
    % the likelihood part w.r.t the data kernel parameters, variational means
    % and covariances (original ones). From the field model.vardist, only
    % vardist.means and vardist.covars and vardist.lantentDimension are used.
    [gKern1, gVarmeans1, gVarcovs1, gInd1] = kernVardistPsi1Gradient(model.kern, model.vardist, model.X_u, gPsi1');
    [gKern2, gVarmeans2, gVarcovs2, gInd2] = kernVardistPsi2Gradient(model.kern, model.vardist, model.X_u, gPsi2);
    [gKern0, gVarmeans0, gVarcovs0] = kernVardistPsi0Gradient(model.kern, model.vardist, gPsi0);
    gKern3 = kernGradient(model.kern, model.X_u, gK_uu);

    % At this point, gKern gVarmeansLik and gVarcovsLik have the derivatives for the
    % likelihood part. Sum all of them to obtain the final result.
    gKern = gKern0 + gKern1 + gKern2 + gKern3;
    gVarmeansLik = gVarmeans0 + gVarmeans1 + gVarmeans2;

    if strcmp(model.kern.type, 'rbfardjit')
        % different derivatives for the variance, which is super-numerically stable for
        % this particular kernel
        if model.learnSigmaf == 1
            gKern(1) = 0.5*model.d*( - model.k+ sum(sum(model.invLat.*model.invLat))/model.beta - model.beta*(model.Psi0-model.TrC)  )...
                + 0.5*tmpV;

            if ~isstruct(model.kern.transforms(1))
                fhandle = str2func([model.kern.transform(1) 'Transform']);
                gKern(1) = gKern(1).*fhandle(model.kern.variance, 'gradfact');
            else
                fhandle = str2func([model.kern.transforms(1).type 'Transform']);
                if ~isfield(model.kern.transforms(1), 'transformsettings')
                    gKern(1) = gKern(1).*fhandle(model.kern.variance, 'gradfact');
                else
                    gKern(1) = gKern(1).*fhandle(model.kern.variance, 'gradfact', model.kern.transforms(1).transformsettings);
                end
            end
        else
            gKern(1) = 0;
        end
    end

    %%% Compute Gradients with respect to X_u %%%
    gKX = kernGradX(model.kern, model.X_u, model.X_u);

    % The 2 accounts for the fact that covGrad is symmetric
    gKX = gKX*2;
    dgKX = kernDiagGradX(model.kern, model.X_u);
    for i = 1:model.k
        gKX(i, :, i) = dgKX(i, :);
    end

    % Allocate space for gX_u
    gX_u = zeros(model.k, model.q);
    % Compute portion associated with gK_u
    for i = 1:model.k
        for j = 1:model.q
            gX_u(i, j) = gKX(:, j, i)'*gK_uu(:, i);
        end
    end

    gInd = gInd1 + gInd2 + gX_u(:)';

    % If the inducing points are fixed (tied to the latent points) then
    % X_u=K_t*dynamics.vardist.means and the derivatives w.r.t theta_t must be
    % amended with the appropriate partial derivatives. gInd must be passed,
    % in that case, as an argument to the function which calculates the
    % derivatives for the reparametrized quantities.
    if isfield(model, 'fixInducing') & model.fixInducing
        gIndRep = gInd;
    else
        gIndRep=[];
    end
    gVarcovsLik = gVarcovs0 + gVarcovs1 + gVarcovs2;

    %----------------   This replaces vargpTimeDynamicsPriorReparamGrads

    % gVarmeansLik and gVarcovsLik are serialized into 1x(NxQ) vectors.
    % Convert them to NxQ matrices, i.e. fold them column-wise.
    gVarmeansLik = reshape(gVarmeansLik,dynModel.N,dynModel.q);
    gVarcovsLik = reshape(gVarcovsLik,dynModel.N,dynModel.q);

    if ~isempty(gIndRep)
        gIndRep = reshape(gIndRep, dynModel.N, dynModel.q); %%%%%
    end

    % The second term in the parenthesis, corresponds to the KL term. The
    % multiplier will set it to zero, if we need only the derivatives
    % corresponding only to the likelihood part.
    gVarmeans = dynModel.Kt * (gVarmeansLik - includeKL* dynModel.vardist.means);
    gVarcovs = zeros(dynModel.N, dynModel.q); % memory preallocation

    if isfield(dynModel, 'seq') & ~isempty(dynModel.seq)
        for q=1:dynModel.q
            LambdaH_q = dynModel.vardist.covars(:,q).^0.5;

            % The calculations are performed for each sequence independently
            % because correlations between sequences are not captured and, thus,
            % most of the matrices involved are block diagonal, so that in each of
            % the following loops only one block is considered.
            seqStart=1;
            for i=1:length(dynModel.seq)
                seqEnd = seq(i);
                Bt_q = eye(seqEnd-seqStart+1) + LambdaH_q(seqStart:seqEnd,1)*LambdaH_q(seqStart:seqEnd,1)'.*dynModel.Kt(seqStart:seqEnd,seqStart:seqEnd);

                % Invert Bt_q
                Lbt_q = jitChol(Bt_q)';
                G1 = Lbt_q \ diag(LambdaH_q(seqStart:seqEnd,1));
                G = G1*dynModel.Kt(seqStart:seqEnd, seqStart:seqEnd);

                % Find Sq
                Sq = dynModel.Kt(seqStart:seqEnd, seqStart:seqEnd) - G'*G;

                gVarcovs(seqStart:seqEnd,q) = - (Sq .* Sq) * (gVarcovsLik(seqStart:seqEnd,q) + ...
                    includeKL* 0.5*dynModel.vardist.covars(seqStart:seqEnd,q));

                % Find the coefficient for the grad. wrt theta_t (params of Kt)
                G1T=G1';
                Bhat=G1T*G1;
                BhatKt=G1T*G;

                if includeKL
                    trGradKL = -0.5*(BhatKt*Bhat + dynModel.vardist.means(seqStart:seqEnd,q) * dynModel.vardist.means(seqStart:seqEnd,q)');
                else
                    trGradKL = 0;
                end
                IBK = eye(seqEnd-seqStart+1) - BhatKt;
                diagVarcovs = repmat(gVarcovsLik(seqStart:seqEnd,q)', seqEnd-seqStart+1,1);
                trGradKL = trGradKL + IBK .*  diagVarcovs * IBK';
                trGradKL = trGradKL + dynModel.vardist.means(seqStart:seqEnd,q) * gVarmeansLik(seqStart:seqEnd,q)';

                % In case gInd is empty then the inducing points are not reparametrized
                % (are not fixed to the variational means) we need not amend further the
                % derivative w.r.t theta_t.
                if ~isempty(gIndRep)
                    trGradKL = trGradKL + dynModel.vardist.means(seqStart:seqEnd,q) * gIndRep(seqStart:seqEnd,q)';
                end
                sumTrGradKL{i} = sumTrGradKL{i} + trGradKL;
                seqStart = seqEnd+1;
            end
            %sumTrGradKL = sumTrGradKL + trGradKL;
        end
    else
        sumTrGradKL = 0;
        for q=1:dynModel.q
            LambdaH_q = dynModel.vardist.covars(:,q).^0.5;
            Bt_q = eye(dynModel.N) + LambdaH_q*LambdaH_q'.*dynModel.Kt;

            % Invert Bt_q
            Lbt_q = jitChol(Bt_q)';
            G1 = Lbt_q \ diag(LambdaH_q);
            G = G1*dynModel.Kt;

            % Find Sq
            Sq = dynModel.Kt - G'*G;

            % If onlyLikelihood is set to 1, then the KL part will be multiplied
            % with zero, thus being ignored.
            gVarcovs(:,q) = - (Sq .* Sq) * (gVarcovsLik(:,q) + includeKL * 0.5*dynModel.vardist.covars(:,q));

            % Find the coefficient for the grad. wrt theta_t (params of Kt)
            G1T=G1';
            Bhat=G1T*G1;
            BhatKt=G1T*G;

            if includeKL
                trGradKL = -0.5*(BhatKt*Bhat + dynModel.vardist.means(:,q) * dynModel.vardist.means(:,q)');
            else
                trGradKL = 0;
            end

            IBK = eye(dynModel.N) - BhatKt;
            diagVarcovs = repmat(gVarcovsLik(:,q)', dynModel.N,1);
            trGradKL = trGradKL + IBK .*  diagVarcovs * IBK';
            trGradKL = trGradKL + dynModel.vardist.means(:,q) * gVarmeansLik(:,q)';

            % In case gInd is empty then the inducing points are not reparametrized
            % (are not fixed to the variational means) we need not amend further the
            % derivative w.r.t theta_t, otherwise we have to do that.
            if ~isempty(gIndRep)
                trGradKL = trGradKL + dynModel.vardist.means(:,q) * gIndRep(:,q)';
            end
            sumTrGradKL = sumTrGradKL + trGradKL;
        end
        gSharedCoeff = gSharedCoeff + sumTrGradKL;
    end
    % Serialize (unfold column-wise) gVarmeans and gVarcovs from NxQ matrices
    % to 1x(NxQ) vectors
    gVarcovs = gVarcovs(:)';
    gVarmeans = gVarmeans(:)';
    %-------------------------------------------------------------------

    % Variational variances are positive: Now that the final covariances
    % are obtained we amend with the partial derivatives due to the
    % exponential transformation to ensure positiveness.
    gVarcovs = (gVarcovs(:).*model.dynamics.vardist.covars(:))';

    if isfield(model, 'fixInducing') & model.fixInducing
        % If there are dynamics the derivative must further be amended with the
        % partial deriv. due to the mean reparametrization.
        if isfield(model, 'dynamics') && ~isempty(model.dynamics)
            gInd = reshape(gInd,model.k,model.q);
            %gInd = gInd' * model.dynamics.Kt;
            gInd =  model.dynamics.Kt * gInd;
            gInd = gInd(:)';
        end
        %gVarmeans(model.inducingIndices, :) = gVarmeans(model.inducingIndices,
        %:) + gInd; % This should work AFTER reshaping the matrices...but here
        %we use all the indices anyway.
        gVarmeans = gVarmeans + gInd;
        gInd = []; % Inducing points are not free variables anymore, they dont have derivatives on their own.
    end
    gVar = [gVarmeans gVarcovs];

    if isfield(model.vardist,'paramGroups')
        gVar = gVar*model.vardist.paramGroups;
    end

    if isfield(model, 'dynamics') && ~isempty(model.dynamics)
        if strcmp(model.dynamics.kern.comp{1}.type,'rbf') || strcmp(model.dynamics.kern.comp{1}.type,'matern32') || strcmp(model.dynamics.kern.comp{1}.type,'rbfperiodic') || strcmp(model.dynamics.kern.comp{1}.type,'rbfperiodic2')
            if ~isfield(model.dynamics, 'learnVariance') || ~model.dynamics.learnVariance
                gDynKern(2) = 0;
            end
        end
        %___NEW: assume that the second rbf/matern etc kernel is last in the
        %compound kernel
        %if numel(model.dynamics.kern.comp) > 3
        if isfield(model.dynamics, 'learnSecondVariance') && ~model.dynamics.learnSecondVariance   %%%%% NEW
            gDynKern(end) = 0;
        end
        %end
        %___
    end

    % In case we are in the phase where the vardistr. is initialised (see above
    % for the variance of the kernel), beta is kept fixed. For backwards
    % compatibility this can be controlled either with the learnBeta field or
    % with the initVardist field. The later overrides the first.
    if isfield(model, 'learnBeta') && model.learnBeta
        gBetaFinal = gBeta;
    else
        gBetaFinal = 0*gBeta;
    end
    if isfield(model, 'initVardist')
        if model.initVardist == 1
            gBetaFinal = 0*gBeta;
        else
            gBetaFinal = gBeta;
        end
    end

    gSharedVar = gSharedVar + gVar;
    % At this point, gDynKern will be [] if there are no dynamics.
    gPrivAll = [gPrivAll gInd gKern gBetaFinal];
end

if isfield(dynModel, 'seq') & ~isempty(dynModel.seq)
    seqStart=1;
    gDynKern = size(dynModel.kern.nParams,1);
    for i=1:length(dynModel.seq)
        seqEnd = seq(i);
        gDynKern = gDynKern + kernGradient(dynModel.kern, dynModel.t(seqStart:seqEnd), sumTrGradKL{i});
        seqStart = seqEnd+1;
    end
else
    gDynKern = kernGradient(dynModel.kern, dynModel.t, gSharedCoeff);
end
g = [gSharedVar gDynKern gPrivAll];

end

function [gK_uu, gPsi0, gPsi1, gPsi2, g_Lambda, gBeta, tmpV] = vargpCovGrads(model)

gPsi1 = model.beta * model.m * model.B';
gPsi1 = gPsi1'; % because it is passed to "kernVardistPsi1Gradient" as gPsi1'...

gPsi2 = (model.beta/2) * model.T1;

gPsi0 = -0.5 * model.beta * model.d;

gK_uu = 0.5 * (model.T1 - (model.beta * model.d) * model.invLmT * model.C * model.invLm);

sigm = 1/model.beta; % beta^-1

PLm = model.invLatT*model.P;
tmpV = sum(sum(PLm.*PLm));
gBeta = 0.5*(model.d*(model.TrC + (model.N-model.k)*sigm -model.Psi0) ...
    - model.TrYY + model.TrPP ...
    + (1/(model.beta^2)) * model.d * sum(sum(model.invLat.*model.invLat)) + sigm*tmpV);

if ~isstruct(model.betaTransform)
    fhandle = str2func([model.betaTransform 'Transform']);
    gBeta = gBeta*fhandle(model.beta, 'gradfact');
else
    fhandle = str2func([model.betaTransform.type 'Transform']);
    gBeta = gBeta*fhandle(model.beta, 'gradfact', model.betaTransform.transformsettings);
end

g_Lambda = repmat(-0.5*model.beta*model.d, 1, model.N);
end

repo: vargplvm


text:
% put a vector in selected positions of a matrix.
%
function [grad] = B_copyVec2Mat(input_layer, curr_layer, future_layers)
input = input_layer.a;

[D,T,N] = size(input);
D1 = curr_layer.targetDims(1);
D2 = curr_layer.targetDims(2);
index2copy = curr_layer.index2copy;

future_grad = GetFutureGrad(future_layers, curr_layer);

if IsInGPU(input(1))
    grad = gpuArray.zeros(size(input));
else
    output = zeros(size(input));
end

for t=1:T
    for n = 1:N
        tmp = future_grad(:,:,t,n);
        grad(:,t,n) = tmp(index2copy);
    end
end

end

author: singaxiong

repo: SignalGraph


text:
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Angle estimation for each peak in detout
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [Resel_agl, vel_ambg_list, rng_excd_list] = angle_estim_dets(detout, ...
    Velocity_FFT, fft_Vel, fft_Ang, Rx, Tx, num_crop)

Resel_agl = [];
vel_ambg_list = [];
rng_excd_list = [];
fft_Rang = size(Velocity_FFT, 2);

for ai = 1:size(detout, 2)
    rx_vect = squeeze(Velocity_FFT(:, detout(2,ai), detout(1,ai)));

    % Phase Compensation on the range-velocity bin for virtual elements
    pha_comp_term = exp(-1i * (pi * (detout(1,ai) - fft_Vel/2 - 1) / fft_Vel));
    rx_vect(Rx+1:Rx*Tx) = rx_vect(Rx+1:Rx*Tx) * pha_comp_term;

    % Estimate Angle on set1
    Angle_FFT1 = fftshift(fft(rx_vect, fft_Ang));
    [MM,II] = max(abs(Angle_FFT1));
    Resel_agl = [Resel_agl, II];

    % Velocity disambiguation on set2 -- flip the sign of the symbols
    % corresponding to Tx2
    rx_vect(Rx+1:Rx*Tx) = - rx_vect(Rx+1:Rx*Tx);
    Angle_FFT1_flip = fftshift(fft(rx_vect, fft_Ang));
    [MM_flip,II_flip] = max(abs(Angle_FFT1_flip));

    if MM_flip > 1.2 * MM
        % now has velocity ambiguration, need to be corrected
        vel_ambg_list = [vel_ambg_list, ai];
    end

    if detout(2,ai) <= num_crop || detout(2,ai) > fft_Rang - num_crop
        rng_excd_list = [rng_excd_list, ai];
    end
end

end

author: Xiangyu-Gao


text:
function d = distance_of_point_to_line(line, x)
% function d = distance_of_point_to_line(line, x)
%
% returns d = ax+by+c
% line: [3x1] (a,b,c)
% x: [2x1] (x,y)

xh = [x(:); 1];
d = line(:)' * xh;

author: zouchuhang

repo: LayoutNet


text:
function i4mat_print ( m, n, a, title )

%*****************************************************************************80
%
%% I4MAT_PRINT prints an I4MAT.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    10 September 2009
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer M, N, the number of rows and columns of the matrix.
%
%    Input, integer A(M,N), an M by N matrix to be printed.
%
%    Input, string TITLE, a title.
%
  i4mat_print_some ( m, n, a, 1, 1, m, n, title );

  return
end

author: johannesgerer

repo: jburkardt-m


text:
function C = calcConc(s,p,s0)
% This function converts a DCE signal intensity time course to the time course of
% tissue concentration of contrast agent
%
% INPUTS
% s : Signal time course vector.
% p : Parameter array.
%     p(1) = R1 (mmolXsec), p(2) = TR (s), p(3) = FA, p(4) = T10 (s)
% Optional -
% s0: Signal at time t=0. If not input, s(1) is used.
%
% OUTPUT
% C : Time course of Gd concentration (mmoles/L) in blood.
% --------------------------------------------------------------------------------------
% Knowing the relaxivity of the contrast agent, the T1 of blood without contrast, and
% the blood signal value prior to contrast, the concentration of contrast
% agent in the blood is:
%                             1/T1  = 1/T10 + R1* C(t)
% Given S/S0, 1/T1 (for each time point) can be obtained by substituting in from the
% partial saturation equation:
%                        S/S0 = (1-exp(-TR/T1)sin(theta))/(1-exp(-TR/T1)cos(theta)
% Then you go back and solve for C(t).
%
% This function returns a vector (C) containing the blood or tissue Gd concentration time course
% units are mmoles/L
% --------------------------------------------------------------------------------------
%
% Kristen Zakian

%Get inputs
R1 = p(1); TR = p(2); FA = p(3); T10 = p(4);
a = FA*pi/180;       % calculate flip angle in radians

if nargin == 2       % if no s0 specified, then use the first point
    s0 = s(1);
end

if s0 ~= 0
    v = s./s0;
else
    s0 = 0.001;      % fudge
    v = s./s0;       % fudge
    errordlg('concformula: s0 = 0!','ERROR');  %Got this error on Acevedo RT-BL
end

if T10 ~= 0
    TT = TR/T10;
    E10 = exp(-TT);
else
    error('concformula: T10 = 0!')
end

u = 1 - E10;
w = 1 - E10*cos(a);

num = w - u.*v.*cos(a);
denom = w - u.*v;
if denom ~= 0
    L = log(num./denom);
    %y = 1/R1*1/TR*(L - TT)*1000;
    C = 1/R1*1/TR*(L - TT);
else
    error('calcconc: denom = 0');
end

for i=1:size(L,1)
    if ~isreal(L(i,:))
        C(i,:) = double(0);
    end
end

C(C <=0) = 0;

end

author: cerr

repo: CERR

save_path: github-repos/MATLAB/cerr-CERR


clear all;

% This script is used to test time and frequency estimation / correction of
% a simulated DroneID burst.  No equalization is done in this script.
% There is a simulated fractional time offset applied, but the script will
% fail if that time offset is anything other than 0 or 0.5.  This is
% something that will likely have to be fixed by the equalizer or by
% upsampling to find the most correct fractional starting time offset

%% Parameters
sample_rate = 15.36e6;
fft_size = get_fft_size(sample_rate);
carrier_spacing = round(sample_rate / fft_size);

% Pick a worst case Parts Per Million (PPM) to support.  20 PPM is the
% value for the HackRF SDR's oscialltor.  Then choose the worst case
% frequency that the SDR will operate at.  In this case 5.9 GHz is the
% highest frequency the radio will need to tune to for DroneID.  Use that
% to then figure out what the worst case frequency offset (in Hz) can be.
% That will be used as the search space for the Integer Frequency Offset
% (IFO) which is done later.
worst_case_ppm = 20;
worst_case_freq = 5.9e9;
max_allowed_freq_offset = worst_case_freq * worst_case_ppm / 1e6;
max_allowed_int_freq_offset = ceil(max_allowed_freq_offset / carrier_spacing);

% Pick a frequency offset to test with
frequency_offset = max_allowed_freq_offset - 3.3e3;

% Calculate the long and short cyclic prefix lengths
[long_cp_len, short_cp_len] = get_cyclic_prefix_lengths(sample_rate);

% Below are the cyclic prefix lengths for each OFDM symbol
cyclic_prefixes = [
    long_cp_len, ...
    short_cp_len, ...
    short_cp_len, ...
    short_cp_len, ...
    short_cp_len, ...
    short_cp_len, ...
    short_cp_len, ...
    short_cp_len, ...
    long_cp_len
];

% Calculate how many samples there are in one DroneID burst
total_burst_sample_count = (fft_size * length(cyclic_prefixes)) + sum(cyclic_prefixes);

% Calculate how many guard carriers there are in one OFDM symbol
[left_guards, right_guards] = get_num_guard_carriers(sample_rate);

% Number of constellation points in the underlying PSK
modulation_order = 4;

% Number of occupied OFDM carriers per symbol
num_data_carriers = 600;

%% OFDM Time Domain Creation
modulator = comm.OFDMModulator( ...
    "FFTLength", fft_size, ...
    "NumGuardBandCarriers", [left_guards; right_guards], ...
    "CyclicPrefixLength", cyclic_prefixes, ...
    "NumSymbols", length(cyclic_prefixes), ...
    "InsertDCNull", true);

% Modulate 9 random OFDM symbols worth of PSK samples
symbols = pskmod( ...
    randi([0, modulation_order - 1], num_data_carriers, length(cyclic_prefixes)), ...
    modulation_order);

% Create the OFDM time domain
modulated_samples = modulator(symbols);

%% Insert ZC Sequence at symbol 4

% Figure out where in the modulated samples that the ZC sequence should
% start (this includes the cyclic prefix)
symbol_4_offset = (fft_size * 3) + sum(cyclic_prefixes(1:3));

% Create the time domain ZC sequence for symbol 4
zc_seq = create_zc(fft_size, 4);

% Tack on the cyclic prefix to build the full symbol
full_zc_seq = [zc_seq(end-cyclic_prefixes(4)+1:end); zc_seq];

% Insert the full ZC sequence symbol in place of the modulated symbol 4
modulated_samples(symbol_4_offset:symbol_4_offset + length(full_zc_seq) - 1) = full_zc_seq;

% Convert to row vector
modulated_samples = reshape(modulated_samples, 1, []);

%% Prepend/Append Zeros

% These zeros are needed to simulate an actual received sample and the
% ambiguity of where the burst actually starts.  The number of samples
% isn't critical, but should be something > about 30 samples
num_zeros = 50;

zero_vec = zeros(1, num_zeros);
modulated_samples = [zero_vec, modulated_samples, zero_vec];

%% Add in a 0.5 sample time offset

% This simulates the clocks of the transmitter and receiver not being
% perfectly synced up.  Upsample by 4, then skip the first two samples, and
% downsample by 4
modulated_samples = resample(modulated_samples, 4, 1);
modulated_samples = [modulated_samples(2:end), 0, 0];
modulated_samples = resample(modulated_samples, 1, 4);

%% Add Noise

% This is not an exact SNR.  Just add gaussian noise to the signal to
% simulate real world reception
modulated_samples = awgn(modulated_samples, 10, 'measured');

figure(1);
plot(10 * log10(abs(modulated_samples).^2));
title('Time Domain with Noise');
%% Apply Frequency Offset

freq_offset_vector = exp(1j * 2 * pi * frequency_offset / sample_rate * [1:length(modulated_samples)]);
freq_offset_samples = modulated_samples .* freq_offset_vector;

figure(2);
subplot(3, 1, 1); plot(10 * log10(abs(fftshift(fft(modulated_samples))).^2));
title('Original Modulated Samples (FFT)')
subplot(3, 1, 2); plot(10 * log10(abs(fftshift(fft(freq_offset_vector))).^2));
title('Frequency Offset Vector (FFT)')
subplot(3, 1, 3); plot(10 * log10(abs(fftshift(fft(freq_offset_samples))).^2));
title('Frequency Offset Samples (FFT)')

%% Find the ZC sequence

% Find the start of the burst by looking at the cyclic prefixes.  Make sure
% to look PAST `num_zeros` since in the real world it will not be known
% where the burst actually starts, so the search space will be rather large
sto_estimate = est_sto(freq_offset_samples, sample_rate, num_zeros * 1.5);

% Calculate how many samples after the start of the burst that the ZC
% sequence should be located (this offset includes the ZC sequence cyclic
% prefix)
zc_seq_offset = (fft_size * 3) + long_cp_len + (short_cp_len * 3);

% Calculate where in the sample vector the ZC sequence starts
auto_corr_index = sto_estimate + zc_seq_offset;

fprintf("Sample offset error: %d\n", sto_estimate - num_zeros - 1);
%% Estimate coarse freq offset and adjust

% Use the cyclic prefix of the ZC sequence to do coarse frequency offset
% estimation (the ZC sequence is the 4th OFDM symbol).
coarse_cp_len = cyclic_prefixes(4);
cp_len_window_size = coarse_cp_len;

% The auto_corr_index points to the start of the ZC sequence, so back off
% by the length of the 4th cyclic prefix
coarse_offset_start = auto_corr_index - coarse_cp_len;

% Extract out `fft_size + coarse_cp_len` samples
window = freq_offset_samples(coarse_offset_start:coarse_offset_start + fft_size + coarse_cp_len - 1);

% Extract the first and last `coarse_cp_len` samples from the window
window_left = window(1:coarse_cp_len);
window_right = window(end - coarse_cp_len + 1:end);

% Calculate the coarse frequency offset in radians and Hz by taking the dot
% product of the two windows
cfo_est_radians = angle(sum(window_left .* conj(window_right))) / fft_size;
cfo_est_hz = cfo_est_radians * sample_rate / (2 * pi);

% Adjust for the frequency offset
cfo_est_adj_vector = exp(1j * cfo_est_radians * [1:length(freq_offset_samples)]);
freq_offset_samples = freq_offset_samples .* cfo_est_adj_vector;

%% Integer frequency offset estimation
% The ZC sequence will be used to find the integer frequency offset.  So,
% extract out just the samples for the ZC sequence (not including the
% cyclic prefix)
received_zc_sym = freq_offset_samples(auto_corr_index:auto_corr_index + fft_size - 1);

figure(3);
plot(abs(received_zc_sym));
title('ZC Sequence (Time Domain Magnitude)');

% Estimate the integer frequency offset
est_int_offset_hz = est_integer_freq_offset(received_zc_sym, sample_rate, max_allowed_int_freq_offset);

% Adjust for the integer frequency offset
ifo_est_adj_vector = exp(1j * 2 * pi * est_int_offset_hz / sample_rate * [1:length(freq_offset_samples)]);
freq_offset_samples = freq_offset_samples .* ifo_est_adj_vector;

% Print out how far off the combination of coarse and integer freq offset
% was from the true value
residual_freq_error_hz = est_int_offset_hz + cfo_est_hz + frequency_offset;
fprintf("Remaining Frequency Offset (Hz): %f\n", residual_freq_error_hz);

%% Extract OFDM Data Carriers
demod = comm.OFDMDemodulator( ...
    "FFTLength", fft_size, ...
    "NumGuardBandCarriers", [left_guards; right_guards], ...
    "CyclicPrefixLength", cyclic_prefixes, ...
    "NumSymbols", length(cyclic_prefixes), ...
    "RemoveDCCarrier", true);

% Extract out just the samples in the burst and run those through the OFDM
% demodulator
burst_samples = freq_offset_samples(sto_estimate:sto_estimate + total_burst_sample_count - 1);
demod_symbols = demod(reshape(burst_samples, [], 1));

% Plot just the data carrying OFDM symbol constellations (the 4th symbol is
% the ZC sequence)
figure(4);
plot(demod_symbols(:,[1,2,3,5,6,7,8,9]), 'o');
title('Demodulated Constellations')


text:
classdef PTKNode < handle
    % PTKNode. A class for storing node data
    %
    %
    %     Licence
    %     -------
    %     Part of the TD Pulmonary Toolkit. https://github.com/tomdoel/pulmonarytoolkit
    %     Author: Tom Doel, 2013.  www.tomdoel.com
    %     Distributed under the GNU GPL v3 licence. Please see website for details.
    %

    properties
        CentrelinePoint
        Parent
        Children
    end
end

author: tomdoel

repo: pulmonarytoolkit


text:
function [command] = path_follower_quad(path, p_drone)

% PATH_FOLLOWER_QUAD - creates velocity command for the quadcopter
%
% Inputs:
%   flag   - if flag==1, follow waypoint path
%            if flag==2, follow orbit
%
%   Va_d   - desired airspeed
%   r      - vect3, inertial position of start of waypoint path
%   q      - vect3, unit vector that defines inertial direction of waypoint path
%   r_next - inertial position of end of waypoint path
%   c      - vect3, center of orbit
%   rho    - radius of orbit
%   lambda - direction of orbit (+1 for CW, -1 for CCW)
%   xhat   - estimated MAV states (pn, pe, pd, Va, alpha, beta, phi, theta, chi, p, q, r, Vg, wn, we, psi)
%
% Outputs:
%

    NN = 0;
%     flag      = path(1+NN);         % 1 for line, 2 for orbit
    Va_d      = path(2+NN);         % base speed
    r_path    = path(3+NN:5+NN);    % inertial position of start of waypoint path
    q_path    = path(6+NN:8+NN);    % unit vector that defines inertial direction of waypoint path
    r_path_next = path(9+NN:11+NN); % inertial position of end of waypoint path
%     c_orbit   = path(9+NN:11+NN);   % center of orbit
%     rho_orbit = path(12+NN);        % radius of orbit
%     lambda_orbit = path(13+NN);     % direction of orbit (+1 for CW, -1 for CCW)
    NN = NN + 16;
    pn        = path(1+NN);
    pe        = path(2+NN);
    h         = path(3+NN);
%     Va        = in(4+NN);
%     alpha     = in(5+NN);
%     beta      = in(6+NN);
%     phi       = in(7+NN);
%     theta     = in(8+NN);
    chi       = path(9+NN);
%     p         = in(10+NN);
%     q         = in(11+NN);
%     r         = in(12+NN);
%     Vg        = in(13+NN);
%     wn        = in(14+NN);
%     we        = in(15+NN);
%     psi       = in(16+NN);
%     NN = NN + 16;
%     t         = in(1+NN);

%     flag = 1;   % only straight lines, valid for a quad
%     switch flag
%         case 1 % follow straight line path specified by r and q
            qn = q_path(1);
            qe = q_path(2);
            chi_q = atan2(qe, qn);

            % angle module 2pi
            while (chi_q - chi) < -pi
                chi_q = chi_q+2*pi;
            end
            while (chi_q - chi) > pi
                chi_q = chi_q-2*pi;
            end

            Rip = [cos(chi_q)     sin(chi_q)  0;
                 -sin(chi_q)    cos(chi_q)  0;
                 0              0           1];
            ep = Rip * ([pn; pe; -h] - r_path);
            epy = ep(2);
            chi_c = chi_q - p_drone.chi_inf * 2/pi*atan(p_drone.k_path * epy);
            h_c = -r_path_next(3);

    % command for quad velocity autopilot
    vn_c = Va_d * cos(chi_c);
    ve_c = Va_d * sin(chi_c);
    vd_c = (h-h_c);
    psi_c = chi_c;  % NOT TRUE! Ref. pag.21 Beard and McLain

    % create output
    command = [psi_c; vn_c; ve_c; vd_c];
end

author: lis-epfl

repo: swarmlab

save_path: github-repos/MATLAB/lis-epfl-swarmlab


text:
% visualize_distortions
%
%
% A script to run in conjunction with calib_gui in TOOLBOX_calib to plot
% the distortion models.
%
% This is a slightly modified version of the script plot_CCT_distortion.m written by Mr. Oshel
% Thank you Mr. Oshel for your contribution!

[mx,my] = meshgrid(0:nx/20:(nx-1),0:ny/20:(ny-1));
[nnx,nny]=size(mx);
px=reshape(mx',nnx*nny,1);
py=reshape(my',nnx*nny,1);
kk_new=[fc(1) alpha_c*fc(1) cc(1);0 fc(2) cc(2);0 0 1];
rays=inv(kk_new)*[px';py';ones(1,length(px))];
x=[rays(1,:)./rays(3,:);rays(2,:)./rays(3,:)];

title2=strcat('Complete Distortion Model');

fh1 = 2;

%if ishandle(fh1),
%    close(fh1);
%end;
figure(fh1); clf;
xd=apply_distortion(x,kc);
px2=fc(1)*(xd(1,:)+alpha_c*xd(2,:))+cc(1);
py2=fc(2)*xd(2,:)+cc(2);
dx=px2'-px;
dy=py2'-py;
Q=quiver(px+1,py+1,dx,dy);
hold on;
plot(cc(1)+1,cc(2)+1,'o');
plot((nx-1)/2+1,(ny-1)/2+1,'x');
dr=reshape(sqrt((dx.*dx)+(dy.*dy)),nny,nnx)';
[C,h]=contour(mx,my,dr,'k');
clabel(C,h);
Mean=mean(mean(dr));
Max=max(max(dr));
title(title2);

axis ij;
axis([1 nx 1 ny])
axis equal;
axis tight;

position=get(gca,'Position');
shr = 0.9;
position(1)=position(1)+position(3)*((1-shr)/2);
position(2)=position(2)+position(4)*(1-shr)+0.03;
position(3:4)=position(3:4)*shr;
set(gca,'position',position);
set(gca,'fontsize',8,'fontname','clean')

gh = gca;

line1=sprintf('Principal Point               = (%0.6g, %0.6g)',cc(1),cc(2));
line2=sprintf('Focal Length                 = (%0.6g, %0.6g)',fc(1),fc(2));
line3=sprintf('Radial coefficients         = (%0.4g, %0.4g, %0.4g)',kc(1),kc(2),kc(5));
line4=sprintf('Tangential coefficients  = (%0.4g, %0.4g)',kc(3),kc(4));
line5=sprintf('+/- [%0.4g, %0.4g]',cc_error(1),cc_error(2));
line6=sprintf('+/- [%0.4g, %0.4g]',fc_error(1),fc_error(2));
line7=sprintf('+/- [%0.4g, %0.4g, %0.4g]',kc_error(1),kc_error(2),kc_error(5));
line8=sprintf('+/- [%0.4g, %0.4g]',kc_error(3),kc_error(4));
line9=sprintf('Pixel error                      = [%0.4g, %0.4g]',err_std(1),err_std(2));
line10=sprintf('Skew                              = %0.4g',alpha_c);
line11=sprintf('+/- %0.4g',alpha_c_error);

axes('position',[0 0 1 1],'visible','off');
th=text(0.11,0,{line9,line2,line1,line10,line3,line4},'horizontalalignment','left','verticalalignment','bottom','fontsize',8,'fontname','clean');
th2=text(0.9,0.,{line6,line5,line11,line7,line8},'horizontalalignment','right','verticalalignment','bottom','fontsize',8,'fontname','clean');
%set(th,'FontName','fixed');
axes(gh);

set(fh1,'color',[1,1,1]);

hold off;

title2=strcat('Tangential Component of the Distortion Model');

fh2 = 3;

%if ishandle(fh2),
%    close(fh2);
%end;
figure(fh2); clf;
xd=apply_distortion(x,[0 0 kc(3) kc(4) 0]);
px2=fc(1)*(xd(1,:)+alpha_c*xd(2,:))+cc(1);
py2=fc(2)*xd(2,:)+cc(2);
dx=px2'-px;
dy=py2'-py;
Q=quiver(px+1,py+1,dx,dy);
hold on;
plot(cc(1)+1,cc(2)+1,'o');
plot((nx-1)/2+1,(ny-1)/2+1,'x');
dr=reshape(sqrt((dx.*dx)+(dy.*dy)),nny,nnx)';
[C,h]=contour(mx,my,dr,'k');
clabel(C,h);
Mean=mean(mean(dr));
Max=max(max(dr));
title(title2);

axis ij;
axis([1 nx 1 ny])
axis equal;
axis tight;

position=get(gca,'Position');
shr = 0.9;
position(1)=position(1)+position(3)*((1-shr)/2);
position(2)=position(2)+position(4)*(1-shr)+0.03;
position(3:4)=position(3:4)*shr;
set(gca,'position',position);
set(gca,'fontsize',8,'fontname','clean')

gh = gca;

line1=sprintf('Principal Point               = (%0.6g, %0.6g)',cc(1),cc(2));
line2=sprintf('Focal Length                 = (%0.6g, %0.6g)',fc(1),fc(2));
line3=sprintf('Radial coefficients         = (%0.4g, %0.4g, %0.4g)',kc(1),kc(2),kc(5));
line4=sprintf('Tangential coefficients  = (%0.4g, %0.4g)',kc(3),kc(4));
line5=sprintf('+/- [%0.4g, %0.4g]',cc_error(1),cc_error(2));
line6=sprintf('+/- [%0.4g, %0.4g]',fc_error(1),fc_error(2));
line7=sprintf('+/- [%0.4g, %0.4g, %0.4g]',kc_error(1),kc_error(2),kc_error(5));
line8=sprintf('+/- [%0.4g, %0.4g]',kc_error(3),kc_error(4));
line9=sprintf('Pixel error                      = [%0.4g, %0.4g]',err_std(1),err_std(2));
line10=sprintf('Skew                              = %0.4g',alpha_c);
line11=sprintf('+/- %0.4g',alpha_c_error);

axes('position',[0 0 1 1],'visible','off');
th=text(0.11,0,{line9,line2,line1,line10,line3,line4},'horizontalalignment','left','verticalalignment','bottom','fontsize',8,'fontname','clean');
th2=text(0.9,0.,{line6,line5,line11,line7,line8},'horizontalalignment','right','verticalalignment','bottom','fontsize',8,'fontname','clean');
%set(th,'FontName','fixed');
axes(gh);

set(fh2,'color',[1,1,1]);

hold off;

title2=strcat('Radial Component of the Distortion Model');

fh3 = 4;

%if ishandle(fh3),
%    close(fh3);
%end;
figure(fh3); clf;
xd=apply_distortion(x,[kc(1) kc(2) 0 0 kc(5)]);
px2=fc(1)*(xd(1,:)+alpha_c*xd(2,:))+cc(1);
py2=fc(2)*xd(2,:)+cc(2);
dx=px2'-px;
dy=py2'-py;
Q=quiver(px+1,py+1,dx,dy);
hold on;
plot(cc(1)+1,cc(2)+1,'o');
plot((nx-1)/2+1,(ny-1)/2+1,'x');
dr=reshape(sqrt((dx.*dx)+(dy.*dy)),nny,nnx)';
[C,h]=contour(mx,my,dr,'k');
clabel(C,h);
Mean=mean(mean(dr));
Max=max(max(dr));
title(title2);

axis ij;
axis([1 nx 1 ny])
axis equal;
axis tight;

position=get(gca,'Position');
shr = 0.9;
position(1)=position(1)+position(3)*((1-shr)/2);
position(2)=position(2)+position(4)*(1-shr)+0.03;
position(3:4)=position(3:4)*shr;
set(gca,'position',position);
set(gca,'fontsize',8,'fontname','clean')

gh = gca;

line1=sprintf('Principal Point               = (%0.6g, %0.6g)',cc(1),cc(2));
line2=sprintf('Focal Length                 = (%0.6g, %0.6g)',fc(1),fc(2));
line3=sprintf('Radial coefficients         = (%0.4g, %0.4g, %0.4g)',kc(1),kc(2),kc(5));
line4=sprintf('Tangential coefficients  = (%0.4g, %0.4g)',kc(3),kc(4));
line5=sprintf('+/- [%0.4g, %0.4g]',cc_error(1),cc_error(2));
line6=sprintf('+/- [%0.4g, %0.4g]',fc_error(1),fc_error(2));
line7=sprintf('+/- [%0.4g, %0.4g, %0.4g]',kc_error(1),kc_error(2),kc_error(5));
line8=sprintf('+/- [%0.4g, %0.4g]',kc_error(3),kc_error(4));
line9=sprintf('Pixel error                      = [%0.4g, %0.4g]',err_std(1),err_std(2));
line10=sprintf('Skew                              = %0.4g',alpha_c);
line11=sprintf('+/- %0.4g',alpha_c_error);

axes('position',[0 0 1 1],'visible','off');
th=text(0.11,0,{line9,line2,line1,line10,line3,line4},'horizontalalignment','left','verticalalignment','bottom','fontsize',8,'fontname','clean');
th2=text(0.9,0.,{line6,line5,line11,line7,line8},'horizontalalignment','right','verticalalignment','bottom','fontsize',8,'fontname','clean');
%set(th,'FontName','fixed');
axes(gh);

set(fh3,'color',[1,1,1]);

hold off;

figure(fh1);

repo: ekfmonoslam


text:
function [rep_motion motion_vector] = fastMV(im1, im2, w)
% Author:    Arash Jalalian
% E-mail:    arash.jalalian@gmail.com
% Arguments: [rep_motion motion_vector] = fastMV(im1, im2, w)
% This is the simplified block matching algorithm(BMA) which is proposed by
% L. Hao. I've simplified this algorithm for use it in a real-time problem.
% We have a dynamic search pattern during finding the motion vector.
% 1. check with big diamond.
% 2. check with one of the hexagon subject to previous results.
% 3. check with small diamond.
% 'im1' is base frame of a video and 'im2' is the second frame. and 'w' is
% the window size. 'rep_motion' is the representative motion vector and
% 'motion_vector' declare motion vectors for each block of image.
% Example:
% im1 = imread('frame001.jpg');
% im2 = imread('frame002.jpg');
% w = 16;
% [rep_m m_vector] = fastMV(im1, im2, w);

%   clear all
%   close all
%   clc
%   im1 = imread('img_1057.pgm');
%   im2 = imread('img_1058.pgm');
%  % dis = 2;
%   w = 8;

% initialization
[r1 c1] = size(im1);
[r2 c2] = size(im2);
if r1 ~= r2 || c1 ~= c2
    error('The images must be in a same size')
end

pat.org    = [0; 0];
pat.diam1  = [2 0 -2 0; 0 2 0 -2];%big diamond
pat.diam2  = [1 0 -1 0; 0 1 0 -1];%small diamond
pat.hexver = [2 1  -1 -2 -1 1; 0 2 2 0 -2 -2];
pat.hexhor = [2 0 -2 -2 0 2 ;1 2 1 -1 -2 -1];

r_time = int16(floor(r1 ./ w));
c_time = int16(floor(c1 ./ w));

% for preventing index exceeding from end of image. 4 is the maximum
% Magnitude of the motion vector
if mod(r1, w) < 4
    r_time = r_time -1;
end
if mod(c1, w) < 4
    c_time = c_time -1;
end
slice1 = cell(r_time, c_time);
slice2 = cell(r_time, c_time);
motion_vector = cell(r_time, c_time);

% creating slices with cell array of slices
for i = 1:r_time
    for j = 1:c_time
        slice1{i, j} = im1(((i-1) * w) + 1:i * w, ((j-1) * w) + 1:j * w);
        slice2{i, j} = im2(((i-1) * w) + 1:i * w, ((j-1) * w) + 1:j * w);
    end
end

% first step checking with 4 points of big diamond and the origin
mad_org = zeros(r_time, c_time);
min_mad = zeros(r_time, c_time);
idx_min_mad = zeros(r_time, c_time);
for i = 1:r_time
    for j = 1:c_time
        s_h_r = ((i-1) * w) + 1;%slice head row
        s_h_c = ((j-1) * w) + 1;%slice head column
        diff.org{i, j} = abs(slice2{i, j} - slice1{i, j});
        [r_diam c_diam] = size(pat.diam1);
        for k = 1:c_diam
            if s_h_r + pat.diam1(1, k) > 0 && s_h_c + pat.diam1(2, k) > 0
                diff.diam1{1, k} = abs(slice1{i, j}- im2(s_h_r + pat.diam1(1, k):...
                    i *w + pat.diam1(1, k),...
                    s_h_c + pat.diam1(2, k):...
                    j * w + pat.diam1(2, k)));
                % calculating MAD = Mean Absolute Difference
                diff.diam1{2, k} = sum(sum(diff.diam1{1,k})) / (w ^ 2);% insert each MAD below each Pat
                mad(k) = sum(sum(diff.diam1{1,k})) / (w ^ 2);
            else
                diff.diam1{1, k} = 9999999999999;
                diff.diam1{2, k} = 9999999999999;
                mad(k) = sum(sum(diff.diam1{1,k})) / (w ^ 2);
            end
        end
        % calculating minimum of mad in 4 pat and the origin and inserting
        % them into a min_mad(i, j). where i, j decler the position of the
        % slice in the image
        mad_org(i , j) = sum(sum(diff.org{i,j})) / (w ^ 2);
        mad = [mad mad_org(i, j)];
        [min_mad(i, j) idx_min_mad(i, j)] = min(mad);
        clear mad

    end
end
% if the idx_min_mad == 5 it means that the origin has the minimum value of
% mad (between 4 big diamond points and the origin the orogin has the
% minimum value) otherwise the index in idx_min_mad shows the pat index that
% has the min mad value.
idx_min_mad2 = zeros(r_time, c_time);
min_mad2 = zeros(r_time, c_time);
for i = 1:r_time
    for j = 1:c_time
        clear mad
        if idx_min_mad(i, j) == 5
            % it means this point didn't moved and it's better to do not use
            % any motion vectors so we must put [0; 0] for these points as a
            % motion vector.accourding to the paper for any slices that
            % idx_min_mad(i, j) ==5 we must use anothe four points as new
            % pat and check the slices with these i and j for mad. and the
            % final solution for these slices is these mad. here we must
            % use pat.diam2 as new patterns and repeat thise process again.
            s_h_r = ((i-1) * w) + 1;%slice head row
            s_h_c = ((j-1) * w) + 1;%slice head column
%           diff.org{i, j} = abs(slice2{i, j} - slice1{i, j});
            [r_diam c_diam] = size(pat.diam1);
            for k = 1:c_diam
                if s_h_r + pat.diam2(1, k) > 0 && s_h_c + pat.diam2(2, k) > 0
                    diff.diam2{1, k} = abs(slice1{i, j}- im2(s_h_r + pat.diam2(1, k):...
                        i *w + pat.diam2(1, k),...
                        s_h_c + pat.diam2(2, k):...
                        j * w + pat.diam2(2, k)));
                    % calculating MAD = Mean Absolute Diffrence
                    diff.diam2{2, k} = sum(sum(diff.diam2{1,k})) / (w ^ 2);% insert each MAD below each Pat
                    mad(k) = sum(sum(diff.diam2{1,k})) / (w ^ 2);
                else
                    diff.diam2{1, k} = 9999999999999;
                    diff.diam2{2, k} = 9999999999999;
                    mad(k) = sum(sum(diff.diam2{1,k})) / (w ^ 2);
                end
            end
            % calculating minimum of mad in 4 pat and the origin and inserting
            % them into a min_mad(i, j). where i, j decler the position of the
            % slice in the image
%            mad_org(i , j) = sum(sum(diff.org{i,j})) / (w ^ 2);
%            mad = [mad mad_org(i, j)];
            [min_mad2(i, j) idx_min_mad2(i, j)] = min(mad);

        end
    end
end
for i = 1:r_time
    for j = 1:c_time
        if idx_min_mad2(i, j) ~= 0
            motion_vector{i , j} = pat.diam2(:, idx_min_mad2(i, j));
        end
    end
end

% other points that their minimum mads idx ~= 5. it means that these slices
% are closer to farder slices.
idx_min_mad3 = zeros(r_time, c_time);
min_mad3 = zeros(r_time, c_time);
for i = 1:r_time
    for j = 1:c_time
        clear mad
        if idx_min_mad(i, j) ~= 5
            s_h_r = ((i-1) * w) + 1;%slice head row
            s_h_c = ((j-1) * w) + 1;%slice head column
%           diff.org{i, j} = abs(slice2{i, j} - slice1{i, j});
            [r_hex c_hex] = size(pat.hexhor);
            for k = 1:c_hex
                if s_h_r + pat.hexhor(1, k) > 0 && s_h_c + pat.hexhor(2, k) > 0 && (idx_min_mad(i, j) == 1 || idx_min_mad(i, j) == 3)
                    diff.hex{1, k} = abs(slice1{i, j}- im2(s_h_r + pat.hexhor(1, k):...
                        i *w + pat.hexhor(1, k),...
                        s_h_c + pat.hexhor(2, k):...
                        j * w + pat.hexhor(2, k)));
                    % calculating MAD = Mean Absolute Diffrence
                    diff.hex{2, k} = sum(sum(diff.hex{1,k})) / (w ^ 2);% insert each MAD below each Pat
                    mad(k) = sum(sum(diff.hex{1,k})) / (w ^ 2);
                elseif s_h_r + pat.hexver(1, k) > 0 && s_h_c + pat.hexver(2, k) > 0 && (idx_min_mad(i, j) == 2 || idx_min_mad(i, j) == 4)
                    diff.hex{1, k} = abs(slice1{i, j}- im2(s_h_r + pat.hexver(1, k):...
                        i *w + pat.hexver(1, k),...
                        s_h_c + pat.hexver(2, k):...
                        j * w + pat.hexver(2, k)));
                    % calculating MAD = Mean Absolute Diffrence
                    diff.hex{2, k} = sum(sum(diff.hex{1,k})) / (w ^ 2);% insert each MAD below each Pat
                    mad(k) = sum(sum(diff.hex{1,k})) / (w ^ 2);
                else
                    diff.hex{1, k} = 9999999999999;
                    diff.hex{2, k} = 9999999999999;
                    mad(k) = sum(sum(diff.hex{1,k})) / (w ^ 2);
                end
            end
            % calculating minimum of mad in 4 pat and inserting
            % them into a min_mad(i, j). where i, j decler the position of the
            % slice in the image
%            mad_org(i , j) = sum(sum(diff.org{i,j})) / (w ^ 2);
%            mad = [mad mad_org(i, j)];
            [min_mad3(i, j) idx_min_mad3(i, j)] = min(mad);

        end
    end
end
% org_mov declare the movement vector for new pattern origins.
org_mov = cell(r_time, c_time);
for i = 1:r_time
    for j = 1:c_time
        if idx_min_mad3(i, j) ~= 0 && (idx_min_mad(i, j) == 1 || idx_min_mad(i, j) == 3)
            org_mov{i , j} = pat.hexhor(:, idx_min_mad3(i, j));
        elseif idx_min_mad3(i, j) ~= 0 && (idx_min_mad(i, j) == 2 || idx_min_mad(i, j) == 4)
            org_mov{i , j} = pat.hexver(:, idx_min_mad3(i, j));
        end
    end
end
% now we must go the theird step. the minimum mad point found in the
% previous search step is repositioned as the center point to form a new
% hexagon nad only three new non overlaped points will be checked as
% candidates each time. so we must define new pattern. this time our
% patterns must have a dynamic behavior. and also these pattern must be
% created based on the old pat.hexver and pat.hexhor
[r_mov c_mov] = size(org_mov);
idx_min_mad4 = zeros(r_mov, c_mov);
min_mad4 = zeros(r_mov, c_mov);
for i = 1:r_mov
    for j = 1:c_mov
        clear mad
        nu = sparse(org_mov{i, j});
        [r_nu c_nu] = size(nu);
        if r_nu ~= 0 || c_nu ~= 0
            for z = 1:c_hex
                pat.hexver_new(:, z) = pat.hexver(:, z) + org_mov{i, j};
                pat.hexhor_new(:, z) = pat.hexhor(:, z) + org_mov{i, j};
            end
            s_h_r = ((i-1) * w) + 1;%slice head row
            s_h_c = ((j-1) * w) + 1;%slice head column
%           diff.org{i, j} = abs(slice2{i, j} - slice1{i, j});
            [r_hex c_hex] = size(pat.hexhor_new);
            for k = 1:c_hex
%                clear mad
                if s_h_r + pat.hexhor_new(1, k) > 0 && s_h_c + pat.hexhor_new(2, k) > 0 && (idx_min_mad(i, j) == 1 || idx_min_mad(i, j) == 3)
                    diff.hex{1, k} = abs(slice1{i, j}- im2(s_h_r + pat.hexhor_new(1, k):...
                        i *w + pat.hexhor_new(1, k),...
                        s_h_c + pat.hexhor_new(2, k):...
                        j * w + pat.hexhor_new(2, k)));
                    % calculating MAD = Mean Absolute Diffrence
                    diff.hex{2, k} = sum(sum(diff.hex{1,k})) / (w ^ 2);% insert each MAD below each Pat
                    mad(k) = sum(sum(diff.hex{1,k})) / (w ^ 2);
                elseif s_h_r + pat.hexver_new(1, k) > 0 && s_h_c + pat.hexver_new(2, k) > 0 && (idx_min_mad(i, j) == 2 || idx_min_mad(i, j) == 4)
                    diff.hex{1, k} = abs(slice1{i, j}- im2(s_h_r + pat.hexver_new(1, k):...
                        i *w + pat.hexver_new(1, k),...
                        s_h_c + pat.hexver_new(2, k):...
                        j * w + pat.hexver_new(2, k)));
                    % calculating MAD = Mean Absolute Diffrence
                    diff.hex{2, k} = sum(sum(diff.hex{1,k})) / (w ^ 2);% insert each MAD below each Pat
                    mad(k) = sum(sum(diff.hex{1,k})) / (w ^ 2);
                else
                    diff.hex{1, k} = 9999999999999;
                    diff.hex{2, k} = 9999999999999;
                    mad(k) = sum(sum(diff.hex{1,k})) / (w ^ 2);
                end
            end
            % calculating minimum of mad in 4 pat and inserting
            % them into a min_mad(i, j). where i, j declare the position of the
            % slice in the image
%            mad_org(i , j) = sum(sum(diff.org{i,j})) / (w ^ 2);
            mad = [mad mad_org(i, j)];
            [min_mad4(i, j) idx_min_mad4(i, j)] = min(mad);
%              clear mad
        end
    end
end
% org_mov declare the movement vector for new pattern origins.
% final motion vector calculation
for i = 1:r_mov
    for j = 1:c_mov
        if idx_min_mad4(i, j) == 7
            motion_vector{i, j} = pat.org;
        elseif idx_min_mad4(i, j) ~= 0 && (idx_min_mad(i, j) == 1 || idx_min_mad(i, j) == 3)
            motion_vector{i , j} = pat.hexhor_new(:, idx_min_mad4(i, j));
        elseif idx_min_mad4(i, j) ~= 0 && (idx_min_mad(i, j) == 2 || idx_min_mad(i, j) == 4)
            motion_vector{i , j} = pat.hexver_new(:, idx_min_mad3(i, j));
        end
    end
end

% now we want to find rep_motion vector
motion_vectors = cat(2, motion_vector{:});
most_motion_vector = zeros(1, r_time .* c_time);
clear temp
temp = motion_vectors;
for i=1:r_time .* c_time
    my_count = 0;
    element(:, 1) = temp(:, i);
    for k = i+1:r_time .* c_time
        if temp(:, k) == element(:, 1)
            my_count = my_count+1;
%             temp(:, k) = [];
        end
    end
    most_motion_vector(i) = my_count;
end
[value, idx] = max(most_motion_vector);
rep_motion= motion_vectors(:, idx);

author: Sable

repo: mcbench-benchmarks


text:
% The OxyGEN proyect by Profoty.xyz
% Together against the Covid-19
% V4.0 rev.17/03/2020
%
% Website/blog: oxygen.protofy.xyz
% Contact: oxygen@protofy.xyz

%GEOMETRICAL PARAMETERS

clear;

%Length from the bearing to the hinge
l0 = 27.5;

%Length of the vertical wall that supports the hinge
h0 = 6.5;

%Lenght of the vertical support of the bearing
hb = 4.0;

%Bearing radius
br = 1.1;

%Array of different max and min positions of the bearing
h1a = [9, 10, 11];
h2a = [20, 20, 20];

%Number of cycles per cam turn (1 to 3).
nc = 1;

%Minimum radius of the camshaft
rmin = 5.5;

%BREATHING CYCLE PARAMETERS

%Duration of the inhale cycle / duration of the whole cycle
lambda1 = 0.500;
lambda2 = 0.100;

%Soft transition between inhale and exhale cycles
dpsi21 = 0.11;
dpsi12 = 0.15;

%Adjust parameters of the inhale curve
ga1 = 3.8;
gb1 = .9;
ff1 = 50;

%Adjust parametes of the exhale curve
ga2 = 3.5;
gb2 = .70;
ff2 = 55;

%GENERATION OF THE CURVES AND THE CAMSHAFT

%Generation of the geometry

l = sqrt(l0^2+hb^2);

for i = 1:numel(h1a);

    ymin(i) = h1a(i);
    ymax(i) = h2a(i);

    alphamin(i) = acos((h0 - h1a(i)) / l);
    alphamax(i) = acos((h0 - h2a(i)) / l);

    xmin(i) = l*sin(alphamin(i));
    xmax(i) = l*sin(alphamax(i));

    d(i) = sqrt((xmin(i)-xmax(i))^2 + (ymin(i)-ymax(i))^2);

    alphatan(i) = (alphamin(i) + alphamax(i)) / 2;

    xtan(i) = l*sin(alphatan(i));
    ytan(i) = h0 - l*cos(alphatan(i));

    xsup(i) = xtan(i) + (d(i)/2)*cos(alphatan(i));
    xinf(i) = xtan(i) - (d(i)/2)*cos(alphatan(i));

    ysup(i) = ytan(i) + (d(i)/2)*sin(alphatan(i));
    yinf(i) = ytan(i) - (d(i)/2)*sin(alphatan(i));

    xcam(i) = xtan(i) + (d(i)/2 + rmin + br)*cos(alphatan(i));
    ycam(i) = ytan(i) + (d(i)/2 + rmin + br)*sin(alphatan(i));

end;

%Time increment
dt = 0.01;

%time coordinate during the whole cycle
theta = 0:dt:2*pi;

%Generation of the soft transition between inhale and exhale curves
psi1 = [];
psi2 = [];

for i = 1:numel(theta);

    if theta(i) < (lambda2-dpsi21/2)*2*pi;
        psi1(i) = 0;

    elseif theta(i) < (lambda2+dpsi21/2)*2*pi;
        psi1(i) = 0.5 + 0.5*cos((theta(i)-(lambda2+dpsi21/2)*2*pi)/(2*dpsi21));

    else theta(i) < (lambda1-dpsi12/2)*2*pi;
        psi1(i) = 1;

    end;

    psi2(i) = 1 - psi1(i);

end;

%Generation of the complete breathing cycle rho(theta)

rho = [];
rho1 = [];
rho2 = [];
rho2next = [];

rhomin = 1000;
rhomax = 0;

for i = 1:numel(theta);

    %Inhale curve
    rho1(i) = ff1*gampdf(theta(i), ga1, gb1);
    rho1next(i) = ff1*gampdf(theta(i)+2*pi, ga1, gb1);

    %Exhale curve
    rho2(i) = ff2*gampdf(theta(i), ga2, gb2);
    rho2next(i) = ff2*gampdf(theta(i)+2*pi, ga2, gb2);

    rho(i) = psi1(i)*rho1(i) + psi2(i)*rho1next(i);

    %Capturing min and max in order to generate the normalized curve
    if rho(i) > rhomax
        rhomax = rho(i);
    end;

    if rho(i) < rhomin
        rhomin = rho(i);
    end;

end;

%Generation of a normalised curve and camshaft

rhonorm = [];
rhocam = [];

for i = 1:numel(theta);

    rhonorm(i) = (rho(i)-rhomin)/(rhomax-rhomin);

    for n = 1:numel(d)

        rhocam(n,i) = rmin + rhonorm(i)*d(n);

    end;

end;

%Generation of the first derivate of the camshaft geometry to analize and
%validate the design

drho = [];
drhonorm = [];
drhocam = [];

a = rmin;
b = 1/dt;

for i = 1 : numel(rho)-1;

    drho(i) = b*(rho(i+1)-rho(i));
    drhonorm(i) = b*(rhonorm(i+1)-rhonorm(i));
    drhocam(i) = b*(rhocam(i+1)-rhocam(i)) + a;

end;

drho(numel(rho)) = b*(rho(1)-rho(numel(rho)));
drhonorm(numel(rhonorm)) = b*(rhonorm(1)-rhonorm(numel(rhonorm)));
drhocam(numel(rhocam)) = b*(rhocam(1)-rhocam(numel(rhocam))) + a;

%X and Y coordinates during two cycles (for 2-cycle camshaft plot)

theta2 = [theta/2, (2*pi+theta)/2];

rhocam2 = [rhocam, rhocam];
drhocam2 = [drho, drho];

%X and Y coordinates during three cycles (for 3-cycle camshaft plot)

theta3 = [theta/3, (2*pi+theta)/3, (4*pi+theta)/3];

rhocam3 = [rhocam, rhocam, rhocam];
drhocam3 = [drho, drho, drho];

%GENERATION OF THE PLOTS

%Trying to print it out in real size (FAIL)
%set(gcf,'PaperUnits','centimeters');
%set(gcf,'PaperSize',[42 29.7]);

fpos = figure('Name', 'Dimensions', 'Units', 'centimeters', 'NumberTitle', 'off');
set(gcf,'PaperUnits','centimeters', 'PaperSize', [42/2 27.9/2]);
set(gcf, 'units', 'centimeters', 'position', [0, 0, 28, 20]);
fpos = gcf;
hold on

xlim([-5 35]);
ylim([0 30]);

for i = 1:numel(xmin)

    plot([0, xmin(i)], [h0, ymin(i)], '-x')
    plot([0, xmax(i)], [h0, ymax(i)], '-x')

    plot([0, xtan(i)], [h0, ytan(i)], '--xb')

    plot([xsup(i), xinf(i)], [ysup(i), yinf(i)], '--xr')
    scatter(xcam(i), ycam(i))

end;

hold off

%Plot of the breathing cycle interpolation by curves

fcycle = figure('Name', 'Breath Cycle curves', 'Units', 'centimeters', 'NumberTitle', 'off');

hold on
plot(theta, psi1, '-k')
plot(theta, psi2, '-k')
plot(theta, rho1, '-g')
plot(theta, rho1next, '-g')
plot(theta, rho2, '-g')
plot(theta, rho2next, '-g')
%plot(theta, drho, '-r')
plot(theta, rho, '-b')
hold off

set(gcf,'PaperUnits','centimeters', 'PaperSize', [42/2 27.9/2]);
set(gcf, 'units', 'centimeters', 'position', [0, 0, 27.9, 27.9]);
fcam3 = gcf;

%Plot of the normalized breathing cycle and first derivate

fnorm = figure('Name', 'Normalized Breath Cycle', 'Units', 'centimeters', 'NumberTitle', 'off');

hold on
%plot(theta, drhonorm, '-r')
plot(theta, rhonorm, '-b')
hold off

set(gcf,'PaperUnits','centimeters', 'PaperSize', [42/2 27.9/2]);
set(gcf, 'units', 'centimeters', 'position', [0, 0, 27.9, 27.9]);
fcam3 = gcf;

%Plot of a 1-cycle camshaft

for n = 1:numel(d)

    fcam1 = figure('Name', '1-Cycle Camshaft', 'Units', 'centimeters', 'NumberTitle', 'off');
    %hold on

    %polarplot(theta, drhocam, '-r')
    polar(theta, rhocam(n,:), '-b')

    %rlim([0 25]);
    hold off

    set(gcf,'PaperUnits','centimeters', 'PaperSize', [42 27.9]);
    set(gcf, 'units', 'centimeters', 'position', [0, 0, 27.9, 27.9]);
    fcam3 = gcf;

end

% %Plot of a 2-cycle camshaft
%
% fcam2 = figure('Name', '2-Cycle', 'Units', 'centimeters', 'NumberTitle', 'off');
%
% %hold on;
% %polar(theta, drhocam2, '-r')
% polar(theta2, rhocam2, '-b')
% %hold off;
%
% set(gcf,'PaperUnits','centimeters', 'PaperSize', [42 27.9]);
% set(gcf, 'units', 'centimeters', 'position', [0, 0, 27.9, 27.9]);
% fcam3 = gcf;
%
%
% %Plot of a 3-cycle camshaft
%
% fcam3 = figure('Name', '3-Cycle', 'Units', 'centimeters', 'NumberTitle', 'off');
%
% %hold on;
% %polar(theta,drhocam3)
% polar(theta3, rhocam3)
% %hold off;
%
% set(gcf,'PaperUnits','centimeters', 'PaperSize', [42 27.9]);
% set(gcf, 'units', 'centimeters', 'position', [0, 0, 27.9, 27.9]);
% fcam3 = gcf;

% The OxyGEN proyect by Profoty.xyz
% Together against the Covid-19
% V4.0 rev.17/03/2020
%
% Website/blog: oxygen.protofy.xyz
% Contact: oxygen@protofy.xyz

author: ProtofyTeam

repo: OxyGEN

save_path: github-repos/MATLAB/ProtofyTeam-OxyGEN


text:
%computes the spectral slope from the magnitude spectrum
%> called by ::ComputeFeature
%>
%> @param X: spectrogram (dimension FFTLength X Observations)
%> @param f_s: sample rate of audio data (unused)
%>
%> @retval vssl spectral slope
% ======================================================================
function [vssl] = FeatureSpectralSlope (X, f_s)

    % compute mean
    mu_X = FeatureSpectralCentroid(X, f_s) * 2 / f_s * (size(X, 1)-1);

    % compute index vector
    kmu = (0:size(X, 1)-1) - (size(X, 1)+1)/2;

    % compute slope
    X = X - repmat(mu_X, size(X, 1), 1);
    vssl = (kmu*X) / (kmu*kmu');
end

author: alexanderlerch

repo: ACA-Code


text:
function centroid = polygon_centroid_2d ( n, v )

%*****************************************************************************80
%
%% POLYGON_CENTROID_2D computes the centroid of a polygon in 2D.
%
%  Discussion:
%
%    Denoting the centroid coordinates by CENTROID, then
%
%      CENTROID(1) = Integral ( Polygon interior ) x dx dy / Area ( Polygon )
%      CENTROID(2) = Integral ( Polygon interior ) y dx dy / Area ( Polygon ).
%
%    Green's theorem states that for continuously differentiable functions
%    M(x,y) and N(x,y),
%
%      Integral ( Polygon boundary ) ( M dx + N dy ) =
%      Integral ( Polygon interior ) ( dN/dx - dM/dy ) dx dy.
%
%    Using M(x,y) = 0 and N(x,y) = x**2/2, we get:
%
%      CENTROID(1) = 0.5 * Integral ( Polygon boundary ) x**2 dy
%                  / Area ( Polygon ),
%
%    which becomes
%
%      CENTROID(1) = 1/6 sum ( 1 <= I <= N )
%        ( X(I+1) + X(I) ) * ( X(I) * Y(I+1) - X(I+1) * Y(I))
%        / Area ( Polygon )
%
%    where, when I = N, the index "I+1" is replaced by 1.
%
%    A similar calculation gives us a formula for CENTROID(2).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    22 May 2005
%
%  Author:
%
%    John Burkardt
%
%  Reference:
%
%    Gerard Bashein and Paul Detmer,
%    Centroid of a Polygon,
%    Graphics Gems IV,
%    edited by Paul Heckbert,
%    AP Professional, 1994.
%
%  Parameters:
%
%    Input, integer N, the number of sides of the polygon.
%
%    Input, real V(2,N), the coordinates of the vertices.
%
%    Output, real CENTROID(2,1), the coordinates of the centroid.
%
  area = 0.0;
  centroid(1:2,1) = 0.0;

  for i = 1 : n

    if ( i < n )
      ip1 = i + 1;
    else
      ip1 = 1;
    end

    temp = ( v(1,i) * v(2,ip1) - v(1,ip1) * v(2,i) );

    area = area + temp;

    centroid(1:2,1) = centroid(1:2,1) + ( v(1:2,ip1) + v(1:2,i) )' * temp;

  end

  area = area / 2.0;

  if ( area == 0.0 )
    centroid(1:2,1) = v(1:2,1);
  else
    centroid(1:2,1) = centroid(1:2,1) / ( 6.0 * area );
  end

  return
end


text:
function fem = update10nbr(fem_struct,jj)
% update10nbr takes a node with 10 elements connected to it
%  (which defines a region) and addes 3 nodes and 6 elements to
%  that region then regroups so that no node has more than six
%  elements connected to it.  The new nodes and elements are sprung.
%
%  NOTE:  fem_struct.nei must be a component.
%
%  Variables
%  fem_struct -- finite element structure from opnml.
%  jj -- the node number that has 10 elements connected to it.
%  fem -- the updated finite element structure.
%        Note:  Only fem.x,fem.y,fem.z,fem.e, and fem.nei
%               get updated.  fem.bnd does not need to be updated.
%
%  Usage -- fem = update10nbr(fem_struct,jj)
%
%  Name: update10nbr.m
%  Written by: Ben Holladay (SEAP Student, 2004)
%  Date: June 22,2004
%  Modified:  Aug. 30, 2004 -- Chris Massey
% Last Modifed:
%         Sept. 19, 2006 -- Chris Massey, NRL Code 7322, S.S.C. MS 39529
%                      % Add test to make sure the correct nodal neighbor
%                        connectivity existed for the requested update
%                        and added a test to create the nodal neighbor list
%                        if not present.
%         April 9, 2007 -- Chris Massey, NRL Code 7322
%                     Fixed an apparent bug in the code that updates
%                     nodal neighbor list.
%         Aug. 13, 2007 -- Chris Massey, NRL Code 7322
%                     Fixed a bug in the code that updates nodal neighbor
%                     list for nodes getting extra connectivity.
%         July 14, 2008 -- Chris Massey, NRL Code 7322
%               moved test for 4 neighbors to after the test for nei.
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%Checks if the fem_struct has an nei. If not one is generated.
try
    nei = fem_struct.nei;
catch
    fem_struct.nei = ele2nei(fem_struct.e,fem_struct.x,fem_struct.y);
    nei = fem_struct.nei;
end

tempnc = sum(~ismember((fem_struct.nei(jj,:)),0));
if tempnc ~= 10
    error(['There are ',num2str(tempnc),' nodal neighbors for node number ',...
           num2str(jj),'. This routine only works for 10 nodal neighbors.']);
end

%Sets the intial fem_struct variables.
x = fem_struct.x;
y = fem_struct.y;
z = fem_struct.z;
enodes = fem_struct.e;
vod = [1:10,1:10];

%Adds new nodes and elements and labels the bad node, neighboring nodes,
%and neighboring elements.
[nnodes,nc] = size(nei);
nelems = size(enodes,1);
newnode = nnodes + [1:3];
newelem = nelems + [1:6];
badnode = jj;
nbrnode = nei(jj,1:10);
[nbrelem,~] = find(enodes == badnode);
inrad = max(sqrt((x(nbrnode)-x(badnode)).^2 + (y(nbrnode)-y(badnode)).^2));

%First guess of the new coordinates and bathymetry.
Bx = sum(x([(nbrnode([1:4])),badnode]))./5;
By = sum(y([(nbrnode([1:4])),badnode]))./5;
Bz = sum(z([(nbrnode([1:4])),badnode]))./5;

Dx = sum(x([(nbrnode([6:9])),badnode]))./5;
Dy = sum(y([(nbrnode([6:9])),badnode]))./5;
Dz = sum(z([(nbrnode([6:9])),badnode]))./5;

Cx = sum([(x([(nbrnode([4:6])),badnode]))',Bx,Dx])./6;
Cy = sum([(y([(nbrnode([4:6])),badnode]))',By,Dy])./6;
Cz = sum([(z([(nbrnode([4:6])),badnode]))',Bz,Dz])./6;

Ax = sum([(x([nbrnode([9,10,1])]))',Bx,Dx,Cx])./6;
Ay = sum([(y([nbrnode([9,10,1])]))',By,Dy,Cy])./6;
Az = sum([(z([nbrnode([9,10,1])]))',Bz,Dz,Cz])./6;

x(badnode) = Ax;
y(badnode) = Ay;
z(badnode) = Az;
x(newnode(1)) = Bx;
y(newnode(1)) = By;
z(newnode(1)) = Bz;
x(newnode(2)) = Cx;
y(newnode(2)) = Cy;
z(newnode(2)) = Cz;
x(newnode(3)) = Dx;
y(newnode(3)) = Dy;
z(newnode(3)) = Dz;

%Updates the effected elements. J variables are used to represent which
%elements are connected each node.
i = 1;
while i <= 8
    j1 = ismember((enodes((nbrelem),:)),(nbrnode(vod(i))));
    j2 = ismember((enodes((nbrelem),:)),(nbrnode(vod(i+1))));
    spb = [1,1,1,2,2,3,3,3];
    j = sum([j1,j2],2);
    temp = nbrelem(find (j == 2));
    if ~isempty(temp)
        enodes(temp(1),:) = ([nbrnode(vod(i)),nbrnode(vod(i+1)),newnode(spb(i))]);
    end
    i = i + 1;
    clear j j1 j2 temp;
end

%Addes the 6 new elements.
enodes(newelem(1),:) = [nbrnode(4),newnode(2),newnode(1)];
enodes(newelem(2),:) = [badnode,newnode(1),newnode(2)];
enodes(newelem(3),:) = [nbrnode(1),newnode(1),badnode];
enodes(newelem(4),:) = [nbrnode(6),newnode(3),newnode(2)];
enodes(newelem(5),:) = [badnode,newnode(2),newnode(3)];
enodes(newelem(6),:) = [nbrnode(9),badnode,newnode(3)];

%Springs the region.
M(1,:) = [Ax,Ay,Az];
M(2,:) = [Bx,By,Bz];
M(3,:) = [Cx,Cy,Cz];
M(4,:) = [Dx,Dy,Dz];

imax = 20;
stoptol = 10e-8 * inrad;
i = 1;
tol = stoptol + 10;
while i <= imax && tol > stoptol
    M2(2,1) = sum([(x([(nbrnode([1:4])),badnode]))',M(3,1)]);
    M2(2,2) = sum([(y([(nbrnode([1:4])),badnode]))',M(3,2)]);
    M2(2,3) = sum([(z([(nbrnode([1:4])),badnode]))',M(3,3)]);

    M2(4,1) = sum([(x([(nbrnode([6:9])),badnode]))',M(3,1)]);
    M2(4,2) = sum([(y([(nbrnode([6:9])),badnode]))',M(3,2)]);
    M2(4,3) = sum([(z([(nbrnode([6:9])),badnode]))',M(3,3)]);

    M2(3,1) = sum([(x([(nbrnode([4:6])),badnode]))',M(2,1),M(4,1)]);
    M2(3,2) = sum([(y([(nbrnode([4:6])),badnode]))',M(2,2),M(4,2)]);
    M2(3,3) = sum([(z([(nbrnode([4:6])),badnode]))',M(2,3),M(4,3)]);

    M2(1,1) = sum([(x([nbrnode([9,10,1])]))',M(2,1),M(3,1),M(4,1)]);
    M2(1,2) = sum([(y([nbrnode([9,10,1])]))',M(2,2),M(3,2),M(4,2)]);
    M2(1,3) = sum([(z([nbrnode([9,10,1])]))',M(2,3),M(3,3),M(4,3)]);
    M2 = M2 ./ 6;

    tol = max(sqrt((M2(:,1)-M(:,1)).^2 + (M2(:,2)-M(:,2)).^2));

    M = M2;
    x([badnode,([newnode(1:3)])]) = ([M(:,1)]);
    y([badnode,([newnode(1:3)])]) = ([M(:,2)]);
    z([badnode,([newnode(1:3)])]) = ([M(:,3)]);
    i = i+1;
end

%Creates the output structure.
fem = fem_struct;
fem.e = enodes;
fem.x = x;
fem.y = y;
fem.z = z;
fem.nei = nei;

%Update the connectivity for the bad node and adds the connections for
%the new nodes.
fem.nei(badnode,1:nc) = ([newnode([1:3]),nbrnode([9,10,1]),zeros(1,nc-6)]);
fem.nei(newnode(1),1:nc) = ([badnode,nbrnode(1:4),newnode(2),zeros(1,nc-6)]);
fem.nei(newnode(2),1:nc) = ([badnode,newnode(1),nbrnode(4:6),newnode(3),zeros(1,nc-6)]);
fem.nei(newnode(3),1:nc) = ([badnode,newnode(2),nbrnode(6:9),zeros(1,nc-6)]);

%Updates the nei for neighbor nodes. These nodes get an additional
%connector.
spb = ([1,4,6,9]);
for i = 1:4
    addconn = ([badnode,newnode([1:3]),badnode]);
    tmp = nei(nbrnode(spb(i)),:);
	ij = find(tmp == badnode);
    ik = min((find(tmp == 0)) + 1);
    if isempty(ik)  % TCM 08/13/2007 -- Added this check in case ik was empty.
        ik= size(nei,2)+1;
    end
    fem.nei(nbrnode(spb(i)),1:ik) = ([tmp(1:(ij-1)),addconn(i+1),addconn(i),tmp((ij+1):(ik-1))]);
end

%Updates the nei for neighbor nodes. These nodes get different connector.
spn = ([2,3,5,7,8]);
for i = 1:5
    addnode = ([newnode(1),newnode(1),newnode(2),newnode(3),newnode(3)]);
    tmp = nei(nbrnode(spn(i)),:);
    ij = find(tmp == badnode);
    % TCM 04/09/2007 -- Begin
    tmp2 = ([tmp(1:(ij-1)),addnode(i),tmp((ij+1):end)]);
    fem.nei(nbrnode(spn(i)),:) = 0;  %Zero out the list
    fem.nei(nbrnode(spn(i)),1:length(tmp2)) = tmp2; %Fill in the list
    %fem.nei(nbrnode(spn(i)),1:end) = ([tmp(1:(ij-1)),addnode(i),tmp((ij+1):end)]);
    % TCM 04/09/2007 -- End
end

%Determine the triqual for the final form to see if has very low quality
%elements.
tmp1 = (nbrelem);
tempL3=(fem.x(fem.e(tmp1,1))-fem.x(fem.e(tmp1,2))).^2+(fem.y(fem.e(tmp1,1))-...
   fem.y(fem.e(tmp1,2))).^2;
tempL1=(fem.x(fem.e(tmp1,2))-fem.x(fem.e(tmp1,3))).^2+(fem.y(fem.e(tmp1,2))-...
   fem.y(fem.e(tmp1,3))).^2;
tempL2=(fem.x(fem.e(tmp1,3))-fem.x(fem.e(tmp1,1))).^2+(fem.y(fem.e(tmp1,3))-...
   fem.y(fem.e(tmp1,1))).^2;
tempLs = ([tempL1 + tempL2 + tempL3]);
xnodes = fem.x(fem.e(tmp1,:));
ynodes = fem.y(fem.e(tmp1,:));
temparea = 0.5*(xnodes(:,1).*(ynodes(:,2)-ynodes(:,3))+xnodes(:,2).*...
   (ynodes(:,3)-ynodes(:,1))+xnodes(:,3).*(ynodes(:,1)-ynodes(:,2)));
fem.ar(tmp1) = temparea;
tempq = (4 * sqrt(3) * temparea) ./ tempLs;
clear tempL3 tempL2 tempL1 tempLs xnodes ynodes temparea;

fem1 = fem;
%Identify very low quality elements and attempts to use a line swap to fix.
poor = find(tempq < .1);
nflag = 1;good = 1;
if ~isempty(poor)
   for it = 1:length(poor)
      je = tmp1(poor(it));
      nodes = fem.e(je,:);
      nflag = 0;

      %Determine the angles to see if an edge can be fliped
      a2 = (x(enodes(je,3))-x(enodes(je,2))).^2+(y(enodes(je,3))-y(enodes(je,2))).^2;
      b2 = (x(enodes(je,1))-x(enodes(je,3))).^2+(y(enodes(je,1))-y(enodes(je,3))).^2;
      c2 = (x(enodes(je,2))-x(enodes(je,1))).^2+(y(enodes(je,2))-y(enodes(je,1))).^2;
      A = (180/pi)*acos((b2+c2-a2)./(2*sqrt(b2).*sqrt(c2)));
      B = (180/pi)*acos((c2+a2-b2)./(2*sqrt(c2).*sqrt(a2)));
      C = (180/pi)*acos((a2+b2-c2)./(2*sqrt(a2).*sqrt(b2)));
      [test,ind] = max([A,B,C]);
      if test > 160
         %Find the element numbers to flip the edge.
         nogood = nodes(ind);
         swap = setdiff(nodes,nogood);
         [temp1,tc] = find(enodes == swap(1) | enodes == swap(2));
         [b,j,k] = unique(temp1);
         temp2 = setdiff(1:length(temp1),j);
         swap = temp1(temp2);
         try
            fem1 = line_swap(fem,swap(1),swap(2));
         catch
            fem1 = fem;
         end

         %Spring the new mesh after the line swap.
         for itt = 1:2
             temp = find(fem1.nei(newnode(1),:) ~= 0);
             tempnei = fem1.nei(newnode(1),temp);
             fem1.x(newnode(1)) = mean(fem1.x(tempnei));
             fem1.y(newnode(1)) = mean(fem1.y(tempnei));
             fem1.z(newnode(1)) = mean(fem1.z(tempnei));
             temp = find(fem1.nei(newnode(2),:) ~= 0);
             tempnei = fem1.nei(newnode(2),temp);
             fem1.x(newnode(2)) = mean(fem1.x(tempnei));
             fem1.y(newnode(2)) = mean(fem1.y(tempnei));
             fem1.z(newnode(2)) = mean(fem1.z(tempnei));
             temp = find(fem1.nei(newnode(3),:) ~= 0);
             tempnei = fem1.nei(newnode(3),temp);
             fem1.x(newnode(3)) = mean(fem1.x(tempnei));
             fem1.y(newnode(3)) = mean(fem1.y(tempnei));
             fem1.z(newnode(3)) = mean(fem1.z(tempnei));
             temp = find(fem1.nei(badnode,:) ~= 0);
             tempnei = fem1.nei(badnode,temp);
             fem1.x(badnode) = mean(fem1.x(tempnei));
             fem1.y(badnode) = mean(fem1.y(tempnei));
             fem1.z(badnode) = mean(fem1.z(tempnei));
         end

         %Use triqual to determine if the new mesh is better quality.
         tmp1 = (nbrelem);
         tempL3=(fem1.x(fem1.e(tmp1,1))-fem1.x(fem1.e(tmp1,2))).^2+(fem1.y(fem1.e(tmp1,1))-...
            fem1.y(fem1.e(tmp1,2))).^2;
         tempL1=(fem1.x(fem1.e(tmp1,2))-fem1.x(fem1.e(tmp1,3))).^2+(fem1.y(fem1.e(tmp1,2))-...
            fem1.y(fem1.e(tmp1,3))).^2;
         tempL2=(fem1.x(fem1.e(tmp1,3))-fem1.x(fem1.e(tmp1,1))).^2+(fem1.y(fem1.e(tmp1,3))-...
            fem1.y(fem1.e(tmp1,1))).^2;
         tempLs = ([tempL1 + tempL2 + tempL3]);
         xnodes = fem1.x(fem1.e(tmp1,:));
         ynodes = fem1.y(fem1.e(tmp1,:));
         temparea = 0.5*(xnodes(:,1).*(ynodes(:,2)-ynodes(:,3))+xnodes(:,2).*...
            (ynodes(:,3)-ynodes(:,1))+xnodes(:,3).*(ynodes(:,1)-ynodes(:,2)));
         fem1.ar(tmp1) = temparea;
         tempq = (4 * sqrt(3) * temparea) ./ tempLs;
         clear tempL3 tempL2 tempL1 tempLs xnodes ynodes temparea;
         if min(tempq) > .4
            fem = fem1;
            nflag = 1;
         else
            nflag = 0;
            good = 6;
         end
      end
   end
end

% If problems with new mesh, then retriangulate

%Correct output if line swap failed to run
if sum(nflag) == 0
   fem = patch_update(fem_struct,fem1,jj);
   good = 6;
end

% Correct output if invalid elements were created.
if sum(fem.ar < 0) > 0
   fem = patch_update(fem_struct,fem1,jj);
   good = 6;
end

%Display message if invalid elements where created.
if good == 6
   disp('The nodally updated mesh contained invalid or badly conditioned');
   disp('elements, therefore the patch was retriangulated which should');
   disp('reduce the connecitivity but is not guaranteed to do so.');
end
if sum(fem.ar < 0) > 0
   disp(' ');
   disp('returning original mesh');
   fem = fem_struct;
end

return

author: CHLNDDEV


text:
%% script for making plots used to demonstrate trx, per-frame, window features

%% set up path

addpath ../perframe;
addpath ../perframe/compute_perframe_features;
addpath ../misc;
addpath ../filehandling;
outfigdir = 'C:\Code\Jdetect\figures\OverviewOut';

%% parameters

rootdatadir = 'C:\Data\JAABA\FlyBowl';
experiment_name = 'pBDPGAL4U_TrpA_Rig1Plate15BowlB_20110922T145928';
fly = 4;
t = 6823;

%% load the data

trx = Trx('trxfilestr','registered_trx.mat','moviefilestr','movie.ufmf','perframedir','perframe');
expdir = fullfile(rootdatadir,experiment_name);
trx.AddExpDir(expdir);
moviename = trx.movienames{1};
[readframe,nframes,fid,headerinfo] = get_readframe_fcn(moviename);

%% show the trajectory

tradius = 50;

hfig = 1;
figure(hfig);
set(hfig,'Units','pixels','Position',[10 10 1026 964]);
clf;

plot(trx(fly).x,trx(fly).y,'-','color',[.4,.4,.4]);
hold on;
idx = t-tradius+trx(fly).off:t+tradius+trx(fly).off;
plot(trx(fly).x(idx),trx(fly).y(idx),'-','linewidth',3,'color','k');
axis equal ij;
axisalmosttight;
set(gca,'Box','off');

SaveFigLotsOfWays(hfig,fullfile(outfigdir,sprintf('ExampleTrajectory_%s_fly%02d',experiment_name,fly)));

%% choose per-frame features

T0 = min(cellfun(@(x)x(1),trx.timestamps));

files = dir(fullfile(trx.expdirs{1},'perframe','*.mat'));
perframefns = cellfun(@(x) x(1:end-4),{files.name},'UniformOutput',false);
ignorefns = {'a','b','x','y','theta','sex','timestamps','dt','x_mm','y_mm','xnose_mm','ynose_mm','arena_angle','arena_r','signdtheta'};
perframefns = setdiff(perframefns,ignorefns);
idx = ~cellfun(@isempty,regexp(perframefns,'^closestfly_.*$'));
perframefns(idx) = [];

%% plot per-frame features

naxr = 5;
naxc = 3;
nax = naxr*naxc;

t0 = t-tradius;
t1 = t+tradius;
i0 = t0+trx(fly).off;
i1 = t1+trx(fly).off;

hfig = 2;
fignum = 1;
for i = 1:nax:numel(perframefns),
  figure(hfig);
  set(hfig,'Units','pixels','Position',[20 20 2243 1030]);
  clf;
  hax = createsubplots(naxr,naxc,[.025,.025;.025,.01]);
  for j = i:min(numel(perframefns),i+nax-1),
    %axes(hax(j-i+1));
    fn = perframefns{j};
    plot(hax(j-i+1),trx(fly).timestamps(i0:i1)-T0,trx(fly).(fn)(i0:i1),'k-');
    axisalmosttight([],hax(j-i+1));
    set(hax(j-i+1),'XLim',[trx(fly).timestamps(i0-1)-T0,trx(fly).timestamps(i1-1)-T0],'Box','off');
    if mod(j-i,naxr) ~= naxr-1,
      set(hax(j-i+1),'XTickLabel',{});
    end
    ylabel(hax(j-i+1),perframefns{j},'Interpreter','none');
  end
  SaveFigLotsOfWays(hfig,fullfile(outfigdir,sprintf('ExamplePerFrameFeatures_%s_fly%02d_%d',experiment_name,fly,fignum)));
  hfig = hfig + 1;
  fignum = fignum+1;
end

%% compute window features

featureparamsfilename = '../perframe/params/WindowFeatures_Touch.xml';
featureconfigfilename = '../perframe/params/featureConfig.xml';

[windowfeaturesparams,windowfeaturescellparams,basicFeatureTable,featureWindowSize] = ...
  ReadPerFrameParams(featureparamsfilename,featureconfigfilename);

perframefns1 = fieldnames(windowfeaturescellparams);

x_curr_all = cell(1,numel(perframefns1));
feature_names_all = cell(1,numel(perframefns1));

for j = 1:numel(perframefns1),
  fn = perframefns1{j};

  % get per-frame data
  perframedata = trx(fly).(fn);

  i11 = min(i1,numel(perframedata));
  [x_curr,feature_names_curr] = ...
    ComputeWindowFeatures(perframedata,windowfeaturescellparams.(fn){:},'t0',i0,'t1',i11);
  if any(imag(x_curr(:)))
    fprintf('Feature values are complex, check input\n');
  end

  if i11 < i1,
    x_curr(:,end+1:end+i1-i11) = nan;
  end

  x_curr_all{j} = x_curr;
  feature_names_all{j} = feature_names_curr;

end

%% plot window features

rng(0);

hfig = 100;

fignum = 0;
nwindowfeatures = naxc;
for i = 1:numel(perframefns1),

  pfn = perframefns1{i};

  if mod(i,naxr) == 1,
    hfig = hfig+1;
    fignum = fignum+1;
    figure(hfig);
    set(hfig,'Units','pixels','Position',[98 143 1804 963]);
    clf;
    hax = reshape(createsubplots(naxr,naxc,[.03,.03;.04,.04]),[naxr,naxc]);
    r = 1;
  else
    r = r+1;
  end

  nw = numel(feature_names_all{i});
  js = sort(randsample(nw,nwindowfeatures))';

  for jj = 1:nwindowfeatures,
    j = js(jj);
    tmp = feature_names_all{i}{j}(5:end);
    tmp(2:2:end) = cellfun(@num2str,tmp(2:2:end),'UniformOutput',false);
    wfn = [sprintf('%s_%s',feature_names_all{i}{j}{2},feature_names_all{i}{j}{4}),...
      sprintf('_%s%s',tmp{:})];
    plot(hax(r,jj),trx(fly).timestamps(i0:i1)-T0,x_curr_all{i}(j,:),'k-');
    axisalmosttight([],hax(r,jj));
    set(hax(r,jj),'XLim',[trx(fly).timestamps(i0-1)-T0,trx(fly).timestamps(i1-1)-T0],'Box','off');
    if r ~= naxr,
      set(hax(r,jj),'XTickLabel',{});
    end
    %if jj == 1,
    ylabel(hax(r,jj),perframefns{i},'Interpreter','none');
    xlabel(hax(r,jj),wfn,'Interpreter','none');
    %else
    %  ylabel(hax(r,jj),wfn,'Interpreter','none');
    %end
  end
  if mod(i,naxr) == 0,
    SaveFigLotsOfWays(hfig,fullfile(outfigdir,sprintf('ExampleWindowFeatures_%s_fly%02d_%02d',experiment_name,fly,fignum)));
  end
end

%%

hfig = 200;
plotall_perframefns = {'velmag_ctr','dnose2ell'};
for fnii = 1:numel(plotall_perframefns),
  fignum = 1;
  pfn = plotall_perframefns{fnii};
  fni = find(strcmp(perframefns1,pfn),1);
  nw = numel(feature_names_all{fni});
  for i = 1:nax:nw,

    figure(hfig);
    set(hfig,'Units','pixels','Position',[98 143 1804 963]);
    clf;
    hax = createsubplots(naxr,naxc,[.03,.03;.04,.04]);
    for j = i:min(nw,i+nax-1),
      %axes(hax(j-i+1));
      tmp = feature_names_all{fni}{j}(5:end);
      tmp(2:2:end) = cellfun(@num2str,tmp(2:2:end),'UniformOutput',false);
      wfn = [sprintf('%s_%s',feature_names_all{fni}{j}{2},feature_names_all{fni}{j}{4}),...
        sprintf('_%s%s',tmp{:})];
      plot(hax(j-i+1),trx(fly).timestamps(i0:i1)-T0,x_curr_all{fni}(j,:),'k-');
      axisalmosttight([],hax(j-i+1));
      set(hax(j-i+1),'XLim',[trx(fly).timestamps(i0-1)-T0,trx(fly).timestamps(i1-1)-T0],'Box','off');
      if mod(j-i,naxr) ~= naxr-1,
        set(hax(j-i+1),'XTickLabel',{});
      end
      %ylabel(hax(j-i+1),pfn,'Interpreter','none');
      xlabel(hax(j-i+1),wfn,'Interpreter','none');
    end
    pause(1);
    SaveFigLotsOfWays(hfig,fullfile(outfigdir,sprintf('ExampleWindowFeatures_%s_%s_fly%02d_%d',pfn,experiment_name,fly,fignum)));
    hfig = hfig + 1;
    fignum = fignum+1;
  end
end

author: kristinbranson

repo: JAABA


text:
function databases = DBList(pattern)

%DBList - List existing databases.
%
%  USAGE
%
%    databases = DBList(pattern)
%
%    pattern        optional name search pattern (see below)
%
%  EXAMPLES
%
%    % List all databases
%    databases = DBList;
%
%    % List all databases starting with 'LFP'
%    databases = DBList('LFP.*');
%
%  SEE
%
%    See also DBConnect.
%

% Copyright (C) 2012-2013 by Michaël Zugaro
%
% This program is free software; you can redistribute it and/or modify
% it under the terms of the GNU General Public License as published by
% the Free Software Foundation; either version 3 of the License, or
% (at your option) any later version.

% Make sure MyM is installed and functional
CheckMyM;

try
	h = mym('show databases;');
catch
	error(['Could not list databases (check DB server connection).']);
end
databases = h.Database;

if nargin >= 1,
	databases = regexp(databases,pattern,'match');
	databases = vertcat(databases{:});
end

author: buzsakilab

repo: buzcode

save_path: github-repos/MATLAB/buzsakilab-buzcode


% Calculate the 16-bit CRC used on the payload for DroneID
%
% This code is a MATLAB port of
% https://github.com/dji-sdk/Guidance-SDK/blob/master/examples/uart_example/crc16.cpp
%
% This function can be used to validate a CRC by running with all bytes including the CRC.  If the purpose is to
% calculate what the CRC should be, then do not include the additional two bytes of zeros and instead just remove the
% bytes that will be the CRC
%
% @param byte_vector Row/Column vector of uint8 values
% @return checksum uint16 CRC output value
function [checksum] = calculate_crc(byte_vector)

    crc_table = [
        0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, ...
        0x8c48, 0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, 0xe97e, 0xf8f7, ...
        0x1081, 0x0108, 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e, ...
        0x9cc9, 0x8d40, 0xbfdb, 0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876, ...
        0x2102, 0x308b, 0x0210, 0x1399, 0x6726, 0x76af, 0x4434, 0x55bd, ...
        0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e, 0xfae7, 0xc87c, 0xd9f5, ...
        0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e, 0x54b5, 0x453c, ...
        0xbdcb, 0xac42, 0x9ed9, 0x8f50, 0xfbef, 0xea66, 0xd8fd, 0xc974, ...
        0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb, ...
        0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3, ...
        0x5285, 0x430c, 0x7197, 0x601e, 0x14a1, 0x0528, 0x37b3, 0x263a, ...
        0xdecd, 0xcf44, 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72, ...
        0x6306, 0x728f, 0x4014, 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9, ...
        0xef4e, 0xfec7, 0xcc5c, 0xddd5, 0xa96a, 0xb8e3, 0x8a78, 0x9bf1, ...
        0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, 0x242a, 0x16b1, 0x0738, ...
        0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, 0x9af9, 0x8b70, ...
        0x8408, 0x9581, 0xa71a, 0xb693, 0xc22c, 0xd3a5, 0xe13e, 0xf0b7, ...
        0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff, ...
        0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036, ...
        0x18c1, 0x0948, 0x3bd3, 0x2a5a, 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e, ...
        0xa50a, 0xb483, 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5, ...
        0x2942, 0x38cb, 0x0a50, 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd, ...
        0xb58b, 0xa402, 0x9699, 0x8710, 0xf3af, 0xe226, 0xd0bd, 0xc134, ...
        0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, 0x6e6e, 0x5cf5, 0x4d7c, ...
        0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1, 0xa33a, 0xb2b3, ...
        0x4a44, 0x5bcd, 0x6956, 0x78df, 0x0c60, 0x1de9, 0x2f72, 0x3efb, ...
        0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232, ...
        0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a, ...
        0xe70e, 0xf687, 0xc41c, 0xd595, 0xa12a, 0xb0a3, 0x8238, 0x93b1, ...
        0x6b46, 0x7acf, 0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9, ...
        0xf78f, 0xe606, 0xd49d, 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330, ...
        0x7bc7, 0x6a4e, 0x58d5, 0x495c, 0x3de3, 0x2c6a, 0x1ef1, 0x0f78 ...
    ];

    assert(~isempty(byte_vector), "Input vector was empty");
    assert(isrow(byte_vector) || iscolumn(byte_vector), "Input vector must be row/col");

    checksum = uint16(0x3692);

    for idx=1:length(byte_vector)
        % Very verbose due to the amount of strange things that need to be done to make MATLAB play nice with bit ops
        new_byte = uint8(byte_vector(idx));
        table_idx = bitxor(checksum, uint16(new_byte));
        table_idx = bitand(table_idx, uint16(0x00ff));
        val = crc_table(table_idx + 1);
        temp_checksum = bitshift(checksum, -8);
        checksum = bitxor(temp_checksum, val);
    end
end


text:
function deri = deri_softmax(deri_cost)
    global config;
    deri = deri_cost / config.batch_size;
end

author: jimmy-ren