optim_private_nlsq.c

ASUFIT－Matlab－全局拟合程序

        NULL));
    /*
     * FIRSTF=CostFunction'*CostFunction;
     */
    mlfAssign(&FIRSTF, mlfMtimes(mlfCtranspose(*CostFunction), *CostFunction));
    /*
     * [OLDX,OLDF]=lsinit(XOUT,CostFunction,verbosity,levMarq);
     */
    mlfAssign(
      &OLDX,
      mlfNlsq_lsinit(&OLDF, NULL, XOUT, *CostFunction, verbosity, levMarq));
    /*
     * PCNT = 0;
     */
    mlfAssign(&PCNT, mlfScalar(0.0));
    /*
     * EstSum=0.5;
     */
    mlfAssign(&EstSum, mlfScalar(0.5));
    /*
     * 
     * % system of equations or overdetermined
     * if nfun >= numberOfVariables
     */
    if (mlfTobool(mlfGe(nfun, numberOfVariables))) {
        /*
         * GradFactor = 0;  
         */
        mlfAssign(&GradFactor, mlfScalar(0.0));
    /*
     * else % underdetermined: singularity in JAC'*JAC or GRAD*GRAD' 
     */
    } else {
        /*
         * GradFactor = 1;
         */
        mlfAssign(&GradFactor, mlfScalar(1.0));
    /*
     * end
     */
    }
    /*
     * 
     * CHG = 1e-7*abs(XOUT)+1e-7*ones(numberOfVariables,1);
     */
    mlfAssign(
      &CHG,
      mlfPlus(
        mlfMtimes(mlfScalar(1e-7), mlfAbs(XOUT)),
        mlfMtimes(
          mlfScalar(1e-7), mlfOnes(numberOfVariables, mlfScalar(1.0), NULL))));
    /*
     * status=-1;
     */
    mlfAssign(&status, mlfScalar(-1.0));
    /*
     * 
     * while status~=1  & OPT_STOP == 0
     */
    for (;;) {
        mxArray * a_ = mclInitialize(mlfNe(status, mlfScalar(1.0)));
        if (mlfTobool(a_)
            && mlfTobool(mlfAnd(a_, mlfEq(OPT_STOP, mlfScalar(0.0))))) {
            mxDestroyArray(a_);
        } else {
            mxDestroyArray(a_);
            break;
        }
        /*
         * iter = iter + 1;
         */
        mlfAssign(&iter, mlfPlus(iter, mlfScalar(1.0)));
        /*
         * % Work Out Gradients
         * if ~(gradflag) | DerivativeCheck
         */
        {
            mxArray * a_ = mclInitialize(mlfNot(gradflag));
            if (mlfTobool(a_) || mlfTobool(mlfOr(a_, DerivativeCheck))) {
                mxDestroyArray(a_);
                /*
                 * JACFD = JAC;  % set to correct size
                 */
                mlfAssign(&JACFD, *JAC);
                /*
                 * OLDF=CostFunction;
                 */
                mlfAssign(&OLDF, *CostFunction);
                /*
                 * CHG = sign(CHG+eps).*min(max(abs(CHG),DiffMinChange),DiffMaxChange);
                 */
                mlfAssign(
                  &CHG,
                  mlfTimes(
                    mlfSign(mlfPlus(CHG, mlfEps())),
                    mlfMin(
                      NULL,
                      mlfMax(NULL, mlfAbs(CHG), DiffMinChange, NULL),
                      DiffMaxChange,
                      NULL)));
                /*
                 * for gcnt=1:numberOfVariables
                 */
                for (mclForStart(
                       &iterator_0, mlfScalar(1.0), numberOfVariables, NULL);
                     mclForNext(&iterator_0, &gcnt);
                     ) {
                    /*
                     * temp = XOUT(gcnt);
                     */
                    mlfAssign(&temp, mlfIndexRef(XOUT, "(?)", gcnt));
                    /*
                     * XOUT(gcnt) = temp +CHG(gcnt);
                     */
                    mlfIndexAssign(
                      &XOUT,
                      "(?)",
                      gcnt,
                      mlfPlus(temp, mlfIndexRef(CHG, "(?)", gcnt)));
                    /*
                     * x(:) = XOUT;
                     */
                    mlfIndexAssign(&x, "(?)", mlfCreateColonIndex(), XOUT);
                    /*
                     * CostFunction = feval(funfcn{3},x,varargin{:});  
                     */
                    mlfAssign(
                      CostFunction,
                      mlfFeval(
                        mclValueVarargout(),
                        mclFevalLookup(
                          mlfIndexRef(funfcn, "{?}", mlfScalar(3.0)),
                          1,
                          local_function_table_),
                        x,
                        mlfIndexRef(varargin, "{?}", mlfCreateColonIndex()),
                        NULL));
                    /*
                     * if ~isempty(YDATA)
                     */
                    if (mlfTobool(mlfNot(mlfIsempty(YDATA)))) {
                        /*
                         * CostFunction = CostFunction - YDATA;
                         */
                        mlfAssign(CostFunction, mlfMinus(*CostFunction, YDATA));
                    /*
                     * end
                     */
                    }
                    /*
                     * CostFunction = CostFunction(:);
                     */
                    mlfAssign(
                      CostFunction,
                      mlfIndexRef(*CostFunction, "(?)", mlfCreateColonIndex()));
                    /*
                     * OPT_STEP = 0; % We're in gradient finding mode
                     */
                    mlfAssign(&OPT_STEP, mlfScalar(0.0));
                    /*
                     * JACFD(:,gcnt)=(CostFunction-OLDF)/(CHG(gcnt));
                     */
                    mlfIndexAssign(
                      &JACFD,
                      "(?,?)",
                      mlfCreateColonIndex(),
                      gcnt,
                      mlfMrdivide(
                        mlfMinus(*CostFunction, OLDF),
                        mlfIndexRef(CHG, "(?)", gcnt)));
                    /*
                     * XOUT(gcnt) = temp;
                     */
                    mlfIndexAssign(&XOUT, "(?)", gcnt, temp);
                /*
                 * end
                 */
                }
                /*
                 * CostFunction = OLDF;
                 */
                mlfAssign(CostFunction, OLDF);
                /*
                 * numFunEvals=numFunEvals+numberOfVariables;
                 */
                mlfAssign(
                  &numFunEvals, mlfPlus(numFunEvals, numberOfVariables));
                /*
                 * % Gradient check
                 * if DerivativeCheck == 1 & gradflag
                 */
                {
                    mxArray * a_ = mclInitialize(
                                     mlfEq(DerivativeCheck, mlfScalar(1.0)));
                    if (mlfTobool(a_) && mlfTobool(mlfAnd(a_, gradflag))) {
                        mxDestroyArray(a_);
                        /*
                         * 
                         * if isa(funfcn{3},'inline') 
                         */
                        if (mlfTobool(
                              mlfFeval(
                                mclValueVarargout(),
                                mlxIsa,
                                mlfIndexRef(funfcn, "{?}", mlfScalar(3.0)),
                                mxCreateString("inline"),
                                NULL))) {
                        /*
                         * % if using inlines, the gradient is in funfcn{4}
                         * %            graderr(JACFD, JAC, formula(funfcn{4})); %
                         * else 
                         */
                        } else {
                            /*
                             * % otherwise fun/grad in funfcn{3}
                             * graderr(JACFD, JAC,  funfcn{3});
                             */
                            mlfFeval(
                              mclAnsVarargout(),
                              mlxGraderr,
                              JACFD,
                              *JAC,
                              mlfIndexRef(funfcn, "{?}", mlfScalar(3.0)),
                              NULL);
                        /*
                         * end
                         */
                        }
                        /*
                         * DerivativeCheck = 0;
                         */
                        mlfAssign(&DerivativeCheck, mlfScalar(0.0));
                    /*
                     * else
                     */
                    } else {
                        mxDestroyArray(a_);
                        /*
                         * JAC = JACFD;
                         */
                        mlfAssign(JAC, JACFD);
                    }
                /*
                 * end
                 */
                }
            /*
             * else
             */
            } else {
                mxDestroyArray(a_);
                /*
                 * x(:) = XOUT;
                 */
                mlfIndexAssign(&x, "(?)", mlfCreateColonIndex(), XOUT);
            }
        /*
         * end
         */
        }
        /*
         * 
         * % Try to set difference to 1e-8 for next iteration
         * if nfun==1  % JAC is a column vector or scalar, don't sum over rows
         */
        if (mlfTobool(mlfEq(nfun, mlfScalar(1.0)))) {
            /*
             * sumabsJAC = abs(JAC);
             */
            mlfAssign(&sumabsJAC, mlfAbs(*JAC));
        /*
         * else  % JAC is a row vector or matrix
         */
        } else {
            /*
             * sumabsJAC = sum(abs(JAC)')';  % Sum over the rows of JAC
             */
            mlfAssign(
              &sumabsJAC,
              mlfCtranspose(mlfSum(mlfCtranspose(mlfAbs(*JAC)), NULL)));
        /*
         * end
         */
        }
        /*
         * nonZeroSum = (sumabsJAC~=0);
         */
        mlfAssign(&nonZeroSum, mlfNe(sumabsJAC, mlfScalar(0.0)));
        /*
         * CHG(~nonZeroSum) = Inf;  % to avoid divide by zero error
         */
        mlfIndexAssign(&CHG, "(?)", mlfNot(nonZeroSum), mlfInf());
        /*
         * CHG(nonZeroSum) = nfun*1e-8./sumabsJAC(nonZeroSum);
         */
        mlfIndexAssign(
          &CHG,
          "(?)",
          nonZeroSum,
          mlfRdivide(
            mlfMtimes(nfun, mlfScalar(1e-8)),
            mlfIndexRef(sumabsJAC, "(?)", nonZeroSum)));
        /*
         * 
         * OPT_STEP = 2; % Entering line search    
         */
        mlfAssign(&OPT_STEP, mlfScalar(2.0));
        /*
         * 
         * GradF = 2*(CostFunction'*JAC)'; %2*GRAD*CostFunction;
         */
        mlfAssign(
          &GradF,
          mlfMtimes(
            mlfScalar(2.0),
            mlfCtranspose(mlfMtimes(mlfCtranspose(*CostFunction), *JAC))));
        /*
         * NewF = CostFunction'*CostFunction;
         */
        mlfAssign(
          &NewF, mlfMtimes(mlfCtranspose(*CostFunction), *CostFunction));
        /*
         * %---------------Initialization of Search Direction------------------
         * if status==-1
         */
        if (mlfTobool(mlfEq(status, mlfScalar(-1.0)))) {
            /*
             * if cond(JAC)>1e8 
             */
            if (mlfTobool(mlfGt(mlfCond(*JAC, NULL), mlfScalar(1e8)))) {
                /*
                 * %         SD=-(GRAD*GRAD'+(norm(GRAD)+1)*(eye(numberOfVariables,numberOfVariables)))\(GRAD*CostFunction);
                 * SD=-(JAC'*JAC+(norm(JAC)+1)*(eye(numberOfVariables,numberOfVariables)))\(CostFunction'*JAC)';
                 */
                mlfAssign(
                  &SD,
                  mlfMldivide(
                    mlfUminus(
                      mlfPlus(