sqpsolve.src

没有说明

    if scalmiss(g);
        if not trapchk(4);
            errorlog "gradient function failed";
        endif;
        retp(x0,f0,LNSlagr,4,iter);
    endif;

    if LNShsprc /= 0;
        h = hsproc(x0);
    else;
        h = hessp(&fnct,x0);
    endif;

    qp_a = {};
    qp_b = {};

    if not scalmiss(LNS_A);
        qp_a = qp_a | LNS_A;
        qp_b = qp_b | (-LNS_A*x0 + LNS_B);
    endif;

    if not scalmiss(LNSeproc);
        qp_a = qp_a | gradp(&EqProc,x0);
        qp_b = qp_b | -EqProc(x0);
    endif;

    if not scalmiss(LNS_C);
        qp_a = qp_a | LNS_C;
        qp_b = qp_b | -LNS_C*x0 + LNS_D;
    endif;

    if not scalmiss(LNSiproc);
        qp_a = qp_a | gradp(&IneqProc,x0);
        qp_b = qp_b | -IneqProc(x0);
    endif;

    if scalmiss(qp_a);
        qp_a = ones(1,rows(x0));
        qp_b = -1e256;
    endif;

    if cols(LNS_Bnds) == 2;
        qp_xl = LNS_Bnds[.,1] - x0;
        qp_xu = LNS_Bnds[.,2] - x0;
    else;
        qp_xl = -qp_t;
        qp_xu = qp_t;
    endif;

    { qp_b,qp_xl,qp_xu,qp_d,qp_ret } = _intqpsolvfcn01(h,-g,qp_a,qp_b,
        qp_xl,qp_xu,qp_d,numeq,qp_maxit,qp_lql);

    if qp_ret < 0;
        if not trapchk(4) and qp_ret < 0;
            errorlog "constraint no. "$+ftos(-qp_ret,"%*.*lf",1,0)$+" incon"\
                "sistent";
        endif;
        retp(x0,f0,LNSlagr,7,iter);

    elseif qp_ret == 1;
        if not trapchk(4);
            errorlog "maximum iterations exceeded in QPSOLVE";
        endif;
    elseif qp_ret == 2;
        if not trapchk(4);
            errorlog "quadratic program iterations halted due to lack"\
                     " of precision";
        endif;
    endif;

    if cols(LNS_Bnds) == 2;
        k1 = qp_d .< (LNS_Bnds[.,1] - x0);
        qp_d = (1 - k1) .* qp_d + k1 .* (LNS_Bnds[.,1] - x0);
        k1 = qp_d .> (LNS_Bnds[.,2] - x0);
        qp_d = (1 - k1) .* qp_d + k1 .* (LNS_Bnds[.,2] - x0);
    endif;

    ky = 1;
    if not scalmiss(LNS_A);
        LNSlagr = vput(LNSlagr,qp_b[ky:rows(LNS_A)],"lineq");
        lagr1 = maxc(abs(qp_b[ky:rows(LNS_A)]));
        ky = ky + rows(LNS_A);
    endif;

    if not scalmiss(LNSeproc);
        LNSlagr = vput(LNSlagr,qp_b[ky:ky+numNlinEqC-1],"nlineq");
        lagr1 = maxc(lagr1|abs(qp_b[ky:rows(LNS_A)]));
        ky = ky + numNlinEqC;
    endif;

    if not scalmiss(LNS_C);
        LNSlagr = vput(LNSlagr,qp_b[ky:ky+rows(LNS_C)-1],"linineq");
        lagr2 = maxc(abs(qp_b[ky:ky+rows(LNS_C)-1]));
        ky = ky + rows(LNS_C);
    endif;

    if not scalmiss(LNSiproc);
        LNSlagr = vput(LNSlagr,qp_b[ky:ky+numNlinIneqC-1],"nlinineq");
        lagr2 = maxc(abs(lagr2|qp_b[ky:ky+numNlinIneqC-1]));
    endif;

    if cols(LNS_Bnds) == 2;
        LNSlagr = vput(LNSlagr,qp_xl~qp_xu,"bounds");
        lagr2 = maxc(abs(qp_xl|qp_xu|lagr2));
    endif;

    if abs(qp_d) < LNSDtol;
        retp(x0,f0,LNSlagr,0,iter);
    endif;


    /*
    **  line search
    */

        delta = 1e-4;   /* must be in (0,1/2) interval */
        ub = 0.5;       /* Upper bound on acceptable reduction in s. */
        lb = 0.1;       /* Lower bound on acceptable reduction in s. */
        cdelta = 1 - delta;
        dg = qp_d' * g;
        if LNSFtest;
            s = _sqp_feasible(x0,1,qp_d,LNS_C,LNS_D,LNSiproc,LNS_bnds);
        endif;
        tt = s * dg;
        f1 = _sqp_meritFunct(x0+s*qp_d,&fnct,lagr1,lagr2,
              LNS_A,LNS_B,LNSeproc,LNS_C,LNS_D,LNSiproc,LNS_Bnds);

        if scalmiss(f1) or (f1 $== __INFp) or (f1 $== __INFn) or (f1 $==
             __INDEFp) or (f1 $== __INDEFn);
                retp(x0,f0,LNSlagr,6,iter);
        endif;

        if (f1 / tt - f0 / tt) < delta;

            s = -dg/(2*(f1-f0-dg));
            if LNSFtest;
                s = _sqp_feasible(x0,maxc(s|lb),qp_d,LNS_C,LNS_D,LNSiproc,
                       LNS_bnds);
            endif;
            f2 = _sqp_meritFunct(x0+s*qp_d,&fnct,lagr1,lagr2,
                    LNS_A,LNS_B,LNSeproc,LNS_C,LNS_D,LNSiproc,LNS_Bnds);

            if scalmiss(f2) or (f2 $== __INFp) or (f2 $== __INFn) or (f2 $==
                 __INDEFp) or (f2 $== __INDEFn);
                    retp(x0,f0,LNSlagr,6,iter);
            endif;

            tt = s * dg;
            w1 = f2 / tt - f0 / tt;

            if w1 < delta or w1 > cdelta;

                sprev = s;
                s2prev = 1;
                rprev = f2;
                r2prev = f1;
#ifUNIX
                for j(1,40,1);   /*  maxtries = 40 */
#else
                j = 1; do until j > MaxTry;
#endif
                    sprev2 = sprev * sprev;
                    s2prev2 = s2prev * s2prev;
                    sp2[1,1] = sprev2;
                    sp2[1,2] = s2prev2;
                    dsprev = sprev - s2prev;

                    vv[2,1] = -s2prev;
                    vv[2,2] = sprev;
                    vv = vv./sp2;

                    zz[1] = rprev - f0 - dg * sprev;
                    zz[2] = r2prev - f0 - dg * s2prev;
                    ab = (1 / dsprev) * vv * zz;
                    a = ab[1,1];
                    b = ab[2,1];

                    if a == 0;
                        s = -dg / (2 * b);
                    else;
                        qv = b * b - 3 * a * dg;
                        if qv < 0;
                            break;
                        endif;          /* terminate if not real root */
                        tt = 3 * a;
                        s = -b / tt + sqrt(qv) / tt;
                    endif;

                    if s > ub * sprev;
                        s = ub * sprev;
                    elseif s < lb * sprev;
                        s = lb * sprev;
                    endif;

                    if LNSFtest;
                        s = _sqp_feasible(x0,s,qp_d,LNS_C,LNS_D,LNSiproc,
                             LNS_bnds);
                    endif;
                    f1 = _sqp_meritFunct(x0+s*qp_d,&fnct,lagr1,lagr2,
                             LNS_A,LNS_B,LNSeproc,LNS_C,LNS_D,LNSiproc,
                             LNS_Bnds);

                    if scalmiss(f1) or (f1 $== __INFp) or (f1 $== __INFn) or
                       (f1 $==  __INDEFp) or (f1 $== __INDEFn);
                          s = error(0);
                          break;
                    endif;

                    tt = s * dg;
                    w1 = f1 / tt - f0 / tt;

                    if w1 >= delta and w1 <= cdelta;
                        break;
                    endif;

                    s2prev = sprev;
                    sprev = s;
                    r2prev = rprev;
                    rprev = f1;
#ifUNIX
                endfor;
#else
                j = j + 1; endo;
#endif

            else;
                 f1 = f2;
            endif;

        endif;

        if scalmiss(s);
            j = 1;
            f1 = 1e250;
            do while f1 > f0;
                rteps = 10^trunc(log(meanc(abs(g)))) * LNSrteps;
                qp_d = rteps*(2*rndu(np,1)-1).*x0;
                f1 = fnct(x0+qp_d);
                if scalmiss(f1) or (f1 $== __INFp) or (f1 $== __INFn) or
                   (f1 $==  __INDEFp) or (f1 $== __INDEFn);
                   f1 = 1e250;
                   qp_d = 1;
                endif;
                j = j + 1;
                if j > MaxTry;
                    break;
                endif;
            endo;
            f0 = f1;
            x0 = x0 + qp_d;
        else;
            x0 = x0 + s * qp_d;
        endif;

        if LNSpiters;

            print;
            print;
            print "---------------------------------------------------";
