예제 #1
0
파일: l1.py 프로젝트: Ovec8hkin/PySAR
    def Fkkt(W):

        # Returns a function f(x, y, z) that solves
        #
        # [ 0  0  P'      -P'      ] [ x[:n] ]   [ bx[:n] ]
        # [ 0  0 -I       -I       ] [ x[n:] ]   [ bx[n:] ]
        # [ P -I -D1^{-1}  0       ] [ z[:m] ] = [ bz[:m] ]
        # [-P -I  0       -D2^{-1} ] [ z[m:] ]   [ bz[m:] ]
        #
        # where D1 = diag(di[:m])^2, D2 = diag(di[m:])^2 and di = W['di'].
        #
        # On entry bx, bz are stored in x, z.
        # On exit x, z contain the solution, with z scaled (di .* z is
        # returned instead of z).

        # Factor A = 4*P'*D*P where D = d1.*d2 ./(d1+d2) and
        # d1 = d[:m].^2, d2 = d[m:].^2.

        di = W['di']
        d1, d2 = di[:m]**2, di[m:]**2
        D = div(mul(d1, d2), d1 + d2)
        Ds = spdiag(2 * sqrt(D))
        base.gemm(Ds, P, Ps)
        blas.syrk(Ps, A, trans='T')
        lapack.potrf(A)

        def f(x, y, z):

            # Solve for x[:n]:
            #
            #    A*x[:n] = bx[:n] + P' * ( ((D1-D2)*(D1+D2)^{-1})*bx[n:]
            #        + (2*D1*D2*(D1+D2)^{-1}) * (bz[:m] - bz[m:]) ).

            blas.copy((mul(div(d1 - d2, d1 + d2), x[n:]) +
                       mul(2 * D, z[:m] - z[m:])), u)
            blas.gemv(P, u, x, beta=1.0, trans='T')
            lapack.potrs(A, x)

            # x[n:] := (D1+D2)^{-1} * (bx[n:] - D1*bz[:m] - D2*bz[m:]
            #     + (D1-D2)*P*x[:n])

            base.gemv(P, x, u)
            x[n:] = div(
                x[n:] - mul(d1, z[:m]) - mul(d2, z[m:]) + mul(d1 - d2, u),
                d1 + d2)

            # z[:m] := d1[:m] .* ( P*x[:n] - x[n:] - bz[:m])
            # z[m:] := d2[m:] .* (-P*x[:n] - x[n:] - bz[m:])

            z[:m] = mul(di[:m], u - x[n:] - z[:m])
            z[m:] = mul(di[m:], -u - x[n:] - z[m:])

        return f
예제 #2
0
파일: l1.py 프로젝트: cuihantao/cvxopt
    def Fkkt(W): 

        # Returns a function f(x, y, z) that solves
        #
        # [ 0  0  P'      -P'      ] [ x[:n] ]   [ bx[:n] ]
        # [ 0  0 -I       -I       ] [ x[n:] ]   [ bx[n:] ]
        # [ P -I -D1^{-1}  0       ] [ z[:m] ] = [ bz[:m] ]
        # [-P -I  0       -D2^{-1} ] [ z[m:] ]   [ bz[m:] ]
        #
        # where D1 = diag(di[:m])^2, D2 = diag(di[m:])^2 and di = W['di'].
        #
        # On entry bx, bz are stored in x, z.
        # On exit x, z contain the solution, with z scaled (di .* z is
        # returned instead of z). 

        # Factor A = 4*P'*D*P where D = d1.*d2 ./(d1+d2) and
        # d1 = d[:m].^2, d2 = d[m:].^2.

        di = W['di']
        d1, d2 = di[:m]**2, di[m:]**2
        D = div( mul(d1,d2), d1+d2 )  
        Ds = spdiag(2 * sqrt(D))
        base.gemm(Ds, P, Ps)
        blas.syrk(Ps, A, trans = 'T')
        lapack.potrf(A)

        def f(x, y, z):

            # Solve for x[:n]:
            #
            #    A*x[:n] = bx[:n] + P' * ( ((D1-D2)*(D1+D2)^{-1})*bx[n:]
            #        + (2*D1*D2*(D1+D2)^{-1}) * (bz[:m] - bz[m:]) ).

            blas.copy(( mul( div(d1-d2, d1+d2), x[n:]) + 
                mul( 2*D, z[:m]-z[m:] ) ), u)
            blas.gemv(P, u, x, beta = 1.0, trans = 'T')
            lapack.potrs(A, x)

            # x[n:] := (D1+D2)^{-1} * (bx[n:] - D1*bz[:m] - D2*bz[m:]
            #     + (D1-D2)*P*x[:n])

            base.gemv(P, x, u)
            x[n:] =  div( x[n:] - mul(d1, z[:m]) - mul(d2, z[m:]) + 
                mul(d1-d2, u), d1+d2 )

            # z[:m] := d1[:m] .* ( P*x[:n] - x[n:] - bz[:m])
            # z[m:] := d2[m:] .* (-P*x[:n] - x[n:] - bz[m:]) 

            z[:m] = mul(di[:m],  u-x[n:]-z[:m])
            z[m:] = mul(di[m:], -u-x[n:]-z[m:])

        return f
