!pip freeze

from sympy import init_printing
init_printing()

from sympy import symbols, Matrix, Symbol, zeros, eye

n = 3 # number of time samples in each cycle (step)
m = 2 # number of cycles (steps)
p = 2 # number of sensors
q = 2 # number of controls

mm = Matrix(q, 1, lambda i, j: Symbol('m_m_q={}'.format(i)))
mm

K = Matrix(q, p, lambda i, j: Symbol('k_q={},p={}'.format(i, j)))
K

ms = Matrix(q, 1, lambda i, j: Symbol('m^*_q={}'.format(i)))
ms

sm = Matrix(p, 1, lambda i, j: Symbol('s_m_p={}'.format(i)))
sm

zero = mm - ms + K * sm
zero

x = K.reshape(q * p, 1).col_join(ms)
x

A = zeros(q, q * p)
for row in range(q):
    A[row, row * p:(row + 1) * p] =  sm.T
A = A.row_join(-eye(q))
A

b = -mm
b

zero2 = A * x - b 
zero2

assert zero == zero2

gain_matrices = []
control_vectors = []
control_star_vectors = []
sensor_vectors = []
for time_step in range(n):
    gain_matrices.append(Matrix(q, p, lambda i, j: Symbol('k^{}_q={},p={}'.format(time_step, i, j))))
    control_vectors.append(Matrix(q, 1, lambda i, j: Symbol('m^{}_m_q={}'.format(time_step, i))))
    control_star_vectors.append(Matrix(q, 1, lambda i, j: Symbol('m^*{}_q={}'.format(time_step, i))))
    sensor_vectors.append(Matrix(p, 1, lambda i, j: Symbol('s^{}_m_p={}'.format(time_step, i))))

gain_matrices

control_vectors

control_star_vectors

sensor_vectors

zero = control_vectors[0] - control_star_vectors[0] + gain_matrices[0] * sensor_vectors[0]
zero

Am = zeros(n * q, n * (q + q * p))
for time_step in range(n):
    An = zeros(q, q * p)
    for row in range(q):
        An[row, row * p:(row + 1) * p] =  sensor_vectors[time_step].T
    An = An.row_join(-eye(q))
    num_rows, num_cols = An.shape
    Am[time_step * num_rows:(time_step + 1) * num_rows, time_step * num_cols:(time_step + 1) * num_cols] = An

Am

bm = -control_vectors[0]
for control in control_vectors[1:]:
    bm = bm.col_join(-control)
bm

xm = gain_matrices[0].reshape(q * p, 1).col_join(control_star_vectors[0])
for gain, control_star in zip(gain_matrices[1:], control_star_vectors[1:]):
    xm = xm.col_join(gain.reshape(q * p, 1)).col_join(control_star)
xm

Am * xm - bm

n * (p*q + q)

1 + p

Am.shape, bm.shape

b = bm
A = Am
for step in range(p):
    b = b.col_join(bm)
    A = A.col_join(Am)

A.shape, b.shape

x = A.LDLsolve(b)
x

type(x[0])