def test_BlockMatrix_Inverse_execution(): k, n = 2, 4 dtype = 'float32' A = sympy.MatrixSymbol('A', n, k) B = sympy.MatrixSymbol('B', n, n) inputs = A, B output = B.I*A cutsizes = {A: [(n/2, n/2), (k/2, k/2)], B: [(n/2, n/2), (n/2, n/2)]} cutinputs = [sympy.blockcut(i, *cutsizes[i]) for i in inputs] cutoutput = output.subs(dict(zip(inputs, cutinputs))) dtypes = dict(zip(inputs, [dtype]*len(inputs))) f = theano_function(inputs, [output], dtypes=dtypes) fblocked = theano_function(inputs, [sympy.block_collapse(cutoutput)], dtypes=dtypes) import numpy ninputs = [numpy.random.rand(*x.shape).astype(dtype) for x in inputs] ninputs = [numpy.arange(n*k).reshape(A.shape).astype(dtype), numpy.eye(n).astype(dtype)] ninputs[1] += numpy.ones(B.shape)*1e-5 assert numpy.allclose(f(*ninputs), fblocked(*ninputs), rtol=1e-5)
def canonical(self): """Returns the LMI positive (semi-)definite form with the matrix at the rhs and zero at the lhs. """ if self.gts.is_Matrix: if self.lts.is_Matrix: diff = self.gts - self.lts else: if isinstance(self, (LMI_PD, LMI_PSD)): return self # self is already in canonical form else: diff = self.gts else: diff = -self.lts diff = block_collapse(diff) if self.is_strict: return LMI_PD(diff, 0) else: return LMI_PSD(diff, 0)
from sympy import MatrixSymbol, BlockMatrix, block_collapse, latex from sympy.abc import n A, B, C, D, E, F, G, H = [MatrixSymbol(a, n, n) for a in 'ABCDEFGH'] X = BlockMatrix([[A, B], [C, D]]) Y = BlockMatrix([[E, F], [G, H]]) print latex(X * Y) print latex(block_collapse(X * Y)) print latex(block_collapse(X.I))
def discretizeF(self): Z33 = ZeroMatrix(3, 3) Z31 = ZeroMatrix(3, 1) Z13 = ZeroMatrix(1, 3) Z11 = ZeroMatrix(1, 1) I33 = Identity(3) I11 = Identity(1) F11 = Z33 F12 = MatrixSymbol('F12', 3, 3) F13 = MatrixSymbol('F13', 3, 3) F14 = Z33 F15 = Z31 F16 = Z31 F17 = Z33 F21 = Z33 F22 = MatrixSymbol('F22', 3, 3) F23 = MatrixSymbol('F23', 3, 3) F24 = MatrixSymbol('F24', 3, 3) F25 = MatrixSymbol('F25', 3, 1) F26 = Z31 F27 = Z33 F31 = Z33 F32 = Z33 F33 = MatrixSymbol('F33', 3, 3) F34 = I33 F35 = Z31 F36 = Z31 F37 = Z33 F41 = Z33 F42 = Z33 F43 = Z33 F44 = MatrixSymbol('F44', 3, 3) F45 = MatrixSymbol('F45', 3, 1) F46 = MatrixSymbol('F46', 3, 1) F47 = MatrixSymbol('F47', 3, 3) F51 = Z13 F52 = Z13 F53 = Z13 F54 = Z13 F55 = Z11 F56 = Z11 F57 = Z13 F61 = Z13 F62 = Z13 F63 = Z13 F64 = Z13 F65 = Z11 F66 = Z11 F67 = Z13 F71 = Z33 F72 = Z33 F73 = Z33 F74 = Z33 F75 = Z31 F76 = Z31 F77 = Z33 F_c = BlockMatrix([[F11, F12, F13, F14, F15, F16, F17], [F21, F22, F23, F24, F25, F26, F27], [F31, F32, F33, F34, F35, F36, F37], [F41, F42, F43, F44, F45, F46, F47], [F51, F52, F53, F54, F55, F56, F57], [F61, F62, F63, F64, F65, F66, F67], [F71, F72, F73, F74, F75, F76, F77]]) print(F_c) I_d = Identity(17) Delta_t = symbols('Delta_t') F_k = block_collapse(I_d + F_c * Delta_t) print(F_k) # k=6 G11 = Z33 G12 = Z33 G13 = Z33 G14 = Z33 G15 = Z33 G16 = Z33 G17 = Z33 G18 = Z33 GZ3 = BlockMatrix([[Z33, Z33, Z33, Z33, Z33, Z33, Z33, Z33]]) G21 = MatrixSymbol('G21', 3, 3) G22 = MatrixSymbol('G22', 3, 3) G23 = MatrixSymbol('G23', 3, 3) G24 = MatrixSymbol('G24', 3, 3) G25 = MatrixSymbol('G25', 3, 3) G26 = MatrixSymbol('G26', 3, 3) G27 = I33 G28 = Z33 G41 = MatrixSymbol('G41', 3, 3) G42 = MatrixSymbol('G42', 3, 3) G43 = MatrixSymbol('G43', 3, 3) G44 = MatrixSymbol('G44', 3, 3) G45 = MatrixSymbol('G45', 3, 3) G46 = MatrixSymbol('G46', 3, 3) G47 = Z33 G48 = I33 G51 = Z13 G52 = Z13 G53 = Z13 G54 = Z13 G55 = Z13 G56 = Z13 G57 = Z13 G58 = Z13 G_c = BlockMatrix([[G11, G12, G13, G14, G15, G16, G17, G18], [G21, G22, G23, G24, G25, G26, G27, G28], [G11, G12, G13, G14, G15, G16, G17, G18], [G41, G42, G43, G44, G45, G46, G47, G48], [G51, G52, G53, G54, G55, G56, G57, G58], [G51, G52, G53, G54, G55, G56, G57, G58], [G11, G12, G13, G14, G15, G16, G17, G18]]) print(G_c) Q11 = MatrixSymbol('sigma2_T', 3, 3) Q22 = Q11 Q33 = Q11 Q44 = Q11 Q55 = Q11 Q66 = Q11 Q77 = MatrixSymbol('sigma2_A', 3, 3) Q88 = MatrixSymbol('sigma2_M', 3, 3) Q_c = BlockDiagMatrix(Q11, Q22, Q33, Q44, Q55, Q66, Q77, Q88) Q_k = block_collapse(G_c * Q_c * G_c.T) Delta_t_2 = symbols('Delta_t_2') Q_k = block_collapse(Q_k * Delta_t_2) print(simplify(Q_k)) # Discussion P_p = MatrixSymbol('P_p', 3, 3) P_v = MatrixSymbol('P_v', 3, 3) P_q = MatrixSymbol('P_q', 3, 3) P_a = MatrixSymbol('P_a', 3, 3) P_t = MatrixSymbol('P_t', 1, 1) P_m = MatrixSymbol('P_m', 1, 1) P_j = MatrixSymbol('P_j', 3, 3) P0 = BlockDiagMatrix(P_p, P_v, P_q, P_a, P_t, P_m, P_j) F_kd = block_collapse(F_k - F_c*Delta_t + F_c) Q_kd = block_collapse(G_c * Q_c * G_c.T) P1 = block_collapse(F_kd * P0 * F_kd.T + Q_kd) #print(P1) P2 = block_collapse(F_kd * P1 * F_kd.T + Q_kd) print(P2) H = BlockMatrix([[I33, Z33, Z33, Z33, Z31, Z31, Z33], [Z33, Z33, I33, Z33, Z31, Z31, Z33]]) S2 = block_collapse(H * P2 * H.T + Identity(6)) #print(S2) K2 = block_collapse(P2 * H.T * S2.I) #print(K2) P2_upd = block_collapse((Identity(17) - K2*H)*P2) print(P2_upd)
def discretizeF(self): Z33 = ZeroMatrix(3, 3) Z31 = ZeroMatrix(3, 1) Z13 = ZeroMatrix(1, 3) Z11 = ZeroMatrix(1, 1) I33 = Identity(3) I11 = Identity(1) F11 = Z33 F12 = MatrixSymbol('F12', 3, 3) F13 = MatrixSymbol('F13', 3, 3) F14 = Z33 F15 = Z31 F16 = Z31 F17 = Z33 F21 = Z33 F22 = MatrixSymbol('F22', 3, 3) F23 = MatrixSymbol('F23', 3, 3) F24 = MatrixSymbol('F24', 3, 3) F25 = MatrixSymbol('F25', 3, 1) F26 = Z31 F27 = Z33 F31 = Z33 F32 = Z33 F33 = MatrixSymbol('F33', 3, 3) F34 = I33 F35 = Z31 F36 = Z31 F37 = Z33 F41 = Z33 F42 = Z33 F43 = Z33 F44 = MatrixSymbol('F44', 3, 3) F45 = MatrixSymbol('F45', 3, 1) F46 = MatrixSymbol('F46', 3, 1) F47 = MatrixSymbol('F47', 3, 3) F51 = Z13 F52 = Z13 F53 = Z13 F54 = Z13 F55 = Z11 F56 = Z11 F57 = Z13 F61 = Z13 F62 = Z13 F63 = Z13 F64 = Z13 F65 = Z11 F66 = Z11 F67 = Z13 F71 = Z33 F72 = Z33 F73 = Z33 F74 = Z33 F75 = Z31 F76 = Z31 F77 = Z33 F_c = BlockMatrix([[F11, F12, F13, F14, F15, F16, F17], [F21, F22, F23, F24, F25, F26, F27], [F31, F32, F33, F34, F35, F36, F37], [F41, F42, F43, F44, F45, F46, F47], [F51, F52, F53, F54, F55, F56, F57], [F61, F62, F63, F64, F65, F66, F67], [F71, F72, F73, F74, F75, F76, F77]]) print(F_c) I_d = Identity(17) Delta_t = symbols('Delta_t') F_k = block_collapse(I_d + F_c * Delta_t) print(F_k) # k=6 G11 = Z33 G12 = Z33 G13 = Z33 G14 = Z33 G15 = Z33 G16 = Z33 G17 = Z33 G18 = Z33 GZ3 = BlockMatrix([[Z33, Z33, Z33, Z33, Z33, Z33, Z33, Z33]]) G21 = MatrixSymbol('G21', 3, 3) G22 = MatrixSymbol('G22', 3, 3) G23 = MatrixSymbol('G23', 3, 3) G24 = MatrixSymbol('G24', 3, 3) G25 = MatrixSymbol('G25', 3, 3) G26 = MatrixSymbol('G26', 3, 3) G27 = I33 G28 = Z33 G41 = MatrixSymbol('G41', 3, 3) G42 = MatrixSymbol('G42', 3, 3) G43 = MatrixSymbol('G43', 3, 3) G44 = MatrixSymbol('G44', 3, 3) G45 = MatrixSymbol('G45', 3, 3) G46 = MatrixSymbol('G46', 3, 3) G47 = Z33 G48 = I33 G51 = Z13 G52 = Z13 G53 = Z13 G54 = Z13 G55 = Z13 G56 = Z13 G57 = Z13 G58 = Z13 G_c = BlockMatrix([[G11, G12, G13, G14, G15, G16, G17, G18], [G21, G22, G23, G24, G25, G26, G27, G28], [G11, G12, G13, G14, G15, G16, G17, G18], [G41, G42, G43, G44, G45, G46, G47, G48], [G51, G52, G53, G54, G55, G56, G57, G58], [G51, G52, G53, G54, G55, G56, G57, G58], [G11, G12, G13, G14, G15, G16, G17, G18]]) print(G_c) Q11 = MatrixSymbol('sigma2_T', 3, 3) Q22 = Q11 Q33 = Q11 Q44 = Q11 Q55 = Q11 Q66 = Q11 Q77 = MatrixSymbol('sigma2_A', 3, 3) Q88 = MatrixSymbol('sigma2_M', 3, 3) Q_c = BlockDiagMatrix(Q11, Q22, Q33, Q44, Q55, Q66, Q77, Q88) Q_k = block_collapse(G_c * Q_c * G_c.T) Delta_t_2 = symbols('Delta_t_2') Q_k = block_collapse(Q_k * Delta_t_2) print(simplify(Q_k)) # Discussion P_p = MatrixSymbol('P_p', 3, 3) P_v = MatrixSymbol('P_v', 3, 3) P_q = MatrixSymbol('P_q', 3, 3) P_a = MatrixSymbol('P_a', 3, 3) P_t = MatrixSymbol('P_t', 1, 1) P_m = MatrixSymbol('P_m', 1, 1) P_j = MatrixSymbol('P_j', 3, 3) P0 = BlockDiagMatrix(P_p, P_v, P_q, P_a, P_t, P_m, P_j) F_kd = block_collapse(F_k - F_c * Delta_t + F_c) Q_kd = block_collapse(G_c * Q_c * G_c.T) P1 = block_collapse(F_kd * P0 * F_kd.T + Q_kd) #print(P1) P2 = block_collapse(F_kd * P1 * F_kd.T + Q_kd) print(P2) H = BlockMatrix([[I33, Z33, Z33, Z33, Z31, Z31, Z33], [Z33, Z33, I33, Z33, Z31, Z31, Z33]]) S2 = block_collapse(H * P2 * H.T + Identity(6)) #print(S2) K2 = block_collapse(P2 * H.T * S2.I) #print(K2) P2_upd = block_collapse((Identity(17) - K2 * H) * P2) print(P2_upd)