optim_private_nlsq.c

ASUFIT－Matlab－全局拟合程序

                                /*
                                 * if SD'*GradF>eps,
                                 */
                                if (mlfTobool(
                                      mlfGt(
                                        mlfMtimes(mlfCtranspose(SD), GradF),
                                        mlfEps()))) {
                                    /*
                                     * how='ERROR- GN not descent direction';
                                     */
                                    mlfAssign(
                                      &how,
                                      mxCreateString(
                                        "ERROR- GN not descent direction"));
                                /*
                                 * end
                                 */
                                }
                                /*
                                 * temp=0;
                                 */
                                mlfAssign(&temp, mlfScalar(0.0));
                            /*
                             * else
                             */
                            } else {
                                mxDestroyArray(a_);
                                /*
                                 * if verbosity > 0
                                 */
                                if (mlfTobool(
                                      mlfGt(verbosity, mlfScalar(0.0)))) {
                                    /*
                                     * disp('Conditioning of Gradient Poor - Switching To LM method')
                                     */
                                    mlfDisp(
                                      mxCreateString(
                                        "Conditioning of Gradient Poor"
                                        " - Switching To LM method"));
                                /*
                                 * end
                                 */
                                }
                                /*
                                 * how='CHG2LM';
                                 */
                                mlfAssign(&how, mxCreateString("CHG2LM"));
                                /*
                                 * levMarq=1;
                                 */
                                mlfAssign(&levMarq, mlfScalar(1.0));
                                /*
                                 * currstepsize=abs(currstepsize);               
                                 */
                                mlfAssign(&currstepsize, mlfAbs(currstepsize));
                            }
                        /*
                         * end
                         * end
                         */
                        }
                        /*
                         * 
                         * if (temp)      
                         */
                        if (mlfTobool(temp)) {
                            /*
                             * % Levenberg_marquardt Method N.B. EstSum is the estimated sum of squares.
                             * %                                 GradFactor is the value of lambda.
                             * % Estimated Residual:
                             * if EstSum>fbest
                             */
                            if (mlfTobool(mlfGt(EstSum, fbest))) {
                                /*
                                 * GradFactor=GradFactor/(1+currstepsize);
                                 */
                                mlfAssign(
                                  &GradFactor,
                                  mlfMrdivide(
                                    GradFactor,
                                    mlfPlus(mlfScalar(1.0), currstepsize)));
                            /*
                             * else
                             */
                            } else {
                                /*
                                 * GradFactor=GradFactor+(fbest-EstSum)/(currstepsize+eps);
                                 */
                                mlfAssign(
                                  &GradFactor,
                                  mlfPlus(
                                    GradFactor,
                                    mlfMrdivide(
                                      mlfMinus(fbest, EstSum),
                                      mlfPlus(currstepsize, mlfEps()))));
                            /*
                             * end
                             */
                            }
                            /*
                             * 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))));
                            /*
                             * currstepsize=1; 
                             */
                            mlfAssign(&currstepsize, mlfScalar(1.0));
                            /*
                             * estf=JAC*SD+CostFunction;
                             */
                            mlfAssign(
                              &estf,
                              mlfPlus(mlfMtimes(*JAC, SD), *CostFunction));
                            /*
                             * EstSum=estf'*estf;
                             */
                            mlfAssign(
                              &EstSum, mlfMtimes(mlfCtranspose(estf), estf));
                            /*
                             * if verbosity> 1
                             */
                            if (mlfTobool(mlfGt(verbosity, mlfScalar(1.0)))) {
                                /*
                                 * num=[num,sprintf('%12.6g ',GradFactor)]; 
                                 */
                                mlfAssign(
                                  &num,
                                  mlfHorzcat(
                                    num,
                                    mlfSprintf(
                                      NULL,
                                      mxCreateString("%12.6g "),
                                      GradFactor,
                                      NULL),
                                    NULL));
                            /*
                             * end
                             */
                            }
                        /*
                         * end
                         */
                        }
                        /*
                         * gdnew=GradF'*SD;
                         */
                        mlfAssign(&gdnew, mlfMtimes(mlfCtranspose(GradF), SD));
                        /*
                         * 
                         * OLDX=XOUT;
                         */
                        mlfAssign(&OLDX, XOUT);
                        /*
                         * % Save Variables
                         * FIRSTF=NewF;
                         */
                        mlfAssign(&FIRSTF, NewF);
                        /*
                         * OLDG=GradF;
                         */
                        mlfAssign(&OLDG, GradF);
                        /*
                         * GDOLD=gdnew;    
                         */
                        mlfAssign(&GDOLD, gdnew);
                        /*
                         * 
                         * % If quadratic interpolation set PCNT
                         * if lineSearchType==0, 
                         */
                        if (mlfTobool(mlfEq(lineSearchType, mlfScalar(0.0)))) {
                            /*
                             * PCNT=1; MATX=zeros(3,1); MATL(1)=NewF; 
                             */
                            mlfAssign(&PCNT, mlfScalar(1.0));
                            mlfAssign(
                              &MATX,
                              mlfZeros(mlfScalar(3.0), mlfScalar(1.0), NULL));
                            mlfIndexAssign(&MATL, "(?)", mlfScalar(1.0), NewF);
                        /*
                         * end
                         */
                        }
                    /*
                     * else 
                     */
                    } else {
                        /*
                         * % Halve Step-length
                         * how='Red_Step';
                         */
                        mlfAssign(&how, mxCreateString("Red_Step"));
                        /*
                         * if NewF==FIRSTF,
                         */
                        if (mlfTobool(mlfEq(NewF, FIRSTF))) {
                            /*
                             * if verbosity>0
                             */
                            if (mlfTobool(mlfGt(verbosity, mlfScalar(0.0)))) {
                                /*
                                 * disp('No improvement in search direction: Terminating')
                                 */
                                mlfDisp(
                                  mxCreateString(
                                    "No improvement in search "
                                    "direction: Terminating"));
                            /*
                             * end
                             */
                            }
                            /*
                             * status=1;
                             */
                            mlfAssign(&status, mlfScalar(1.0));
                            /*
                             * EXITFLAG = -1;
                             */
                            mlfAssign(EXITFLAG, mlfScalar(-1.0));
                        /*
                         * else
                         */
                        } else {
                            /*
                             * currstepsize=currstepsize/8;
                             */
                            mlfAssign(
                              &currstepsize,
                              mlfMrdivide(currstepsize, mlfScalar(8.0)));
                            /*
                             * if currstepsize<1e-8
                             */
                            if (mlfTobool(
                                  mlfLt(currstepsize, mlfScalar(1e-8)))) {
                                /*
                                 * currstepsize=-currstepsize;
                                 */
                                mlfAssign(
                                  &currstepsize, mlfUminus(currstepsize));
                            /*
                             * end
                             */
                            }
                        /*
                         * end
                         */
                        }
                    }
                }
            /*
             * end
             */
            }
            /*
             * XOUT=OLDX+currstepsize*SD;
             */
            mlfAssign(&XOUT, mlfPlus(OLDX, mlfMtimes(currstepsize, SD)));
            /*
             * if isinf(verbosity)
             */
            if (mlfTobool(mlfIsinf(verbosity))) {
                /*
                 * disp([num,'       ',how])
                 */
                mlfDisp(mlfHorzcat(num, mxCreateString("       "), how, NULL));
            /*
             * elseif verbosity>1
             */
            } else if (mlfTobool(mlfGt(verbosity, mlfScalar(1.0)))) {
                /*
                 * disp(num)
                 */
                mlfDisp(num);
            /*
             * end
             */
            }
        /*
         * 
         * end %----------End of Direction Update-------------------
         */
        }
        /*
         * if lineSearchType==0, 
         */
        if (mlfTobool(mlfEq(lineSearchType, mlfScalar(0.0)))) {
            /*
             * PCNT=1; MATX=zeros(3,1);  MATL(1)=NewF; 
             */
            mlfAssign(&PCNT, mlfScalar(1.0));
            mlfAssign(&MATX, mlfZeros(mlfScalar(3.0), mlfScalar(1.0), NULL));
            mlfIndexAssign(&MATL, "(?)", mlfScalar(1.0), NewF);
        /*
         * end
         */
        }
        /*
         * % Check Termination 
         * if max(abs(SD))< tolX 
         */
        if (mlfTobool(mlfLt(mlfMax(NULL, mlfAbs(SD), NULL, NULL), tolX))) {
            /*
             * if verbosity > 0
             */
            if (mlfTobool(mlfGt(verbosity, mlfScalar(0.0)))) {
                /*
                 * disp('Optimization terminated successfully:') 
                 */
                mlfDisp(
                  mxCreateString("Optimization terminated successfully:"));
                /*
                 * disp(' Search direction less than tolX')     
                 */
                mlfDisp(mxCreateString(" Search direction less than tolX"));
            /*
             * end
             */
            }
            /*
             * status=1; EXITFLAG=1;
             */
            mlfAssign(&status, mlfScalar(1.0));
            mlfAssign(EXITFLAG, mlfScalar(1.0));
        /*
         * elseif (GradF'*SD) < tolFun & ...
         */