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)