function [A,xHyp,PHyp,GateMat]=makeStandardCartOnlyLRMatHyps(xPred,SPred,zCart,SRCart,SRCartMax,PD,lambda,rPred,gammaVal,measJacobDet)
%%MAKESTANDARDCARTONLYLRMATHYPS Create the likelihood ratio matrix used for
%         2D assignment under standard Gaussian approximations in the
%         coordinate system of the target states using Gaussian-
%         approximated Cartesian position-only measurements and provide
%         updated state estimates when tracks gate with the measurements.
%         Brute-force gating is used. This function is appropriate for use
%         in Cartesian-converted measurement tracking without range rate
%         information. This assumes that the false alarms occur due to a
%         Poisson point process with a constant density lambda that is
%         either in the coordinate system of the states (Cartesian) or in
%         the coordinate system of a converted measurement when appropriate
%         parameters are passed. This implements the unitless ratio of [1],
%         except the missed detection probability with gating is 1-PD*PG
%         and not just 1-PD due to Section 3.5.3 of [2] (for multiple
%         gammaVal values, the larged PG is chosen). Additionally, the
%         covariance inflation due to gating applied to a missed detection
%         hypothesis (due to [3], Equation 28) can be used, if desired.
%
%INPUTS: xPred A numDimXnumTar set of predicted target states. The
%              first posDim Components are position.
%        SPred A numDimXnumDimXnumTar set of lower-triangular square-root
%              state covariance matrices.
%        zCart The zDimXnumMeas set of measurements in Cartesian
%              coordinates.
%       SRCart A zDimXzDimXnumMeas set of lower-triangular square-root
%              measurement covariance matrices. If all of the covariance
%              matrices are the same, then SRCart can be a single zDimXzDim
%              lower triangular matrix.
%    SRCartMax A zDimXzDimXnumTar set of maximum measurement covariance
%              matrices that can be taken into consideration for gating and
%              will be used to compute the covariance inflation of [3]
%              (Equation 28) for missed detection hypotheses. If only a
%              single zDimXzDim matrix is passed, it is assumed the same
%              for all targets. If an empty matrix is passed, then the
%              covariance inflation is not performed. The inflation is
%              insignificant if the gate probability PG is almost 1 and
%              PD is less than 1 by a decent amount. On the other hand, if
%              PD is high but PG is low, then this can be the difference
%              between track divergence or not.
%           PD The target detection probabilities. This can either be a
%              numTarX1 vector if the targets have different probabilities,
%              or this can be a scalar value if all of the target detection
%              probabilities are the same. This value does not include the
%              effects of gating (set by gammaVal) eliminating measurements
%              from consideration.
%       lambda The false alarm density. When given in Cartesian
%              coordinates, this has units of # false alarm/volume, for
%              example, false alarms/m^3. When given in the coordinate
%              system of the measurement (anticipating a conversion at a
%              single point using a Jacobian), this has units of inverse of
%              the "volume" in the measurement coordinate system. For
%              example, for polar measurements, this might be inverse
%              meters*radians. If this is not in Cartesian coordinates,
%              then the inputs measJacobDet must be provided.
%        rPred When using a single-scan tracking algorithm with integrated
%              target existence probabilities, this is the numTarX1 set of
%              target existence probabilities. If omitted or an empty
%              matrix is passed, then rPred=1 is used (everything exists).
%     gammaVal This is the threshold for gating. Gating is performed
%              by a brute-force evaluation of Mahalanobis distances between
%              predicted target locations and the measurements. gammaVal is
%              related to the probability region about the target as
%              described in Chatper 2.3.2 of [2]. For a zDim-dimensional
%              measurement, one can obtain the value of gammaVal for a
%              99.97% probability region using
%              ChiSquareD.invCDF(0.9997,zDim). The default if this
%              parameter is omitted or an empty matrix is passed is Inf,
%              meaning that everything gates.
% measJacobDet If this input is omitted, then it is assumed that lambda is
%              given in the same coordinate system as the target state
%              (typically Cartesian). Otherwise, this is a length numMeas
%              array such that measJacobDet(i) is
%              det(measJacob(zNative(:,i))) where measJacob is a function
%              that computes the Jacobian matrix of the measurement and
%              zNative is the measurement in the original coordinate system
%              of the measurement, not the coordinate system of the state.
%              For example, if the original measurements were in spherical
%              coordinates, measJacob could be calcSpherConvJacob(z,0), and
%              thus measJacobDet would be determinates of the outputs of
%              calcSpherConvJacob evaluated at all of the measurements.
%
%OUTPUTS: A A numTarX(numMeas+numTar) matrix of target-measurement and
%           missed detection likelihood ratios computed as in [1] for
%           Gaussian states/(converted) measurements. Columns > numMeas
%           hold missed-detection likelihoods. Thus, off-diagonal terms
%           for columns > numMeas are set to 0 and the diagonal terms
%           set to the costs of a missed detection for each given target.
%           When given rPred, PD is just multiplied by these values before
%           computing the assignment matrix. If a measurement does not
%           gate, then the corresponding entry in A is explicitly set to
%           0.
%      xHyp A numDimXnumTarX(numMeas+1) set of conditionally updated target
%           states (using the prior, the measurement, and the
%           sqrtKalmanMeasPred and sqrtKalmanUpdateWithPred functions,
%           which are equivalent to the sqrtKalmanUpdate function), one for
%           each association hypothesis in A. For hypotheses that do not
%           gate, then the corresponding entry in xHyp is all zeros. The
%           last hypothesis (missed detection) is the same as the
%           prediction value, but the covariance might be inflated as in
%           [3] if the SRCartMax input is provided.
%      PHyp A numDimXnumDimXnumTarX(numMeas+1) set of conditionally updated
%           target covariance matrices corresponding to the state estimate
%           in xHyp. Hypotheses are not provided in square root form so as
%           to simplify their usage in JPDAF-style filters.
%   GateMat A numTarXnumMeas boolean matrix indicating whether each target
%           gates with each measurement.
%
%We are using the dimensionless score function from [1]. This formulation
%in terms of likelihood ratios eliminates the need to compute the volumes
%of detection gates. Of course, if these likelihood ratios are computed in
%Cartesian coordinates, but lambda is in a measurement coordinate system,
%this is just an approximation.
%
%The assignment matrix returned by this function is commonly used in
%functions such as assign2D, singleScanUpdate, and
%singleScanUpdateWithExistence. This function is useful when performing
%tracking using Cartesian-converted measurements.
%
%Developments of the score function are given in [1] and relevant parts of
%Section 3.5.3 and other sections of [2].
%
%REFERENCES:
%[1] Y. Bar-Shalom, S. S. Blackman, and R. J. Fitzgerald, "Dimensionless
%    score function for multiple hypothesis tracking," IEEE Transactions on
%    Aerospace and Electronic Systems, vol. 43, no. 1, pp. 392-400, Jan.
%    2007.
%[2] Y. Bar-Shalom, P. K. Willett, and X. Tian, Tracking and Data Fusion.
%    Storrs, CT: YBS Publishing, 2011.
%[3] X. R. Li, "Tracking in clutter with strongest neighbor measurements -
%    Part i: Theoretical analysis," IEEE Transactions on Automatic Control,
%    vol. 43, no. 11, pp. 1560-1578, Nov. 1998.
%
%June 2018 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

xDim=size(xPred,1);

if(nargin<10)
    measJacobDet=[];
else
    %They must all be positive.
    measJacobDet=abs(measJacobDet);
end

if(nargin<9||isempty(gammaVal))
    gammaVal=Inf;
end

numTar=size(xPred,2);
zDim=size(zCart,1);
numMeas=size(zCart,2);

if(nargin<8||isempty(rPred))
    %Assume all targets exist with probability 1.
    rPred=ones(numTar,1);
end

%If only one is given, assume they are all the same.
if(size(SRCart,3)==1)
    SRCart=repmat(SRCart,[1,1,numMeas]);
end

%If only one is given, assume they are all the same.
if(~isempty(SRCartMax)&&size(SRCartMax,3)==1)
    SRCartMax=repmat(SRCartMax,[1,1,numTar]);
end

if(~isempty(SRCartMax)&&isempty(zCart))
    %Get the correct measurement dimensionality to use to update the missed
    %detection covariance matrix with PG for the possibility that the
    %measurement is actually outside of the gate.
    zDim=size(SRCartMax,1);
end

if(isscalar(PD))
    PD=PD*ones(numTar,1);
end

%The gate probability (from an inverse chi-squared PDF). Choose PG to be
%the maximum gate when multiple gammaVal values are given.
PG=ChiSquareD.CDF(max(gammaVal(:)),zDim);

%Whether or not the target is detected is affected by whether it exists
%(the rPred).
PD=PD(:).*rPred(:);

%The measurement matrix.
H=[eye(zDim,zDim),zeros(zDim,xDim-zDim)];

numHyp=numMeas+1;%The extra one is the missed detection hypothesis.

%The likelihood ratio matrix.
A=zeros(numTar,numMeas+numTar);
%The gating matrix
GateMat=false(numTar,numMeas);