예제 #3
0
    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
예제 #4
0
    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
예제 #5
0
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}
예제 #6
0
    def factor(W, H=None, Df=None):

        if F['firstcall']:
            if type(G) is matrix:
                F['Gs'] = matrix(0.0, G.size)
            else:
                F['Gs'] = spmatrix(0.0, G.I, G.J, G.size)
            if mnl:
                if type(Df) is matrix:
                    F['Dfs'] = matrix(0.0, Df.size)
                else:
                    F['Dfs'] = spmatrix(0.0, Df.I, Df.J, Df.size)
            if (mnl and type(Df) is matrix) or type(G) is matrix or \
                    type(H) is matrix:
                F['S'] = matrix(0.0, (n, n))
                F['K'] = matrix(0.0, (p, p))
            else:
                F['S'] = spmatrix([], [], [], (n, n), 'd')
                F['Sf'] = None
                if type(A) is matrix:
                    F['K'] = matrix(0.0, (p, p))
                else:
                    F['K'] = spmatrix([], [], [], (p, p), 'd')

        # Dfs = Wnl^{-1} * Df
        if mnl:
            base.gemm(spmatrix(W['dnli'], list(range(mnl)),
                               list(range(mnl))), Df, F['Dfs'], partial=True)

        # Gs = Wl^{-1} * G.
        base.gemm(spmatrix(W['di'], list(range(ml)), list(range(ml))),
                  G, F['Gs'], partial=True)

        if F['firstcall']:
            base.syrk(F['Gs'], F['S'], trans='T')
            if mnl:
                base.syrk(F['Dfs'], F['S'], trans='T', beta=1.0)
            if H is not None:
                F['S'] += H
            try:
                if type(F['S']) is matrix:
                    lapack.potrf(F['S'])
                else:
                    F['Sf'] = cholmod.symbolic(F['S'])
                    cholmod.numeric(F['S'], F['Sf'])
            except ArithmeticError:
                F['singular'] = True
                if type(A) is matrix and type(F['S']) is spmatrix:
                    F['S'] = matrix(0.0, (n, n))
                base.syrk(F['Gs'], F['S'], trans='T')
                if mnl:
                    base.syrk(F['Dfs'], F['S'], trans='T', beta=1.0)
                base.syrk(A, F['S'], trans='T', beta=1.0)
                if H is not None:
                    F['S'] += H
                if type(F['S']) is matrix:
                    lapack.potrf(F['S'])
                else:
                    F['Sf'] = cholmod.symbolic(F['S'])
                    cholmod.numeric(F['S'], F['Sf'])
            F['firstcall'] = False

        else:
            base.syrk(F['Gs'], F['S'], trans='T', partial=True)
            if mnl:
                base.syrk(F['Dfs'], F['S'], trans='T', beta=1.0,
                          partial=True)
            if H is not None:
                F['S'] += H
            if F['singular']:
                base.syrk(A, F['S'], trans='T', beta=1.0, partial=True)
            if type(F['S']) is matrix:
                lapack.potrf(F['S'])
            else:
                cholmod.numeric(F['S'], F['Sf'])

        if type(F['S']) is matrix:
            # Asct := L^{-1}*A'.  Factor K = Asct'*Asct.
            if type(A) is matrix:
                Asct = A.T
            else:
                Asct = matrix(A.T)
            blas.trsm(F['S'], Asct)
            blas.syrk(Asct, F['K'], trans='T')
            lapack.potrf(F['K'])

        else:
            # Asct := L^{-1}*P*A'.  Factor K = Asct'*Asct.
            if type(A) is matrix:
                Asct = A.T
                cholmod.solve(F['Sf'], Asct, sys=7)
                cholmod.solve(F['Sf'], Asct, sys=4)
                blas.syrk(Asct, F['K'], trans='T')
                lapack.potrf(F['K'])
            else:
                Asct = cholmod.spsolve(F['Sf'], A.T, sys=7)
                Asct = cholmod.spsolve(F['Sf'], Asct, sys=4)
                base.syrk(Asct, F['K'], trans='T')
                Kf = cholmod.symbolic(F['K'])
                cholmod.numeric(F['K'], Kf)

        def solve(x, y, z):

            # Solve
            #
            #     [ H          A'  GG'*W^{-1} ]   [ ux   ]   [ bx        ]
            #     [ A          0   0          ] * [ uy   ] = [ by        ]
            #     [ W^{-T}*GG  0   -I         ]   [ W*uz ]   [ W^{-T}*bz ]
            #
            # and return ux, uy, W*uz.
            #
            # If not F['singular']:
            #
            #     K*uy = A * S^{-1} * ( bx + GG'*W^{-1}*W^{-T}*bz ) - by
            #     S*ux = bx + GG'*W^{-1}*W^{-T}*bz - A'*uy
            #     W*uz = W^{-T} * ( GG*ux - bz ).
            #
            # If F['singular']:
            #
            #     K*uy = A * S^{-1} * ( bx + GG'*W^{-1}*W^{-T}*bz + A'*by )
            #            - by
            #     S*ux = bx + GG'*W^{-1}*W^{-T}*bz + A'*by - A'*y.
            #     W*uz = W^{-T} * ( GG*ux - bz ).

            # z := W^{-1} * z = W^{-1} * bz
            scale(z, W, trans='T', inverse='I')

            # If not F['singular']:
            #     x := L^{-1} * P * (x + GGs'*z)
            #        = L^{-1} * P * (x + GG'*W^{-1}*W^{-T}*bz)
            #
            # If F['singular']:
            #     x := L^{-1} * P * (x + GGs'*z + A'*y))
            #        = L^{-1} * P * (x + GG'*W^{-1}*W^{-T}*bz + A'*y)

            if mnl:
                base.gemv(F['Dfs'], z, x, trans='T', beta=1.0)
            base.gemv(F['Gs'], z, x, offsetx=mnl, trans='T',
                      beta=1.0)
            if F['singular']:
                base.gemv(A, y, x, trans='T', beta=1.0)
            if type(F['S']) is matrix:
                blas.trsv(F['S'], x)
            else:
                cholmod.solve(F['Sf'], x, sys=7)
                cholmod.solve(F['Sf'], x, sys=4)

            # y := K^{-1} * (Asc*x - y)
            #    = K^{-1} * (A * S^{-1} * (bx + GG'*W^{-1}*W^{-T}*bz) - by)
            #      (if not F['singular'])
            #    = K^{-1} * (A * S^{-1} * (bx + GG'*W^{-1}*W^{-T}*bz +
            #      A'*by) - by)
            #      (if F['singular']).

            base.gemv(Asct, x, y, trans='T', beta=-1.0)
            if type(F['K']) is matrix:
                lapack.potrs(F['K'], y)
            else:
                cholmod.solve(Kf, y)

            # x := P' * L^{-T} * (x - Asc'*y)
            #    = S^{-1} * (bx + GG'*W^{-1}*W^{-T}*bz - A'*y)
            #      (if not F['singular'])
            #    = S^{-1} * (bx + GG'*W^{-1}*W^{-T}*bz + A'*by - A'*y)
            #      (if F['singular'])

            base.gemv(Asct, y, x, alpha=-1.0, beta=1.0)
            if type(F['S']) is matrix:
                blas.trsv(F['S'], x, trans='T')
            else:
                cholmod.solve(F['Sf'], x, sys=5)
                cholmod.solve(F['Sf'], x, sys=8)

            # W*z := GGs*x - z = W^{-T} * (GG*x - bz)
            if mnl:
                base.gemv(F['Dfs'], x, z, beta=-1.0)
            base.gemv(F['Gs'], x, z, beta=-1.0, offsety=mnl)

        return solve
예제 #7
0
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}
예제 #8
0
    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)]
        B[:] = B[Itrans]
        B.size = (d,a)
        for ii in xrange(p*N):
            A[:,ii] = A[Itrans,ii]
    
    n = p*N
    if a < d:
        p, q = d, a
    else:
예제 #9
0
    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)]
        B[:] = B[Itrans]
        B.size = (d, a)
        for ii in xrange(p * N):
            A[:, ii] = A[Itrans, ii]

    n = p * N
    if a < d:
        p, q = d, a
    else: