pseudorank.m

基于matlab的约束非线性规划算法库

function [GNlambda, lambda, r, p, beta, lenJp, JpTf] = PseudoRank(Aset,Jc,Teq,T,J,Tnfixed,nfixed,fixed,Abounds,b,f,m,n)
%Call:
%[GNlambda, lambda, r, p, beta, lenJp, JpTf] = PseudoRank(Aset,Jc,Teq,T,J,Tnfixed,nfixed,fixed,Abounds,b,f,m,n)
%Compute GnLambda, r and p the solution of the constrained subproblem.
%GNlambda is a kind of 2:nd order approx. to Lagrange multipliers
%lambda is a first order estimate of Lagrange multipliers
%r is the residual A'*GNlambda - J'f (should be zeros at a KT-point)
%If the Jacobian in question is rank-deficient a subspace metod is used
%to compute the searchdirection.
%
epsrank = sqrt(eps);
disp('In PseudoRank:')
if Tnfixed == 0 %unconstrained subproblem
    [Q, R, P1] = qr(J);
    d = -Q'*f;
    ShouldBeN = min(m,n);
    R11 = R(1:ShouldBeN,1:n);
    D = abs(diag(R11)/R11(1,1));
    great = (D >= epsrank);
    prank = nnz(great);
    p = P1*[R11(1:prank,1:prank)\d(1:prank); zeros(n-prank,1)];
    temp=J*p;
    JpTf = dot(temp,f)
    lenJp = norm(temp)
    beta = norm(d(1:prank))
    GNlambda = 0;
    lambda = 0;
    r = 0;
    pause
else
    %Form the Jacobian corresponding to the current active set
    %First the rows corresponding to fixed variables
    JcB = zeros(nfixed,n);
    if nfixed > 0
       for kk=1:nfixed
          JcB(kk,fixed(kk)) = Abounds(fixed(kk));
       end
    end
    JcB
    %Then the complete Jacobians
    i = Aset(Teq+1:T);
    A = [Jc(1:Teq,:); Jc(i,:); JcB];
    [Q1, R, P1] = qr(A');
    L11 = R(1:Tnfixed,1:Tnfixed)';
    D = abs(diag(L11)/L11(1,1));
    great = (D >= epsrank);
    Tbar = nnz(great)
    if Tbar == Tnfixed
       disp('Full rank on A')
    else
       disp('NOT FULL RANK ON A')
       pause
    end
    [Q2, R11, P2] = qr(L11(1:Tnfixed,1:Tbar));
    btrans = -Q2'*P1'*[b(:); zeros(nfixed,1)];
    DeltaP1 = R11(1:Tbar,1:Tbar)\btrans(1:Tbar);
    p1 = P2*DeltaP1(:);
    Jtrans = J*Q1;
    J1 = Jtrans(1:m,1:Tbar);
    J2 = Jtrans(1:m,Tbar+1:n);
    R1 = J1*P2;
    d = -f-R1*p1;
    [mJ2, nJ2]=size(J2);
    if isempty(J2)
        p2 = [];
        s = n;
    elseif mJ2<nJ2
           p2 = J2\d;
           s = mJ2;
       else
        [Q3, R2, P3] = qr(J2);
        R22 = R2(1:n-Tbar,1:n-Tbar)
        D = abs(diag(R22)/R22(1,1));
        great = (D >= epsrank);
        s = nnz(great)
        d = Q3'*d;
        DeltaP2 = R2(1:s,1:s)\d(1:s);
        p2 = P3*[DeltaP2(:); zeros(n-Tbar-s,1)];
    end
    p = Q1*[p1;p2]
    beta = norm(d(1:s))
    nrmpGN = norm(p)
    JpGN = J*p;
    grad = J'*f;
    lambda = A'\grad-(A*A')\[b(:); zeros(nfixed,1)]
    GNlambda = A'\(J'*(JpGN+f))
    r = A'*lambda-grad;
    nrmFOLres = norm(r)
    rGN1 = A'*GNlambda-grad;
    nrmGN1res = norm(rGN1)
    rGN2 = A'*GNlambda-J'*(JpGN+f);
    nrmGN2res = norm(rGN2)
    JpTf = JpGN'*f
    lenJp = norm(JpGN)
    pause
end