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 Y
FR 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 T
T generating
l
L.
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];
};