#ifUNIX
            print " iter "$+ftos(iter+0,"%*.*lf",1,0);;
#else
            print " iter "$+ftos(iter,"%*.*lf",1,0);;
#endif
            print "          function = "$+ftos(fnct(x0),"%*.*lf",15,8);
            print "---------------------------------------------------";
            print;
            print "       parameter      direction        gradient";
            call printfmt(x0~qp_d~g,1);
        endif;


        ky = key;
        do while ky;
            if upper(chrs(ky)) $== "C";
                retp(x0,f0,LNSlagr,1,iter);
            elseif upper(chrs(ky)) $== "P";
                LNSPiters = 1 - LNSPiters;
            endif;
        endo;

#ifUNIX
    endfor;
#else
    iter = iter + 1; endo;
#endif

    call ndpcntrl(old,0xffff);
    ndpclex;

    retp(x0,f0,2,LNSlagr,LNSmiter);

endp;


proc _sqp_feasible(x,l,d,LNS_C,LNS_D,LNSiproc,LNS_bnds);
    local m0, t, ineqproc;

    m0 = 0;
    do until m0 > 200;
        m0 = m0 + 1;
        t = x + l * d;
        if not scalmiss(LNS_C);
            if not((LNS_C*t - LNS_D) >= 0);
                l = .9 * l;
                continue;
            endif;
        endif;
        if not scalmiss(LNSiproc);
            IneqProc = LNSiproc;
            local ineqproc:proc;
            if not(IneqProc(t) >= 0);
                l = .9 * l;
                continue;
            endif;
        endif;
        if cols(LNS_Bnds) == 2;
            if not((t - LNS_Bnds[.,1]) >= 0);
                l = .9 * l;
                continue;
            endif;
            if not((-t + LNS_Bnds[.,2]) >= 0);
                l = .9 * l;
                continue;
            endif;
        endif;
        retp(l);
    endo;
    if not trapchk(4);
        errorlog "feasible step length could not be found";
    endif;
    retp(error(0));
endp;



proc _sqp_meritFunct(x,fnct,lagr1,lagr2,
       LNS_A,LNS_B,LNSeproc,LNS_C,LNS_D,LNSiproc,LNS_Bnds);

    local f0, zz, eqproc, ineqproc;
    local fnct:proc;

    f0 = fnct(x);
    if lagr1 /= 0;
        if not scalmiss(LNS_A);
            f0 = f0 + lagr1 * sumc(abs(LNS_A*x - LNS_B));
        endif;
        if not scalmiss(LNSeproc);
            EqProc = LNSeproc;
            local eqproc:proc;
            f0 = f0 + lagr1 * sumc(abs(EqProc(x)));
        endif;
    endif;
    if lagr2 /= 0;
        if not scalmiss(LNS_C);
            zz = LNS_C*x - LNS_D;
            zz = zz .* (zz .< 0);
            f0 = f0 - lagr2 * sumc(zz);
        endif;
        if not scalmiss(LNSiproc);
            ineqproc = LNSiproc;
            local ineqproc:proc;
            zz = IneqProc(x);
            zz = zz .* (zz .< 0);
            f0 = f0 - lagr2 * sumc(zz);
        endif;
        if cols(LNS_Bnds) == 2;
            zz = x - LNS_Bnds[.,1];
            zz = zz .* (zz .< 0);
            f0 = f0 - lagr2 * sumc(zz);
            zz = -x + LNS_Bnds[.,2];
            zz = zz .* (zz .< 0);
            f0 = f0 - lagr2 * sumc(zz);
        endif;

    endif;
    retp(f0);
endp;




proc(0) = sqpSolveset;
    gausset;
    _sqp_ParNames = 0;       /* parameter names */
    _sqp_DirTol = 1e-5;      /* convergence tolerance */
    _sqp_HessProc = 0;       /* procedure to compute hessian */
    _sqp_GradProc = 0;       /* procedure to compute gradient */
    _sqp_MaxIters = 1e+5;    /* maximum number of iterations */
    _sqp_PrintIters = 0;
    _sqp_FeasibleTest = 1;
    _sqp_A = { . };
    _sqp_B = { . };
    _sqp_C = { . };
    _sqp_D = { . };
    _sqp_Bounds = { . };
    _sqp_EqProc = { . };
    _sqp_IneqProc = { . };
endp;