QPSolveImpl.mesa
Copyright Ó 1990, 1992 by Xerox Corporation. All rights reserved.
Ken Shoemake, April 17, 1990 11:43 pm PDT
DIRECTORY
QPSolve, RealFns
;
QPSolveImpl: CEDAR PROGRAM
IMPORTS RealFns
EXPORTS QPSolve
~ BEGIN
OPEN QPSolve;
Procedures
NewMatrix: PUBLIC PROC [m, n: NAT] RETURNS [M: Matrix]
~ {
M ¬ NEW[MatrixRep[m]];
M.m ¬ m;
FOR i: NAT IN [0..m) DO M[i] ¬ NEW[RVectorRep[n]]; M[i].n ¬ n ENDLOOP;
};
QPSolve: PUBLIC PROC [
c: RVector, A: Matrix, lobd: RVector, x: RVector, iVar: IVector, nFR: NAT]
RETURNS [minFR: NAT]
Solve simple Quadratic Program: minimize ý xTx + cTx, subject to Ax = 0 and x e lobd.
Iterations start with initial feasible vector x, with x[i  iFX] fixed on their bounds and the rest, x[i  iFR], free. Elements [0..nFR) of iVar store iFR, and [nFR..n) store iFX.
~ {
Method is to maintain an active set of constaints, which includes the equality constraints and some of the bounds. The TQ factors of the currently active columns of the matrix A are maintained by incremental updating whenever a bound is added or deleted. The T matrix is reverse lower triangular: Tij = 0 for i+j < m-1. Part of Q forms a matrix Z, a basis for the null space of A, and the remaining columns, Y, form a basis for the range space of A. Successive iterations use Z to compute a search direction, p.
If p is zero, then current x is a constrained minimum of active set. Lagrange multipliers at x are estimated from the gradient g = x+c by solving first TTlL = YFRTgFR for lL, and then lFX = gFX-AFXTlL. If lk e 0 for all bounds k, x is desired minimum; else delete a bound k with lk < 0, and do another iteration.
If p is non-zero, find nearest bound the proposed step will encounter by computing q = MINi[qi], where qi = (si-xi)/pi, i  iFR. If q < 1, add encountered bound i to active set. Step x ← x + p MIN[1,q], and do another iteration.
The rows and columns of the data structures (vectors and matrices) are indexed indirectly through iFX and iFR.
n: NAT ~ x.n;   -- number of variables
m: NAT ~ A.m;   -- number of general (equality) constraints
mL: NAT ¬ m;   -- number of active non-bounds constraints
nZ: NAT ¬ nFR-mL; -- number of degrees of freedom active in x
<<These are indexed indirectly via iVar.>>
Q: Matrix ¬ NewMatrix[n, n];  -- will contain ((Z Y 0)T(0 0 IFX)T)T = Q
T: Matrix ¬ NewMatrix[m, n];  -- will contain T of TQ decomposition of A
<<These are indexed directly.>>
g: RVector ¬ NEW[RVectorRep[n]]; -- gradient at current point
Zg: RVector ¬ NEW[RVectorRep[n]]; -- projected gradient at current point
p: RVector ¬ NEW[RVectorRep[n]]; -- step direction
toMin: REAL ¬ 0.0;     -- step magnitude to projected minimum or nearest bound
lL: RVector ¬ NEW[RVectorRep[m]]; -- Lagrange multiplier estimates for general constraints
lFX: RVector ¬ NEW[RVectorRep[n]]; -- Lagrange multiplier estimates for bound constraints
b: RVector ¬ NEW[RVectorRep[m]];  -- temporary storage vector
<<I'm liable to go nuts keeping track of index indirection, so I'll use procedures to hide it.>>
GetMat: TYPE ~ PROC [i, j: NAT] RETURNS [REAL];
PutMat: TYPE ~ PROC [i, j: NAT, val: REAL];
getA: GetMat ~ {RETURN[A[i][iVar[j]]]};
getQ: GetMat ~ {RETURN[Q[iVar[i]][j]]};
putQ: PutMat ~ {Q[iVar[i]][j] ¬ val};
getT: GetMat ~ {RETURN[T[i][j]]};
putT: PutMat ~ {T[i][j] ¬ val};
fullProject: BOOL ¬ FALSE;
eps: REAL ¬ 5.0e-5;
gMag: REAL;
EstimateLagrange: PROC
Lagrange multipliers at x are estimated from g by solving first TTlL = YFRTgFR for lL, and then lFX = gFX-AFXTlL.
~ {
<<Compute b = YFRTgFR.>>
FOR i: NAT IN [0..mL) DO   -- Iterate across columns of YFR generating b.
dot: REAL ¬ 0.0;     -- Dot column of YFR with gFR.
FOR j: NAT IN [0..nFR) DO dot ¬ dot + getQ[j, nZ+i]*g[j] ENDLOOP;
b[i] ¬ dot;
ENDLOOP;
<<Solve TTlL = b for lL.>>
FOR i: NAT IN [0..mL) DO   -- Iterate down rows of TT generating lL.
sum: REAL ¬ b[i];    -- Dot row of TT with lL.
FOR j: NAT IN [mL-i..mL) DO sum ¬ sum - getT[j, nZ+i]*lL[j] ENDLOOP;
lL[mL-1-i] ¬ sum/getT[mL-1-i, nZ+i];
ENDLOOP;
<<Now compute lFX = gFX-AFXTlL.>>
FOR i: NAT IN [nFR..n) DO
sum: REAL ¬ g[i];    -- Dot row of AFXT with lL.
FOR j: NAT IN [0..mL) DO sum ¬ sum - getA[j, i]*lL[j] ENDLOOP;
lFX[i] ¬ sum;
ENDLOOP;
};
NegativeLagrange: PROC RETURNS [k: NAT]
~ {
mostNeg: REAL ¬ -eps;
k ¬ n;
FOR i: NAT IN [nFR..n) DO
IF lFX[i] < mostNeg THEN {k ¬ i; mostNeg ¬ lFX[i]};
ENDLOOP;
};
UpdateGradient: PROC
~ {
gMag ¬ 0.0;
FOR i: NAT IN [0..n) DO gMag ¬ MAX[gMag, ABS[g[i] ¬ x[iVar[i]] + c[iVar[i]]]] ENDLOOP
};
ProjectGradient: PROC
Set direction pZ to -ZTgFR.
~ {
FOR i: NAT IN [0..nZ) DO
dot: REAL ¬ 0.0;
FOR j: NAT IN [0..nFR) DO dot ¬ dot + getQ[j, i]*g[j] ENDLOOP;
Zg[i] ¬ -dot;
ENDLOOP;
};
NegligibleZGradient: PROC RETURNS [BOOL]
~ {
toMin ¬ 0.0;
IF fullProject
THEN {
ProjectGradient[];
FOR i: NAT IN [0..nZ) DO IF ABS[Zg[i]] > eps THEN toMin ¬ 1.0 ENDLOOP;
}
ELSE {FOR j: NAT IN [0..nFR) DO toMin ¬ toMin+g[j]*getQ[j, nFR-1] ENDLOOP};
RETURN[ABS[toMin] < eps];
};
ComputeDirection: PROC
~ {
IF fullProject
THEN {
FOR i: NAT IN [0..nFR) DO p[i] ¬ 0.0 ENDLOOP;
FOR i: NAT IN [0..nZ) DO
FOR j: NAT IN [0..nFR) DO p[j] ¬ p[j] + getQ[j, i]*Zg[i] ENDLOOP;
ENDLOOP;
}
ELSE {
IF toMin > 0.0
THEN {FOR j: NAT IN [0..nFR) DO p[j] ¬ getQ[j, nFR-1] ENDLOOP}
ELSE {FOR j: NAT IN [0..nFR) DO p[j] ¬ -getQ[j, nFR-1] ENDLOOP};
toMin ¬ ABS[toMin];
};
};
NearestBound: PROC RETURNS [k: NAT]
~ {
toK: REAL;
fudge: REAL ¬ eps;
k ¬ nFR;
FOR i: NAT IN [0..nFR) DO
IF p[i] < -fudge AND (toK ¬ (lobd[iVar[i]]-x[iVar[i]])/p[i]) < toMin AND toK > 0.0
THEN {toMin ¬ toK; k ¬ i};
ENDLOOP;
};
StepToMin: PROC
~ {FOR i: NAT IN [0..nFR) DO x[iVar[i]] ¬ x[iVar[i]] + toMin*p[i] ENDLOOP};
MakeRot: PROC [x, y: REAL] RETURNS [c, s: REAL]
Compute c and s so that (x y) ((c s)T (-s c)T)T = (0 w); w2 = x2+y2; w e 0.
~ {
Signum: PROC [v: REAL] RETURNS [REAL]
~ INLINE {RETURN[IF v < 0.0 THEN -1.0 ELSE 1.0]};
IF x = 0.0
THEN {c ¬ IF y < 0.0 THEN -1.0 ELSE 1.0; s ¬ 0.0}
ELSE {
IF ABS[y] > ABS[x]
THEN {t: REAL ¬ x/y; c ¬ Signum[y]/RealFns.SqRt[1+t*t]; s ¬ c*t}
ELSE {t: REAL ¬ y/x; s ¬ Signum[x]/RealFns.SqRt[1+t*t]; c ¬ s*t};
};
};
RotCols: PROC [c, s: REAL, getM: GetMat, putM: PutMat, loRow, hiRow, xCol, yCol: NAT]
Apply rotation (x y) ((c s)T (-s c)T)T to rows [loRow..hiRow) of columns x and y of M.
~ {
IF c = 1.0 THEN RETURN;
FOR i: NAT IN [loRow..hiRow) DO
x: REAL ¬ getM[i, xCol];
y: REAL ¬ getM[i, yCol];
xRot: REAL ¬ x*c - y*s;
yRot: REAL ¬ x*s + y*c;
putM[i, xCol, xRot];
putM[i, yCol, yRot];
ENDLOOP;
};
FixVar: PROC [k: NAT]
Put variable iVar[k] in fixed set iFX, and update TQ decomposition in Q and T.
~ {
Move variable k to end of iFR. Decrement nFR, effectively moving k to iFX.
kV: NAT ¬ iVar[k];
FOR i: NAT IN [k..nFR-1) DO iVar[i] ¬ iVar[i+1] ENDLOOP;
iVar[nFR-1] ¬ kV;
nFR ¬ nFR-1;
nZ ¬ nZ-1;
Apply rotations to Q to make row k of Q (0 0 ... 0 1). Apply last mL rots to T.
FOR i: NAT IN [0..nFR) DO
x: REAL ¬ getQ[nFR, i];
y: REAL ¬ getQ[nFR, i+1];
c, s: REAL;
[c, s] ¬ MakeRot[x, y];
RotCols[c, s, getQ, putQ, 0, nFR+1, i, i+1];
SELECT i FROM
< nZ => {}; -- In general algorithm, would update Cholesky R when i <= nZ.
= nZ => {
FOR j: NAT IN [0..mL) DO putT[j, i, 0.0] ENDLOOP;
RotCols[c, s, getT, putT, mL-1, mL, i, i+1];
};
> nZ => RotCols[c, s, getT, putT, nFR-1-i, mL, i, i+1];
ENDCASE;
ENDLOOP;
<<In general algorithm, would now restore triangularity of Cholesky R.>>
};
FreeVar: PROC [k: NAT]
Put variable iVar[k] in free set iFR, and update TQ decomposition in Q and T.
~ {
Move variable k to head of iFX. Increment nFR, effectively moving k to iFR.
kV: NAT ¬ iVar[k];
FOR i: NAT DECREASING IN [nFR..k) DO iVar[i+1] ¬ iVar[i] ENDLOOP;
iVar[nFR] ¬ kV;
nFR ¬ nFR+1;
nZ ¬ nZ+1;
New row and column of Q are (0 0 ... 0 1).
FOR j: NAT IN [0..nFR-1) DO putQ[j, nFR-1, 0.0]; putQ[nFR-1, j, 0.0] ENDLOOP;
putQ[nFR-1, nFR-1, 1.0];
Apply mL rotations to triangularize (T ak). Rotations also affect augmented Q.
FOR j: NAT IN [0..mL) DO putT[j, nFR-1, getA[j, nFR-1]] ENDLOOP;
FOR i: NAT DECREASING IN [nZ..nFR) DO
x: REAL ¬ getT[nFR-1-i, i-1];
y: REAL ¬ getT[nFR-1-i, i];
c, s: REAL;
[c, s] ¬ MakeRot[x, y];
RotCols[c, s, getQ, putQ, 0, nFR, i-1, i];
RotCols[c, s, getT, putT, nFR-1-i, mL, i-1, i];
ENDLOOP;
<<In general algorithm, would now augment then restore triangularity of Cholesky R.>>
};
InitTQ: PROC
Perform TQ factorization of initially active columns of A matrix.
~ {
Copy nFR columns of A to T. Use Givens rotations, saved in Q, to triangularize it.
FOR i: NAT IN [0..nFR) DO
FOR j: NAT IN [0..mL) DO putT[j, i, getA[j, i]] ENDLOOP;
FOR j: NAT IN [0..nFR) DO putQ[j, i, IF i=j THEN 1.0 ELSE 0.0] ENDLOOP;
ENDLOOP;
FOR j: NAT IN [0..mL) DO
FOR i: NAT IN [1..nFR-j) DO
c, s: REAL;
[c, s] ¬ MakeRot[getT[j, i-1], getT[j, i]];
RotCols[c, s, getT, putT, j, mL, i-1, i];
RotCols[c, s, getQ, putQ, 0, nFR, i-1, i];
ENDLOOP;
ENDLOOP;
};
iter: NAT ¬ 0;
fullProject ¬ nFR > 1;
InitTQ[];
FOR iter IN [1..5*n*n] DO
k: NAT;
UpdateGradient[];
IF NegligibleZGradient[]
THEN {
EstimateLagrange[];
IF (k ¬ NegativeLagrange[]) < n THEN FreeVar[k] ELSE EXIT;
}
ELSE {
ComputeDirection[];
k ¬ NearestBound[];
StepToMin[];
IF k < nFR THEN FixVar[k];
};
ENDLOOP;
RETURN[nFR];
};
Start Code
END..