def checksol(sol, A, B, C = None, d = None, G = None, h = None):
"""
Check optimality conditions
C * x + G' * z + A'(Z) + d = 0
G * x <= h
z >= 0, || Z || < = 1
z' * (h - G*x) = 0
tr (Z' * (A(x) + B)) = || A(x) + B ||_*.
"""
p, q = B.size
n = A.size[1]
if G is None: G = spmatrix([], [], [], (0, n))
if h is None: h = matrix(0.0, (0, 1))
m = h.size[0]
if C is None: C = spmatrix(0.0, [], [], (n,n))
if d is None: d = matrix(0.0, (n, 1))
if sol['status'] is 'optimal':
res = +d
base.symv(C, sol['x'], res, beta = 1.0)
base.gemv(G, sol['z'], res, beta = 1.0, trans = 'T')
base.gemv(A, sol['Z'], res, beta = 1.0, trans = 'T')
print "Dual residual: %e" %blas.nrm2(res)
if m:
print "Minimum primal slack (scalar inequalities): %e" \
%min(h - G*sol['x'])
print "Minimum dual slack (scalar inequalities): %e" \
%min(sol['z'])
if p:
s = matrix(0.0, (p,1))
X = matrix(A*sol['x'], (p, q)) + B
lapack.gesvd(+X, s)
nrmX = sum(s)
lapack.gesvd(+sol['Z'], s)
nrmZ = max(s)
print "Norm of Z: %e" %nrmZ
print "Nuclear norm of A(x) + B: %e" %nrmX
print "Inner product of Z and A(x) + B: %e" \
%blas.dot(sol['Z'], X)
elif sol['status'] is 'primal infeasible':
res = matrix(0.0, (n,1))
base.gemv(G, sol['z'], res, beta = 1.0, trans = 'T')
print "Dual residual: %e" %blas.nrm2(res)
print "h' * z = %e" %blas.dot(h, sol['z'])
print "Minimum dual slack (scalar inequalities): %e" \
%min(sol['z'])
else:
pass
def checksol(sol, A, B, C=None, d=None, G=None, h=None):
"""
Check optimality conditions
C * x + G' * z + A'(Z) + d = 0
G * x <= h
z >= 0, || Z || < = 1
z' * (h - G*x) = 0
tr (Z' * (A(x) + B)) = || A(x) + B ||_*.
"""
p, q = B.size
n = A.size[1]
if G is None: G = spmatrix([], [], [], (0, n))
if h is None: h = matrix(0.0, (0, 1))
m = h.size[0]
if C is None: C = spmatrix(0.0, [], [], (n, n))
if d is None: d = matrix(0.0, (n, 1))
if sol['status'] is 'optimal':
res = +d
base.symv(C, sol['x'], res, beta=1.0)
base.gemv(G, sol['z'], res, beta=1.0, trans='T')
base.gemv(A, sol['Z'], res, beta=1.0, trans='T')
print "Dual residual: %e" % blas.nrm2(res)
if m:
print "Minimum primal slack (scalar inequalities): %e" \
%min(h - G*sol['x'])
print "Minimum dual slack (scalar inequalities): %e" \
%min(sol['z'])
if p:
s = matrix(0.0, (p, 1))
X = matrix(A * sol['x'], (p, q)) + B
lapack.gesvd(+X, s)
nrmX = sum(s)
lapack.gesvd(+sol['Z'], s)
nrmZ = max(s)
print "Norm of Z: %e" % nrmZ
print "Nuclear norm of A(x) + B: %e" % nrmX
print "Inner product of Z and A(x) + B: %e" \
%blas.dot(sol['Z'], X)
elif sol['status'] is 'primal infeasible':
res = matrix(0.0, (n, 1))
base.gemv(G, sol['z'], res, beta=1.0, trans='T')
print "Dual residual: %e" % blas.nrm2(res)
print "h' * z = %e" % blas.dot(h, sol['z'])
print "Minimum dual slack (scalar inequalities): %e" \
%min(sol['z'])
else:
pass
def F(W):
"""
Create a solver for the linear equations
C * ux + G' * uzl - 2*A'(uzs21) = bx
-uzs11 = bX1
-uzs22 = bX2
G * ux - Dl^2 * uzl = bzl
[ -uX1 -A(ux)' ] [ uzs11 uzs21' ]
[ ] - r*r' * [ ] * r*r' = bzs
[ -A(ux) -uX2 ] [ uzs21 uzs22 ]
where Dl = diag(W['l']), r = W['r'][0].
On entry, x = (bx, bX1, bX2) and z = [ bzl; bzs[:] ].
On exit, x = (ux, uX1, uX2) and z = [ Dl*uzl; (r'*uzs*r)[:] ].
1. Compute matrices V1, V2 such that (with T = r*r')
[ V1 0 ] [ T11 T21' ] [ V1' 0 ] [ I S' ]
[ ] [ ] [ ] = [ ]
[ 0 V2' ] [ T21 T22 ] [ 0 V2 ] [ S I ]
and S = [ diag(s); 0 ], s a positive q-vector.
2. Factor the mapping X -> X + S * X' * S:
X + S * X' * S = L( L'( X )).
3. Compute scaled mappings: a matrix As with as its columns the
coefficients of the scaled mapping
L^-1( V2' * A() * V1' )
and the matrix Gs = Dl^-1 * G.
4. Cholesky factorization of H = C + Gs'*Gs + 2*As'*As.
"""
# 1. Compute V1, V2, s.
r = W['r'][0]
# LQ factorization R[:q, :] = L1 * Q1.
lapack.lacpy(r, Q1, m = q)
lapack.gelqf(Q1, tau1)
lapack.lacpy(Q1, L1, n = q, uplo = 'L')
lapack.orglq(Q1, tau1)
# LQ factorization R[q:, :] = L2 * Q2.
lapack.lacpy(r, Q2, m = p, offsetA = q)
lapack.gelqf(Q2, tau2)
lapack.lacpy(Q2, L2, n = p, uplo = 'L')
lapack.orglq(Q2, tau2)
# V2, V1, s are computed from an SVD: if
#
# Q2 * Q1' = U * diag(s) * V',
#
# then V1 = V' * L1^-1 and V2 = L2^-T * U.
# T21 = Q2 * Q1.T
blas.gemm(Q2, Q1, T21, transB = 'T')
# SVD T21 = U * diag(s) * V'. Store U in V2 and V' in V1.
lapack.gesvd(T21, s, jobu = 'A', jobvt = 'A', U = V2, Vt = V1)
# # Q2 := Q2 * Q1' without extracting Q1; store T21 in Q2
# this will requires lapack.ormlq or lapack.unmlq
# V2 = L2^-T * U
blas.trsm(L2, V2, transA = 'T')
# V1 = V' * L1^-1
blas.trsm(L1, V1, side = 'R')
# 2. Factorization X + S * X' * S = L( L'( X )).
#
# The factor L is stored as a diagonal matrix D and a sparse lower
# triangular matrix P, such that
#
# L(X)[:] = D**-1 * (I + P) * X[:]
# L^-1(X)[:] = D * (I - P) * X[:].
# SS is q x q with SS[i,j] = si*sj.
blas.scal(0.0, SS)
blas.syr(s, SS)
# For a p x q matrix X, P*X[:] is Y[:] where
#
# Yij = si * sj * Xji if i < j
# = 0 otherwise.
#
P.V = SS[Itril2]
# For a p x q matrix X, D*X[:] is Y[:] where
#
# Yij = Xij / sqrt( 1 - si^2 * sj^2 ) if i < j
# = Xii / sqrt( 1 + si^2 ) if i = j
# = Xij otherwise.
#
DV[Idiag] = sqrt(1.0 + SS[::q+1])
DV[Itriu] = sqrt(1.0 - SS[Itril3]**2)
D.V = DV**-1
# 3. Scaled linear mappings
# Ask := V2' * Ask * V1'
blas.scal(0.0, As)
base.axpy(A, As)
for i in xrange(n):
# tmp := V2' * As[i, :]
blas.gemm(V2, As, tmp, transA = 'T', m = p, n = q, k = p,
ldB = p, offsetB = i*p*q)
# As[:,i] := tmp * V1'
blas.gemm(tmp, V1, As, transB = 'T', m = p, n = q, k = q,
ldC = p, offsetC = i*p*q)
# As := D * (I - P) * As
# = L^-1 * As.
blas.copy(As, As2)
base.gemm(P, As, As2, alpha = -1.0, beta = 1.0)
base.gemm(D, As2, As)
# Gs := Dl^-1 * G
blas.scal(0.0, Gs)
base.axpy(G, Gs)
for k in xrange(n):
blas.tbmv(W['di'], Gs, n = m, k = 0, ldA = 1, offsetx = k*m)
# 4. Cholesky factorization of H = C + Gs' * Gs + 2 * As' * As.
blas.syrk(As, H, trans = 'T', alpha = 2.0)
blas.syrk(Gs, H, trans = 'T', beta = 1.0)
base.axpy(C, H)
lapack.potrf(H)
def f(x, y, z):
"""
Solve
C * ux + G' * uzl - 2*A'(uzs21) = bx
-uzs11 = bX1
-uzs22 = bX2
G * ux - D^2 * uzl = bzl
[ -uX1 -A(ux)' ] [ uzs11 uzs21' ]
[ ] - T * [ ] * T = bzs.
[ -A(ux) -uX2 ] [ uzs21 uzs22 ]
On entry, x = (bx, bX1, bX2) and z = [ bzl; bzs[:] ].
On exit, x = (ux, uX1, uX2) and z = [ D*uzl; (r'*uzs*r)[:] ].
Define X = uzs21, Z = T * uzs * T:
C * ux + G' * uzl - 2*A'(X) = bx
[ 0 X' ] [ bX1 0 ]
T * [ ] * T - Z = T * [ ] * T
[ X 0 ] [ 0 bX2 ]
G * ux - D^2 * uzl = bzl
[ -uX1 -A(ux)' ] [ Z11 Z21' ]
[ ] - [ ] = bzs
[ -A(ux) -uX2 ] [ Z21 Z22 ]
Return x = (ux, uX1, uX2), z = [ D*uzl; (rti'*Z*rti)[:] ].
We use the congruence transformation
[ V1 0 ] [ T11 T21' ] [ V1' 0 ] [ I S' ]
[ ] [ ] [ ] = [ ]
[ 0 V2' ] [ T21 T22 ] [ 0 V2 ] [ S I ]
and the factorization
X + S * X' * S = L( L'(X) )
to write this as
C * ux + G' * uzl - 2*A'(X) = bx
L'(V2^-1 * X * V1^-1) - L^-1(V2' * Z21 * V1') = bX
G * ux - D^2 * uzl = bzl
[ -uX1 -A(ux)' ] [ Z11 Z21' ]
[ ] - [ ] = bzs,
[ -A(ux) -uX2 ] [ Z21 Z22 ]
or
C * ux + Gs' * uuzl - 2*As'(XX) = bx
XX - ZZ21 = bX
Gs * ux - uuzl = D^-1 * bzl
-As(ux) - ZZ21 = bbzs_21
-uX1 - Z11 = bzs_11
-uX2 - Z22 = bzs_22
if we introduce scaled variables
uuzl = D * uzl
XX = L'(V2^-1 * X * V1^-1)
= L'(V2^-1 * uzs21 * V1^-1)
ZZ21 = L^-1(V2' * Z21 * V1')
and define
bbzs_21 = L^-1(V2' * bzs_21 * V1')
[ bX1 0 ]
bX = L^-1( V2' * (T * [ ] * T)_21 * V1').
[ 0 bX2 ]
Eliminating Z21 gives
C * ux + Gs' * uuzl - 2*As'(XX) = bx
Gs * ux - uuzl = D^-1 * bzl
-As(ux) - XX = bbzs_21 - bX
-uX1 - Z11 = bzs_11
-uX2 - Z22 = bzs_22
and eliminating uuzl and XX gives
H * ux = bx + Gs' * D^-1 * bzl + 2*As'(bX - bbzs_21)
Gs * ux - uuzl = D^-1 * bzl
-As(ux) - XX = bbzs_21 - bX
-uX1 - Z11 = bzs_11
-uX2 - Z22 = bzs_22.
In summary, we can use the following algorithm:
1. bXX := bX - bbzs21
[ bX1 0 ]
= L^-1( V2' * ((T * [ ] * T)_21 - bzs_21) * V1')
[ 0 bX2 ]
2. Solve H * ux = bx + Gs' * D^-1 * bzl + 2*As'(bXX).
3. From ux, compute
uuzl = Gs*ux - D^-1 * bzl and
X = V2 * L^-T(-As(ux) + bXX) * V1.
4. Return ux, uuzl,
rti' * Z * rti = r' * [ -bX1, X'; X, -bX2 ] * r
and uX1 = -Z11 - bzs_11, uX2 = -Z22 - bzs_22.
"""
# Save bzs_11, bzs_22, bzs_21.
lapack.lacpy(z, bz11, uplo = 'L', m = q, n = q, ldA = p+q,
offsetA = m)
lapack.lacpy(z, bz21, m = p, n = q, ldA = p+q, offsetA = m+q)
lapack.lacpy(z, bz22, uplo = 'L', m = p, n = p, ldA = p+q,
offsetA = m + (p+q+1)*q)
# zl := D^-1 * zl
# = D^-1 * bzl
blas.tbmv(W['di'], z, n = m, k = 0, ldA = 1)
# zs := r' * [ bX1, 0; 0, bX2 ] * r.
# zs := [ bX1, 0; 0, bX2 ]
blas.scal(0.0, z, offset = m)
lapack.lacpy(x[1], z, uplo = 'L', m = q, n = q, ldB = p+q,
offsetB = m)
lapack.lacpy(x[2], z, uplo = 'L', m = p, n = p, ldB = p+q,
offsetB = m + (p+q+1)*q)
# scale diagonal of zs by 1/2
blas.scal(0.5, z, inc = p+q+1, offset = m)
# a := tril(zs)*r
blas.copy(r, a)
blas.trmm(z, a, side = 'L', m = p+q, n = p+q, ldA = p+q, ldB =
p+q, offsetA = m)
# zs := a'*r + r'*a
blas.syr2k(r, a, z, trans = 'T', n = p+q, k = p+q, ldB = p+q,
ldC = p+q, offsetC = m)
# bz21 := L^-1( V2' * ((r * zs * r')_21 - bz21) * V1')
#
# [ bX1 0 ]
# = L^-1( V2' * ((T * [ ] * T)_21 - bz21) * V1').
# [ 0 bX2 ]
# a = [ r21 r22 ] * z
# = [ r21 r22 ] * r' * [ bX1, 0; 0, bX2 ] * r
# = [ T21 T22 ] * [ bX1, 0; 0, bX2 ] * r
blas.symm(z, r, a, side = 'R', m = p, n = p+q, ldA = p+q,
ldC = p+q, offsetB = q)
# bz21 := -bz21 + a * [ r11, r12 ]'
# = -bz21 + (T * [ bX1, 0; 0, bX2 ] * T)_21
blas.gemm(a, r, bz21, transB = 'T', m = p, n = q, k = p+q,
beta = -1.0, ldA = p+q, ldC = p)
# bz21 := V2' * bz21 * V1'
# = V2' * (-bz21 + (T*[bX1, 0; 0, bX2]*T)_21) * V1'
blas.gemm(V2, bz21, tmp, transA = 'T', m = p, n = q, k = p,
ldB = p)
blas.gemm(tmp, V1, bz21, transB = 'T', m = p, n = q, k = q,
ldC = p)
# bz21[:] := D * (I-P) * bz21[:]
# = L^-1 * bz21[:]
# = bXX[:]
blas.copy(bz21, tmp)
base.gemv(P, bz21, tmp, alpha = -1.0, beta = 1.0)
base.gemv(D, tmp, bz21)
# Solve H * ux = bx + Gs' * D^-1 * bzl + 2*As'(bXX).
# x[0] := x[0] + Gs'*zl + 2*As'(bz21)
# = bx + G' * D^-1 * bzl + 2 * As'(bXX)
blas.gemv(Gs, z, x[0], trans = 'T', alpha = 1.0, beta = 1.0)
blas.gemv(As, bz21, x[0], trans = 'T', alpha = 2.0, beta = 1.0)
# x[0] := H \ x[0]
# = ux
lapack.potrs(H, x[0])
# uuzl = Gs*ux - D^-1 * bzl
blas.gemv(Gs, x[0], z, alpha = 1.0, beta = -1.0)
# bz21 := V2 * L^-T(-As(ux) + bz21) * V1
# = X
blas.gemv(As, x[0], bz21, alpha = -1.0, beta = 1.0)
blas.tbsv(DV, bz21, n = p*q, k = 0, ldA = 1)
blas.copy(bz21, tmp)
base.gemv(P, tmp, bz21, alpha = -1.0, beta = 1.0, trans = 'T')
blas.gemm(V2, bz21, tmp)
blas.gemm(tmp, V1, bz21)
# zs := -zs + r' * [ 0, X'; X, 0 ] * r
# = r' * [ -bX1, X'; X, -bX2 ] * r.
# a := bz21 * [ r11, r12 ]
# = X * [ r11, r12 ]
blas.gemm(bz21, r, a, m = p, n = p+q, k = q, ldA = p, ldC = p+q)
# z := -z + [ r21, r22 ]' * a + a' * [ r21, r22 ]
# = rti' * uzs * rti
blas.syr2k(r, a, z, trans = 'T', beta = -1.0, n = p+q, k = p,
offsetA = q, offsetC = m, ldB = p+q, ldC = p+q)
# uX1 = -Z11 - bzs_11
# = -(r*zs*r')_11 - bzs_11
# uX2 = -Z22 - bzs_22
# = -(r*zs*r')_22 - bzs_22
blas.copy(bz11, x[1])
blas.copy(bz22, x[2])
# scale diagonal of zs by 1/2
blas.scal(0.5, z, inc = p+q+1, offset = m)
# a := r*tril(zs)
blas.copy(r, a)
blas.trmm(z, a, side = 'R', m = p+q, n = p+q, ldA = p+q, ldB =
p+q, offsetA = m)
# x[1] := -x[1] - a[:q,:] * r[:q, :]' - r[:q,:] * a[:q,:]'
# = -bzs_11 - (r*zs*r')_11
blas.syr2k(a, r, x[1], n = q, alpha = -1.0, beta = -1.0)
# x[2] := -x[2] - a[q:,:] * r[q:, :]' - r[q:,:] * a[q:,:]'
# = -bzs_22 - (r*zs*r')_22
blas.syr2k(a, r, x[2], n = p, alpha = -1.0, beta = -1.0,
offsetA = q, offsetB = q)
# scale diagonal of zs by 1/2
blas.scal(2.0, z, inc = p+q+1, offset = m)
return f
def construct_Ab(constr_list,mapping,n,options):
"""
Description
-----------
Constructs matrix A and vector b from a list
of equality constraints. The program is assumed
to be expanded and transformed so all the
equality constraints are affine.
Arguments
---------
constr_list: cvxpy_list of equality constraints.
Mapping: Dictionary mapping variable to index.
n: Number of variables.
"""
# Create matrices
A = opt.spmatrix(0.0,[],[],(0,n))
b = opt.matrix(0.0,(0,1))
H_top = opt.sparse([[A],[b]])
rank = 0
counter = 1
for constr in constr_list:
# Get elements
left = constr.left
op = constr.op
right = constr.right
# New rows
A_row = opt.spmatrix(0.0,[],[],(1,n))
b_row = opt.matrix(0.0,(1,1))
# Deal with right element
if(right.type == CONSTANT):
b_row[0] += right.data*1.0
elif(right.type == VARIABLE):
A_row[0,mapping[right]] += -1.0
else:
raise ValueError('Bad equality: Cannot construct A')
# Left is a variable
if(left.type == VARIABLE):
A_row[0,mapping[left]] += 1.0
# Left is a constant
elif(left.type == CONSTANT):
b_row[0] += -left.data*1.0
# Left is an operation tree
elif(left.type == TREE and
left.item.type == OPERATOR):
# Get operator name
name = left.item.name
# Addition
if(name == '+'):
for arg in left.children:
if(arg.type == TREE and
arg.item.name == '*'):
ch1 = arg.children[1]
ch0 = arg.children[0]
A_row[0,mapping[ch1]] += ch0.data
elif(arg.type == VARIABLE):
A_row[0,mapping[arg]] += 1.0
elif(arg.type == CONSTANT):
b_row[0] += -arg.data*1.0
else:
raise ValueError('Bad equality: Cannot construct A')
# Multiplication
elif(name == '*'):
# Constant
op1 = left.children[0]
# Variable
op2 = left.children[1]
# Process
A_row[0,mapping[op2]] += 1.0*op1.data
# Error
else:
raise ValueError('Bad equality: Cannot construct A')
# Error
else:
raise ValueError('Bad equality: Cannot construct A')
# Rank check
if(options['check redundancy']):
H_botton = opt.sparse([[A_row],[b_row]])
H = opt.sparse([H_top,H_botton])
svd = opt.matrix(0.0,(1,np.min([H.size[0],H.size[1]])))
lapack.gesvd(opt.matrix(H),svd)
new_rank = len([x for x in svd if x > 1e-8])
if(new_rank > rank):
# Append
A = opt.sparse([A,A_row])
b = opt.matrix([b,b_row])
# Update rank and H_top
rank = new_rank
H_top = H
else:
if(options['show steps'] and not quiet):
print 'Redundant equality eliminated [',counter,']'
counter += 1
else:
# Append
A = opt.sparse([A,A_row])
b = opt.matrix([b,b_row])
# Return matrices
return A,b
def F(W):
"""
Create a solver for the linear equations
C * ux + G' * uzl - 2*A'(uzs21) = bx
-uzs11 = bX1
-uzs22 = bX2
G * ux - Dl^2 * uzl = bzl
[ -uX1 -A(ux)' ] [ uzs11 uzs21' ]
[ ] - r*r' * [ ] * r*r' = bzs
[ -A(ux) -uX2 ] [ uzs21 uzs22 ]
where Dl = diag(W['l']), r = W['r'][0].
On entry, x = (bx, bX1, bX2) and z = [ bzl; bzs[:] ].
On exit, x = (ux, uX1, uX2) and z = [ Dl*uzl; (r'*uzs*r)[:] ].
1. Compute matrices V1, V2 such that (with T = r*r')
[ V1 0 ] [ T11 T21' ] [ V1' 0 ] [ I S' ]
[ ] [ ] [ ] = [ ]
[ 0 V2' ] [ T21 T22 ] [ 0 V2 ] [ S I ]
and S = [ diag(s); 0 ], s a positive q-vector.
2. Factor the mapping X -> X + S * X' * S:
X + S * X' * S = L( L'( X )).
3. Compute scaled mappings: a matrix As with as its columns the
coefficients of the scaled mapping
L^-1( V2' * A() * V1' )
and the matrix Gs = Dl^-1 * G.
4. Cholesky factorization of H = C + Gs'*Gs + 2*As'*As.
"""
# 1. Compute V1, V2, s.
r = W['r'][0]
# LQ factorization R[:q, :] = L1 * Q1.
lapack.lacpy(r, Q1, m=q)
lapack.gelqf(Q1, tau1)
lapack.lacpy(Q1, L1, n=q, uplo='L')
lapack.orglq(Q1, tau1)
# LQ factorization R[q:, :] = L2 * Q2.
lapack.lacpy(r, Q2, m=p, offsetA=q)
lapack.gelqf(Q2, tau2)
lapack.lacpy(Q2, L2, n=p, uplo='L')
lapack.orglq(Q2, tau2)
# V2, V1, s are computed from an SVD: if
#
# Q2 * Q1' = U * diag(s) * V',
#
# then V1 = V' * L1^-1 and V2 = L2^-T * U.
# T21 = Q2 * Q1.T
blas.gemm(Q2, Q1, T21, transB='T')
# SVD T21 = U * diag(s) * V'. Store U in V2 and V' in V1.
lapack.gesvd(T21, s, jobu='A', jobvt='A', U=V2, Vt=V1)
# # Q2 := Q2 * Q1' without extracting Q1; store T21 in Q2
# this will requires lapack.ormlq or lapack.unmlq
# V2 = L2^-T * U
blas.trsm(L2, V2, transA='T')
# V1 = V' * L1^-1
blas.trsm(L1, V1, side='R')
# 2. Factorization X + S * X' * S = L( L'( X )).
#
# The factor L is stored as a diagonal matrix D and a sparse lower
# triangular matrix P, such that
#
# L(X)[:] = D**-1 * (I + P) * X[:]
# L^-1(X)[:] = D * (I - P) * X[:].
# SS is q x q with SS[i,j] = si*sj.
blas.scal(0.0, SS)
blas.syr(s, SS)
# For a p x q matrix X, P*X[:] is Y[:] where
#
# Yij = si * sj * Xji if i < j
# = 0 otherwise.
#
P.V = SS[Itril2]
# For a p x q matrix X, D*X[:] is Y[:] where
#
# Yij = Xij / sqrt( 1 - si^2 * sj^2 ) if i < j
# = Xii / sqrt( 1 + si^2 ) if i = j
# = Xij otherwise.
#
DV[Idiag] = sqrt(1.0 + SS[::q + 1])
DV[Itriu] = sqrt(1.0 - SS[Itril3]**2)
D.V = DV**-1
# 3. Scaled linear mappings
# Ask := V2' * Ask * V1'
blas.scal(0.0, As)
base.axpy(A, As)
for i in xrange(n):
# tmp := V2' * As[i, :]
blas.gemm(V2,
As,
tmp,
transA='T',
m=p,
n=q,
k=p,
ldB=p,
offsetB=i * p * q)
# As[:,i] := tmp * V1'
blas.gemm(tmp,
V1,
As,
transB='T',
m=p,
n=q,
k=q,
ldC=p,
offsetC=i * p * q)
# As := D * (I - P) * As
# = L^-1 * As.
blas.copy(As, As2)
base.gemm(P, As, As2, alpha=-1.0, beta=1.0)
base.gemm(D, As2, As)
# Gs := Dl^-1 * G
blas.scal(0.0, Gs)
base.axpy(G, Gs)
for k in xrange(n):
blas.tbmv(W['di'], Gs, n=m, k=0, ldA=1, offsetx=k * m)
# 4. Cholesky factorization of H = C + Gs' * Gs + 2 * As' * As.
blas.syrk(As, H, trans='T', alpha=2.0)
blas.syrk(Gs, H, trans='T', beta=1.0)
base.axpy(C, H)
lapack.potrf(H)
def f(x, y, z):
"""
Solve
C * ux + G' * uzl - 2*A'(uzs21) = bx
-uzs11 = bX1
-uzs22 = bX2
G * ux - D^2 * uzl = bzl
[ -uX1 -A(ux)' ] [ uzs11 uzs21' ]
[ ] - T * [ ] * T = bzs.
[ -A(ux) -uX2 ] [ uzs21 uzs22 ]
On entry, x = (bx, bX1, bX2) and z = [ bzl; bzs[:] ].
On exit, x = (ux, uX1, uX2) and z = [ D*uzl; (r'*uzs*r)[:] ].
Define X = uzs21, Z = T * uzs * T:
C * ux + G' * uzl - 2*A'(X) = bx
[ 0 X' ] [ bX1 0 ]
T * [ ] * T - Z = T * [ ] * T
[ X 0 ] [ 0 bX2 ]
G * ux - D^2 * uzl = bzl
[ -uX1 -A(ux)' ] [ Z11 Z21' ]
[ ] - [ ] = bzs
[ -A(ux) -uX2 ] [ Z21 Z22 ]
Return x = (ux, uX1, uX2), z = [ D*uzl; (rti'*Z*rti)[:] ].
We use the congruence transformation
[ V1 0 ] [ T11 T21' ] [ V1' 0 ] [ I S' ]
[ ] [ ] [ ] = [ ]
[ 0 V2' ] [ T21 T22 ] [ 0 V2 ] [ S I ]
and the factorization
X + S * X' * S = L( L'(X) )
to write this as
C * ux + G' * uzl - 2*A'(X) = bx
L'(V2^-1 * X * V1^-1) - L^-1(V2' * Z21 * V1') = bX
G * ux - D^2 * uzl = bzl
[ -uX1 -A(ux)' ] [ Z11 Z21' ]
[ ] - [ ] = bzs,
[ -A(ux) -uX2 ] [ Z21 Z22 ]
or
C * ux + Gs' * uuzl - 2*As'(XX) = bx
XX - ZZ21 = bX
Gs * ux - uuzl = D^-1 * bzl
-As(ux) - ZZ21 = bbzs_21
-uX1 - Z11 = bzs_11
-uX2 - Z22 = bzs_22
if we introduce scaled variables
uuzl = D * uzl
XX = L'(V2^-1 * X * V1^-1)
= L'(V2^-1 * uzs21 * V1^-1)
ZZ21 = L^-1(V2' * Z21 * V1')
and define
bbzs_21 = L^-1(V2' * bzs_21 * V1')
[ bX1 0 ]
bX = L^-1( V2' * (T * [ ] * T)_21 * V1').
[ 0 bX2 ]
Eliminating Z21 gives
C * ux + Gs' * uuzl - 2*As'(XX) = bx
Gs * ux - uuzl = D^-1 * bzl
-As(ux) - XX = bbzs_21 - bX
-uX1 - Z11 = bzs_11
-uX2 - Z22 = bzs_22
and eliminating uuzl and XX gives
H * ux = bx + Gs' * D^-1 * bzl + 2*As'(bX - bbzs_21)
Gs * ux - uuzl = D^-1 * bzl
-As(ux) - XX = bbzs_21 - bX
-uX1 - Z11 = bzs_11
-uX2 - Z22 = bzs_22.
In summary, we can use the following algorithm:
1. bXX := bX - bbzs21
[ bX1 0 ]
= L^-1( V2' * ((T * [ ] * T)_21 - bzs_21) * V1')
[ 0 bX2 ]
2. Solve H * ux = bx + Gs' * D^-1 * bzl + 2*As'(bXX).
3. From ux, compute
uuzl = Gs*ux - D^-1 * bzl and
X = V2 * L^-T(-As(ux) + bXX) * V1.
4. Return ux, uuzl,
rti' * Z * rti = r' * [ -bX1, X'; X, -bX2 ] * r
and uX1 = -Z11 - bzs_11, uX2 = -Z22 - bzs_22.
"""
# Save bzs_11, bzs_22, bzs_21.
lapack.lacpy(z, bz11, uplo='L', m=q, n=q, ldA=p + q, offsetA=m)
lapack.lacpy(z, bz21, m=p, n=q, ldA=p + q, offsetA=m + q)
lapack.lacpy(z,
bz22,
uplo='L',
m=p,
n=p,
ldA=p + q,
offsetA=m + (p + q + 1) * q)
# zl := D^-1 * zl
# = D^-1 * bzl
blas.tbmv(W['di'], z, n=m, k=0, ldA=1)
# zs := r' * [ bX1, 0; 0, bX2 ] * r.
# zs := [ bX1, 0; 0, bX2 ]
blas.scal(0.0, z, offset=m)
lapack.lacpy(x[1], z, uplo='L', m=q, n=q, ldB=p + q, offsetB=m)
lapack.lacpy(x[2],
z,
uplo='L',
m=p,
n=p,
ldB=p + q,
offsetB=m + (p + q + 1) * q)
# scale diagonal of zs by 1/2
blas.scal(0.5, z, inc=p + q + 1, offset=m)
# a := tril(zs)*r
blas.copy(r, a)
blas.trmm(z,
a,
side='L',
m=p + q,
n=p + q,
ldA=p + q,
ldB=p + q,
offsetA=m)
# zs := a'*r + r'*a
blas.syr2k(r,
a,
z,
trans='T',
n=p + q,
k=p + q,
ldB=p + q,
ldC=p + q,
offsetC=m)
# bz21 := L^-1( V2' * ((r * zs * r')_21 - bz21) * V1')
#
# [ bX1 0 ]
# = L^-1( V2' * ((T * [ ] * T)_21 - bz21) * V1').
# [ 0 bX2 ]
# a = [ r21 r22 ] * z
# = [ r21 r22 ] * r' * [ bX1, 0; 0, bX2 ] * r
# = [ T21 T22 ] * [ bX1, 0; 0, bX2 ] * r
blas.symm(z,
r,
a,
side='R',
m=p,
n=p + q,
ldA=p + q,
ldC=p + q,
offsetB=q)
# bz21 := -bz21 + a * [ r11, r12 ]'
# = -bz21 + (T * [ bX1, 0; 0, bX2 ] * T)_21
blas.gemm(a,
r,
bz21,
transB='T',
m=p,
n=q,
k=p + q,
beta=-1.0,
ldA=p + q,
ldC=p)
# bz21 := V2' * bz21 * V1'
# = V2' * (-bz21 + (T*[bX1, 0; 0, bX2]*T)_21) * V1'
blas.gemm(V2, bz21, tmp, transA='T', m=p, n=q, k=p, ldB=p)
blas.gemm(tmp, V1, bz21, transB='T', m=p, n=q, k=q, ldC=p)
# bz21[:] := D * (I-P) * bz21[:]
# = L^-1 * bz21[:]
# = bXX[:]
blas.copy(bz21, tmp)
base.gemv(P, bz21, tmp, alpha=-1.0, beta=1.0)
base.gemv(D, tmp, bz21)
# Solve H * ux = bx + Gs' * D^-1 * bzl + 2*As'(bXX).
# x[0] := x[0] + Gs'*zl + 2*As'(bz21)
# = bx + G' * D^-1 * bzl + 2 * As'(bXX)
blas.gemv(Gs, z, x[0], trans='T', alpha=1.0, beta=1.0)
blas.gemv(As, bz21, x[0], trans='T', alpha=2.0, beta=1.0)
# x[0] := H \ x[0]
# = ux
lapack.potrs(H, x[0])
# uuzl = Gs*ux - D^-1 * bzl
blas.gemv(Gs, x[0], z, alpha=1.0, beta=-1.0)
# bz21 := V2 * L^-T(-As(ux) + bz21) * V1
# = X
blas.gemv(As, x[0], bz21, alpha=-1.0, beta=1.0)
blas.tbsv(DV, bz21, n=p * q, k=0, ldA=1)
blas.copy(bz21, tmp)
base.gemv(P, tmp, bz21, alpha=-1.0, beta=1.0, trans='T')
blas.gemm(V2, bz21, tmp)
blas.gemm(tmp, V1, bz21)
# zs := -zs + r' * [ 0, X'; X, 0 ] * r
# = r' * [ -bX1, X'; X, -bX2 ] * r.
# a := bz21 * [ r11, r12 ]
# = X * [ r11, r12 ]
blas.gemm(bz21, r, a, m=p, n=p + q, k=q, ldA=p, ldC=p + q)
# z := -z + [ r21, r22 ]' * a + a' * [ r21, r22 ]
# = rti' * uzs * rti
blas.syr2k(r,
a,
z,
trans='T',
beta=-1.0,
n=p + q,
k=p,
offsetA=q,
offsetC=m,
ldB=p + q,
ldC=p + q)
# uX1 = -Z11 - bzs_11
# = -(r*zs*r')_11 - bzs_11
# uX2 = -Z22 - bzs_22
# = -(r*zs*r')_22 - bzs_22
blas.copy(bz11, x[1])
blas.copy(bz22, x[2])
# scale diagonal of zs by 1/2
blas.scal(0.5, z, inc=p + q + 1, offset=m)
# a := r*tril(zs)
blas.copy(r, a)
blas.trmm(z,
a,
side='R',
m=p + q,
n=p + q,
ldA=p + q,
ldB=p + q,
offsetA=m)
# x[1] := -x[1] - a[:q,:] * r[:q, :]' - r[:q,:] * a[:q,:]'
# = -bzs_11 - (r*zs*r')_11
blas.syr2k(a, r, x[1], n=q, alpha=-1.0, beta=-1.0)
# x[2] := -x[2] - a[q:,:] * r[q:, :]' - r[q:,:] * a[q:,:]'
# = -bzs_22 - (r*zs*r')_22
blas.syr2k(a,
r,
x[2],
n=p,
alpha=-1.0,
beta=-1.0,
offsetA=q,
offsetB=q)
# scale diagonal of zs by 1/2
blas.scal(2.0, z, inc=p + q + 1, offset=m)
return f
def sysid(y, u, vsig, svth=None):
"""
System identification using the subspace method and nuclear norm
optimization. Estimate a linear time-invariant state-space model
given inputs and outputs. The algorithm is described in [1].
INPUT
y 'd' matrix of size (p, N). y are the measured outputs, p is
the number of outputs, and N is the number of data points
measured.
u 'd' matrix of size (m, N). u are the inputs, m is the number
of inputs, and N is the number of data points.
vsig a weighting parameter in the nuclear norm optimization, its
value is approximately the 1-sigma output noise level
svth an optional parameter, if specified, the model order is
determined as the number of singular values greater than svth
times the maximum singular value. The default value is 1E-3
OUTPUT
sol a dictionary with the following words
-- 'A', 'B', 'C', 'D' are the state-space matrices
-- 'svN', the original singular values of the Hankel matrix
-- 'sv', the optimized singular values of the Hankel matrix
-- 'x0', the initial state x(0)
-- 'n', the model order
[1] Zhang Liu and Lieven Vandenberghe. "Interior-point method for
nuclear norm approximation with application to system
identification."
"""
m, N, p = u.size[0], u.size[1], y.size[0]
if y.size[1] != N:
raise ValueError, "y and u must have the same length"
# Y = G*X + H*U + V, Y has size a x b, U has size c x b, Un has b x d
r = min(int(30 / p), int((N + 1.0) / (p + m + 1) + 1.0))
a = r * p
c = r * m
b = N - r + 1
d = b - c
# construct Hankel matrix Y
Y = Hankel(y, r, b, p=p, q=1)
# construct Hankel matrix U
U = Hankel(u, r, b, p=m, q=1)
# compute Un = null(U) and YUn = Y*Un
Vt = matrix(0.0, (b, b))
Stemp = matrix(0.0, (c, 1))
Un = matrix(0.0, (b, d))
YUn = matrix(0.0, (a, d))
lapack.gesvd(U, Stemp, jobvt='A', Vt=Vt)
Un[:, :] = Vt.T[:, c:]
blas.gemm(Y, Un, YUn)
# compute original singular values
svN = matrix(0.0, (min(a, d), 1))
lapack.gesvd(YUn, svN)
# variable, [y(1);...;y(N)]
# form the coefficient matrices for the nuclear norm optimization
# minimize | Yh * Un |_* + alpha * | y - yh |_F
AA = Hankel_basis(r, b, p=p, q=1)
A = matrix(0.0, (a * d, p * N))
temp = spmatrix([], [], [], (a, b), 'd')
temp2 = matrix(0.0, (a, d))
for ii in xrange(p * N):
temp[:] = AA[:, ii]
base.gemm(temp, Un, temp2)
A[:, ii] = temp2[:]
B = matrix(0.0, (a, d))
# flip the matrix if columns is more than rows
if a < d:
Itrans = [i + j * a for i in xrange(a) for j in xrange(d)]
B[:] = B[Itrans]
B.size = (d, a)
for ii in xrange(p * N):
A[:, ii] = A[Itrans, ii]
# regularized term
x0 = y[:]
Qd = matrix(2.0 * svN[0] / p / N / (vsig**2), (p * N, 1))
# solve the nuclear norm optimization
sol = nrmapp(A, B, C=base.spdiag(Qd), d=-base.mul(x0, Qd))
status = sol['status']
x = sol['x']
# construct YhUn and take the svd
YhUn = matrix(B)
blas.gemv(A, x, YhUn, beta=1.0)
if a < d:
YhUn = YhUn.T
Uh = matrix(0.0, (a, d))
sv = matrix(0.0, (d, 1))
lapack.gesvd(YhUn, sv, jobu='S', U=Uh)
# determine model order
if svth is None:
svth = 1E-3
svthn = sv[0] * svth
n = 1
while sv[n] >= svthn and n < 10:
n = n + 1
# estimate A, C
Uhn = Uh[:, :n]
for ii in xrange(n):
blas.scal(sv[ii], Uhn, n=a, offset=ii * a)
syseC = Uhn[:p, :]
Als = Uhn[:-p, :]
Bls = Uhn[p:, :]
lapack.gels(Als, Bls)
syseA = Bls[:n, :]
Als[:, :] = Uhn[:-p, :]
Bls[:, :] = Uhn[p:, :]
blas.gemm(Als, syseA, Bls, beta=-1.0)
Aerr = blas.nrm2(Bls)
# stabilize A
Sc = matrix(0.0, (n, n), 'z')
w = matrix(0.0, (n, 1), 'z')
Vs = matrix(0.0, (n, n), 'z')
def F(w):
return (abs(w) < 1.0)
Sc[:, :] = syseA
ns = lapack.gees(Sc, w, Vs, select=F)
while ns < n:
#print "stabilize matrix A"
w[ns:] = w[ns:]**-1
Sc[::n + 1] = w
Sc = Vs * Sc * Vs.H
syseA[:, :] = Sc.real()
Sc[:, :] = syseA
ns = lapack.gees(Sc, w, Vs, select=F)
# estimate B,D,x0 stored in vector [x0; vec(D); vec(B)]
F1 = matrix(0.0, (p * N, n))
F1[:p, :] = syseC
for ii in xrange(1, N):
F1[ii * p:(ii + 1) * p, :] = F1[(ii - 1) * p:ii * p, :] * syseA
F2 = matrix(0.0, (p * N, p * m))
ut = u.T
for ii in xrange(p):
F2[ii::p, ii::p] = ut
F3 = matrix(0.0, (p * N, n * m))
F3t = matrix(0.0, (p * (N - 1), n * m))
for ii in xrange(1, N):
for jj in xrange(p):
for kk in xrange(n):
F3t[jj:jj + (N - ii) * p:p,
kk::n] = ut[:N - ii, :] * F1[(ii - 1) * p + jj, kk]
F3[ii * p:, :] = F3[ii * p:, :] + F3t[:(N - ii) * p, :]
F = matrix([[F1], [F2], [F3]])
yls = y[:]
Sls = matrix(0.0, (F.size[1], 1))
Uls = matrix(0.0, (F.size[0], F.size[1]))
Vtls = matrix(0.0, (F.size[1], F.size[1]))
lapack.gesvd(F, Sls, jobu='S', jobvt='S', U=Uls, Vt=Vtls)
Frank = len([ii for ii in xrange(Sls.size[0]) if Sls[ii] >= 1E-6])
#print 'Rank deficiency = ', F.size[1] - Frank
xx = matrix(0.0, (F.size[1], 1))
xx[:Frank] = Uls.T[:Frank, :] * yls
xx[:Frank] = base.mul(xx[:Frank], Sls[:Frank]**-1)
xx[:] = Vtls.T[:, :Frank] * xx[:Frank]
blas.gemv(F, xx, yls, beta=-1.0)
xxerr = blas.nrm2(yls)
x0 = xx[:n]
syseD = xx[n:n + p * m]
syseD.size = (p, m)
syseB = xx[n + p * m:]
syseB.size = (n, m)
return {'A': syseA, 'B': syseB, 'C': syseC, 'D': syseD, 'svN': svN, 'sv': \
sv, 'x0': x0, 'n': n, 'Aerr': Aerr, 'xxerr': xxerr}
# with n variables, and matrices A(x), B of size p x q. setseed(0) p, q, n = 100, 100, 100 A = normal(p*q, n) B = normal(p, q) # options['feastol'] = 1e-6 # options['refinement'] = 3 sol = nucnrm.nrmapp(A, B) x = sol['x'] Z = sol['Z'] s = matrix(0.0, (p,1)) X = matrix(A *x, (p, q)) + B lapack.gesvd(+X, s) nrmX = sum(s) lapack.gesvd(+Z, s) nrmZ = max(s) res = matrix(0.0, (n, 1)) blas.gemv(A, Z, res, beta = 1.0, trans = 'T') print "\nNuclear norm of A(x) + B: %e" %nrmX print "Inner product of B and Z: %e" %blas.dot(B, Z) print "Maximum singular value of Z: %e" %nrmZ print "Euclidean norm of A'(Z): %e" %blas.nrm2(res)
def mrcompletion(A, reordered=True):
"""
Minimum rank positive semidefinite completion. The routine takes a
positive semidefinite cspmatrix :math:`A` and returns a dense
matrix :math:`Y` with :math:`r` columns that satisfies
.. math::
P( YY^T ) = A
where
.. math::
r = \max_{i} |\gamma_i|
is the clique number (the size of the largest clique).
:param A: :py:class:`cspmatrix`
:param reordered: boolean
"""
assert isinstance(A, cspmatrix) and A.is_factor is False, "A must be a cspmatrix"
symb = A.symb
n = symb.n
snpost = symb.snpost
snptr = symb.snptr
chptr = symb.chptr
chidx = symb.chidx
relptr = symb.relptr
relidx = symb.relidx
blkptr = symb.blkptr
blkval = A.blkval
stack = []
r = 0
maxr = symb.clique_number
Y = matrix(0.0,(n,maxr)) # storage for factorization
Z = matrix(0.0,(maxr,maxr)) # storage for EVD of cliques
w = matrix(0.0,(maxr,1)) # storage for EVD of cliques
P = matrix(0.0,(maxr,maxr)) # storage for left singular vectors
Q1t = matrix(0.0,(maxr,maxr)) # storage for right singular vectors (1)
Q2t = matrix(0.0,(maxr,maxr)) # storage for right singular vectors (2)
S = matrix(0.0,(maxr,1)) # storage for singular values
V = matrix(0.0,(maxr,maxr))
Ya = matrix(0.0,(maxr,maxr))
# visit supernodes in reverse topological order
for k in range(symb.Nsn-1,-1,-1):
nn = snptr[k+1]-snptr[k] # |Nk|
na = relptr[k+1]-relptr[k] # |Ak|
nj = na + nn
# allocate F and copy X_{Jk,Nk} to leading columns of F
F = matrix(0.0, (nj,nj))
lapack.lacpy(blkval, F, offsetA = blkptr[k], ldA = nj, m = nj, n = nn, uplo = 'L')
# if supernode k is not a root node:
if na > 0:
# copy Vk to 2,2 block of F
Vk = stack.pop()
lapack.lacpy(Vk, F, offsetB = nn*nj+nn, m = na, n = na, uplo = 'L')
# if supernode k has any children:
for ii in range(chptr[k],chptr[k+1]):
stack.append(frontal_get_update(F,relidx,relptr,chidx[ii]))
# Compute factorization of F
lapack.syevr(F, w, jobz='V', range='A', uplo='L', Z=Z, n=nj,ldZ=maxr)
rk = sum([1 for wi in w[:nj] if wi > 1e-14*w[nj-1]]) # determine rank of clique k
r = max(rk,r) # update rank
# Scale last rk cols of Z and copy parts to Yn
for j in range(nj-rk,nj):
Z[:nj,j] *= sqrt(w[j])
In = symb.snrowidx[symb.sncolptr[k]:symb.sncolptr[k]+nn]
Y[In,:rk] = Z[:nn,nj-rk:nj]
# if supernode k is not a root node:
if na > 0:
# Extract data
Ia = symb.snrowidx[symb.sncolptr[k]+nn:symb.sncolptr[k+1]]
Ya[:na,:r] = Y[Ia,:r]
V[:na,:rk] = Z[nn:nj,nj-rk:nj]
V[:na,rk:r] *= 0.0
# Compute SVDs: V = P*S*Q1t and Ya = P*S*Q2t
lapack.gesvd(V,S,jobu='A',jobvt='A',U=P,Vt=Q1t,ldU=maxr,ldVt=maxr,m=na,n=r,ldA=maxr)
lapack.gesvd(Ya,S,jobu='N',jobvt='A',Vt=Q2t,ldVt=maxr,m=na,n=r,ldA=maxr)
# Scale Q2t
for i in range(min(na,rk)):
if S[i] > 1e-14*S[0]: Q2t[i,:r] = P[:na,i].T*Y[Ia,:r]/S[i]
# Scale Yn
Y[In,:r] = Y[In,:r]*Q1t[:r,:r].T*Q2t[:r,:r]
if reordered:
return Y[:,:r]
else:
return Y[symb.ip,:r]
def sysid(y, u, vsig, svth = None):
"""
System identification using the subspace method and nuclear norm
optimization. Estimate a linear time-invariant state-space model
given inputs and outputs. The algorithm is described in [1].
INPUT
y 'd' matrix of size (p, N). y are the measured outputs, p is
the number of outputs, and N is the number of data points
measured.
u 'd' matrix of size (m, N). u are the inputs, m is the number
of inputs, and N is the number of data points.
vsig a weighting parameter in the nuclear norm optimization, its
value is approximately the 1-sigma output noise level
svth an optional parameter, if specified, the model order is
determined as the number of singular values greater than svth
times the maximum singular value. The default value is 1E-3
OUTPUT
sol a dictionary with the following words
-- 'A', 'B', 'C', 'D' are the state-space matrices
-- 'svN', the original singular values of the Hankel matrix
-- 'sv', the optimized singular values of the Hankel matrix
-- 'x0', the initial state x(0)
-- 'n', the model order
[1] Zhang Liu and Lieven Vandenberghe. "Interior-point method for
nuclear norm approximation with application to system
identification."
"""
m, N, p = u.size[0], u.size[1], y.size[0]
if y.size[1] != N:
raise ValueError, "y and u must have the same length"
# Y = G*X + H*U + V, Y has size a x b, U has size c x b, Un has b x d
r = min(int(30/p),int((N+1.0)/(p+m+1)+1.0))
a = r*p
c = r*m
b = N-r+1
d = b-c
# construct Hankel matrix Y
Y = Hankel(y,r,b,p=p,q=1)
# construct Hankel matrix U
U = Hankel(u,r,b,p=m,q=1)
# compute Un = null(U) and YUn = Y*Un
Vt = matrix(0.0,(b,b))
Stemp = matrix(0.0,(c,1))
Un = matrix(0.0,(b,d))
YUn = matrix(0.0,(a,d))
lapack.gesvd(U,Stemp,jobvt='A',Vt=Vt)
Un[:,:] = Vt.T[:,c:]
blas.gemm(Y,Un,YUn)
# compute original singular values
svN = matrix(0.0,(min(a,d),1))
lapack.gesvd(YUn,svN)
# variable, [y(1);...;y(N)]
# form the coefficient matrices for the nuclear norm optimization
# minimize | Yh * Un |_* + alpha * | y - yh |_F
AA = Hankel_basis(r,b,p=p,q=1)
A = matrix(0.0,(a*d,p*N))
temp = spmatrix([],[],[],(a,b),'d')
temp2 = matrix(0.0,(a,d))
for ii in xrange(p*N):
temp[:] = AA[:,ii]
base.gemm(temp,Un,temp2)
A[:,ii] = temp2[:]
B = matrix(0.0,(a,d))
# flip the matrix if columns is more than rows
if a < d:
Itrans = [i+j*a for i in xrange(a) for j in xrange(d)]
B[:] = B[Itrans]
B.size = (d,a)
for ii in xrange(p*N):
A[:,ii] = A[Itrans,ii]
# regularized term
x0 = y[:]
Qd = matrix(2.0*svN[0]/p/N/(vsig**2),(p*N,1))
# solve the nuclear norm optimization
sol = nrmapp(A, B, C = base.spdiag(Qd), d = -base.mul(x0, Qd))
status = sol['status']
x = sol['x']
# construct YhUn and take the svd
YhUn = matrix(B)
blas.gemv(A,x,YhUn,beta=1.0)
if a < d:
YhUn = YhUn.T
Uh = matrix(0.0,(a,d))
sv = matrix(0.0,(d,1))
lapack.gesvd(YhUn,sv,jobu='S',U=Uh)
# determine model order
if svth is None:
svth = 1E-3
svthn = sv[0]*svth
n=1
while sv[n] >= svthn and n < 10:
n=n+1
# estimate A, C
Uhn = Uh[:,:n]
for ii in xrange(n):
blas.scal(sv[ii],Uhn,n=a,offset=ii*a)
syseC = Uhn[:p,:]
Als = Uhn[:-p,:]
Bls = Uhn[p:,:]
lapack.gels(Als,Bls)
syseA = Bls[:n,:]
Als[:,:] = Uhn[:-p,:]
Bls[:,:] = Uhn[p:,:]
blas.gemm(Als,syseA,Bls,beta=-1.0)
Aerr = blas.nrm2(Bls)
# stabilize A
Sc = matrix(0.0,(n,n),'z')
w = matrix(0.0, (n,1), 'z')
Vs = matrix(0.0, (n,n), 'z')
def F(w):
return (abs(w) < 1.0)
Sc[:,:] = syseA
ns = lapack.gees(Sc, w, Vs, select = F)
while ns < n:
#print "stabilize matrix A"
w[ns:] = w[ns:]**-1
Sc[::n+1] = w
Sc = Vs*Sc*Vs.H
syseA[:,:] = Sc.real()
Sc[:,:] = syseA
ns = lapack.gees(Sc, w, Vs, select = F)
# estimate B,D,x0 stored in vector [x0; vec(D); vec(B)]
F1 = matrix(0.0,(p*N,n))
F1[:p,:] = syseC
for ii in xrange(1,N):
F1[ii*p:(ii+1)*p,:] = F1[(ii-1)*p:ii*p,:]*syseA
F2 = matrix(0.0,(p*N,p*m))
ut = u.T
for ii in xrange(p):
F2[ii::p,ii::p] = ut
F3 = matrix(0.0,(p*N,n*m))
F3t = matrix(0.0,(p*(N-1),n*m))
for ii in xrange(1,N):
for jj in xrange(p):
for kk in xrange(n):
F3t[jj:jj+(N-ii)*p:p,kk::n] = ut[:N-ii,:]*F1[(ii-1)*p+jj,kk]
F3[ii*p:,:] = F3[ii*p:,:] + F3t[:(N-ii)*p,:]
F = matrix([[F1],[F2],[F3]])
yls = y[:]
Sls = matrix(0.0,(F.size[1],1))
Uls = matrix(0.0,(F.size[0],F.size[1]))
Vtls = matrix(0.0,(F.size[1],F.size[1]))
lapack.gesvd(F, Sls, jobu='S', jobvt='S', U=Uls, Vt=Vtls)
Frank=len([ii for ii in xrange(Sls.size[0]) if Sls[ii] >= 1E-6])
#print 'Rank deficiency = ', F.size[1] - Frank
xx = matrix(0.0,(F.size[1],1))
xx[:Frank] = Uls.T[:Frank,:] * yls
xx[:Frank] = base.mul(xx[:Frank],Sls[:Frank]**-1)
xx[:] = Vtls.T[:,:Frank]*xx[:Frank]
blas.gemv(F,xx,yls,beta=-1.0)
xxerr = blas.nrm2(yls)
x0 = xx[:n]
syseD = xx[n:n+p*m]
syseD.size = (p,m)
syseB = xx[n+p*m:]
syseB.size = (n,m)
return {'A': syseA, 'B': syseB, 'C': syseC, 'D': syseD, 'svN': svN, 'sv': \
sv, 'x0': x0, 'n': n, 'Aerr': Aerr, 'xxerr': xxerr}
def F(W):
"""
Generate a solver for
A'(uz0) = bx[0]
-uz0 - uz1 = bx[1]
A(ux[0]) - ux[1] - r0*r0' * uz0 * r0*r0' = bz0
- ux[1] - r1*r1' * uz1 * r1*r1' = bz1.
uz0, uz1, bz0, bz1 are symmetric m x m-matrices.
ux[0], bx[0] are n-vectors.
ux[1], bx[1] are symmetric m x m-matrices.
We first calculate a congruence that diagonalizes r0*r0' and r1*r1':
U' * r0 * r0' * U = I, U' * r1 * r1' * U = S.
We then make a change of variables
usx[0] = ux[0],
usx[1] = U' * ux[1] * U
usz0 = U^-1 * uz0 * U^-T
usz1 = U^-1 * uz1 * U^-T
and define
As() = U' * A() * U'
bsx[1] = U^-1 * bx[1] * U^-T
bsz0 = U' * bz0 * U
bsz1 = U' * bz1 * U.
This gives
As'(usz0) = bx[0]
-usz0 - usz1 = bsx[1]
As(usx[0]) - usx[1] - usz0 = bsz0
-usx[1] - S * usz1 * S = bsz1.
1. Eliminate usz0, usz1 using equations 3 and 4,
usz0 = As(usx[0]) - usx[1] - bsz0
usz1 = -S^-1 * (usx[1] + bsz1) * S^-1.
This gives two equations in usx[0] an usx[1].
As'(As(usx[0]) - usx[1]) = bx[0] + As'(bsz0)
-As(usx[0]) + usx[1] + S^-1 * usx[1] * S^-1
= bsx[1] - bsz0 - S^-1 * bsz1 * S^-1.
2. Eliminate usx[1] using equation 2:
usx[1] + S * usx[1] * S
= S * ( As(usx[0]) + bsx[1] - bsz0 ) * S - bsz1
i.e., with Gamma[i,j] = 1.0 + S[i,i] * S[j,j],
usx[1] = ( S * As(usx[0]) * S ) ./ Gamma
+ ( S * ( bsx[1] - bsz0 ) * S - bsz1 ) ./ Gamma.
This gives an equation in usx[0].
As'( As(usx[0]) ./ Gamma )
= bx0 + As'(bsz0) +
As'( (S * ( bsx[1] - bsz0 ) * S - bsz1) ./ Gamma )
= bx0 + As'( ( bsz0 - bsz1 + S * bsx[1] * S ) ./ Gamma ).
"""
# Calculate U s.t.
#
# U' * r0*r0' * U = I, U' * r1*r1' * U = diag(s).
# Cholesky factorization r0 * r0' = L * L'
blas.syrk(W['r'][0], L)
lapack.potrf(L)
# SVD L^-1 * r1 = U * diag(s) * V'
blas.copy(W['r'][1], U)
blas.trsm(L, U)
lapack.gesvd(U, s, jobu = 'O')
# s := s**2
s[:] = s**2
# Uti := U
blas.copy(U, Uti)
# U := L^-T * U
blas.trsm(L, U, transA = 'T')
# Uti := L * Uti = U^-T
blas.trmm(L, Uti)
# Us := U * diag(s)^-1
blas.copy(U, Us)
for i in range(m):
blas.tbsv(s, Us, n = m, k = 0, ldA = 1, incx = m, offsetx = i)
# S is m x m with lower triangular entries s[i] * s[j]
# sqrtG is m x m with lower triangular entries sqrt(1.0 + s[i]*s[j])
# Upper triangular entries are undefined but nonzero.
blas.scal(0.0, S)
blas.syrk(s, S)
Gamma = 1.0 + S
sqrtG = sqrt(Gamma)
# Asc[i] = (U' * Ai * * U ) ./ sqrtG, for i = 1, ..., n
# = Asi ./ sqrt(Gamma)
blas.copy(A, Asc)
misc.scale(Asc, # only 'r' part of the dictionary is used
{'dnl': matrix(0.0, (0, 1)), 'dnli': matrix(0.0, (0, 1)),
'd': matrix(0.0, (0, 1)), 'di': matrix(0.0, (0, 1)),
'v': [], 'beta': [], 'r': [ U ], 'rti': [ U ]})
for i in range(n):
blas.tbsv(sqrtG, Asc, n = msq, k = 0, ldA = 1, offsetx = i*msq)
# Convert columns of Asc to packed storage
misc.pack2(Asc, {'l': 0, 'q': [], 's': [ m ]})
# Cholesky factorization of Asc' * Asc.
H = matrix(0.0, (n, n))
blas.syrk(Asc, H, trans = 'T', k = mpckd)
lapack.potrf(H)
def solve(x, y, z):
"""
1. Solve for usx[0]:
Asc'(Asc(usx[0]))
= bx0 + Asc'( ( bsz0 - bsz1 + S * bsx[1] * S ) ./ sqrtG)
= bx0 + Asc'( ( bsz0 + S * ( bsx[1] - bssz1) S )
./ sqrtG)
where bsx[1] = U^-1 * bx[1] * U^-T, bsz0 = U' * bz0 * U,
bsz1 = U' * bz1 * U, bssz1 = S^-1 * bsz1 * S^-1
2. Solve for usx[1]:
usx[1] + S * usx[1] * S
= S * ( As(usx[0]) + bsx[1] - bsz0 ) * S - bsz1
usx[1]
= ( S * (As(usx[0]) + bsx[1] - bsz0) * S - bsz1) ./ Gamma
= -bsz0 + (S * As(usx[0]) * S) ./ Gamma
+ (bsz0 - bsz1 + S * bsx[1] * S ) . / Gamma
= -bsz0 + (S * As(usx[0]) * S) ./ Gamma
+ (bsz0 + S * ( bsx[1] - bssz1 ) * S ) . / Gamma
Unscale ux[1] = Uti * usx[1] * Uti'
3. Compute usz0, usz1
r0' * uz0 * r0 = r0^-1 * ( A(ux[0]) - ux[1] - bz0 ) * r0^-T
r1' * uz1 * r1 = r1^-1 * ( -ux[1] - bz1 ) * r1^-T
"""
# z0 := U' * z0 * U
# = bsz0
__cngrnc(U, z, trans = 'T')
# z1 := Us' * bz1 * Us
# = S^-1 * U' * bz1 * U * S^-1
# = S^-1 * bsz1 * S^-1
__cngrnc(Us, z, trans = 'T', offsetx = msq)
# x[1] := Uti' * x[1] * Uti
# = bsx[1]
__cngrnc(Uti, x[1], trans = 'T')
# x[1] := x[1] - z[msq:]
# = bsx[1] - S^-1 * bsz1 * S^-1
blas.axpy(z, x[1], alpha = -1.0, offsetx = msq)
# x1 = (S * x[1] * S + z[:msq] ) ./ sqrtG
# = (S * ( bsx[1] - S^-1 * bsz1 * S^-1) * S + bsz0 ) ./ sqrtG
# = (S * bsx[1] * S - bsz1 + bsz0 ) ./ sqrtG
# in packed storage
blas.copy(x[1], x1)
blas.tbmv(S, x1, n = msq, k = 0, ldA = 1)
blas.axpy(z, x1, n = msq)
blas.tbsv(sqrtG, x1, n = msq, k = 0, ldA = 1)
misc.pack2(x1, {'l': 0, 'q': [], 's': [m]})
# x[0] := x[0] + Asc'*x1
# = bx0 + Asc'( ( bsz0 - bsz1 + S * bsx[1] * S ) ./ sqrtG)
# = bx0 + As'( ( bz0 - bz1 + S * bx[1] * S ) ./ Gamma )
blas.gemv(Asc, x1, x[0], m = mpckd, trans = 'T', beta = 1.0)
# x[0] := H^-1 * x[0]
# = ux[0]
lapack.potrs(H, x[0])
# x1 = Asc(x[0]) .* sqrtG (unpacked)
# = As(x[0])
blas.gemv(Asc, x[0], tmp, m = mpckd)
misc.unpack(tmp, x1, {'l': 0, 'q': [], 's': [m]})
blas.tbmv(sqrtG, x1, n = msq, k = 0, ldA = 1)
# usx[1] = (x1 + (x[1] - z[:msq])) ./ sqrtG**2
# = (As(ux[0]) + bsx[1] - bsz0 - S^-1 * bsz1 * S^-1)
# ./ Gamma
# x[1] := x[1] - z[:msq]
# = bsx[1] - bsz0 - S^-1 * bsz1 * S^-1
blas.axpy(z, x[1], -1.0, n = msq)
# x[1] := x[1] + x1
# = As(ux) + bsx[1] - bsz0 - S^-1 * bsz1 * S^-1
blas.axpy(x1, x[1])
# x[1] := x[1] / Gammma
# = (As(ux) + bsx[1] - bsz0 + S^-1 * bsz1 * S^-1 ) / Gamma
# = S^-1 * usx[1] * S^-1
blas.tbsv(Gamma, x[1], n = msq, k = 0, ldA = 1)
# z[msq:] := r1' * U * (-z[msq:] - x[1]) * U * r1
# := -r1' * U * S^-1 * (bsz1 + ux[1]) * S^-1 * U * r1
# := -r1' * uz1 * r1
blas.axpy(x[1], z, n = msq, offsety = msq)
blas.scal(-1.0, z, offset = msq)
__cngrnc(U, z, offsetx = msq)
__cngrnc(W['r'][1], z, trans = 'T', offsetx = msq)
# x[1] := S * x[1] * S
# = usx1
blas.tbmv(S, x[1], n = msq, k = 0, ldA = 1)
# z[:msq] = r0' * U' * ( x1 - x[1] - z[:msq] ) * U * r0
# = r0' * U' * ( As(ux) - usx1 - bsz0 ) * U * r0
# = r0' * U' * usz0 * U * r0
# = r0' * uz0 * r0
blas.axpy(x1, z, -1.0, n = msq)
blas.scal(-1.0, z, n = msq)
blas.axpy(x[1], z, -1.0, n = msq)
__cngrnc(U, z)
__cngrnc(W['r'][0], z, trans = 'T')
# x[1] := Uti * x[1] * Uti'
# = ux[1]
__cngrnc(Uti, x[1])
return solve
def F(W):
# SVD R[j] = U[j] * diag(sig[j]) * Vt[j]
lapack.gesvd(+W['r'][0], sv, jobu='A', jobvt='A', U=U, Vt=Vt)
# Vt[j] := diag(sig[j])^-1 * Vt[j]
for k in xrange(ns):
blas.tbsv(sv, Vt, n=ns, k=0, ldA=1, offsetx=k * ns)
# Gamma[j] is an ns[j] x ns[j] symmetric matrix
#
# (sig[j] * sig[j]') ./ sqrt(1 + rho * (sig[j] * sig[j]').^2)
# S = sig[j] * sig[j]'
S = matrix(0.0, (ns, ns))
blas.syrk(sv, S)
Gamma = div(S, sqrt(1.0 + rho * S**2))
symmetrize(Gamma, ns)
# As represents the scaled mapping
#
# As(x) = A(u * (Gamma .* x) * u')
# As'(y) = Gamma .* (u' * A'(y) * u)
#
# stored in a similar format as A, except that we use packed
# storage for the columns of As[i][j].
if type(A) is spmatrix:
blas.scal(0.0, As)
try:
As[VecAIndex] = +A['s'][VecAIndex]
except:
As[VecAIndex] = +A[VecAIndex]
else:
blas.copy(A, As)
# As[i][j][:,k] = diag( diag(Gamma[j]))*As[i][j][:,k]
# As[i][j][l,:] = Gamma[j][l,l]*As[i][j][l,:]
for k in xrange(ms):
cngrnc(U, As, trans='T', offsetx=k * (ns2))
blas.tbmv(Gamma, As, n=ns2, k=0, ldA=1, offsetx=k * (ns2))
misc.pack(As, Aspkd, {'l': 0, 'q': [], 's': [ns] * ms})
# H is an m times m block matrix with i, k block
#
# Hik = sum_j As[i,j]' * As[k,j]
#
# of size ms[i] x ms[k]. Hik = 0 if As[i,j] or As[k,j]
# are zero for all j
H = matrix(0.0, (ms, ms))
blas.syrk(Aspkd, H, trans='T', beta=1.0, k=ns * (ns + 1) / 2)
lapack.potrf(H)
def solve(x, y, z):
"""
Returns solution of
rho * ux + A'(uy) - r^-T * uz * r^-1 = bx
A(ux) = by
-ux - r * uz * r' = bz.
On entry, x = bx, y = by, z = bz.
On exit, x = ux, y = uy, z = uz.
"""
# bz is a copy of z in the format of x
blas.copy(z, bz)
blas.axpy(bz, x, alpha=rho)
# x := Gamma .* (u' * x * u)
# = Gamma .* (u' * (bx + rho * bz) * u)
cngrnc(U, x, trans='T', offsetx=0)
blas.tbmv(Gamma, x, n=ns2, k=0, ldA=1, offsetx=0)
# y := y - As(x)
# := by - As( Gamma .* u' * (bx + rho * bz) * u)
#blas.copy(x,xp)
#pack_ip(xp,n = ns,m=1,nl=nl)
misc.pack(x, xp, {'l': 0, 'q': [], 's': [ns]})
blas.gemv(Aspkd, xp, y, trans = 'T',alpha = -1.0, beta = 1.0, \
m = ns*(ns+1)/2, n = ms,offsetx = 0)
# y := -y - A(bz)
# = -by - A(bz) + As(Gamma .* (u' * (bx + rho * bz) * u)
Af(bz, y, alpha=-1.0, beta=-1.0)
# y := H^-1 * y
# = H^-1 ( -by - A(bz) + As(Gamma.* u'*(bx + rho*bz)*u) )
# = uy
blas.trsv(H, y)
blas.trsv(H, y, trans='T')
# bz = Vt' * vz * Vt
# = uz where
# vz := Gamma .* ( As'(uy) - x )
# = Gamma .* ( As'(uy) - Gamma .* (u'*(bx + rho *bz)*u) )
# = Gamma.^2 .* ( u' * (A'(uy) - bx - rho * bz) * u ).
#blas.copy(x,xp)
#pack_ip(xp,n=ns,m=1,nl=nl)
misc.pack(x, xp, {'l': 0, 'q': [], 's': [ns]})
blas.scal(-1.0, xp)
blas.gemv(Aspkd,
y,
xp,
alpha=1.0,
beta=1.0,
m=ns * (ns + 1) / 2,
n=ms,
offsety=0)
# bz[j] is xp unpacked and multiplied with Gamma
misc.unpack(xp, bz, {'l': 0, 'q': [], 's': [ns]})
blas.tbmv(Gamma, bz, n=ns2, k=0, ldA=1, offsetx=0)
# bz = Vt' * bz * Vt
# = uz
cngrnc(Vt, bz, trans='T', offsetx=0)
symmetrize(bz, ns, offset=0)
# x = -bz - r * uz * r'
# z contains r.h.s. bz; copy to x
blas.copy(z, x)
blas.copy(bz, z)
cngrnc(W['r'][0], bz, offsetx=0)
blas.axpy(bz, x)
blas.scal(-1.0, x)
return solve
def F(W):
for j in xrange(N):
# SVD R[j] = U[j] * diag(sig[j]) * Vt[j]
lapack.gesvd(+W['r'][j],
sv[j],
jobu='A',
jobvt='A',
U=U[j],
Vt=Vt[j])
# Vt[j] := diag(sig[j])^-1 * Vt[j]
for k in xrange(ns[j]):
blas.tbsv(sv[j], Vt[j], n=ns[j], k=0, ldA=1, offsetx=k * ns[j])
# Gamma[j] is an ns[j] x ns[j] symmetric matrix
#
# (sig[j] * sig[j]') ./ sqrt(1 + rho * (sig[j] * sig[j]').^2)
# S = sig[j] * sig[j]'
S = matrix(0.0, (ns[j], ns[j]))
blas.syrk(sv[j], S)
Gamma[j][:] = div(S, sqrt(1.0 + rho * S**2))[:]
symmetrize(Gamma[j], ns[j])
# As represents the scaled mapping
#
# As(x) = A(u * (Gamma .* x) * u')
# As'(y) = Gamma .* (u' * A'(y) * u)
#
# stored in a similar format as A, except that we use packed
# storage for the columns of As[i][j].
for i in xrange(M):
if (type(A[i][j]) is matrix) or (type(A[i][j]) is spmatrix):
# As[i][j][:,k] = vec(
# (U[j]' * mat( A[i][j][:,k] ) * U[j]) .* Gamma[j])
copy(A[i][j], As[i][j])
As[i][j] = matrix(As[i][j])
for k in xrange(ms[i]):
cngrnc(U[j],
As[i][j],
trans='T',
offsetx=k * (ns[j]**2),
n=ns[j])
blas.tbmv(Gamma[j],
As[i][j],
n=ns[j]**2,
k=0,
ldA=1,
offsetx=k * (ns[j]**2))
# pack As[i][j] in place
#pack_ip(As[i][j], ns[j])
for k in xrange(As[i][j].size[1]):
tmp = +As[i][j][:, k]
misc.pack2(tmp, {'l': 0, 'q': [], 's': [ns[j]]})
As[i][j][:, k] = tmp
else:
As[i][j] = 0.0
# H is an m times m block matrix with i, k block
#
# Hik = sum_j As[i,j]' * As[k,j]
#
# of size ms[i] x ms[k]. Hik = 0 if As[i,j] or As[k,j]
# are zero for all j
H = spmatrix([], [], [], (sum(ms), sum(ms)))
rowid = 0
for i in xrange(M):
colid = 0
for k in xrange(i + 1):
sparse_block = True
Hik = matrix(0.0, (ms[i], ms[k]))
for j in xrange(N):
if (type(As[i][j]) is matrix) and \
(type(As[k][j]) is matrix):
sparse_block = False
# Hik += As[i,j]' * As[k,j]
if i == k:
blas.syrk(As[i][j],
Hik,
trans='T',
beta=1.0,
k=ns[j] * (ns[j] + 1) / 2,
ldA=ns[j]**2)
else:
blas.gemm(As[i][j],
As[k][j],
Hik,
transA='T',
beta=1.0,
k=ns[j] * (ns[j] + 1) / 2,
ldA=ns[j]**2,
ldB=ns[j]**2)
if not (sparse_block):
H[rowid:rowid+ms[i], colid:colid+ms[k]] \
= sparse(Hik)
colid += ms[k]
rowid += ms[i]
HF = cholmod.symbolic(H)
cholmod.numeric(H, HF)
def solve(x, y, z):
"""
Returns solution of
rho * ux + A'(uy) - r^-T * uz * r^-1 = bx
A(ux) = by
-ux - r * uz * r' = bz.
On entry, x = bx, y = by, z = bz.
On exit, x = ux, y = uy, z = uz.
"""
# bz is a copy of z in the format of x
blas.copy(z, bz)
# x := x + rho * bz
# = bx + rho * bz
blas.axpy(bz, x, alpha=rho)
# x := Gamma .* (u' * x * u)
# = Gamma .* (u' * (bx + rho * bz) * u)
offsetj = 0
for j in xrange(N):
cngrnc(U[j], x, trans='T', offsetx=offsetj, n=ns[j])
blas.tbmv(Gamma[j], x, n=ns[j]**2, k=0, ldA=1, offsetx=offsetj)
offsetj += ns[j]**2
# y := y - As(x)
# := by - As( Gamma .* u' * (bx + rho * bz) * u)
blas.copy(x, xp)
offsetj = 0
for j in xrange(N):
misc.pack2(xp, {'l': offsetj, 'q': [], 's': [ns[j]]})
offsetj += ns[j]**2
offseti = 0
for i in xrange(M):
offsetj = 0
for j in xrange(N):
if type(As[i][j]) is matrix:
blas.gemv(As[i][j],
xp,
y,
trans='T',
alpha=-1.0,
beta=1.0,
m=ns[j] * (ns[j] + 1) / 2,
n=ms[i],
ldA=ns[j]**2,
offsetx=offsetj,
offsety=offseti)
offsetj += ns[j]**2
offseti += ms[i]
# y := -y - A(bz)
# = -by - A(bz) + As(Gamma .* (u' * (bx + rho * bz) * u)
Af(bz, y, alpha=-1.0, beta=-1.0)
# y := H^-1 * y
# = H^-1 ( -by - A(bz) + As(Gamma.* u'*(bx + rho*bz)*u) )
# = uy
cholmod.solve(HF, y)
# bz = Vt' * vz * Vt
# = uz where
# vz := Gamma .* ( As'(uy) - x )
# = Gamma .* ( As'(uy) - Gamma .* (u'*(bx + rho *bz)*u) )
# = Gamma.^2 .* ( u' * (A'(uy) - bx - rho * bz) * u ).
blas.copy(x, xp)
offsetj = 0
for j in xrange(N):
# xp is -x[j] = -Gamma .* (u' * (bx + rho*bz) * u)
# in packed storage
misc.pack2(xp, {'l': offsetj, 'q': [], 's': [ns[j]]})
offsetj += ns[j]**2
blas.scal(-1.0, xp)
offsetj = 0
for j in xrange(N):
# xp += As'(uy)
offseti = 0
for i in xrange(M):
if type(As[i][j]) is matrix:
blas.gemv(As[i][j], y, xp, alpha = 1.0,
beta = 1.0, m = ns[j]*(ns[j]+1)/2, \
n = ms[i],ldA = ns[j]**2, \
offsetx = offseti, offsety = offsetj)
offseti += ms[i]
# bz[j] is xp unpacked and multiplied with Gamma
#unpack(xp, bz[j], ns[j])
misc.unpack(xp,
bz, {
'l': 0,
'q': [],
's': [ns[j]]
},
offsetx=offsetj,
offsety=offsetj)
blas.tbmv(Gamma[j],
bz,
n=ns[j]**2,
k=0,
ldA=1,
offsetx=offsetj)
# bz = Vt' * bz * Vt
# = uz
cngrnc(Vt[j], bz, trans='T', offsetx=offsetj, n=ns[j])
symmetrize(bz, ns[j], offset=offsetj)
offsetj += ns[j]**2
# x = -bz - r * uz * r'
blas.copy(z, x)
blas.copy(bz, z)
offsetj = 0
for j in xrange(N):
cngrnc(+W['r'][j], bz, offsetx=offsetj, n=ns[j])
offsetj += ns[j]**2
blas.axpy(bz, x)
blas.scal(-1.0, x)
return solve
elif data is "sysid":
iddata = pickle.load(open("CD_player_arm.bin","r"))
N = 100
u = iddata['u'][:,:N]
y = iddata['y'][:,:N]
m, N, p = u.size[0], u.size[1], y.size[0]
r = min(int(30/p),int((N+1.0)/(p+m+1)+1.0))
a = r*p
c = r*m
b = N-r+1
d = b-c
U = sysid.Hankel(u,r,b,p=m,q=1)
Vt = matrix(0.0,(b,b))
Stemp = matrix(0.0,(c,1))
Un = matrix(0.0,(b,d))
lapack.gesvd(U,Stemp,jobvt='A',Vt=Vt)
Un[:,:] = Vt.T[:,c:]
AA = sysid.Hankel_basis(r,b,p=p,q=1)
A = matrix(0.0,(a*d,p*N))
temp = spmatrix([],[],[],(a,b),'d')
temp2 = matrix(0.0,(a,d))
for ii in xrange(p*N):
temp[:] = AA[:,ii]
base.gemm(temp,Un,temp2)
A[:,ii] = temp2[:]
B = matrix(0.0,(a,d))
# flip the matrix if columns is more than rows
if a < d:
Itrans = [i+j*a for i in xrange(a) for j in xrange(d)]
elif data is "sysid":
iddata = pickle.load(open("CD_player_arm.bin", "r"))
N = 100
u = iddata['u'][:, :N]
y = iddata['y'][:, :N]
m, N, p = u.size[0], u.size[1], y.size[0]
r = min(int(30 / p), int((N + 1.0) / (p + m + 1) + 1.0))
a = r * p
c = r * m
b = N - r + 1
d = b - c
U = sysid.Hankel(u, r, b, p=m, q=1)
Vt = matrix(0.0, (b, b))
Stemp = matrix(0.0, (c, 1))
Un = matrix(0.0, (b, d))
lapack.gesvd(U, Stemp, jobvt='A', Vt=Vt)
Un[:, :] = Vt.T[:, c:]
AA = sysid.Hankel_basis(r, b, p=p, q=1)
A = matrix(0.0, (a * d, p * N))
temp = spmatrix([], [], [], (a, b), 'd')
temp2 = matrix(0.0, (a, d))
for ii in xrange(p * N):
temp[:] = AA[:, ii]
base.gemm(temp, Un, temp2)
A[:, ii] = temp2[:]
B = matrix(0.0, (a, d))
# flip the matrix if columns is more than rows
if a < d:
Itrans = [i + j * a for i in xrange(a) for j in xrange(d)]
def construct_Ab(constr_list, mapping, n, options):
"""
Description
-----------
Constructs matrix A and vector b from a list
of equality constraints. The program is assumed
to be expanded and transformed so all the
equality constraints are affine.
Arguments
---------
constr_list: cvxpy_list of equality constraints.
Mapping: Dictionary mapping variable to index.
n: Number of variables.
"""
# Create matrices
A = opt.spmatrix(0.0, [], [], (0, n))
b = opt.matrix(0.0, (0, 1))
H_top = opt.sparse([[A], [b]])
rank = 0
counter = 1
for constr in constr_list:
# Get elements
left = constr.left
op = constr.op
right = constr.right
# New rows
A_row = opt.spmatrix(0.0, [], [], (1, n))
b_row = opt.matrix(0.0, (1, 1))
# Deal with right element
if (right.type == CONSTANT):
b_row[0] += right.data * 1.0
elif (right.type == VARIABLE):
A_row[0, mapping[right]] += -1.0
else:
raise ValueError('Bad equality: Cannot construct A')
# Left is a variable
if (left.type == VARIABLE):
A_row[0, mapping[left]] += 1.0
# Left is a constant
elif (left.type == CONSTANT):
b_row[0] += -left.data * 1.0
# Left is an operation tree
elif (left.type == TREE and left.item.type == OPERATOR):
# Get operator name
name = left.item.name
# Addition
if (name == '+'):
for arg in left.children:
if (arg.type == TREE and arg.item.name == '*'):
ch1 = arg.children[1]
ch0 = arg.children[0]
A_row[0, mapping[ch1]] += ch0.data
elif (arg.type == VARIABLE):
A_row[0, mapping[arg]] += 1.0
elif (arg.type == CONSTANT):
b_row[0] += -arg.data * 1.0
else:
raise ValueError('Bad equality: Cannot construct A')
# Multiplication
elif (name == '*'):
# Constant
op1 = left.children[0]
# Variable
op2 = left.children[1]
# Process
A_row[0, mapping[op2]] += 1.0 * op1.data
# Error
else:
raise ValueError('Bad equality: Cannot construct A')
# Error
else:
raise ValueError('Bad equality: Cannot construct A')
# Rank check
if (options['check redundancy']):
H_botton = opt.sparse([[A_row], [b_row]])
H = opt.sparse([H_top, H_botton])
svd = opt.matrix(0.0, (1, np.min([H.size[0], H.size[1]])))
lapack.gesvd(opt.matrix(H), svd)
new_rank = len([x for x in svd if x > 1e-8])
if (new_rank > rank):
# Append
A = opt.sparse([A, A_row])
b = opt.matrix([b, b_row])
# Update rank and H_top
rank = new_rank
H_top = H
else:
if (options['show steps'] and not quiet):
print 'Redundant equality eliminated [', counter, ']'
counter += 1
else:
# Append
A = opt.sparse([A, A_row])
b = opt.matrix([b, b_row])
# Return matrices
return A, b
def F(W):
"""
Generate a solver for
A'(uz0) = bx[0]
-uz0 - uz1 = bx[1]
A(ux[0]) - ux[1] - r0*r0' * uz0 * r0*r0' = bz0
- ux[1] - r1*r1' * uz1 * r1*r1' = bz1.
uz0, uz1, bz0, bz1 are symmetric m x m-matrices.
ux[0], bx[0] are n-vectors.
ux[1], bx[1] are symmetric m x m-matrices.
We first calculate a congruence that diagonalizes r0*r0' and r1*r1':
U' * r0 * r0' * U = I, U' * r1 * r1' * U = S.
We then make a change of variables
usx[0] = ux[0],
usx[1] = U' * ux[1] * U
usz0 = U^-1 * uz0 * U^-T
usz1 = U^-1 * uz1 * U^-T
and define
As() = U' * A() * U'
bsx[1] = U^-1 * bx[1] * U^-T
bsz0 = U' * bz0 * U
bsz1 = U' * bz1 * U.
This gives
As'(usz0) = bx[0]
-usz0 - usz1 = bsx[1]
As(usx[0]) - usx[1] - usz0 = bsz0
-usx[1] - S * usz1 * S = bsz1.
1. Eliminate usz0, usz1 using equations 3 and 4,
usz0 = As(usx[0]) - usx[1] - bsz0
usz1 = -S^-1 * (usx[1] + bsz1) * S^-1.
This gives two equations in usx[0] an usx[1].
As'(As(usx[0]) - usx[1]) = bx[0] + As'(bsz0)
-As(usx[0]) + usx[1] + S^-1 * usx[1] * S^-1
= bsx[1] - bsz0 - S^-1 * bsz1 * S^-1.
2. Eliminate usx[1] using equation 2:
usx[1] + S * usx[1] * S
= S * ( As(usx[0]) + bsx[1] - bsz0 ) * S - bsz1
i.e., with Gamma[i,j] = 1.0 + S[i,i] * S[j,j],
usx[1] = ( S * As(usx[0]) * S ) ./ Gamma
+ ( S * ( bsx[1] - bsz0 ) * S - bsz1 ) ./ Gamma.
This gives an equation in usx[0].
As'( As(usx[0]) ./ Gamma )
= bx0 + As'(bsz0) +
As'( (S * ( bsx[1] - bsz0 ) * S - bsz1) ./ Gamma )
= bx0 + As'( ( bsz0 - bsz1 + S * bsx[1] * S ) ./ Gamma ).
"""
# Calculate U s.t.
#
# U' * r0*r0' * U = I, U' * r1*r1' * U = diag(s).
# Cholesky factorization r0 * r0' = L * L'
blas.syrk(W['r'][0], L)
lapack.potrf(L)
# SVD L^-1 * r1 = U * diag(s) * V'
blas.copy(W['r'][1], U)
blas.trsm(L, U)
lapack.gesvd(U, s, jobu='O')
# s := s**2
s[:] = s**2
# Uti := U
blas.copy(U, Uti)
# U := L^-T * U
blas.trsm(L, U, transA='T')
# Uti := L * Uti = U^-T
blas.trmm(L, Uti)
# Us := U * diag(s)^-1
blas.copy(U, Us)
for i in range(m):
blas.tbsv(s, Us, n=m, k=0, ldA=1, incx=m, offsetx=i)
# S is m x m with lower triangular entries s[i] * s[j]
# sqrtG is m x m with lower triangular entries sqrt(1.0 + s[i]*s[j])
# Upper triangular entries are undefined but nonzero.
blas.scal(0.0, S)
blas.syrk(s, S)
Gamma = 1.0 + S
sqrtG = sqrt(Gamma)
# Asc[i] = (U' * Ai * * U ) ./ sqrtG, for i = 1, ..., n
# = Asi ./ sqrt(Gamma)
blas.copy(A, Asc)
misc.scale(
Asc, # only 'r' part of the dictionary is used
{
'dnl': matrix(0.0, (0, 1)),
'dnli': matrix(0.0, (0, 1)),
'd': matrix(0.0, (0, 1)),
'di': matrix(0.0, (0, 1)),
'v': [],
'beta': [],
'r': [U],
'rti': [U]
})
for i in range(n):
blas.tbsv(sqrtG, Asc, n=msq, k=0, ldA=1, offsetx=i * msq)
# Convert columns of Asc to packed storage
misc.pack2(Asc, {'l': 0, 'q': [], 's': [m]})
# Cholesky factorization of Asc' * Asc.
H = matrix(0.0, (n, n))
blas.syrk(Asc, H, trans='T', k=mpckd)
lapack.potrf(H)
def solve(x, y, z):
"""
1. Solve for usx[0]:
Asc'(Asc(usx[0]))
= bx0 + Asc'( ( bsz0 - bsz1 + S * bsx[1] * S ) ./ sqrtG)
= bx0 + Asc'( ( bsz0 + S * ( bsx[1] - bssz1) S )
./ sqrtG)
where bsx[1] = U^-1 * bx[1] * U^-T, bsz0 = U' * bz0 * U,
bsz1 = U' * bz1 * U, bssz1 = S^-1 * bsz1 * S^-1
2. Solve for usx[1]:
usx[1] + S * usx[1] * S
= S * ( As(usx[0]) + bsx[1] - bsz0 ) * S - bsz1
usx[1]
= ( S * (As(usx[0]) + bsx[1] - bsz0) * S - bsz1) ./ Gamma
= -bsz0 + (S * As(usx[0]) * S) ./ Gamma
+ (bsz0 - bsz1 + S * bsx[1] * S ) . / Gamma
= -bsz0 + (S * As(usx[0]) * S) ./ Gamma
+ (bsz0 + S * ( bsx[1] - bssz1 ) * S ) . / Gamma
Unscale ux[1] = Uti * usx[1] * Uti'
3. Compute usz0, usz1
r0' * uz0 * r0 = r0^-1 * ( A(ux[0]) - ux[1] - bz0 ) * r0^-T
r1' * uz1 * r1 = r1^-1 * ( -ux[1] - bz1 ) * r1^-T
"""
# z0 := U' * z0 * U
# = bsz0
__cngrnc(U, z, trans='T')
# z1 := Us' * bz1 * Us
# = S^-1 * U' * bz1 * U * S^-1
# = S^-1 * bsz1 * S^-1
__cngrnc(Us, z, trans='T', offsetx=msq)
# x[1] := Uti' * x[1] * Uti
# = bsx[1]
__cngrnc(Uti, x[1], trans='T')
# x[1] := x[1] - z[msq:]
# = bsx[1] - S^-1 * bsz1 * S^-1
blas.axpy(z, x[1], alpha=-1.0, offsetx=msq)
# x1 = (S * x[1] * S + z[:msq] ) ./ sqrtG
# = (S * ( bsx[1] - S^-1 * bsz1 * S^-1) * S + bsz0 ) ./ sqrtG
# = (S * bsx[1] * S - bsz1 + bsz0 ) ./ sqrtG
# in packed storage
blas.copy(x[1], x1)
blas.tbmv(S, x1, n=msq, k=0, ldA=1)
blas.axpy(z, x1, n=msq)
blas.tbsv(sqrtG, x1, n=msq, k=0, ldA=1)
misc.pack2(x1, {'l': 0, 'q': [], 's': [m]})
# x[0] := x[0] + Asc'*x1
# = bx0 + Asc'( ( bsz0 - bsz1 + S * bsx[1] * S ) ./ sqrtG)
# = bx0 + As'( ( bz0 - bz1 + S * bx[1] * S ) ./ Gamma )
blas.gemv(Asc, x1, x[0], m=mpckd, trans='T', beta=1.0)
# x[0] := H^-1 * x[0]
# = ux[0]
lapack.potrs(H, x[0])
# x1 = Asc(x[0]) .* sqrtG (unpacked)
# = As(x[0])
blas.gemv(Asc, x[0], tmp, m=mpckd)
misc.unpack(tmp, x1, {'l': 0, 'q': [], 's': [m]})
blas.tbmv(sqrtG, x1, n=msq, k=0, ldA=1)
# usx[1] = (x1 + (x[1] - z[:msq])) ./ sqrtG**2
# = (As(ux[0]) + bsx[1] - bsz0 - S^-1 * bsz1 * S^-1)
# ./ Gamma
# x[1] := x[1] - z[:msq]
# = bsx[1] - bsz0 - S^-1 * bsz1 * S^-1
blas.axpy(z, x[1], -1.0, n=msq)
# x[1] := x[1] + x1
# = As(ux) + bsx[1] - bsz0 - S^-1 * bsz1 * S^-1
blas.axpy(x1, x[1])
# x[1] := x[1] / Gammma
# = (As(ux) + bsx[1] - bsz0 + S^-1 * bsz1 * S^-1 ) / Gamma
# = S^-1 * usx[1] * S^-1
blas.tbsv(Gamma, x[1], n=msq, k=0, ldA=1)
# z[msq:] := r1' * U * (-z[msq:] - x[1]) * U * r1
# := -r1' * U * S^-1 * (bsz1 + ux[1]) * S^-1 * U * r1
# := -r1' * uz1 * r1
blas.axpy(x[1], z, n=msq, offsety=msq)
blas.scal(-1.0, z, offset=msq)
__cngrnc(U, z, offsetx=msq)
__cngrnc(W['r'][1], z, trans='T', offsetx=msq)
# x[1] := S * x[1] * S
# = usx1
blas.tbmv(S, x[1], n=msq, k=0, ldA=1)
# z[:msq] = r0' * U' * ( x1 - x[1] - z[:msq] ) * U * r0
# = r0' * U' * ( As(ux) - usx1 - bsz0 ) * U * r0
# = r0' * U' * usz0 * U * r0
# = r0' * uz0 * r0
blas.axpy(x1, z, -1.0, n=msq)
blas.scal(-1.0, z, n=msq)
blas.axpy(x[1], z, -1.0, n=msq)
__cngrnc(U, z)
__cngrnc(W['r'][0], z, trans='T')
# x[1] := Uti * x[1] * Uti'
# = ux[1]
__cngrnc(Uti, x[1])
return solve
# # with n variables, and matrices A(x), B of size p x q. setseed(0) p, q, n = 100, 100, 100 A = normal(p * q, n) B = normal(p, q) # options['feastol'] = 1e-6 # options['refinement'] = 3 sol = nucnrm.nrmapp(A, B) x = sol['x'] Z = sol['Z'] s = matrix(0.0, (p, 1)) X = matrix(A * x, (p, q)) + B lapack.gesvd(+X, s) nrmX = sum(s) lapack.gesvd(+Z, s) nrmZ = max(s) res = matrix(0.0, (n, 1)) blas.gemv(A, Z, res, beta=1.0, trans='T') print "\nNuclear norm of A(x) + B: %e" % nrmX print "Inner product of B and Z: %e" % blas.dot(B, Z) print "Maximum singular value of Z: %e" % nrmZ print "Euclidean norm of A'(Z): %e" % blas.nrm2(res)