%Conditional hypothesis updates.
xHyp=zeros(xDim,numTar,numHyp);
PHyp=zeros(xDim,xDim,numTar,numHyp);
for curTar=1:numTar
    xPredCur=xPred(:,curTar);
    SPredCur=SPred(:,:,curTar);

    %The missed detection likelihood (includes the gating probability).
    A(curTar,numMeas+curTar)=(1-PD(curTar)*PG);
    xHyp(:,curTar,numHyp)=xPredCur;
    PHyp(:,:,curTar,numHyp)=SPredCur*SPredCur';

    if(numMeas>0||~isempty(SRCartMax))
        %Get the filter measurement prediction once (not for all
        %measurements). This is independent of the measurement covariance
        %matrix.
        [zPred,PzPred,otherInfo]=sqrtKalmanMeasPred(xPredCur,SPredCur,H);

        %If upper bounds on the measurement covariances to consider for
        %each target are provided.
        if(~isempty(SRCartMax))
            %The gain corresponding to a gate with the maximum considered
            %Cartesian covariance matrix for that target.
            W=calcSqrtKalmanGain(SRCartMax(:,:,curTar),otherInfo);
            Pzz=PzPred+(SRCartMax(:,:,curTar)*SRCartMax(:,:,curTar)');
            PMissed=calcMissedGateCov(PHyp(:,:,curTar,numHyp),Pzz,W,PD(curTar),gammaVal,PG);
            PHyp(:,:,curTar,numHyp)=PMissed;
        end
    end

    for curMeas=1:numMeas
        %First, we evaluate the Mahalanobis distance necessary for gating,
        %then we evaluate the measurement update and the likelihood ratio
        %if the target gates.
        innov=zCart(:,curMeas)-zPred;

        Pzz=PzPred+(SRCart(:,:,curMeas)*SRCart(:,:,curMeas)');
        mahabDist=innov'*inv(Pzz)*innov;
        if(mahabDist>gammaVal)
            %If it does not gate, then assign a zero likelihood ratio.
            A(curTar,curMeas)=0;
            continue;
        end

        %It gates.
        GateMat(curTar,curMeas)=true;

        %Perform the measurement update.
        [xUpdate, SUpdate,innov,Szz]=sqrtKalmanUpdateWithPred(zCart(:,curMeas),SRCart(:,:,curMeas),zPred,otherInfo);

        xHyp(:,curTar,curMeas)=xUpdate;
        PHyp(:,:,curTar,curMeas)=SUpdate*SUpdate';

        if(~isempty(measJacobDet))
            %If the clutter density is given in some local measurement
            %coordinate system, then use the Jacobian to do a single-
            %point transformation.
            JDet=measJacobDet(curMeas);
        else
            JDet=1;
        end

        %Evaluate the likelihood ratio.
        A(curTar,curMeas)=PD(curTar)/(JDet*lambda)*GaussianD.PDFS(innov,zeros(zDim,1),Szz);
    end
end
end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


classdef testInverseSymmetricFourthOrderTensor < testInverseFourthOrderTensor

    methods (Access = protected)
        function createRandomFourthOrderTensor(obj)
            obj.tensor = Stiffness3DTensor;
            obj.tensor.createRandomTensor();
        end
    end

    methods (Static, Access = protected)

        function Id = computeIdentityTensor(I,i,j,k,l)
            Id = 0.5*(I(i,k)*I(j,l) + I(i,l)*I(j,k));
        end
    end

end


function h=ref_pfilt_1(f,g,a)
%REF_PFILT_1  Reference PFILT implementation by FFT
%
%   This is the old reference pfilt from before the struct filters where
%   introduced.

[L W]=size(f);

g=fir2long(g,L);

% Force FFT along dimension 1, since we have permuted the dimensions
% manually
if isreal(f) && isreal(g)
  h=ifftreal(fftreal(f,L,1).*repmat(fftreal(g,L,1),1,W),L,1);
else
  h=ifft(fft(f,L,1).*repmat(fft(g,L,1),1,W),L,1);
end;

h=h(1:a:end,:);


function [ stg_Ld, stg_Rd, att ] = filter_damping_design( stg_L, stg_C, fs, Zmax)
% function [ stg_Ld, stg_Rd, att ] = filter_damping_design( stg_L, stg_C, fs, Zmax)
% Design the optimal damping for a passive filter in power electronics applications.
% Written by Dr. Yoash Levron
%
% This function designs the optimal L-R damping network for a multistage passive filter.
% It compues the optimal damping network, that achieves the best
% attenuation at a desired switching frequency fs, and has an output impedance
% smaller than a specified threshold Zmax, over all frequencies.
% the number of stages may be 1,2 or 3.
% The damping inductors, if placed at all, are chosen to be equal,
% so practical filters can be built using the same inductor, reducing cost.
% The attached photo shows the topology and component names.
%
% Input:
% stg_L - [H] specifies the primary inductors at each stage.
%                    This is a vector with 1, 2 or 3 components.
% stg_C - [F] specifies the primary capacitors at each stage.
%                     It is the same length as stg_L.
% fs - [Hz] desired switching frequency to attenuate.
% Zmax - [ohm] desired maximal output impedance (over all frequencies).
%
% Output:
% stg_Ld - [H] the resulting damping inductors.
% stg_Rd - [ohm] the resulting damping resistors.
% att - [dB] the resulting optimal attenuation at frequency fs

% numeric parameters
NLd = 10; % number of damping inductors to compare
NRd = 20;  % number of damping resistors to compare
min_resistor_factor = 0.2;
max_resistor_factor = 20;

% default outputs
stg_Ld = NaN;
stg_Rd = NaN;
att = NaN;
Zmax_out = NaN;

% check inputs:
if (length(stg_L) == length(stg_C))
    NS = length(stg_L);  % number of stages
else
    disp('filter_damping_design - error : unequal number of inductors and capacitors');
    beep; return
end
if (NS >3)
    disp('filter_damping_design - error : Three stages maximum');
    beep; return
end

Ltot = sum(stg_L);
Ctot = sum(stg_C);
Rnom = (Ltot/Ctot)^0.5;
Ldvec = linspace((max(stg_L)/NLd),max(stg_L),NLd);
Rdvec = logspace( log10(min_resistor_factor*Rnom), ...
    log10(max_resistor_factor*Rnom),  (NRd-1));
Rdvec(NRd) = 10e6;  % add an infinite resistor

ws = 2*pi*fs;

%%%% design for a first order filter %%%%
if (NS ==1)
    L1 =  stg_L(1);
    C1 =  stg_C(1);
    Ldvec = [0 Ldvec];   NLd=NLd+1;  % add a zero to the possible inductor values
    MAT = zeros(NLd,NRd);  % matrix of results
    for ii1 = 1:NLd  % index for damping inductor
        xx = (ii1-1) / NLd;
        done = floor(1000*xx)/10;
        disp(sprintf('%3.1f%% done',done));

        for ii2 = 1:NRd % index for 1st damping resistor
            % determine components
            Ld1 = Ldvec(ii1);
            Rd1 = Rdvec(ii2);

            % compute coefficients
            a1 = (L1 + Ld1)/Rd1;
            d1 = L1;
            d2 = (L1*Ld1)/Rd1;
            b1 = (L1 + Ld1)/Rd1;
            b2 =  C1*L1;
            b3 = (C1*L1*Ld1)/Rd1;

            % compute the resonance frequencies - vector Wr
            Wr = [(1/b2) (b1/b3)].^0.5;
            ind = find(~isinf(Wr));
            Wr = Wr(ind);

            % compute the impedace at resonance
            s = i*Wr;
            Zout = (d1*s + d2*s.^2) ./ (1 + b1*s + b2*s.^2 + b3*s.^3);
            ind = find(abs(Zout) > Zmax);
            output_impedance_too_big = ~isempty(ind);

            % compute the attenuation
            s = i*ws;
            H = (1 + a1*s) / (1 + b1*s + b2*s^2 + b3*s^3);
            H_dB = 20*log10(abs(H));

            MAT(ii1,ii2) = H_dB + 1e10*output_impedance_too_big;
        end
    end
    %%% find the best filter
    [att, ind] = min(MAT(:));
    if (att > 1e9)
        disp('filter_damping_design - error : Design failed. Output impedance too high');
        beep; return;
    end
    [ii1,ii2] = ind2sub(size(MAT),ind);  % indexes of optimal filter
    Ld1 = Ldvec(ii1);
    Rd1 = Rdvec(ii2);
    stg_Ld = [Ld1];
    stg_Rd = [Rd1];
end

%%%% design for a second order filter %%%%
if (NS ==2)
    disp('This may take a few minutes. please wait ...');
    L1 =  stg_L(1);  L2 = stg_L(2);
    C1 =  stg_C(1);  C2 = stg_C(2);
    MAT = zeros(NLd,NRd,NRd,2,2);  % matrix of results
    for ii1 = 1:NLd  % index for damping inductor
        for ii2 = 1:NRd % index for 1st damping resistor
            xx = ((ii1-1)*NRd+(ii2-1)) / (NLd*NRd);
            done = floor(1000*xx)/10;
            disp(sprintf('%3.1f%% done',done));

            for ii3 = 1:NRd % index for 2st damping resistor
                for ii5 = 1:2  % index for the existance of 1st damping inductor
                    for ii6 = 1:2  % index for the existance of 2st damping inductor
                        % determine components
                        Ld1 = Ldvec(ii1) * (ii5-1);
                        Ld2 = Ldvec(ii1) * (ii6-1);
                        Rd1 = Rdvec(ii2);
                        Rd2 = Rdvec(ii3);

                        % compute coefficients
                        a1 = (L1 + Ld1)/Rd1 + (L2 + Ld2)/Rd2;
                        a2 = ((L1 + Ld1)*(L2 + Ld2))/(Rd1*Rd2);
                        d1 = L1 + L2;
                        d2 = (L1*L2 + L1*Ld1 + L2*Ld1)/Rd1 + (L1*L2 + L1*Ld2 + L2*Ld2)/Rd2;
                        d3 = (L1*L2*Ld1 + L1*L2*Ld2 + L1*Ld1*Ld2 + L2*Ld1*Ld2)/(Rd1*Rd2) + C1*L1*L2;
                        d4 = (C1*L1*L2*(Ld1*Rd2 + Ld2*Rd1))/(Rd1*Rd2);
                        d5 = (C1*L1*L2*Ld1*Ld2)/(Rd1*Rd2);
                        b1 = (L1 + Ld1)/Rd1 + (L2 + Ld2)/Rd2;
                        b2 = C1*L1 + C2*L1 + C2*L2 + ((L1 + Ld1)*(L2 + Ld2))/(Rd1*Rd2);
                        b3 = (C2*L1*L2 + C1*L1*Ld1 + C2*L1*Ld1 + C2*L2*Ld1)/Rd1 + ...
                            (C1*L1*L2 + C2*L1*L2 + C1*L1*Ld2 + C2*L1*Ld2 + C2*L2*Ld2)/Rd2;
                        b4 = (C1*L1*L2*Ld1 + C2*L1*L2*Ld1 + C2*L1*L2*Ld2 + C1*L1*Ld1*Ld2 ...
                            + C2*L1*Ld1*Ld2 + C2*L2*Ld1*Ld2)/(Rd1*Rd2) + C1*C2*L1*L2;
                        b5 = (C1*C2*L1*L2*(Ld1*Rd2 + Ld2*Rd1))/(Rd1*Rd2);
                        b6 = (C1*C2*L1*L2*Ld1*Ld2)/(Rd1*Rd2);

                        % compute the resonance frequencies - vector Wr
                        P1 = [-b6, +b4, -b2, +1];
                        P2 = [+b5, -b3, +b1];
                        WrA = (roots(P1)).^0.5;
                        WrB = (roots(P2)).^0.5;
                        ind1 = find(real(WrA) > 1e6*imag(WrA) );
                        ind2 = find(real(WrB) > 1e6*imag(WrB));
                        Wr = [real(WrA(ind1)).' real(WrB(ind2)).'];

                        % compute the impedace at resonance
                        s = i*Wr;
                        Zout = (d1*s + d2*s.^2 + d3*s.^3 + d4*s.^4 + d5*s.^5) ./ ...
                            (1 + b1*s + b2*s.^2 + b3*s.^3 + b4*s.^4 + b5*s.^5 + b6*s.^6);
                        ind = find(abs(Zout) > Zmax);
                        output_impedance_too_big = ~isempty(ind);

                        % compute the attenuation
                        s = i*ws;
                        H = (1 + a1*s + a2*s^2) / ...
                            (1 + b1*s + b2*s^2 + b3*s^3 + b4*s^4 + b5*s^5 + b6*s^6);
                        H_dB = 20*log10(abs(H));

                        MAT(ii1,ii2,ii3,ii5,ii6) = H_dB + 1e10*output_impedance_too_big;
                    end
                end
            end
        end
    end
    %%% find the best filter
    [att, ind] = min(MAT(:));
    if (att > 1e9)
        disp('filter_damping_design - error : Design failed. Output impedance too high');
        beep; return;
    end
    [ii1,ii2,ii3,ii5,ii6] = ind2sub(size(MAT),ind);  % indexes of optimal filter
    Ld1 = Ldvec(ii1) * (ii5-1);       Ld2 = Ldvec(ii1) * (ii6-1);
    Rd1 = Rdvec(ii2);       Rd2 = Rdvec(ii3);
    stg_Ld = [Ld1 Ld2];
    stg_Rd = [Rd1 Rd2];
end

%%%% design for a third order filter %%%%
if (NS == 3)
    disp('This may take a few minutes. please wait ...');
    L1 =  stg_L(1);  L2 = stg_L(2);  L3 = stg_L(3);
    C1 =  stg_C(1);  C2 = stg_C(2);  C3 = stg_C(3);
    MAT = zeros(NLd,NRd,NRd,NRd,2,2,2);  % matrix of results
    for ii1 = 1:NLd  % index for damping inductor
        for ii2 = 1:NRd % index for 1st damping resistor
            xx = ((ii1-1)*NRd+(ii2-1)) / (NLd*NRd);
            done = floor(1000*xx)/10;
            disp(sprintf('%3.1f%% done',done));

            for ii3 = 1:NRd % index for 2st damping resistor
                for ii4 = 1:NRd % index for 3rd damping resistor
                    for ii5 = 1:2  % index for the existance of 1st damping inductor
                        for ii6 = 1:2  % index for the existance of 2st damping inductor
                            for ii7 = 1:2  % index for the existance of 3rd damping inductor
                                % determine components
                                Ld1 = Ldvec(ii1) * (ii5-1);
                                Ld2 = Ldvec(ii1) * (ii6-1);
                                Ld3 = Ldvec(ii1) * (ii7-1);
                                Rd1 = Rdvec(ii2);
                                Rd2 = Rdvec(ii3);
                                Rd3 = Rdvec(ii4);

                                % compute coefficients
                                a1 = (L1 + Ld1)/Rd1 + (L2 + Ld2)/Rd2 + (L3 + Ld3)/Rd3;
                                a2 = ((L1 + Ld1)/(Rd1*Rd3) + (L2 + Ld2)/(Rd2*Rd3))*(L3 + Ld3) +...
                                    ((L1 + Ld1)*(L2 + Ld2))/(Rd1*Rd2);
                                a3 = ((L1 + Ld1)*(L2 + Ld2)*(L3 + Ld3))/(Rd1*Rd2*Rd3);
                                d1 = L1 + L2 + L3;
                                d2 = (L1*L2*Rd1 + L1*L2*Rd2 + L1*L3*Rd2 + L2*L3*Rd1 + ...
                                    L1*Ld1*Rd2 + L1*Ld2*Rd1 + L2*Ld1*Rd2 + L2*Ld2*Rd1 + ...
                                    L3*Ld1*Rd2 + L3*Ld2*Rd1)/(Rd1*Rd2) +...
                                    (L1*L3*Rd1*Rd2 + L2*L3*Rd1*Rd2 + L1*Ld3*Rd1*Rd2 + ...
                                    L2*Ld3*Rd1*Rd2 + L3*Ld3*Rd1*Rd2)/(Rd1*Rd2*Rd3);
                                d3 =  (L1*L2*L3 + L1*L2*Ld1 + L1*L2*Ld2 + L1*L3*Ld2 + ...
                                    L2*L3*Ld1 + L1*Ld1*Ld2 + L2*Ld1*Ld2 + L3*Ld1*Ld2 + ...
                                    C1*L1*L2*Rd1*Rd2 + C1*L1*L3*Rd1*Rd2 + C2*L1*L3*Rd1*Rd2 + ...
                                    C2*L2*L3*Rd1*Rd2)/(Rd1*Rd2) + (L1*L2*L3*Rd1 + L1*L2*L3*Rd2 + ...
                                    L1*L2*Ld3*Rd1 + L1*L3*Ld1*Rd2 + L1*L3*Ld2*Rd1 + L1*L2*Ld3*Rd2 +...
                                    L2*L3*Ld1*Rd2 + L2*L3*Ld2*Rd1 + L1*L3*Ld3*Rd2 + L2*L3*Ld3*Rd1 +...
                                    L1*Ld1*Ld3*Rd2 + L1*Ld2*Ld3*Rd1 + L2*Ld1*Ld3*Rd2 + L2*Ld2*Ld3*Rd1 +...
                                    L3*Ld1*Ld3*Rd2 + L3*Ld2*Ld3*Rd1)/(Rd1*Rd2*Rd3);
                                d4 = (C1*L1*L2*L3*Rd1 + C2*L1*L2*L3*Rd1 + C2*L1*L2*L3*Rd2 + ...
                                    C1*L1*L2*Ld1*Rd2 + C1*L1*L2*Ld2*Rd1 + C1*L1*L3*Ld1*Rd2 + ...
                                    C1*L1*L3*Ld2*Rd1 + C2*L1*L3*Ld1*Rd2 + C2*L1*L3*Ld2*Rd1 + ...
                                    C2*L2*L3*Ld1*Rd2 + C2*L2*L3*Ld2*Rd1)/(Rd1*Rd2) +...
                                    (L1*L2*L3*Ld1 + L1*L2*L3*Ld2 + L1*L2*L3*Ld3 + L1*L2*Ld1*Ld3 +...
                                    L1*L3*Ld1*Ld2 + L1*L2*Ld2*Ld3 + L2*L3*Ld1*Ld2 + L1*L3*Ld2*Ld3 +...
                                    L2*L3*Ld1*Ld3 + L1*Ld1*Ld2*Ld3 + L2*Ld1*Ld2*Ld3 + L3*Ld1*Ld2*Ld3 +...
                                    C1*L1*L2*L3*Rd1*Rd2 + C1*L1*L2*Ld3*Rd1*Rd2 + C1*L1*L3*Ld3*Rd1*Rd2 +...
                                    C2*L1*L3*Ld3*Rd1*Rd2 + C2*L2*L3*Ld3*Rd1*Rd2)/(Rd1*Rd2*Rd3);
                                d5 =  (C1*L1*L2*L3*Ld1 + C2*L1*L2*L3*Ld1 + ...
                                    C2*L1*L2*L3*Ld2 + C1*L1*L2*Ld1*Ld2 + C1*L1*L3*Ld1*Ld2 +...
                                    C2*L1*L3*Ld1*Ld2 + C2*L2*L3*Ld1*Ld2 + ...
                                    C1*C2*L1*L2*L3*Rd1*Rd2)/(Rd1*Rd2) + (C1*L1*L2*L3*Ld1*Rd2 +...
                                    C1*L1*L2*L3*Ld2*Rd1 + C1*L1*L2*L3*Ld3*Rd1 + C2*L1*L2*L3*Ld3*Rd1 +...
                                    C2*L1*L2*L3*Ld3*Rd2 + C1*L1*L2*Ld1*Ld3*Rd2 + C1*L1*L2*Ld2*Ld3*Rd1 +...
                                    C1*L1*L3*Ld1*Ld3*Rd2 + C1*L1*L3*Ld2*Ld3*Rd1 + C2*L1*L3*Ld1*Ld3*Rd2 +...
                                    C2*L1*L3*Ld2*Ld3*Rd1 + C2*L2*L3*Ld1*Ld3*Rd2 + ...
                                    C2*L2*L3*Ld2*Ld3*Rd1)/(Rd1*Rd2*Rd3);
                                d6 = (C1*L1*L2*L3*Ld1*Ld2 + C1*L1*L2*L3*Ld1*Ld3 + ...
                                    C2*L1*L2*L3*Ld1*Ld3 + C2*L1*L2*L3*Ld2*Ld3 + C1*L1*L2*Ld1*Ld2*Ld3 +...
                                    C1*L1*L3*Ld1*Ld2*Ld3 + C2*L1*L3*Ld1*Ld2*Ld3 + C2*L2*L3*Ld1*Ld2*Ld3 +...
                                    C1*C2*L1*L2*L3*Ld3*Rd1*Rd2)/(Rd1*Rd2*Rd3) +...
                                    (C1*C2*L1*L2*L3*(Ld1*Rd2 + Ld2*Rd1))/(Rd1*Rd2);
                                d7 =  (C1*C2*L1*L2*L3*(Ld1*Ld2*Rd3 + Ld1*Ld3*Rd2 +...
                                    Ld2*Ld3*Rd1))/(Rd1*Rd2*Rd3);
                                d8 = (C1*C2*L1*L2*L3*Ld1*Ld2*Ld3)/(Rd1*Rd2*Rd3);
                                b1 = (L1 + Ld1)/Rd1 + (L2*Rd3 + L3*Rd2 + Ld2*Rd3 + Ld3*Rd2)/(Rd2*Rd3);
                                b2 = (L2*L3 + L2*Ld3 + L3*Ld2 + Ld2*Ld3 + C1*L1*Rd2*Rd3 + C2*L1*Rd2*Rd3 +...
                                    C2*L2*Rd2*Rd3 + C3*L1*Rd2*Rd3 + C3*L2*Rd2*Rd3 + ...
                                    C3*L3*Rd2*Rd3)/(Rd2*Rd3) + ((L1 + Ld1)*(L2*Rd3 +...
                                    L3*Rd2 + Ld2*Rd3 + Ld3*Rd2))/(Rd1*Rd2*Rd3);
                                b3 =  (C1*L1*L2*Rd3 + C1*L1*L3*Rd2 + C2*L1*L2*Rd3 + C2*L1*L3*Rd2 +...
                                    C2*L2*L3*Rd2 + C3*L1*L2*Rd3 + C3*L1*L3*Rd2 + C3*L2*L3*Rd2 +...
                                    C3*L2*L3*Rd3 + C1*L1*Ld2*Rd3 + C1*L1*Ld3*Rd2 + C2*L1*Ld2*Rd3 +...
                                    C2*L1*Ld3*Rd2 + C2*L2*Ld2*Rd3 + C2*L2*Ld3*Rd2 + C3*L1*Ld2*Rd3 +...
                                    C3*L1*Ld3*Rd2 + C3*L2*Ld2*Rd3 + C3*L2*Ld3*Rd2 + C3*L3*Ld2*Rd3 +...
                                    C3*L3*Ld3*Rd2)/(Rd2*Rd3) + (L1*L2*L3 + L1*L2*Ld3 + L1*L3*Ld2 +...
                                    L2*L3*Ld1 + L1*Ld2*Ld3 + L2*Ld1*Ld3 + L3*Ld1*Ld2 + Ld1*Ld2*Ld3 +...
                                    C2*L1*L2*Rd2*Rd3 + C3*L1*L2*Rd2*Rd3 + C3*L1*L3*Rd2*Rd3 +...
                                    C1*L1*Ld1*Rd2*Rd3 + C2*L1*Ld1*Rd2*Rd3 + C2*L2*Ld1*Rd2*Rd3 +...
                                    C3*L1*Ld1*Rd2*Rd3 + C3*L2*Ld1*Rd2*Rd3 +...
                                    C3*L3*Ld1*Rd2*Rd3)/(Rd1*Rd2*Rd3);
                                b4 = (C1*L1*L2*L3 + C2*L1*L2*L3 + C3*L1*L2*L3 + C1*L1*L2*Ld3 +...
                                    C1*L1*L3*Ld2 + C2*L1*L2*Ld3 + C2*L1*L3*Ld2 + C2*L2*L3*Ld2 +...
                                    C3*L1*L2*Ld3 + C3*L1*L3*Ld2 + C3*L2*L3*Ld2 + C3*L2*L3*Ld3 +...
                                    C1*L1*Ld2*Ld3 + C2*L1*Ld2*Ld3 + C2*L2*Ld2*Ld3 + C3*L1*Ld2*Ld3 +...
                                    C3*L2*Ld2*Ld3 + C3*L3*Ld2*Ld3 + C1*C2*L1*L2*Rd2*Rd3 +...
                                    C1*C3*L1*L2*Rd2*Rd3 + C1*C3*L1*L3*Rd2*Rd3 + C2*C3*L1*L3*Rd2*Rd3 +...
                                    C2*C3*L2*L3*Rd2*Rd3)/(Rd2*Rd3) + (C2*L1*L2*L3*Rd2 + C3*L1*L2*L3*Rd2 +...
                                    C3*L1*L2*L3*Rd3 + C1*L1*L2*Ld1*Rd3 + C1*L1*L3*Ld1*Rd2 +...
                                    C2*L1*L2*Ld1*Rd3 + C2*L1*L3*Ld1*Rd2 + C2*L1*L2*Ld2*Rd3 + ...
                                    C2*L1*L2*Ld3*Rd2 + C2*L2*L3*Ld1*Rd2 + C3*L1*L2*Ld1*Rd3 +...
                                    C3*L1*L3*Ld1*Rd2 + C3*L1*L2*Ld2*Rd3 + C3*L1*L2*Ld3*Rd2 + ...
                                    C3*L2*L3*Ld1*Rd2 + C3*L1*L3*Ld2*Rd3 + C3*L1*L3*Ld3*Rd2 + ...
                                    C3*L2*L3*Ld1*Rd3 + C1*L1*Ld1*Ld2*Rd3 + C1*L1*Ld1*Ld3*Rd2 + ...
                                    C2*L1*Ld1*Ld2*Rd3 + C2*L1*Ld1*Ld3*Rd2 + C2*L2*Ld1*Ld2*Rd3 + ...
                                    C2*L2*Ld1*Ld3*Rd2 + C3*L1*Ld1*Ld2*Rd3 + C3*L1*Ld1*Ld3*Rd2 + ...
                                    C3*L2*Ld1*Ld2*Rd3 + C3*L2*Ld1*Ld3*Rd2 + C3*L3*Ld1*Ld2*Rd3 + ...
                                    C3*L3*Ld1*Ld3*Rd2)/(Rd1*Rd2*Rd3);
                                b5 = (C1*C2*L1*L2*L3*Rd2 + C1*C3*L1*L2*L3*Rd2 + C1*C3*L1*L2*L3*Rd3 + ...
                                    C2*C3*L1*L2*L3*Rd3 + C1*C2*L1*L2*Ld2*Rd3 + C1*C2*L1*L2*Ld3*Rd2 + ...
                                    C1*C3*L1*L2*Ld2*Rd3 + C1*C3*L1*L2*Ld3*Rd2 + C1*C3*L1*L3*Ld2*Rd3 + ...
                                    C1*C3*L1*L3*Ld3*Rd2 + C2*C3*L1*L3*Ld2*Rd3 + C2*C3*L1*L3*Ld3*Rd2 + ...
                                    C2*C3*L2*L3*Ld2*Rd3 + C2*C3*L2*L3*Ld3*Rd2)/(Rd2*Rd3) + ...
                                    (C1*L1*L2*L3*Ld1 + C2*L1*L2*L3*Ld1 + C2*L1*L2*L3*Ld2 + C3*L1*L2*L3*Ld1 +...
                                    C3*L1*L2*L3*Ld2 + C3*L1*L2*L3*Ld3 + C1*L1*L2*Ld1*Ld3 + C1*L1*L3*Ld1*Ld2 +...
                                    C2*L1*L2*Ld1*Ld3 + C2*L1*L3*Ld1*Ld2 + C2*L1*L2*Ld2*Ld3 + ...
                                    C2*L2*L3*Ld1*Ld2 + C3*L1*L2*Ld1*Ld3 + C3*L1*L3*Ld1*Ld2 + ...
                                    C3*L1*L2*Ld2*Ld3 + C3*L2*L3*Ld1*Ld2 + C3*L1*L3*Ld2*Ld3 + ...
                                    C3*L2*L3*Ld1*Ld3 + C1*L1*Ld1*Ld2*Ld3 + C2*L1*Ld1*Ld2*Ld3 + ...
                                    C2*L2*Ld1*Ld2*Ld3 + C3*L1*Ld1*Ld2*Ld3 + C3*L2*Ld1*Ld2*Ld3 + ...
                                    C3*L3*Ld1*Ld2*Ld3 + C2*C3*L1*L2*L3*Rd2*Rd3 + C1*C2*L1*L2*Ld1*Rd2*Rd3 +...
                                    C1*C3*L1*L2*Ld1*Rd2*Rd3 + C1*C3*L1*L3*Ld1*Rd2*Rd3 + ...
                                    C2*C3*L1*L3*Ld1*Rd2*Rd3 + C2*C3*L2*L3*Ld1*Rd2*Rd3)/(Rd1*Rd2*Rd3);
                                b6 = (C1*C2*L1*L2*L3*Ld2 + C1*C3*L1*L2*L3*Ld2 + C1*C3*L1*L2*L3*Ld3 + ...
                                    C2*C3*L1*L2*L3*Ld3 + C1*C2*L1*L2*Ld2*Ld3 + C1*C3*L1*L2*Ld2*Ld3 + ...
                                    C1*C3*L1*L3*Ld2*Ld3 + C2*C3*L1*L3*Ld2*Ld3 + C2*C3*L2*L3*Ld2*Ld3 + ...
                                    C1*C2*C3*L1*L2*L3*Rd2*Rd3)/(Rd2*Rd3) + (C1*C2*L1*L2*L3*Ld1*Rd2 + ...
                                    C1*C3*L1*L2*L3*Ld1*Rd2 + C1*C3*L1*L2*L3*Ld1*Rd3 + C2*C3*L1*L2*L3*Ld1*Rd3 +...
                                    C2*C3*L1*L2*L3*Ld2*Rd3 + C2*C3*L1*L2*L3*Ld3*Rd2 + C1*C2*L1*L2*Ld1*Ld2*Rd3 +...
                                    C1*C2*L1*L2*Ld1*Ld3*Rd2 + C1*C3*L1*L2*Ld1*Ld2*Rd3 + ...
                                    C1*C3*L1*L2*Ld1*Ld3*Rd2 + C1*C3*L1*L3*Ld1*Ld2*Rd3 + ...
                                    C1*C3*L1*L3*Ld1*Ld3*Rd2 + C2*C3*L1*L3*Ld1*Ld2*Rd3 + ...
                                    C2*C3*L1*L3*Ld1*Ld3*Rd2 + C2*C3*L2*L3*Ld1*Ld2*Rd3 + ...
                                    C2*C3*L2*L3*Ld1*Ld3*Rd2)/(Rd1*Rd2*Rd3);
                                b7 =  (C1*C2*L1*L2*L3*Ld1*Ld2 + C1*C3*L1*L2*L3*Ld1*Ld2 + ...
                                    C1*C3*L1*L2*L3*Ld1*Ld3 + C2*C3*L1*L2*L3*Ld1*Ld3 + ...
                                    C2*C3*L1*L2*L3*Ld2*Ld3 + C1*C2*L1*L2*Ld1*Ld2*Ld3 + ...
                                    C1*C3*L1*L2*Ld1*Ld2*Ld3 + C1*C3*L1*L3*Ld1*Ld2*Ld3 + ...
                                    C2*C3*L1*L3*Ld1*Ld2*Ld3 + C2*C3*L2*L3*Ld1*Ld2*Ld3 + ...
                                    C1*C2*C3*L1*L2*L3*Ld3*Rd1*Rd2)/(Rd1*Rd2*Rd3) + ...
                                    (C1*C2*C3*L1*L2*L3*(Ld1*Rd2 + Ld2*Rd1))/(Rd1*Rd2);
                                b8 = (C1*C2*C3*L1*L2*L3*(Ld1*Ld2*Rd3 + Ld1*Ld3*Rd2 + ...
                                    Ld2*Ld3*Rd1))/(Rd1*Rd2*Rd3);
                                b9 = (C1*C2*C3*L1*L2*L3*Ld1*Ld2*Ld3)/(Rd1*Rd2*Rd3);

                                % compute the resonance frequencies - vector Wr
                                P1 = [+b8, -b6, +b4, -b2, +1];
                                P2 = [+b9, -b7, +b5, -b3, +b1];
                                WrA = (roots(P1)).^0.5;
                                WrB = (roots(P2)).^0.5;
                                ind1 = find(real(WrA) > 1e6*imag(WrA) );
                                ind2 = find(real(WrB) > 1e6*imag(WrB));
                                Wr = [real(WrA(ind1)).' real(WrB(ind2)).'];

                                % compute the impedace at resonance
                                s = i*Wr;
                                Zout = (d1*s + d2*s.^2 + d3*s.^3 + d4*s.^4 + d5*s.^5 + d6*s.^6 + d7*s.^7 + d8*s.^8) ./ ...
                                    (1 + b1*s + b2*s.^2 + b3*s.^3 + b4*s.^4 + b5*s.^5 + b6*s.^6 + b7*s.^7 + b8*s.^8 + b9*s.^9);
                                ind = find(abs(Zout) > Zmax);
                                output_impedance_too_big = ~isempty(ind);

                                % compute the attenuation
                                s = i*ws;
                                H = (1 + a1*s + a2*s^2 + a3*s^3) / ...
                                    (1 + b1*s + b2*s^2 + b3*s^3 + b4*s^4 + b5*s^5 + b6*s^6 + b7*s^7 + b8*s^8 + b9*s^9);
                                H_dB = 20*log10(abs(H));

                                MAT(ii1,ii2,ii3,ii4,ii5,ii6,ii7) = H_dB + 1e10*output_impedance_too_big;
                            end
                        end
                    end
                end
            end
        end
    end
    %%% find the best filter
    [att, ind] = min(MAT(:));
    if (att > 1e9)
        disp('filter_damping_design - error : Design failed. Output impedance too high');
        beep; return;
    end
    [ii1,ii2,ii3,ii4,ii5,ii6,ii7] = ind2sub(size(MAT),ind);  % indexes of optimal filter
    Ld1 = Ldvec(ii1) * (ii5-1);       Ld2 = Ldvec(ii1) * (ii6-1);       Ld3 = Ldvec(ii1) * (ii7-1);
    Rd1 = Rdvec(ii2);       Rd2 = Rdvec(ii3);       Rd3 = Rdvec(ii4);
    stg_Ld = [Ld1 Ld2 Ld3];
    stg_Rd = [Rd1 Rd2 Rd3];
end

% mark infinite damping resistors
ind = find(stg_Rd > 9e6);
stg_Rd(ind) = inf;
stg_Ld(ind)  = 0;
end

%%% Transfer functions of the filters
% %%% 1 stage %%%
% H = (1 + a1*s) / (1 + b1*s + b2*s^2 + b3*s^3);
% Zout = (d1*s + d2*s^2) / (1 + b1*s + b2*s^2 + b3*s^3);

% %%% 2 stages %%%
% H = (1 + a1*s + a2*s^2) / ...
%     (1 + b1*s + b2*s^2 + b3*s^3 + b4*s^4 + b5*s^5 + b6*s^6);
% Zout = (d1*s + d2*s^2 + d3*s^3 + d4*s^4 + d5*s^5) / ...
%     (1 + b1*s + b2*s^2 + b3*s^3 + b4*s^4 + b5*s^5 + b6*s^6);

% %%% 3 stages %%%
% H = (1 + a1*s + a2*s^2 + a3*s^3) / ...
%     (1 + b1*s + b2*s^2 + b3*s^3 + b4*s^4 + b5*s^5 + b6*s^6 + b7*s^7 + b8*s^8 + b9*s^9);
% Zout = (d1*s + d2*s^2 + d3*s^3 + d4*s^4 + d5*s^5 + d6*s^6 + d7*s^7 + d8*s^8) / ...
%     (1 + b1*s + b2*s^2 + b3*s^3 + b4*s^4 + b5*s^5 + b6*s^6 + b7*s^7 + b8*s^8 + b9*s^9);


%LIBSVC Trainable classifier: LIBSVM
%
% 	[W,J] = LIBSVC(A,KERNEL,C)
% 	[W,J] = A*LIBSVC([],KERNEL,C)
% 	[W,J] = A*LIBSVC(KERNEL,C)
%
% INPUT
%   A	    Dataset
%   KERNEL  Mapping to compute kernel by A*MAP(A,KERNEL)
%           or string to compute kernel by FEVAL(KERNEL,A,A)
%           or cell array with strings and parameters to compute kernel by
%           FEVAL(KERNEL{1},A,A,KERNEL{2:END})
%           Default: linear kernel (PROXM([],'P',1))
%   C       Trade_off parameter in the support vector classifier.
%           Default C = 1;
%
% OUTPUT
%   W       Mapping: Support Vector Classifier
%   J       Object idences of support objects. Can be also obtained as W{4}
%
% DESCRIPTION
% Optimizes a support vector classifier for the dataset A by the libsvm
% package, see http://www.csie.ntu.edu.tw/~cjlin/libsvm/. LIBSVC calls the
% svmtrain routine of libsvm for training. Classifier execution for a
% test dataset B may be done by D = B*W; In D posterior probabilities are
% given as computed by svmpredict using the '-b 1' option.
%
% The kernel may be supplied in KERNEL by
% - an untrained mapping, e.g. a call to PROXM like W = LIBSVC(A,PROXM([],'R',1))
% - a string with the name of the routine to compute the kernel from A
% - a cell-array with this name and additional parameters.
% This will be used for the evaluation of a dataset B by B*W or PRMAP(B,W) as
% well.
%
% If KERNEL = 0 (or not given) it is assumed that A is already the
% kernelmatrix (square). In this also a kernel matrix should be supplied at
% evaluation by B*W or PRMAP(B,W). However, the kernel has to be computed with
% respect to support objects listed in J (the order of objects in J does matter).
%
% EXAMPLE
% a = gendatb;                     % generate banana classes
% [w,J] = a*libsvc(proxm('p',3));  % compute svm with 3rd order polynomial
% a*w*testc                        % show error on train set
% scatterd(a)                      % show scatterplot
% plotc(w)                         % plot classifier
% hold on;
% scatterd(a(J,:),'o')             % show support objcts
%
% REFERENCES
% R.-E. Fan, P.-H. Chen, and C.-J. Lin. Working set selection using the second order
% information for training SVM. Journal of Machine Learning Research 6, 1889-1918, 2005
%
% SEE ALSO (PRTools Guide)
% MAPPINGS, DATASETS, SVC, PROXM

% Copyright: R.P.W. Duin, r.p.w.duin@37steps.com
% Faculty EWI, Delft University of Technology
% P.O. Box 5031, 2600 GA Delft, The Netherlands

function [W,J,u] = libsvc(varargin)

	checktoolbox('libsvm');

  mapname = 'LIBSVM';
  argin = shiftargin(varargin,{'prmapping','char','cell'});
  argin = setdefaults(argin,[],proxm([],'p',1),1,1);

  if mapping_task(argin,'definition')

    W = define_mapping(argin,'untrained',mapname);

	elseif mapping_task(argin,'training')			% Train a mapping.

    [a,kernel,C] = check_for_old_call(argin);
    opt = ['-s 0 -t 4 -b 1 -e 1e-3 -c ',num2str(C), ' -q'];

    islabtype(a,'crisp');
    isvaldfile(a,1,2); % at least 1 object per class, 2 classes
    a = testdatasize(a,'objects');
    [m,k,c] = getsize(a);
    nlab = getnlab(a);

    K = compute_kernel(a,a,kernel);
    K = min(K,K');   % make sure kernel is symmetric
    K = [[1:m]' K];  % as libsvm wants it
                     % call libsvm
    u = svmtrain(nlab,K,opt);
		if isempty(u)
			prwarning(1,'libsvc: no solution for SVM, pseudo-inverse will be used')
			W = lkc(prdataset(K(:,2:end),getlabels(a)),0);
			J = [1:m]';
			return
		end
                     % Store the results:
    J = full(u.SVs);
    if isempty(J) | J == 0
      % LIBSVM failed, use fisher
      W = fisherc(a);
      prwarning(1,'LIBSVC failed, Fisher used instead')
      return
    end
    if isequal(kernel,0)
      s = [];
      in_size = 0; % to allow old and new style calls
    else
      s = a(J,:);
      in_size = k;
    end

    lablist = getlablist(a);
    W = prmapping(mfilename,'trained',{u,s,kernel,J,opt},lablist(u.Label,:),in_size,c);

    W = setname(W,'LIBSVM Classifier');
    W = setcost(W,a);

  else % Evaluation

    [a,W] = deal(argin{1:2});
    [u,s,kernel,J,opt] = getdata(W);
    m = size(a,1);

    K = compute_kernel(a,s,kernel);
    k = size(K,2);
    if k ~= length(J)
      if isequal(kernel,0)
        if (k > length(J)) &  (k >= max(J))
          % precomputed kernel; old style call
          prwarning(2,'Old style execution call: The precomputed kernel was calculated on a test set and the whole training set!')
        else
          error(['Inappropriate precomputed kernel!' newline ...
              'For the execution the kernel matrix should be computed on a test set' ...
              newline 'and the set of support objects']);
        end
      else
        error('Kernel matrix has the wrong number of columns');
      end
    else
      % kernel was computed with respect to the support objects
      % we make an approprite correction in the libsvm structure
      u.SVs = sparse((1:length(J))');
    end
    K = [[1:m]' K];  % as libsvm wants it
    %[lab,acc,d] = svmpredict(getnlab(a),K,u,' -b 1');
    [lab,acc,d] = svmpredict(ones(m,1),K,u,' -b 1');
    W = setdat(a,d,W);
  end

return;

function K = compute_kernel(a,s,kernel)

	% compute a kernel matrix for the objects a w.r.t. the support objects s
	% given a kernel description

	if  isstr(kernel) % routine supplied to compute kernel
		K = feval(kernel,a,s);
	elseif iscell(kernel)
		K = feval(kernel{1},a,s,kernel{2:end});
	elseif ismapping(kernel)
		K = a*prmap(s,kernel);
	elseif kernel == 0 % we have already a kernel
		K = a;
	else
		error('Do not know how to compute kernel matrix')
	end

	K = +K;

return

function [a,kernel,C] = check_for_old_call(argin)

[a,kernel,C,par] = deal(argin{:});
if ischar(kernel) && exist(kernel,'file') ~= 2
  kernel = proxm(kernel,C);
  C = par;
end


function Z = spinv(A, B)
% SPINV    Evaluate the sparse inverse matrix
%
% z = sinv(A)  returns the elements of inv(A)_ij, for which A_ij
%      is different from zero.
%
% z = sinv(LD, 1)  returns the elements of inv(A)_ij, for which A_ij
%     is different from zero, and where LD is the LDL cholesky
%     decomposition of A. LD has to be in the form returned by ldlchol
%     in SuiteSparse by Tim Davis.
%
%   Note! If z = sinv(LD, 1) is used LD must not be modified in Matlab
%   after ldlchol. Matlab destroys the symbolic sparsity structure in
%   the Cholesky decomposition, which is needed in the spinv
%   algorithm. If LD is modified the worst scenario is memory corruption.
%
%
%     For details, see:
%     Jarno Vanhatalo and Aki Vehtari (2008). Modelling local and
%     global phenomena with sparse Gaussian processes. Proceedings of
%     the 24th Conference on Uncertainty in Artificial Intelligence
%

% Copyright (c) 2008-2010      Jarno Vanhatalo

% This software is distributed under the GNU General Public
% License (version 3 or later); please refer to the file
% License.txt, included with the software, for details.

    n = size(A,1);

    if nargin == 1
        [LD, p, q] = ldlchol(A);
    else
        LD = A;
    end

    [I,J,ld] = find(LD);
    temp = [I(:) J(:) ; J(:) I(:)];
    temp = sortrows(unique(temp,'rows'),2);
    Iz = temp(:,1); Jz = temp(:,2);

    % Find the column starting points
    a1=zeros(n,1);
    a2 = cumsum(histc(J,1:n));
    a1(1) = 1; a1(2:end) = a2(1:end-1) + 1;
    az1=zeros(n,1);
    az2 = cumsum(histc(Jz,1:n));
    az1(1) = 1; az1(2:end) = az2(1:end-1) + 1;

    for j=1:n
        indaz{j} = az1(j):az2(j);
        indIz{j} = Iz(indaz{j})';
    end

    % Evaluate the sparse inverse
    z = zeros(size(Iz));
    z(end) = 1./ld(end);
    % Allocate memory
    cindit=zeros(n,1);
    for jj = n-1:-1:1
        fil = ld(a1(jj)+1:a1(jj+1)-1);
        fi = I(a1(jj)+1:a1(jj+1)-1);
        lfi = length(fi);
        Zt = zeros(lfi,lfi);
        indz = cumsum(histc(indIz{jj},[0 ; fi]));
        indz = az1(jj) + indz(1:end-1);

        i4=0;
        for i1 = 1:lfi
            cind1=indaz{fi(i1)};
            Icind1=indIz{fi(i1)};
            indfi = lfi;
            i2=length(Icind1);
            go = true;
            while go
                if Icind1(i2)==jj  % Find the indeces for the jj'th rows in fi columns
                    i4=i4+1;
                    cindit(i4)=cind1(i2);
                    go = false;
                end
                if indfi >= 1 && fi(indfi) == Icind1(i2) % Find the indeces for the fi'th rows in i2'nd columns
                    Zt(indfi,i1) = z(cind1(i2));
                    indfi = indfi-1;
                end
                i2 = i2-1;
            end
        end
        % remove extras
        cindi=cindit(1:i4);

        zij = -fil'*Zt;
        z(cindi) = zij;
        z(indz) = zij;
        zij = 1./ld(a1(jj)) - fil'*z(indz);
        z(az1(jj)-1+find(indIz{jj}==jj,1)) = zij;
    end

    Z = sparse(Iz,Jz,z);

    if nargin == 1
        r(q) = 1:n;
        Z = Z(r,r);
    end
end


classdef L2RegressionLoss < dagnn.Loss
    methods
        function outputs = forward(obj, inputs, params)
            sz = size(inputs{2});
            mass = sz(1) * sz(2) + 1;

            outputs{1} = vl_nnloss_regression(inputs{1}, inputs{2}, [], ...
                'loss', obj.loss, ...
                'instanceWeights', 1./mass) ;
            n = obj.numAveraged ;
            m = n + size(inputs{1},4) ;
            obj.average = (n * obj.average + double(gather(outputs{1}))) / m ;
            obj.numAveraged = m ;
        end

        function [derInputs, derParams] = backward(obj, inputs, params, derOutputs)
            sz = size(inputs{2});
            mass = sz(1) * sz(2) + 1;
            derInputs{1} = vl_nnloss_regression(inputs{1}, inputs{2}, derOutputs{1}, ...
                'loss', obj.loss, ...
                'instanceWeights', 1./mass) ;
            derInputs{2} = [] ;
            derParams = {} ;
        end

        function obj = L2RegressionLoss(varargin)
            obj.load(varargin) ;
        end
    end
end


function [cl] = gal2cl(gal)
% Convert volume from US liquid gallons to centiliters.
% Chad Greene 2012
cl = gal*378.5411784;


function [x cutvalue cutvalue_upperbound Y] = maxcut_octave(L, r)
% Algorithm to (try to) compute a maximum cut of a graph, via SDP approach.
%
% function x = maxcut_octave(L)
% function [x cutvalue cutvalue_upperbound Y] = maxcut_octave(L, r)
%
% See examples/maxcut.m for help about the math behind this example. This
% file is here to illustrate how to use Manopt within Octave.
%
% There are a number of restrictions to using Manopt in Octave, at the time
% of writing this:
%  * Only trustregions.m works as a solver yet.
%  * Only elliptopefactory.m works as a manifold factory yet.
%  * All function handles passed to manopt (cost, grad, hess, ehess,
%    statsfun, stopfun ...) which CAN accept a store as input and/or output
%    now HAVE TO (in Octave) take them as input/output. Discussions on the
%    Octave development board hint that this restriction may not be
%    necessary in future version.
%  * You cannot define those functions as nested functions. Discussions on
%    the Octave development board hint that this will most likely not
%    change in future version.
%
% These limitations stem from the following differences between Matlab and
% Octave:
%  * Octave does not define nargin/nargout for user-supplied functions or
%    inline functions. This will likely change.
%  * Octave has no nested functions support. This will likely not change.
% Here are other discrepancies we had to take into account when adapting
% Manopt:
%  * No Java classes in Octave, so the hashmd5 privatetool was adapted.
%  * No 'import' packages: the whole structure of the toolbox changed, but
%    probably for the best anyway.
%  * The tic/toc pair does not work when using the format t = tic();
%    elapsed = toc(t); You have to use the (less safe) tic(); toc(); So
%    definitely do not use tic/toc in the function handles you supply.
%  * try/catch blocks do not give the catch an exception object.
%  * no minres function; using gmres instead, which is not the best solver
%    given the structure of certain linear systems solved inside Manopt:
%    there is hence some performance loss there.
%
% See also: maxcut

% This file is part of Manopt and is copyrighted. See the license file.
%
% Main author: Nicolas Boumal, Aug. 22, 2013
% Contributors:
%
% Change log:
%

    % If no inputs are provided, generate a random Laplacian.
    % This is for illustration purposes only.
    if ~exist('L', 'var') || isempty(L)
        n = 20;
        A = triu(randn(n) <= .4, 1);
        A = A+A';
        D = diag(sum(A, 2));
        L = D-A;
    end

    n = size(L, 1);
    assert(size(L, 2) == n, 'L must be square.');

    if ~exist('r', 'var') || isempty(r) || r > n
        r = n;
    end

    % We will let the rank increase. Each rank value will generate a cut.
    % We have to go up in the rank to eventually find a certificate of SDP
    % optimality. This in turn will give us an upperbound on the MAX CUT
    % value and assure us that we're doing well, according to Goemans and
    % Williamson's argument. In practice though, the good cuts often come
    % up for low rank values, so we better keep track of the best one.
    best_x = ones(n, 1);
    best_cutvalue = 0;
    cutvalue_upperbound = NaN;

    time = [];
    cost = [];

    for rr = 2 : r

        manifold = elliptopefactory(n, rr);

        if rr == 2

            % At first, for rank 2, generate a random point.
            Y0 = manifold.rand();

        else

            % To increase the rank, we could just add a column of zeros to
            % the Y matrix. Unfortunately, this lands us in a saddle point.
            % To escape from the saddle, we may compute an eigenvector of
            % Sy associated to a negative eigenvalue: that will yield a
            % (second order) descent direction Z. See Journee et al ; Sy is
            % linked to dual certificates for the SDP.
            Y0 = [Y zeros(n, 1)];
            LY0 = L*Y0;
            Dy = spdiags(sum(LY0.*Y0, 2), 0, n, n);
            Sy = (Dy - L)/4;
            % Find the smallest (the "most negative") eigenvalue of Sy.
            [v, s] = eigs(Sy, 1, 'SA');
            % If there is no negative eigenvalue for Sy, than we are not at
            % a saddle point: we're actually done!
            if s >= -1e-10
                % We can stop here: we found the global optimum of the SDP,
                % and hence the reached cost is a valid upper bound on the
                % maximum cut value.
                cutvalue_upperbound = max(-[info.cost]);
                break;
            end

            % This is our escape direction.
            Z = manifold.proj(Y0, [zeros(n, rr-1) v]);

            % % These instructions can be uncommented to see what the cost
            % % function looks like at a saddle point.
            % plotprofile(problem, Y0, Z, linspace(-1, 1, 101));
            % drawnow; pause;

            % Now make a step in the Z direction to escape from the saddle.
            % It is not obvious that it is ok to do a unit step ... perhaps
            % need to be cautious here with the stepsize. It's not too
            % critical though: the important point is to leave the saddle
            % point. But it's nice to guarantee monotone decrease of the
            % cost, and we can't do that with a constant step (at least,
            % not without a proper argument to back it up).
            stepsize = 1.0;
            Y0 = manifold.retr(Y0, Z, stepsize);

        end

        % Use the Riemannian optimization based algorithm lower in this
        % file to reach a critical point (typically a local optimizer) of
        % the max cut cost with fixed rank, starting from Y0.
        [Y info] = maxcut_fixedrank(L, Y0);

        % Some info logging.
        thistime = [info.time];
        if ~isempty(time)
            thistime = time(end) + thistime;
        end
        time = [time thistime]; %#ok<AGROW>
        cost = [cost [info.cost]]; %#ok<AGROW>

        % Time to turn the matrix Y into a cut.
        % We can either do the random rounding as follows:
        % x = sign(Y*randn(rr, 1));
        % or extract the "PCA direction" of the points in Y and cut
        % orthogonally to that direction, as follows:
        [u, ~, ~] = svds(Y, 1);
        x = sign(u);

        cutvalue = (x'*L*x)/4;
        if cutvalue > best_cutvalue
            best_x = x;
            best_cutvalue = cutvalue;
        end

    end

    x = best_x;
    cutvalue = best_cutvalue;

    plot(time, -cost, '.-');
    xlabel('Time [s]');
    ylabel('Relaxed cut value');
    title('The relaxed cut value is an upper bound on the optimal cut value.');

end

function [Y info] = maxcut_fixedrank(L, Y)
% Try to solve the (fixed) rank r relaxed max cut program, based on the
% Laplacian of the graph L and an initial guess Y. L is nxn and Y is nxr.

    [n r] = size(Y);
    assert(all(size(L) == n));

    % The fixed rank elliptope geometry describes symmetric, positive
    % semidefinite matrices of size n with rank r and all diagonal entries
    % are 1.
    manifold = elliptopefactory(n, r);

    % % If you want to compare the performance of the elliptope geometry
    % % against the (conceptually simpler) oblique manifold geometry,
    % % uncomment this line.
    % manifold = obliquefactory(r, n, true);

    problem.M = manifold;

    % % Unfortunately, you cannot code things this way in Octave, because
    % you have to accept the store as input AND return it as second output.
    % problem.cost = @(Y)  -trace(Y'*L*Y)/4;
    % problem.egrad = @(Y) -(L*Y)/2;
    % problem.ehess = @(Y, U) -(L*U)/2;

    % Instead of the prototyping version, the functions below describe the
    % cost, gradient and Hessian using the caching system (the store
    % structure). This alows to execute exactly the required number of
    % multiplications with the matrix L.

    problem.cost = @(Y, store) cost(L, Y, store);

    problem.grad = @(Y, store) grad(manifold, L, Y, store);

    problem.hess = @(Y, U, store) hess(manifold, L, Y, U, store);

    % % Diagnostics tools: to make sure the gradient and Hessian are
    % % correct during the prototyping stage.
    % checkgradient(problem); pause;
    % checkhessian(problem); pause;

    % % To investigate the effect of the rotational invariance when using
    % % the oblique or the elliptope geometry, or to study the saddle point
    % % issue mentioned above, it is sometimes interesting to look at the
    % % spectrum of the Hessian. For large dimensions, this is slow!
    % stairs(sort(hessianspectrum(problem, Y)));
    % drawnow; pause;

    % % When facing a saddle point issue as described in the master
    % % function, and when no sure mechanism exists to find an escape
    % % direction, it may be helpful to set useRand to true and raise
    % % miniter to more than 1, when using trustregions. This will tell the
    % % solver to not stop before at least miniter iterations were
    % % accomplished (thus disregarding the zero gradient at the saddle
    % % point) and to use random search directions to kick start the inner
    % % solve (tCG) step. It is not as efficient as finding a sure escape
    % % direction, but sometimes it's the best we have.
    % options.useRand = true;
    % options.miniter = 5;

    options.verbosity = 2;
    % profile clear; profile on;
    [Y Ycost info] = trustregions(problem, Y, options); %#ok
    % profile off; profile report;

end

function store = prepare(L, Y, store)
    if ~isfield(store, 'LY')
        store.LY = L*Y;
    end
end

function [f store] = cost(L, Y, store)
    store = prepare(L, Y, store);
    LY = store.LY;
    f = -(Y(:)'*LY(:))/4; % = -trace(Y'*LY)/4;
end

function [g store] = grad(manifold, L, Y, store)
    store = prepare(L, Y, store);
    LY = store.LY;
    g = manifold.egrad2rgrad(Y, -LY/2);
end

function [h store] = hess(manifold, L, Y, U, store)
    store = prepare(L, Y, store);
    LY = store.LY;
    LU = L*U;
    h = manifold.ehess2rhess(Y, -LY/2, -LU/2, U);
end


function step = stepsize_alg(iter, options)
% stepsize control algorithm.
%
% Inputs:
%       iter        number of iterations
%       options     options
% Output:
%       step        stepsize
%
% This file is part of SGDLibrary.
%
% Created by H.Kasai on Sep. 25, 2017
% Modified by H.Kasai on Sep. 28, 2017

    % extract options
    if ~isfield(options, 'step_init')
        step_init = 0.1;
    else
        step_init = options.step_init;
    end

    if ~isfield(options, 'step_alg')
        step_alg = 'fix';
    else
        if strcmp(options.step_alg, 'decay')
            step_alg = 'decay';
        elseif strcmp(options.step_alg, 'decay-2')
            step_alg = 'decay-2';
        elseif strcmp(options.step_alg, 'decay-3')
            step_alg = 'decay-3';
        elseif strcmp(options.step_alg, 'fix')
            step_alg = 'fix';
        else
            step_alg = 'decay';
        end
    end

    if ~isfield(options, 'lambda')
        lambda = 0.1;
    else
        lambda = options.lambda;
    end

    % update step-size
    if strcmp(step_alg, 'fix')
        step = step_init;
    elseif strcmp(step_alg, 'decay')
        step = step_init / (1 + step_init * lambda * iter);
    elseif strcmp(step_alg, 'decay-2')
        step = step_init / (1 + iter);
    elseif strcmp(step_alg, 'decay-3')
        step = step_init / (lambda + iter);
    end

end


function pde = elli3DorthocircIntf(bm,bp,rx,ry,rz)
%% USAGE: polynomial solution for Poisson equation
%  Last Modified: 02/21/2020 by Xu Zhang

%% PDE Structure
pde = struct('intf',@intf,'f',@f,'fm',@fm,'fp',@fp,'exactu',@exactu,...
    'um',@um,'up',@up,'Dxu',@Dxu,'Dxum',@Dxum,'Dxup',@Dxup,'Dyu',@Dyu,...
    'Dyum',@Dyum,'Dyup',@Dyup,'Dzu',@Dzu,'Dzum',@Dzum,'Dzup',@Dzup,...
    'A',@A,'Am',@Am,'Ap',@Ap,'one',@one,'gD',@gD);

pde.bm = bm;
pde.bp = bp;
%% interface function
    function u = intf(x,y,z)
        u = log(F1(x,y,z).*F2(x,y,z).*F3(x,y,z)-r(x,y,z)+1);
    end

%% exact solution
    function u = exactu(x,y,z)
        u = um(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = up(x(id),y(id),z(id));
    end
    function u = um(x,y,z)
        u = intf(x,y,z)/bm;
    end
    function u = up(x,y,z)
        u = intf(x,y,z)/bp;
    end
%% Boundary Function
    function u = gD(x,y,z)
        u = exactu(x,y,z);
    end
%% Derivative of the exact solution
    function u = Dxu(x,y,z)
        u = Dxum(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Dxup(x(id),y(id),z(id));
    end
    function u = Dxum(x,y,z)
        fx = (F1x(x,y,z).*F2(x,y,z).*F3(x,y,z)+F1(x,y,z).*F2x(x,y,z).*F3(x,y,z)+...
            F1(x,y,z).*F2(x,y,z).*F3x(x,y,z)) - Dxr(x,y,z);
        u = (F1(x,y,z).*F2(x,y,z).*F3(x,y,z)-r(x,y,z)+1).^(-1).*fx/bm;
    end
    function u = Dxup(x,y,z)
        fx = (F1x(x,y,z).*F2(x,y,z).*F3(x,y,z)+F1(x,y,z).*F2x(x,y,z).*F3(x,y,z)+...
            F1(x,y,z).*F2(x,y,z).*F3x(x,y,z)) - Dxr(x,y,z);
        u = (F1(x,y,z).*F2(x,y,z).*F3(x,y,z)-r(x,y,z)+1).^(-1).*fx/bp;
    end

    function u = Dyu(x,y,z)
        u = Dyum(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Dyup(x(id),y(id),z(id));
    end
    function u = Dyum(x,y,z)
        fy = (F1y(x,y,z).*F2(x,y,z).*F3(x,y,z)+F1(x,y,z).*F2y(x,y,z).*F3(x,y,z)+...
            F1(x,y,z).*F2(x,y,z).*F3y(x,y,z)) - Dyr(x,y,z);
        u = (F1(x,y,z).*F2(x,y,z).*F3(x,y,z)-r(x,y,z)+1).^(-1).*fy/bm;
    end
    function u = Dyup(x,y,z)
        fy = (F1y(x,y,z).*F2(x,y,z).*F3(x,y,z)+F1(x,y,z).*F2y(x,y,z).*F3(x,y,z)+...
            F1(x,y,z).*F2(x,y,z).*F3y(x,y,z)) - Dyr(x,y,z);
        u = (F1(x,y,z).*F2(x,y,z).*F3(x,y,z)-r(x,y,z)+1).^(-1).*fy/bp;
    end

    function u = Dzu(x,y,z)
        u = Dzum(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Dzup(x(id),y(id),z(id));
    end
    function u = Dzum(x,y,z)
        fz = (F1z(x,y,z).*F2(x,y,z).*F3(x,y,z)+F1(x,y,z).*F2z(x,y,z).*F3(x,y,z)+...
            F1(x,y,z).*F2(x,y,z).*F3z(x,y,z)) - Dzr(x,y,z);
        u = (F1(x,y,z).*F2(x,y,z).*F3(x,y,z)-r(x,y,z)+1).^(-1).*fz/bm;
    end
    function u = Dzup(x,y,z)
        fz = (F1z(x,y,z).*F2(x,y,z).*F3(x,y,z)+F1(x,y,z).*F2z(x,y,z).*F3(x,y,z)+...
            F1(x,y,z).*F2(x,y,z).*F3z(x,y,z)) - Dzr(x,y,z);
        u = (F1(x,y,z).*F2(x,y,z).*F3(x,y,z)-r(x,y,z)+1).^(-1).*fz/bp;
    end

%% right hand side function
    function u = f(x,y,z)
        u = fm(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = fp(x(id),y(id),z(id));
    end
    function u = fm(x,y,z)
        r_xx = ry^2*(rz*2);
        r_yy = ry^2*(rz*2);
        r_zz = ry^2*(rz*2);

        fx = (F1x(x,y,z).*F2(x,y,z).*F3(x,y,z)+F1(x,y,z).*F2x(x,y,z).*F3(x,y,z)+...
            F1(x,y,z).*F2(x,y,z).*F3x(x,y,z)) - Dxr(x,y,z);
        fy = (F1y(x,y,z).*F2(x,y,z).*F3(x,y,z)+F1(x,y,z).*F2y(x,y,z).*F3(x,y,z)+...
            F1(x,y,z).*F2(x,y,z).*F3y(x,y,z)) - Dyr(x,y,z);
        fz = (F1z(x,y,z).*F2(x,y,z).*F3(x,y,z)+F1(x,y,z).*F2z(x,y,z).*F3(x,y,z)+...
            F1(x,y,z).*F2(x,y,z).*F3z(x,y,z)) - Dzr(x,y,z);

        fxx = F1xx(x,y,z).*F2(x,y,z).*F3(x,y,z) + F1x(x,y,z).*F2x(x,y,z).*F3(x,y,z) + F1x(x,y,z).*F2(x,y,z).*F3x(x,y,z) +...
            F1(x,y,z).*F2xx(x,y,z).*F3(x,y,z) + F1x(x,y,z).*F2x(x,y,z).*F3(x,y,z) + F1(x,y,z).*F2x(x,y,z).*F3x(x,y,z) +...
            F1(x,y,z).*F2(x,y,z).*F3xx(x,y,z) + F1(x,y,z).*F2x(x,y,z).*F3x(x,y,z) + F1x(x,y,z).*F2(x,y,z).*F3x(x,y,z) - r_xx;

        fyy = F1yy(x,y,z).*F2(x,y,z).*F3(x,y,z) + F1y(x,y,z).*F2y(x,y,z).*F3(x,y,z) + F1y(x,y,z).*F2(x,y,z).*F3y(x,y,z) +...
            F1(x,y,z).*F2yy(x,y,z).*F3(x,y,z) + F1y(x,y,z).*F2y(x,y,z).*F3(x,y,z) + F1(x,y,z).*F2y(x,y,z).*F3y(x,y,z) +...
            F1(x,y,z).*F2(x,y,z).*F3yy(x,y,z) + F1(x,y,z).*F2y(x,y,z).*F3y(x,y,z) + F1y(x,y,z).*F2(x,y,z).*F3y(x,y,z) - r_yy;

        fzz = F1zz(x,y,z).*F2(x,y,z).*F3(x,y,z) + F1z(x,y,z).*F2z(x,y,z).*F3(x,y,z) + F1z(x,y,z).*F2(x,y,z).*F3z(x,y,z) +...
            F1(x,y,z).*F2zz(x,y,z).*F3(x,y,z) + F1z(x,y,z).*F2z(x,y,z).*F3(x,y,z) + F1(x,y,z).*F2z(x,y,z).*F3z(x,y,z) +...
            F1(x,y,z).*F2(x,y,z).*F3zz(x,y,z) + F1(x,y,z).*F2z(x,y,z).*F3z(x,y,z) + F1z(x,y,z).*F2(x,y,z).*F3z(x,y,z) - r_zz;

        u = -(F1(x,y,z).*F2(x,y,z).*F3(x,y,z)-r(x,y,z)+1).^(-1).*(fxx+fyy+fzz) +...
            (F1(x,y,z).*F2(x,y,z).*F3(x,y,z)-r(x,y,z)+1).^(-2).*(fx.^2+fy.^2+fz.^2);
    end
    function u = fp(x,y,z)
        u = fm(x,y,z);
    end

%% Diffusion coefficient function
    function u = A(x,y,z)
        u = Am(x,y,z);
        id = intf(x,y,z) > 0;
        u(id) = Ap(x(id),y(id),z(id));
    end
    function u = Am(x,y,z)
        u = bm*ones(size(x));
    end
    function u = Ap(x,y,z)
        u = bp*ones(size(x));
    end

%% Other function
    function u = one(x,y,z)
        u = ones(size(x));
    end
    function u = r(x,y,z)
        u = ry^2*(1+rz*(x.^2 + y.^2 + z.^2));
    end
    function u = Dxr(x,y,z)
        u = ry^2*(rz*2*x);
    end
    function u = Dyr(x,y,z)
        u = ry^2*(rz*2*y);
    end
    function u = Dzr(x,y,z)
        u = ry^2*(rz*2*z);
    end
    function u = F1(x,y,z)
        u = (x.^2 + y.^2 -rx^2).^2 + z.^2;
    end
    function u = F2(x,y,z)
        u = (x.^2 + z.^2 -rx^2).^2 + y.^2;
    end
    function u = F3(x,y,z)
        u = (y.^2 + z.^2 -rx^2).^2 + x.^2;
    end
    function u = F1x(x,y,z)
        u = 2*(x.^2 + y.^2 -rx^2).*(2*x);
    end
    function u = F1xx(x,y,z)
        u = 2*(x.^2 + y.^2 -rx^2).*(2) + 8*x.^2;
    end
    function u = F2x(x,y,z)
        u = 2*(x.^2 + z.^2 -rx^2).*(2*x);
    end
    function u = F2xx(x,y,z)
        u = 2*(x.^2 + z.^2 -rx^2).*(2) + 8*x.^2;
    end
    function u = F3x(x,y,z)
        u = 2*x;
    end
    function u = F3xx(x,y,z)
        u = 2*ones(size(x));
    end
    function u = F1y(x,y,z)
        u = 2*(x.^2 + y.^2 -rx^2).*(2*y);
    end
    function u = F1yy(x,y,z)
        u = 2*(x.^2 + y.^2 -rx^2).*(2) + 8*y.^2;
    end
    function u = F2y(x,y,z)
        u = 2*y;
    end
    function u = F2yy(x,y,z)
        u = 2*ones(size(y));
    end
    function u = F3y(x,y,z)
        u = 2*(y.^2 + z.^2 -rx^2).*(2*y);
    end
    function u = F3yy(x,y,z)
        u = 2*(y.^2 + z.^2 -rx^2)*(2) + 8*y.^2;
    end
    function u = F1z(x,y,z)
        u = 2*z;
    end
    function u = F1zz(x,y,z)
        u = 2*ones(size(z));
    end
    function u = F2z(x,y,z)
        u = 2*(x.^2 + z.^2 -rx^2).*(2*z);
    end
    function u = F2zz(x,y,z)
        u = 2*(x.^2 + z.^2 -rx^2).*(2) + 8*z.^2;
    end
    function u = F3z(x,y,z)
        u = 2*(y.^2 + z.^2 -rx^2).*(2*z);
    end
    function u = F3zz(x,y,z)
        u = 2*(y.^2 + z.^2 -rx^2).*(2) + 8*z.^2;
    end
end


function  a = i4vec_indicator0 ( n )

%*****************************************************************************80
%
%% I4VEC_INDICATOR0 sets an I4VEC to the indicator vector (0,1,2,...).
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    27 September 2014
%
%  Author:
%
%    John Burkardt
%
%  Parameters:
%
%    Input, integer N, the number of entries in the vector.
%
%    Output, integer A(N), the vector.
%
  a = ( 0 : n - 1 );

  return
end


function [kN] = kip2kN(kip)
% Convert force from kip to kilonewtons.
% Chad A. Greene 2012
kN = kip*4.4482216;


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

%%begin

% Generate a Reeds-Shepp path for 3-point turn
dl = 0.05;
q0 = [0 0 0]'; qf = [0 0 pi]';
maxcurv = 1/5;   % 5m turning circle
rs = ReedsShepp(q0, qf, maxcurv, dl)

% set up a vehicle model
[car.image,~,car.alpha] = imread('car2.png');
car.rotation = 180;
car.centre = [81,110];
car.centre = [648; 173];
car.length = 4.2;

% now animate
clf; plotvol([-4 8 -6 6])

a = gca;
a.XLimMode = 'manual';
a.YLimMode = 'manual';
set(gcf, 'Color', 'w')
grid on
a = gca;
xyzlabel

plot_vehicle(rs.path, 'model', car, 'trail', 'r:');


function [pop] = initializega(num, bounds, evalFN,evalOps,options)
% function [pop]=initializega(populationSize, variableBounds,evalFN,
%                           evalOps,options)
%    initializega creates a matrix of random numbers with
%    a number of rows equal to the populationSize and a number
%    columns equal to the number of rows in bounds plus 1 for
%    the f(x) value which is found by applying the evalFN.
%    This is used by the ga to create the population if it
%    is not supplied.
%
% pop            - the initial, evaluated, random population
% populatoinSize - the size of the population, i.e. the number to create
% variableBounds - a matrix which contains the bounds of each variable, i.e.
%                  [var1_high var1_low; var2_high var2_low; ....]
% evalFN         - the evaluation fn, usually the name of the .m file for
%                  evaluation
% evalOps        - any options to be passed to the eval function defaults []
% options        - options to the initialize function, ie.
%                  [type prec] where eps is the epsilon value
%                  and the second option is 1 for float and 0 for binary,
%                  prec is the precision of the variables defaults [1e-6 1]

% Binary and Real-Valued Simulation Evolution for Matlab GAOT V2
% Copyright (C) 1998 C.R. Houck, J.A. Joines, M.G. Kay
%
% C.R. Houck, J.Joines, and M.Kay. A genetic algorithm for function
% optimization: A Matlab implementation. ACM Transactions on Mathmatical
% Software, Submitted 1996.
%
% 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 1, or (at your option)
% any later version.
%
% This program is distributed in the hope that it will be useful,
% but WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
% GNU General Public License for more details. A copy of the GNU
% General Public License can be obtained from the
% Free Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.

if nargin<5
  options=[1e-6 1];
end
if nargin<4
  evalOps=[];
end

if any(evalFN<48) %Not a .m file
  if options(2)==1 %Float GA
    estr=['x=pop(i,1); pop(i,xZomeLength)=', evalFN ';'];
  else %Binary GA
    estr=['x=b2f(pop(i,:),bounds,bits); pop(i,xZomeLength)=', evalFN ';'];
  end
else %A .m file
  if options(2)==1 %Float GA
    estr=['[ pop(i,:) pop(i,xZomeLength)]=' evalFN '(pop(i,:),[0 evalOps]);'];
  else %Binary GA
    estr=['x=b2f(pop(i,:),bounds,bits);[x v]=' evalFN ...
	'(x,[0 evalOps]); pop(i,:)=[f2b(x,bounds,bits) v];'];
    end
end

numVars     = size(bounds,1); 		%Number of variables
rng         = (bounds(:,2)-bounds(:,1))'; %The variable ranges'

if options(2)==1 %Float GA
  xZomeLength = numVars+1; 		%Length of string is numVar + fit
  pop         = zeros(num,xZomeLength); 	%Allocate the new population
  pop(:,1:numVars)=(ones(num,1)*rng).*(rand(num,numVars))+...
    (ones(num,1)*bounds(:,1)');
else %Binary GA
  bits=calcbits(bounds,options(1));
  xZomeLength = sum(bits)+1; 		%Length of string is numVar + fit
  pop = round(rand(num,sum(bits)+1));
end

for i=1:num
  eval(estr);
end


%==============================================================================
% This code is part of the Matlab-based toolbox
% FAIR - Flexible Algorithms for Image Registration.
% For details see
% - https://github.com/C4IR and
% - http://www.siam.org/books/fa06/
% ##2
%==============================================================================
% function [Tc,dT] = linearInter(T,omega,x,varargin)
%
% linear interpolator for the data T given on a cell-centered grid
% to be evaluated at locations x
%
% Input
%  T        coefficients for image model img : \R^{dim*n} \to\R^{n)},
%           for j=1:n, img(j)  = sum T_i b_i(x_j); end;
%  omega    specification of domain
%           Omega = (omega(1),omega(2)) x ... x (omega(2*dim-1,omega(2*dim)),
%           spatial dimension dim = length(omega/2)
%  x        location evaluation points
%           x = reshape(x,n,dim), x(j,:) coordinates of j-th point
%  varargin additional parameters such as doDerivative,
%##4 cleanup
%==============================================================================

function [Tc,dT] = linearInter(T,omega,x,varargin)

Tc = mfilename('fullpath'); dT = [];

if nargin == 0,
  help(mfilename);
  testOneImgModel(mfilename);
  return;
elseif nargin == 1 && isempty(T),
  return;
end;

% flag for computing the derivative
doDerivative = (nargout>1);
matrixFree   = 0;
for k=1:2:length(varargin), % overwrite default parameter
  eval([varargin{k},'=varargin{',int2str(k+1),'};']);
end;

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

% get data size m, cell size h, dimension d, and number n of interpolation points
dim = length(omega)/2;
m   = size(T);         if dim == 1, m = numel(T); end;
h   = (omega(2:2:end)-omega(1:2:end))./m;
n   = length(x)/dim;
x   = reshape(x,n,dim);

% map x from [h/2,omega-h/2] -> [1,m],
for i=1:dim, x(:,i) = (x(:,i)-omega(2*i-1))/h(i) + 0.5; end;

Tc = zeros(n,1); dT = [];                     % initialize output
if doDerivative, dT = zeros(n,dim);  end;     % allocate memory in column format
Valid = @(j) (0<x(:,j) & x(:,j)<m(j)+1);      % determine indices of valid points

switch dim,
  case 1, valid = find( Valid(1) );
  case 2, valid = find( Valid(1) & Valid(2) );
  case 3, valid = find( Valid(1) & Valid(2) & Valid(3) );
end;

if isempty(valid),
  if doDerivative, dT = sparse(n,dim*n); end; % allocate memory in column format
  return;
end;

pad = 1; TP = zeros(m+2*pad);                 % pad data to reduce cases

P = floor(x); x = x-P;                        % split x into integer/remainder
p = @(j) P(valid,j); xi = @(j) x(valid,j);

% increments for linearized ordering
i1 = 1; i2 = size(T,1)+2*pad; i3 = (size(T,1)+2*pad)*(size(T,2)+2*pad);

switch dim,
  case 1,
    TP(pad+(1:m)) = reshape(T,m,1);
    clear T;
    p = pad + p(1);
    Tc(valid) = TP(p).* (1-xi(1)) + TP(p+1).*xi(1);   % compute weighted sum

    if ~doDerivative, return; end;
    % compute and format the derivative
    dT(valid) = TP(p+1)-TP(p);
  case 2,
    TP(pad+(1:m(1)),pad+(1:m(2))) = T;
    clear T;
    p  = (pad + p(1)) + i2*(pad + p(2) - 1);
    % compute Tc as weighted sum
    Tc(valid) = (TP(p)   .* (1-xi(1)) + TP(p+i1)    .*xi(1)) .* (1-xi(2)) ...
      + (TP(p+i2) .* (1-xi(1)) + TP(p+i1+i2) .*xi(1)) .* (xi(2));

    if ~doDerivative, return; end;
    dT(valid,1) = (TP(p+i1)-TP(p)).*(1-xi(2)) + (TP(p+i1+i2)-TP(p+i2)).*xi(2);
    dT(valid,2) = (TP(p+i2)-TP(p)).*(1-xi(1)) + (TP(p+i1+i2)-TP(p+i1)).*xi(1);
  case 3,
    TP(pad+(1:m(1)),pad+(1:m(2)),pad+(1:m(3))) = T;
    clear T;
    p  = (pad + p(1)) + i2*(pad + p(2) - 1) + i3*(pad + p(3) -1);
    % compute Tc as weighted sum
    Tc(valid) = ((TP(p).*(1-xi(1))+TP(p+i1).*xi(1)).*(1-xi(2))...
      +(TP(p+i2).*(1-xi(1))+TP(p+i1+i2).*xi(1)).*(xi(2))).*(1-xi(3)) ...
      +((TP(p+i3).*(1-xi(1))+TP(p+i1+i3).*xi(1)).*(1-xi(2)) ...
      +(TP(p+i2+i3).*(1-xi(1))+TP(p+i1+i2+i3).*xi(1)).*(xi(2))).*(xi(3));

    if ~doDerivative, return; end;
    dT(valid,1) = ((TP(p+i1)-TP(p)).*(1-xi(2))+(TP(p+i1+i2)-TP(p+i2)).*xi(2)).*(1-xi(3)) ...
      +((TP(p+i1+i3)-TP(p+i3)).*(1-xi(2))+(TP(p+i1+i2+i3)-TP(p+i2+i3)).*xi(2)).*(xi(3));
    dT(valid,2) = ((TP(p+i2)-TP(p)).*(1-xi(1))+(TP(p+i1+i2)-TP(p+i1)).*xi(1)).*(1-xi(3)) ...
      +((TP(p+i2+i3)-TP(p+i3)).*(1-xi(1))+(TP(p+i1+i2+i3)-TP(p+i1+i3)).*xi(1)).*(xi(3));
    dT(valid,3) = ((TP(p+i3).*(1-xi(1))+TP(p+i1+i3).*xi(1)).*(1-xi(2)) ...
      +(TP(p+i2+i3).*(1-xi(1))+TP(p+i1+i2+i3).*xi(1)).*(xi(2))) ....
      -((TP(p).*(1-xi(1))+TP(p+i1).*xi(1)).*(1-xi(2)) ...
      +(TP(p+i2).*(1-xi(1))+TP(p+i1+i2).*xi(1)).*(xi(2)));
end;
if doDerivative
    for i=1:dim, dT(:,i) = dT(:,i)/h(i); end
    if not(matrixFree)
        dT = spdiags(dT,n*(0:(dim-1)),n,dim*n);
    end
end
%==============================================================================


function [ chg mag geometry ] = import_chgcar( filename )
%IMPORT_CHGCAR Import a VASP CHGCAR file.
%   [chg,mag,geometry] = import_chgcar(filename)
%   Import a VASP CHGCAR file. If no filename is specified, data is read
%   from CHGCAR. chg and mag are three dimensional arrays containing the
%   charge magnetization densities in Bohr magneton per cubic Angstrom.
%   Note that these are not the same units as the CHGCAR file. geometry is
%   a struct describing the cell geometry; see IMPORT_POSCAR for a detailed
%   description.
%
%   See also IMPORT_POSCAR, IMPORT_LOCPOT.

% todo:
% check compatibility with non-spin-polarized files
% extract chemical symbols
% what about AECAR and ELFCAR files?

  if nargin == 0
      filename='CHGCAR';
  end

  fid = fopen(filename);
  if fid==-1
    error(['File ' filename ' not found']);
  end

    geometry = import_poscar(fid);

    vol = abs(dot(geometry.lattice(1,:),cross(geometry.lattice(2,:),geometry.lattice(3,:))));
    natoms = sum(geometry.atomcount);

    fgetl(fid); % blank line

    gridsize = fscanf(fid, '%d %d %d', [3 1])';

    chg = fscanf(fid, '%f', [prod(gridsize,2) 1])';
    chg = reshape(chg,gridsize);
    chg = chg/vol;

    fgetl(fid); % empty string (or padding)

    pos = ftell(fid);
    line = fgetl(fid);
    fseek(fid,pos,'bof');
    if line(1)=='a'
        for i = 1:natoms
            line = fgetl(fid);
            nentries = sscanf(line,['augmentation occupancies ' num2str(i)...
                ' %d']); % number of occupancy entries
            fscanf(fid, '%f', [nentries 1]);
            fgetl(fid); % empty string
        end
        fscanf(fid, '%f', [natoms 1]); % don't know what these are
        fgetl(fid); % empty string
    end

    pos = ftell(fid);
    fgetl(fid);
    if ~feof(fid)
        fseek(fid,pos,'bof');
        gridsize = fscanf(fid, '%d %d %d', [3 1])';

        mag = fscanf(fid, '%f', [prod(gridsize,2) 1])';
        mag = reshape(mag,gridsize);
        mag = mag/vol;
    else
        mag = [];
    end

    fclose(fid);

end


%% set kalman parameters
function k = klmi(ocn, ncn, nzn, k, frame, st)
if frame  == st.st.st                                    % all objects in the first frame
k(size(ocn, 2)).s = [];                                  % initialize kalman filter
for      i = 1 : size(ocn, 2)                            % for every detected objects
k(i).s     = kalmani(ocn(:, i));                         % initialize every kalman filter
end
elseif frame ~= st.st.st                                 % not associated objects
n          = size(k, 2);
for      i = n + 1 : n + size(ncn, 2)                    % for every new objects
k(i).s     = kalmani(ncn(:, i - n));                     % initialize every new kalman filter
k(i).sz    = nzn(:, i - n);
end
end
end
%% initialize kalman
function s = kalmani(int)
dt         = 0.1;                                        % time step
s.A        = [1 dt 0 0; 0 1 0 0; 0 0 1 dt; 0 0 0 1];     % state transition matrix
s.H        = [1 0 0 0; 0 0 1 0];                         % observation matrix
s.Q        = 5 * eye(4);                                 % process noise covariance
s.R        = [0.5  0; 0 0.5];                            % measurement error covariance
s.x        = [int(1), 0, int(2), 0]';                    % a priori 'state vector' estimate
s.P        = 5 * eye(4);                                 % a priori estimate 'error covariance'
end


function xPosVel=makeConstSpeedLevelCurvedEarthTraj(llhStart,headingStart,speed,timeOffsets,useRhumb,trajParams)
%%MAKECONSTSPEEDLEVELCURVEDEARTHTRAJ Create a non-maneuvering, constant
%   (local tangent plane) speed, constant ellipsoidal height trajectory
%   over an ellipsoidal Earth, with position and velocity sampled at
%   specified times. By default, this is a geodesic path, but a rhumb
%   trajectory (constant heading) can also be used. Note that rhumb lines
%   will get stuck at the geographic poles.
%
%INPUTS: llhStart The [latitude;longitude;ellipsoidal height] of the
%                 starting point (time=0) of the trajectory.
%    headingStart The initial heading of the trajectory in radians East of
%                 North.
%           speed The positive speed of the target. This will typically be
%                 in meters.
%     timeOffsets A length numPts list of times where the position and
%                 velocity of the target should be obtained. This will
%                 typically be in seconds.
%        useRhumb If this is true, a rhumb (constant heading) trajectory is
%                 created as opposed to a geodesic (straightest)
%                 trajectory. The default if omitted or an empty matrix is
%                 passed is false.
%      trajParams An optional structure containing values that change how
%                 the trajectory is generated. Possible field and their
%                 defaults (if omitted or empty matrices are passed) are:
%                 a The semi-major axis of the reference ellipsoid. The
%                   default is Constants.WGS84SemiMajorAxis.
%                 f The flattening factor of the reference ellipsoid. The
%                   default is Constants.WGS84Flattening.
%                 useHeightApprox If true, an approximation for targets
%                   with nonzero height is used that is much faster than a
%                   numerical integration-based technique. The default is
%                   true.
%                 numSteps4Circ If useHeightApprox is false, then numerical
%                   integration is performed and this is the number of
%                   Runge-Kutta steps that would be taken when
%                   circumnavigating the equator. The default is 2000,
%                   though 6000 is often better.
%
%OUTPUTS: xPosVel The 6XnumPts set of [position;velocity] vectors of the
%                 target at the specified times.
%
%This function obtains positions using directRhumbProbGen or
%directGeodeticProbGen and then converts the local heading to ECEF, which
%when given magnitude speed is an approximation to the instantaneous
%velocity vector.
%
%October 2021 David F. Crouse, Naval Research Laboratory, Washington D.C.
%(UNCLASSIFIED) DISTRIBUTION STATEMENT A. Approved for public release.

if(nargin<5||isempty(useRhumb))
    useRhumb=false;
end

useHeightApprox=true;
numSteps4Circ=2000;
a=Constants.WGS84SemiMajorAxis;
f=Constants.WGS84Flattening;

if(nargin>5&&~isempty(trajParams))
    if(isfield(trajParams,'a'))
        a=trajParams.a;
    end

    if(isfield(trajParams,'f'))
        f=trajParams.f;
    end

    if(isfield(trajParams,'useHeightApprox'))
        useHeightApprox=trajParams.useHeightApprox;
    end

    if(isfield(trajParams,'numSteps4Circ'))
        numSteps4Circ=trajParams.numSteps4Circ;
    end
end

numPts=length(timeOffsets);
height=llhStart(3);
latLonStart=llhStart(1:2);

xPosVel=zeros(6,numPts);
if(useRhumb)
    %The instantaneous local ENU velocity never changes, so find it outside
    %the loop.
    vENULocal=geogHeading2ENUVec(headingStart,0)*speed;

    for curOffset=1:numPts
        curDist=timeOffsets(curOffset)*speed;
        latLonEnd=directRhumbProbGen(latLonStart,headingStart,curDist,height,useHeightApprox,a,f,numSteps4Circ);
        vECEF=ENUVec2ECEFVec(latLonEnd,vENULocal,a,f);
        posCart=ellips2Cart([latLonEnd;height],a,f);
        xPosVel(:,curOffset)=[posCart;vECEF];
    end
else
    %A geodesic trajectory.
    for curOffset=1:numPts
        curDist=timeOffsets(curOffset)*speed;
        [latLonEnd,azEnd]=directGeodeticProbGen(latLonStart,headingStart,curDist,height,useHeightApprox,a,f,numSteps4Circ);

        vENULocal=geogHeading2ENUVec(azEnd,0)*speed;
        vECEF=ENUVec2ECEFVec(latLonEnd,vENULocal,a,f);
        posCart=ellips2Cart([latLonEnd;height],a,f);
        xPosVel(:,curOffset)=[posCart;vECEF];
    end
end
end

%LICENSE:
%
%The source code is in the public domain and not licensed or under
%copyright. The information and software may be used freely by the public.
%As required by 17 U.S.C. 403, third parties producing copyrighted works
%consisting predominantly of the material produced by U.S. government
%agencies must provide notice with such work(s) identifying the U.S.
%Government material incorporated and stating that such material is not
%subject to copyright protection.
%
%Derived works shall not identify themselves in a manner that implies an
%endorsement by or an affiliation with the Naval Research Laboratory.
%
%RECIPIENT BEARS ALL RISK RELATING TO QUALITY AND PERFORMANCE OF THE
%SOFTWARE AND ANY RELATED MATERIALS, AND AGREES TO INDEMNIFY THE NAVAL
%RESEARCH LABORATORY FOR ALL THIRD-PARTY CLAIMS RESULTING FROM THE ACTIONS
%OF RECIPIENT IN THE USE OF THE SOFTWARE.


function yth = SB_CoarseGrain(y,howtocg,numGroups)
% SB_CoarseGrain   Coarse-grains a continuous time series to a discrete alphabet.
%
%---INPUTS:
% howtocg, the method of coarse-graining
%
% numGroups, either specifies the size of the alphabet for 'quantile' and 'updown'
%       or sets the time delay for the embedding subroutines

% ------------------------------------------------------------------------------
% Copyright (C) 2020, Ben D. Fulcher <ben.d.fulcher@gmail.com>,
% <http://www.benfulcher.com>
%
% If you use this code for your research, please cite the following two papers:
%
% (1) B.D. Fulcher and N.S. Jones, "hctsa: A Computational Framework for Automated
% Time-Series Phenotyping Using Massive Feature Extraction, Cell Systems 5: 527 (2017).
% DOI: 10.1016/j.cels.2017.10.001
%
% (2) B.D. Fulcher, M.A. Little, N.S. Jones, "Highly comparative time-series
% analysis: the empirical structure of time series and their methods",
% J. Roy. Soc. Interface 10(83) 20130048 (2013).
% DOI: 10.1098/rsif.2013.0048
%
% This function is free software: you can redistribute it and/or modify it under
% the terms of the GNU General Public License as published by the Free Software
% Foundation, either version 3 of the License, or (at your option) any later
% version.
%
% This program is distributed in the hope that it will be useful, but WITHOUT
% ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS
% FOR A PARTICULAR PURPOSE. See the GNU General Public License for more
% details.
%
% You should have received a copy of the GNU General Public License along with
% this program. If not, see <http://www.gnu.org/licenses/>.
% ------------------------------------------------------------------------------

% ------------------------------------------------------------------------------
% Check inputs, preliminaries:
% ------------------------------------------------------------------------------

% Quantile puts an equal number into each bin
if nargin < 3
    howtocg = 'quantile';
end

N = length(y); % length of the input sequence

if ~ismember(howtocg,{'updown','quantile','embed2quadrants','embed2octants'})
    error('Unknown coarse-graining method ''%s''',howtocg);
end

% ------------------------------------------------------------------------------
% Some coarse-graining/symbolization methods require initial processing:
% ------------------------------------------------------------------------------
switch howtocg
case 'updown'
   y = diff(y);
   N = N - 1; % the time series is one value shorter than the input because of differencing
   howtocg = 'quantile'; % successive differences and then quantiles

case {'embed2quadrants','embed2octants'}
	% Construct the embedding

	if strcmp(numGroups,'tau')
        % First zero-crossing of the autocorrelation function
        tau = CO_FirstCrossing(y,'ac',0,'discrete');
    else
        tau = numGroups;
	end
	if tau > N/25; tau = floor(N/25); end
	m1 = y(1:end-tau);
	m2 = y(1+tau:end);

	% Look at which points are in which angular 'quadrant'
	upr = find(m2 >= 0); % points above the axis
	downr = find(m2 < 0); % points below the axis

	q1r = upr(m1(upr) >= 0); % points in quadrant 1
	q2r = upr(m1(upr) < 0); % points in quadrant 2
	q3r = downr(m1(downr) < 0); % points in quadrant 3
	q4r = downr(m1(downr) >= 0); % points in quadrant 4
end

% ------------------------------------------------------------------------------
% Do the coarse graining
% ------------------------------------------------------------------------------
switch howtocg
    case 'quantile'
        th = quantile(y,linspace(0,1,numGroups+1)); % thresholds for dividing the time-series values
        th(1) = th(1)-1; % this ensures the first point is included
        % turn the time series into a set of numbers from 1:numGroups
        yth = zeros(N,1);
        for i = 1:numGroups
            yth(y > th(i) & y <= th(i+1)) = i;
        end

    case 'embed2quadrants' % divides based on quadrants in a 2-D embedding space
		% create alphabet in quadrants -- {1,2,3,4}
		yth = zeros(length(m1),1);
		yth(q1r) = 1; yth(q2r) = 2; yth(q3r) = 3; yth(q4r) = 4;

	case 'embed2octants' % divide based on octants in 2-D embedding space
		o1r = q1r(m2(q1r)<m1(q1r)); % points in octant 1
		o2r = q1r(m2(q1r)>=m1(q1r)); % points in octant 2
		o3r = q2r(m2(q2r)>=-m1(q2r)); % points in octant 3
		o4r = q2r(m2(q2r)<-m1(q2r)); % points in octant 4
		o5r = q3r(m2(q3r)>=m1(q3r)); % points in octant 5
		o6r = q3r(m2(q3r)<m1(q3r)); % points in octant 6
		o7r = q4r(m2(q4r)<-m1(q4r)); % points in octant 7
		o8r = q4r(m2(q4r)>=-m1(q4r)); % points in octant 8

		% create alphabet in octants -- {1,2,3,4,5,6,7,8}
		yth = zeros(length(m1),1);
		yth(o1r) = 1; yth(o2r) = 2; yth(o3r) = 3; yth(o4r) = 4;
		yth(o5r) = 5; yth(o6r) = 6; yth(o7r) = 7; yth(o8r) = 8;
end

if any(yth == 0)
    error('All values in the sequence were not assigned to a group')
end

end


function test_failed=test_rangecompress
%TEST_RANGECOMPRESS Test range compression and expansion

test_failed=0;

disp(' ===============  TEST_RANGECOMPRESS ================');

x=tester_crand(5,11);

y=rangecompress(x,'mulaw');
x_r=rangeexpand(y,'mulaw');

res=norm(x-x_r,'fro');

[test_failed,fail]=ltfatdiditfail(res,test_failed);
fprintf(['RANGECOMPRESS MULAW %0.5g %s\n'],res,fail);

y=rangecompress(x,'alaw');
x_r=rangeexpand(y,'alaw');

res=norm(x-x_r,'fro');

[test_failed,fail]=ltfatdiditfail(res,test_failed);
fprintf(['RANGECOMPRESS  ALAW %0.5g %s\n'],res,fail);


% Copyright (C) 1994-2015 John W. Eaton
%
% This file is part of Octave.
%
% Octave 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.
%
% Octave is distributed in the hope that it will be useful, but
% WITHOUT ANY WARRANTY; without even the implied warranty of
% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
% General Public License for more details.
%
% You should have received a copy of the GNU General Public License
% along with Octave; see the file COPYING.  If not, see
% <http://www.gnu.org/licenses/>.

% -*- texinfo -*-
% @deftypefn  {Function File} {} fftfilt (@var{b}, @var{x})
% @deftypefnx {Function File} {} fftfilt (@var{b}, @var{x}, @var{n})
% Filter @var{x} with the FIR filter @var{b} using the FFT.
%
% If @var{x} is a matrix, filter each column of the matrix.
%
% Given the optional third argument, @var{n}, @code{fftfilt} uses the
% overlap-add method to filter @var{x} with @var{b} using an N-point FFT@.
% The FFT size must be an even power of 2 and must be greater than or equal to
% the length of @var{b}.  If the specified @var{n} does not meet these
% criteria, it is automatically adjusted to the nearest value that does.
%
% @seealso{filter, filter2}
% @end deftypefn

% Author: Kurt Hornik <Kurt.Hornik@wu-wien.ac.at>
% Created: 3 September 1994
% Adapted-By: jwe

function y = oc_fftfilt (b, x, n)

  % If N is not specified explicitly, we do not use the overlap-add
  % method at all because loops are really slow.  Otherwise, we only
  % ensure that the number of points in the FFT is the smallest power
  % of two larger than N and length(b).  This could result in length
  % one blocks, but if the user knows better ...

  if (nargin < 2 || nargin > 3)
    print_usage ();
  end

  transpose = (size(x,1) == 1);

  if (transpose)
    x = x.';
  end

  [r_x, c_x] = size (x);
  [r_b, c_b] = size (b);

  if (~isvector (b))
    error ('fftfilt: B must be a vector');
  end

  if (ndims (x) ~= 2)
    error ('fftfilt: X must be a 1-D or 2-D array');
  end

  l_b = r_b * c_b;
  b = reshape (b, l_b, 1);

  if (nargin == 2)
    % Use FFT with the smallest power of 2 which is >= length (x) +
    % length (b) - 1 as number of points ...
    n = 2 ^ nextpow2 (r_x + l_b - 1);
    B = fft (b, n);
    y = ifft (fft (x, n) .* B(:, ones (1, c_x)));
  else
    % Use overlap-add method ...
    if (~(isscalar (n)))
      error ('fftfilt: N has to be a scalar');
    end
    n = 2 ^ nextpow2 (max ([n, l_b]));
    L = n - l_b + 1;
    B = fft (b, n);
    B = B(:, ones (c_x,1));
    R = ceil (r_x / L);
    y = zeros (r_x, c_x);
    for r = 1:R;
      lo = (r - 1) * L + 1;
      hi = min (r * L, r_x);
      tmp = zeros (n, c_x);
      tmp(1:(hi-lo+1),:) = x(lo:hi,:);
      tmp = ifft (fft (tmp) .* B);
      hi  = min (lo+n-1, r_x);
      y(lo:hi,:) = y(lo:hi,:) + tmp(1:(hi-lo+1),:);
    end
  end

  y = y(1:r_x, :);

  % Final cleanups:

  % - If both b and x are real, y should be real.
  % - If b is real and x is imaginary, y should be imaginary.
  % - If b is imaginary and x is real, y should be imaginary.
  % - If both b and x are imaginary, y should be real.
  xisreal = all (imag (x) == 0);
  xisimag = all (real (x) == 0);

  if (all (imag (b) == 0))
    y (:,xisreal) = real (y (:,xisreal));
    y (:,xisimag) = complex (real (y (:,xisimag)) * 0, imag (y (:,xisimag)));
  elseif (all (real (b) == 0))
    y (:,xisreal) = complex (real (y (:,xisreal)) * 0, imag (y (:,xisreal)));
    y (:,xisimag) = real (y (:,xisimag));
  end

  % - If both x and b are integer in both real and imaginary
  %   components, y should be integer.
  if (~any(b - fix (b)))
    idx = find (~any(x - fix (x)));
    y (:, idx) = round (y (:, idx));
  end

  % Transpose after cleanup, otherwise rounding fails.
  if (transpose)
    y = y.';
  end

end


clear all; close all;
bw=imread('text.png');
se=strel('line', 11, 90);
bw2=imdilate(bw, se);
figure;
subplot(121);  imshow(bw);
subplot(122);  imshow(bw2);


classdef IMMOEA_F1 < PROBLEM
% <multi> <real> <large/none>
% Benchmark MOP for testing IM-MOEA

%------------------------------- Reference --------------------------------
% R. Cheng, Y. Jin, K. Narukawa, and B. Sendhoff, A multiobjective
% evolutionary algorithm using Gaussian process-based inverse modeling,
% IEEE Transactions on Evolutionary Computation, 2015, 19(6): 838-856.
%------------------------------- Copyright --------------------------------
% Copyright (c) 2023 BIMK Group. You are free to use the PlatEMO for
% research purposes. All publications which use this platform or any code
% in the platform should acknowledge the use of "PlatEMO" and reference "Ye
% Tian, Ran Cheng, Xingyi Zhang, and Yaochu Jin, PlatEMO: A MATLAB platform
% for evolutionary multi-objective optimization [educational forum], IEEE
% Computational Intelligence Magazine, 2017, 12(4): 73-87".
%--------------------------------------------------------------------------

    methods
        %% Default settings of the problem
        function Setting(obj)
            obj.M = 2;
            if isempty(obj.D); obj.D = 30; end
            obj.lower    = zeros(1,obj.D);
            obj.upper    = ones(1,obj.D);
            obj.encoding = ones(1,obj.D);
        end
        %% Calculate objective values
        function PopObj = CalObj(obj,X)
            t = (1+5*repmat(2:obj.D,size(X,1),1)/obj.D).*X(:,2:obj.D) - repmat(X(:,1),1,obj.D-1);
            g = 1 + 9*mean(t.^2,2);
            PopObj(:,1) = X(:,1);
            PopObj(:,2) = g.*(1-sqrt(PopObj(:,1)./g));
        end
        %% Generate points on the Pareto front
        function R = GetOptimum(obj,N)
            R(:,1) = linspace(0,1,N)';
            R(:,2) = 1 - sqrt(R(:,1));
        end
        %% Generate the image of Pareto front
        function R = GetPF(obj)
            R = obj.GetOptimum(100);
        end
    end
end


%%%%% Representacion en esfera (radio variable, posicion (0,0,0),escribe por el "polo sur"):

function [xyz, lin]=en_esfera(P2)
    global radio;
    global precision;

    %precision=0.001;
    P=[P2(1) -P2(2) -radio];
    V=[0,0,radio]-P;
    for i=0:1:10000;
        recta(i+1,:)=P+V.*(i*precision/10);
        if abs((recta(i+1,1))^2+recta(i+1,2)^2+(recta(i+1,3))^2-radio^2) < precision
            xyz=recta(i+1,:);
            %display('exito')
            lin=recta;
            break
        end
    end
%     figure
%     hold on
%     [x,y,z] = sphere;
%     surf(x,y,z)
%    plot3(recta(:,1),recta(:,2),recta(:,3),'*')
    %hold on
    %plot3(xyz(1),xyz(2),xyz(3),'*')
end


% test for CS minoration

path(path, 'toolbox/');

% number of measurements
n = 280;
% number of atoms
m = 300;
% sparsity
s = 5;

% random measurements matrix
D = compute_compressed_sensing_matrix(n,m);

Delta = compute_cs_bounds(D,s)


function linplus_test1565 ( )

%*****************************************************************************80
%
%% TEST1565 tests R8BUT_INDICATOR.
%
%  Licensing:
%
%    This code is distributed under the GNU LGPL license.
%
%  Modified:
%
%    14 March 2009
%
%  Author:
%
%    John Burkardt
%
  n = 6
  mu = 2

  fprintf ( 1, '\n' );
  fprintf ( 1, 'TEST1565\n' );
  fprintf ( 1, '  R8BUT_INDICATOR sets up a R8BUT indicator matrix.\n' );
  fprintf ( 1, '\n' );
  fprintf ( 1, '  Matrix order N     = %d\n', n );
  fprintf ( 1, '  Upper bandwidth MU = %d\n', mu );
%
%  Set the matrix.
%
  a = r8but_indicator ( n, mu );

  r8but_print ( n, mu, a, '  The indicator matrix:' );

  return
end