optim_private_nlsq.c

ASUFIT－Matlab－全局拟合程序

                        mlfMtimes(mlfCtranspose(*JAC), *JAC),
                        mlfMtimes(
                          mlfPlus(mlfNorm(*JAC, NULL), mlfScalar(1.0)),
                          mlfEye(numberOfVariables, numberOfVariables)))),
                    mlfCtranspose(
                      mlfMtimes(mlfCtranspose(*CostFunction), *JAC))));
                /*
                 * if levMarq 
                 */
                if (mlfTobool(levMarq)) {
                    /*
                     * GradFactor=norm(JAC)+1; 
                     */
                    mlfAssign(
                      &GradFactor,
                      mlfPlus(mlfNorm(*JAC, NULL), mlfScalar(1.0)));
                /*
                 * end
                 */
                }
                /*
                 * how='COND';
                 */
                mlfAssign(&how, mxCreateString("COND"));
            /*
             * else
             */
            } else {
                /*
                 * % SD=JAC\(JAC*X-F)-X;
                 * SD=-(JAC'*JAC+GradFactor*(eye(numberOfVariables,numberOfVariables)))\(CostFunction'*JAC)';
                 */
                mlfAssign(
                  &SD,
                  mlfMldivide(
                    mlfUminus(
                      mlfPlus(
                        mlfMtimes(mlfCtranspose(*JAC), *JAC),
                        mlfMtimes(
                          GradFactor,
                          mlfEye(numberOfVariables, numberOfVariables)))),
                    mlfCtranspose(
                      mlfMtimes(mlfCtranspose(*CostFunction), *JAC))));
            /*
             * end
             */
            }
            /*
             * FIRSTF=NewF;
             */
            mlfAssign(&FIRSTF, NewF);
            /*
             * OLDJ = JAC;
             */
            mlfAssign(&OLDJ, *JAC);
            /*
             * GDOLD=GradF'*SD;
             */
            mlfAssign(&GDOLD, mlfMtimes(mlfCtranspose(GradF), SD));
            /*
             * % currstepsize controls the initial starting step-size.
             * % If currstepsize has been set externally then it will
             * % be non-zero, otherwise set to 1.
             * if currstepsize == 0, 
             */
            if (mlfTobool(mlfEq(currstepsize, mlfScalar(0.0)))) {
                /*
                 * currstepsize=1; 
                 */
                mlfAssign(&currstepsize, mlfScalar(1.0));
            /*
             * end
             */
            }
            /*
             * if verbosity>1
             */
            if (mlfTobool(mlfGt(verbosity, mlfScalar(1.0)))) {
                /*
                 * disp(sprintf(formatstr,iter,numFunEvals,NewF,currstepsize,GDOLD));
                 */
                mlfDisp(
                  mlfSprintf(
                    NULL,
                    formatstr,
                    iter,
                    numFunEvals,
                    NewF,
                    currstepsize,
                    GDOLD,
                    NULL));
            /*
             * end
             */
            }
            /*
             * XOUT=XOUT+currstepsize*SD;
             */
            mlfAssign(&XOUT, mlfPlus(XOUT, mlfMtimes(currstepsize, SD)));
            /*
             * if levMarq
             */
            if (mlfTobool(levMarq)) {
                /*
                 * newf=JAC*SD+CostFunction;
                 */
                mlfAssign(&newf, mlfPlus(mlfMtimes(*JAC, SD), *CostFunction));
                /*
                 * GradFactor=newf'*newf;
                 */
                mlfAssign(&GradFactor, mlfMtimes(mlfCtranspose(newf), newf));
                /*
                 * SD=-(JAC'*JAC+GradFactor*(eye(numberOfVariables,numberOfVariables)))\(CostFunction'*JAC)'; 
                 */
                mlfAssign(
                  &SD,
                  mlfMldivide(
                    mlfUminus(
                      mlfPlus(
                        mlfMtimes(mlfCtranspose(*JAC), *JAC),
                        mlfMtimes(
                          GradFactor,
                          mlfEye(numberOfVariables, numberOfVariables)))),
                    mlfCtranspose(
                      mlfMtimes(mlfCtranspose(*CostFunction), *JAC))));
            /*
             * end
             */
            }
            /*
             * newf=JAC*SD+CostFunction;
             */
            mlfAssign(&newf, mlfPlus(mlfMtimes(*JAC, SD), *CostFunction));
            /*
             * XOUT=XOUT+currstepsize*SD;
             */
            mlfAssign(&XOUT, mlfPlus(XOUT, mlfMtimes(currstepsize, SD)));
            /*
             * EstSum=newf'*newf;
             */
            mlfAssign(&EstSum, mlfMtimes(mlfCtranspose(newf), newf));
            /*
             * status=0;
             */
            mlfAssign(&status, mlfScalar(0.0));
            /*
             * if lineSearchType==0; 
             */
            if (mlfTobool(mlfEq(lineSearchType, mlfScalar(0.0)))) {
                /*
                 * PCNT=1; 
                 */
                mlfAssign(&PCNT, mlfScalar(1.0));
            /*
             * end
             */
            }
        /*
         * 
         * else
         */
        } else {
            /*
             * %-------------Direction Update------------------
             * gdnew=GradF'*SD;
             */
            mlfAssign(&gdnew, mlfMtimes(mlfCtranspose(GradF), SD));
            /*
             * if verbosity> 1 
             */
            if (mlfTobool(mlfGt(verbosity, mlfScalar(1.0)))) {
                /*
                 * num=sprintf(formatstr,iter,numFunEvals,NewF,currstepsize,gdnew);
                 */
                mlfAssign(
                  &num,
                  mlfSprintf(
                    NULL,
                    formatstr,
                    iter,
                    numFunEvals,
                    NewF,
                    currstepsize,
                    gdnew,
                    NULL));
            /*
             * end
             */
            }
            /*
             * if gdnew>0 & NewF>FIRSTF
             */
            {
                mxArray * a_ = mclInitialize(mlfGt(gdnew, mlfScalar(0.0)));
                if (mlfTobool(a_)
                    && mlfTobool(mlfAnd(a_, mlfGt(NewF, FIRSTF)))) {
                    mxDestroyArray(a_);
                    /*
                     * % Case 1: New function is bigger than last and gradient w.r.t. SD -ve
                     * % ... interpolate. 
                     * how='inter';
                     */
                    mlfAssign(&how, mxCreateString("inter"));
                    /*
                     * [stepsize]=cubici1(NewF,FIRSTF,gdnew,GDOLD,currstepsize);
                     */
                    mlfAssign(
                      &stepsize,
                      mlfCubici1(NewF, FIRSTF, gdnew, GDOLD, currstepsize));
                    /*
                     * currstepsize=0.9*stepsize;
                     */
                    mlfAssign(
                      &currstepsize, mlfMtimes(mlfScalar(0.9), stepsize));
                /*
                 * elseif NewF<FIRSTF
                 */
                } else {
                    mxDestroyArray(a_);
                    if (mlfTobool(mlfLt(NewF, FIRSTF))) {
                        /*
                         * %  New function less than old fun. and OK for updating 
                         * %         .... update and calculate new direction. 
                         * [newstep,fbest] =cubici3(NewF,FIRSTF,gdnew,GDOLD,currstepsize);
                         */
                        mlfAssign(
                          &newstep,
                          mlfCubici3(
                            &fbest, NewF, FIRSTF, gdnew, GDOLD, currstepsize));
                        /*
                         * if fbest>NewF,
                         */
                        if (mlfTobool(mlfGt(fbest, NewF))) {
                            /*
                             * fbest=0.9*NewF; 
                             */
                            mlfAssign(&fbest, mlfMtimes(mlfScalar(0.9), NewF));
                        /*
                         * end 
                         */
                        }
                        /*
                         * if gdnew<0
                         */
                        if (mlfTobool(mlfLt(gdnew, mlfScalar(0.0)))) {
                            /*
                             * how='incstep';
                             */
                            mlfAssign(&how, mxCreateString("incstep"));
                            /*
                             * if newstep<currstepsize,  
                             */
                            if (mlfTobool(mlfLt(newstep, currstepsize))) {
                                /*
                                 * newstep=(2*currstepsize+1e-4); how=[how,'IF']; 
                                 */
                                mlfAssign(
                                  &newstep,
                                  mlfPlus(
                                    mlfMtimes(mlfScalar(2.0), currstepsize),
                                    mlfScalar(1e-4)));
                                mlfAssign(
                                  &how,
                                  mlfHorzcat(how, mxCreateString("IF"), NULL));
                            /*
                             * end
                             */
                            }
                            /*
                             * currstepsize=abs(newstep);
                             */
                            mlfAssign(&currstepsize, mlfAbs(newstep));
                        /*
                         * else 
                         */
                        } else {
                            /*
                             * if currstepsize>0.9
                             */
                            if (mlfTobool(
                                  mlfGt(currstepsize, mlfScalar(0.9)))) {
                                /*
                                 * how='int_step';
                                 */
                                mlfAssign(&how, mxCreateString("int_step"));
                                /*
                                 * currstepsize=min([1,abs(newstep)]);
                                 */
                                mlfAssign(
                                  &currstepsize,
                                  mlfMin(
                                    NULL,
                                    mlfHorzcat(
                                      mlfScalar(1.0), mlfAbs(newstep), NULL),
                                    NULL,
                                    NULL));
                            /*
                             * end
                             */
                            }
                        /*
                         * end
                         */
                        }
                        /*
                         * % SET DIRECTION.      
                         * % Gauss-Newton Method    
                         * temp=1;
                         */
                        mlfAssign(&temp, mlfScalar(1.0));
                        /*
                         * if ~levMarq
                         */
                        if (mlfTobool(mlfNot(levMarq))) {
                            /*
                             * if currstepsize>1e-8 & cond(JAC)<1e8
                             */
                            mxArray * a_ = mclInitialize(
                                             mlfGt(
                                               currstepsize, mlfScalar(1e-8)));
                            if (mlfTobool(a_)
                                && mlfTobool(
                                     mlfAnd(
                                       a_,
                                       mlfLt(
                                         mlfCond(*JAC, NULL),
                                         mlfScalar(1e8))))) {
                                mxDestroyArray(a_);
                                /*
                                 * SD=JAC\(JAC*XOUT-CostFunction)-XOUT;
                                 */
                                mlfAssign(
                                  &SD,
                                  mlfMinus(
                                    mlfMldivide(
                                      *JAC,
                                      mlfMinus(
                                        mlfMtimes(*JAC, XOUT), *CostFunction)),
                                    XOUT));