def check_primal_feasibility(self, places):
     all_cons = [c for c in self.constraints]  # shallow copy
     for x in self.prob.variables():
         attrs = x.attributes
         if attrs['nonneg'] or attrs['pos']:
             all_cons.append(x >= 0)
         elif attrs['nonpos'] or attrs['neg']:
             all_cons.append(x <= 0)
         elif attrs['imag']:
             all_cons.append(x + cp.conj(x) == 0)
         elif attrs['symmetric']:
             all_cons.append(x == x.T)
         elif attrs['diag']:
             all_cons.append(x - cp.diag(cp.diag(x)) == 0)
         elif attrs['PSD']:
             all_cons.append(x >> 0)
         elif attrs['NSD']:
             all_cons.append(x << 0)
         elif attrs['hermitian']:
             all_cons.append(x == cp.conj(x.T))
         elif attrs['boolean'] or attrs['integer']:
             round_val = np.round(x.value)
             all_cons.append(x == round_val)
     for con in all_cons:
         viol = con.violation()
         if isinstance(viol, np.ndarray):
             viol = np.linalg.norm(viol, ord=2)
         self.tester.assertAlmostEqual(viol, 0, places)
Ejemplo n.º 2
0
    def test_affine_atoms_canon(self):
        """Test canonicalization for affine atoms.
        """
        # Scalar.
        x = Variable()
        expr = cvx.imag(x + 1j * x)
        prob = Problem(Minimize(expr), [x >= 0])
        result = prob.solve()
        self.assertAlmostEqual(result, 0)
        self.assertAlmostEqual(x.value, 0)

        x = Variable(imag=True)
        expr = 1j * x
        prob = Problem(Minimize(expr), [cvx.imag(x) <= 1])
        result = prob.solve()
        self.assertAlmostEqual(result, -1)
        self.assertAlmostEqual(x.value, 1j)

        x = Variable(2)
        expr = x / 1j
        prob = Problem(Minimize(expr[0] * 1j + expr[1] * 1j),
                       [cvx.real(x + 1j) >= 1])
        result = prob.solve()
        self.assertAlmostEqual(result, -np.inf)
        prob = Problem(Minimize(expr[0] * 1j + expr[1] * 1j),
                       [cvx.real(x + 1j) <= 1])
        result = prob.solve()
        self.assertAlmostEqual(result, -2)
        self.assertItemsAlmostEqual(x.value, [1, 1])
        prob = Problem(
            Minimize(expr[0] * 1j + expr[1] * 1j),
            [cvx.real(x + 1j) >= 1, cvx.conj(x) <= 0])
        result = prob.solve()
        self.assertAlmostEqual(result, np.inf)

        x = Variable((2, 2))
        y = Variable((3, 2), complex=True)
        expr = cvx.vstack([x, y])
        prob = Problem(Minimize(cvx.sum(cvx.imag(cvx.conj(expr)))),
                       [x == 0, cvx.real(y) == 0,
                        cvx.imag(y) <= 1])
        result = prob.solve()
        self.assertAlmostEqual(result, -6)
        self.assertItemsAlmostEqual(y.value, 1j * np.ones((3, 2)))
        self.assertItemsAlmostEqual(x.value, np.zeros((2, 2)))

        x = Variable((2, 2))
        y = Variable((3, 2), complex=True)
        expr = cvx.vstack([x, y])
        prob = Problem(Minimize(cvx.sum(cvx.imag(expr.H))),
                       [x == 0, cvx.real(y) == 0,
                        cvx.imag(y) <= 1])
        result = prob.solve()
        self.assertAlmostEqual(result, -6)
        self.assertItemsAlmostEqual(y.value, 1j * np.ones((3, 2)))
        self.assertItemsAlmostEqual(x.value, np.zeros((2, 2)))
Ejemplo n.º 3
0
    def test_hermitian(self):
        """Test Hermitian variables.
        """
        with self.assertRaises(Exception) as cm:
            v = Variable((4, 3), hermitian=True)
        self.assertEqual(
            str(cm.exception),
            "Invalid dimensions (4, 3). Must be a square matrix.")

        v = Variable((2, 2), hermitian=True)
        assert v.is_hermitian()
        # v = Variable((2,2), PSD=True)
        # assert v.is_symmetric()
        # v = Variable((2,2), NSD=True)
        # assert v.is_symmetric()
        v = Variable((2, 2), diag=True)
        assert v.is_hermitian()

        v = Variable((2, 2), hermitian=True)
        expr = v + v
        assert expr.is_hermitian()
        expr = -v
        assert expr.is_hermitian()
        expr = v.T
        assert expr.is_hermitian()
        expr = cp.real(v)
        assert expr.is_hermitian()
        expr = cp.imag(v)
        assert expr.is_hermitian()
        expr = cp.conj(v)
        assert expr.is_hermitian()
        expr = cp.promote(Variable(), (2, 2))
        assert expr.is_hermitian()
Ejemplo n.º 4
0
def bqpd_c(choi_ideal, choi_mats, n_qubits, c_fac_budget=None, cp_constraint=True, tp_constraint=False, eps=1e-6, use_mosek=False):
    """ approximate QPD with gamma factor factor budget c_fac_budget"""
    n_ops = len(choi_mats)
    dim = choi_ideal.shape[0]
    halfdim = int(true_np.sqrt(dim))
    Y0 = cvxpy.Variable((dim,dim), hermitian=True)
    Y1 = cvxpy.Variable((dim,dim), hermitian=True)
    a = list()
    for i in range(n_ops): a.append(cvxpy.Variable())

    C1 = sum([a[i]*choi_mats[i] for i in range(n_ops)])
    C = C1 - choi_ideal
    bigmatrix = cvxpy.bmat([[Y0, -C],[-cvxpy.conj(C.T), Y1]])
    constraints = [
        bigmatrix >> 0
    ]
    if cp_constraint: constraints.append( C1 >> 0)
    if tp_constraint: constraints.append( cvxpy_partial_trace(C1, (2**n_qubits,2**n_qubits), 1) == true_np.identity(2**n_qubits) )
    if c_fac_budget is not None:
        constraints.append( sum([cvxpy.abs(a[i]) for i in range(n_ops)]) <= c_fac_budget )
    loss = 0.5*cvxpy.norm(cvxpy_partial_trace(Y0, (halfdim,halfdim), 1)) + 0.5*cvxpy.norm(cvxpy_partial_trace(Y1, (halfdim,halfdim), 1))

    prob = cvxpy.Problem(cvxpy.Minimize(loss), constraints)
    if use_mosek:
        prob.solve(solver="MOSEK")
    else:
        prob.solve(max_iters=int(1e8), eps=eps, solver="SCS")
    assert prob.status == "optimal"
    return [float(a[i].value) for i in range(n_ops)], loss.value
Ejemplo n.º 5
0
    def test_symmetric(self) -> None:
        """Test symmetric variables.
        """
        with self.assertRaises(Exception) as cm:
            v = Variable((4, 3), symmetric=True)
        self.assertEqual(
            str(cm.exception),
            "Invalid dimensions (4, 3). Must be a square matrix.")

        v = Variable((2, 2), symmetric=True)
        assert v.is_symmetric()
        v = Variable((2, 2), PSD=True)
        assert v.is_symmetric()
        v = Variable((2, 2), NSD=True)
        assert v.is_symmetric()
        v = Variable((2, 2), diag=True)
        assert v.is_symmetric()
        assert self.a.is_symmetric()
        assert not self.A.is_symmetric()

        v = Variable((2, 2), symmetric=True)
        expr = v + v
        assert expr.is_symmetric()
        expr = -v
        assert expr.is_symmetric()
        expr = v.T
        assert expr.is_symmetric()
        expr = cp.real(v)
        assert expr.is_symmetric()
        expr = cp.imag(v)
        assert expr.is_symmetric()
        expr = cp.conj(v)
        assert expr.is_symmetric()
        expr = cp.promote(Variable(), (2, 2))
        assert expr.is_symmetric()
Ejemplo n.º 6
0
 def test_conj(self) -> None:
     """Test conj.
     """
     v = cp.Variable((4, ))
     obj = cp.Minimize(cp.sum(v))
     prob = cp.Problem(obj, [cp.conj(v) >= 1])
     prob.solve(solver=cp.SCS)
     assert np.allclose(v.value, np.ones((4, )))
Ejemplo n.º 7
0
 def test_conj(self):
     """Test imag.
     """
     A = np.ones((2, 2))
     expr = Constant(A) + 1j*Constant(A)
     expr = cvx.conj(expr)
     assert not expr.is_real()
     assert expr.is_complex()
     assert not expr.is_imag()
     self.assertItemsAlmostEqual(expr.value, A - 1j*A)
    def _quantum_rel_entropy_problem(self, rho, key_map_povm, m, k):
        mat_size = rho.shape[0]
        opt_mat_sh = (mat_size**2, mat_size**2)
        # Define cvxpy variables
        M = [cvx.Variable(opt_mat_sh, hermitian=True) for _ in range(k)]
        tau = cvx.Variable()
        T = cvx.Variable(m)

        key_map_povm_ = [
            np.kron(povm, np.eye(self.dim_B)) for povm in key_map_povm
        ]
        cq_rho = np.sum(povm @ rho @ povm for povm in key_map_povm_)

        X = general_kron(rho, np.eye(mat_size))
        Y = cvx.kron(np.eye(mat_size), cvx.conj(cq_rho))
        M.insert(0, Y)  # M[0] = Y
        Z = M[-1]  # M[k] = Z

        # Constraints related to matrix geometric mean cone
        const_geo_mean_cone = []
        for i in range(k):
            M_matrix = cvx.bmat([[M[i], M[i + 1]], [M[i + 1], X]])
            const_geo_mean_cone.append((M_matrix >> 0))

        # Constraints related to operator relative entropy cone
        e = np.reshape(np.eye(mat_size), (-1, 1), order="C")
        s, w = self.leggauss_zero_to_one(m)

        const_rel_entropy = []
        eXe = e.T @ X @ e
        eX = e.T @ X
        Xe = X @ e
        for j in range(m):
            T_matrix = cvx.bmat([[eXe - s[j] * T[j] / w[j], eX],
                                 [Xe, X + s[j] * (Z - X)]])
            const_rel_entropy.append((T_matrix >> 0))
        const_rel_entropy.append((np.power(2, k) * cvx.sum(T) + tau >= 0))
        constraints = const_geo_mean_cone + const_rel_entropy

        obj = cvx.Minimize(tau)
        problem = cvx.Problem(obj, constraints)
        return problem
Ejemplo n.º 9
0
    def get_primal_problem(self, m=2, k=2):

        mat_size = np.prod(self.dims)  # size of rho_AB
        opt_mat_sh = (mat_size**2, mat_size**2)

        # Define cvxpy variables
        M = [cvx.Variable(opt_mat_sh, hermitian=True) for _ in range(k)]
        rho_AB = cvx.Variable((mat_size, mat_size), hermitian=True)
        tau = cvx.Variable()
        T = cvx.Variable(m)

        cq_rho_AB = np.sum(povm @ rho_AB @ povm for povm in self._key_map_povm)

        X = general_kron(rho_AB, np.eye(mat_size))
        Y = cvx.kron(np.eye(mat_size), cvx.conj(cq_rho_AB))
        M.insert(0, Y)  # M[0] = Y
        Z = M[-1]  # M[k] = Z

        # Constraints related to matrix geometric mean cone
        const_geo_mean_cone = []
        for i in range(k):
            M_matrix = cvx.bmat([[M[i], M[i + 1]], [M[i + 1], X]])
            const_geo_mean_cone.append((M_matrix >> 0))

        # Constraints related to operator relative entropy cone
        e = np.reshape(np.eye(mat_size), (-1, 1), order="C")
        s, w = self.leggauss_zero_to_one(m)

        const_rel_entropy = []
        eXe = e.T @ X @ e
        eX = e.T @ X
        Xe = X @ e
        for j in range(m):
            T_matrix = cvx.bmat([[eXe - s[j] * T[j] / w[j], eX],
                                 [Xe, X + s[j] * (Z - X)]])
            const_rel_entropy.append((T_matrix >> 0))
        const_rel_entropy.append((np.power(2, k) * cvx.sum(T) + tau >= 0))

        # Constraints related to the state rho_AB
        const_rho_AB = const_rho_AB_ub = const_rho_AB_lb = []
        if self.Gamma_exact is not None:
            const_rho_AB = [(cvx.trace(rho_AB * G) == g)
                            for g, G in zip(self.gamma, self.Gamma_exact)]
        if self.Gamma_inexact is not None:
            const_rho_AB_ub = [
                (cvx.real(cvx.trace(rho_AB * G)) <= g_ub)
                for g_ub, G in zip(self.gamma_ub, self.Gamma_inexact)
            ]
            const_rho_AB_lb = [
                (cvx.real(cvx.trace(rho_AB * G)) >= g_lb)
                for g_lb, G in zip(self.gamma_lb, self.Gamma_inexact)
            ]

        # Other constraints
        const_rho_normalized = (cvx.trace(rho_AB) == 1)
        const_rho_pos = (rho_AB >> 0)
        constraints = [const_rho_normalized, const_rho_pos] + \
            const_rho_AB + const_rho_AB_ub + const_rho_AB_lb + \
            const_geo_mean_cone + const_rel_entropy

        obj = cvx.Minimize(tau)
        problem = cvx.Problem(obj, constraints)
        return problem
def getControllerParams(y, M):
    """
    This function generates the response time optimal controller parameters for a permanent
    magnet motor, resulting in zero torque ripple under no measurement noise. It is assumed, for simplicity,
    that the control inputs are the currents instead of voltages. These currents, once known, can be
    used to determine the control voltage inputs.
    :param y: This is the torque vs theta function, which makes the permanent magnet motor
    model nonlinear.
    :param M: This is the order of the Fourier series approximation of the nonlinearity.
    :return: Returns the controller parameters, without the proportionality constant.
    """
    plt.figure()
    plt.plot(np.linspace(-np.pi, np.pi, len(y)), y)
    plt.title('The plot of f(theta)')

    p = np.zeros(M)
    q = np.zeros(M)
    for k in range(M):
        p[k] = 2 * (np.cos(
            (k + 1) * np.linspace(-np.pi, np.pi, len(y))) @ y) / len(y)
        q[k] = 2 * (np.sin(
            (k + 1) * np.linspace(-np.pi, np.pi, len(y))) @ y) / len(y)

    s = np.zeros(len(y))
    x = np.linspace(-np.pi, np.pi, len(y))
    for k in range(len(y)):
        s[k] = (np.cos(np.linspace(1, M, M) * x[k]) @ p +
                np.sin(np.linspace(1, M, M) * x[k]) @ q)

    plt.figure()
    plt.plot(s)
    plt.plot(y)
    plt.title('Comparison with the Fourier truncation')

    Z = np.zeros([4 * M + 1, 2 * M])

    #cos and cos
    for k in range(1, M + 1):
        for l in range(1, M + 1):
            if (l == k):
                Z[0, l - 1] = Z[0, l - 1] + p[k - 1] / 2
                Z[2 * (k + l) - 1,
                  l - 1] = Z[2 * (k + l) - 1, l - 1] + p[k - 1] / 2
            else:
                Z[2 * (k + l) - 1,
                  l - 1] = Z[2 * (k + l) - 1, l - 1] + p[k - 1] / 2
                Z[2 * np.abs(k - l) - 1,
                  l - 1] = Z[2 * abs(k - l) - 1, l - 1] + p[k - 1] / 2

    #cos and sin
    for k in range(1, M + 1):
        for l in range(1, M + 1):
            if (l == k):
                Z[2 * (k + l) + 1 - 1,
                  l + M - 1] = Z[2 * (k + l) + 1 - 1, l + M - 1] + p[k - 1] / 2
            else:
                Z[2 * (k + l) + 1 - 1,
                  l + M - 1] = Z[2 * (k + l) + 1 - 1, l + M - 1] + p[k - 1] / 2
                Z[2 * abs(k - l) + 1 - 1,
                  l + M - 1] = Z[2 * abs(k - l) + 1 - 1,
                                 l + M - 1] + np.sign(l - k) * p[k - 1] / 2

    # sin and cos
    for k in range(1, M + 1):
        for l in range(1, M + 1):
            if (l == k):
                Z[2 * (k + l) + 1 - 1,
                  l - 1] = Z[2 * (k + l) + 1 - 1, l - 1] + q[k - 1] / 2
            else:
                Z[2 * (k + l) + 1 - 1,
                  l - 1] = Z[2 * (k + l) + 1 - 1, l - 1] + q[k - 1] / 2
                Z[2 * np.abs(k - l) + 1 - 1,
                  l - 1] = Z[2 * np.abs(k - l) + 1 - 1,
                             l - 1] + np.sign(k - l) * q[k - 1] / 2

    # sin and sin
    for k in range(1, M + 1):
        for l in range(1, M + 1):
            if (l == k):
                Z[1 - 1, l + M - 1] = Z[1 - 1, l + M - 1] + q[k - 1] / 2
                Z[2 * (k + l) - 1,
                  l + M - 1] = Z[2 * (k + l) - 1, l + M - 1] - q[k - 1] / 2
            else:
                Z[2 * (k + l) - 1,
                  l + M - 1] = Z[2 * (k + l) - 1, l + M - 1] - q[k - 1] / 2
                Z[2 * abs(k - l) - 1,
                  l + M - 1] = Z[2 * abs(k - l) - 1, l + M - 1] + q[k - 1] / 2

    A = np.zeros([4 * M, 4 * M])
    for i in range(1, 2 * M + 1):
        A[2 * (i - 1), 2 * (i - 1)] = np.cos(2 * np.pi * i / 3)
        A[2 * (i - 1), 2 * i - 1] = np.sin(2 * np.pi * i / 3)
        A[2 * i - 1, 2 * (i - 1)] = -np.sin(2 * np.pi * i / 3)
        A[2 * i - 1, 2 * i - 1] = np.cos(2 * np.pi * i / 3)

    A = np.vstack((np.hstack(
        (1, np.zeros(4 * M))), np.hstack((np.zeros([4 * M, 1]), A))))

    B = np.zeros([4 * M, 4 * M])
    for i in range(1, 2 * M + 1):
        B[2 * (i - 1), 2 * (i - 1)] = np.cos(4 * np.pi * i / 3)
        B[2 * (i - 1), 2 * i - 1] = np.sin(4 * np.pi * i / 3)
        B[2 * i - 1, 2 * (i - 1)] = -np.sin(4 * np.pi * i / 3)
        B[2 * i - 1, 2 * i - 1] = np.cos(4 * np.pi * i / 3)

    B = np.vstack((np.hstack(
        (1, np.zeros(4 * M))), np.hstack((np.zeros([4 * M, 1]), B))))

    G = np.hstack((Z, A @ Z, B @ Z))

    As1 = np.vstack((np.hstack(
        (np.zeros(M - 1), 0)), np.hstack((np.eye(M - 1), np.zeros([M - 1,
                                                                   1])))))
    Bs1 = np.vstack((1, np.zeros([M - 1, 1])))
    As2 = np.vstack((np.hstack(
        (np.zeros(M - 1), 0)), np.hstack((np.eye(M - 1), np.zeros([M - 1,
                                                                   1])))))
    Bs2 = np.vstack((1, np.zeros([M - 1, 1])))
    As3 = np.vstack((np.hstack(
        (np.zeros(M - 1), 0)), np.hstack((np.eye(M - 1), np.zeros([M - 1,
                                                                   1])))))
    Bs3 = np.vstack((1, np.zeros([M - 1, 1])))

    p1 = cvx.Variable((1, M))
    q1 = cvx.Variable((1, M))
    p2 = cvx.Variable((1, M))
    q2 = cvx.Variable((1, M))
    p3 = cvx.Variable((1, M))
    q3 = cvx.Variable((1, M))
    Q1l = cvx.Variable((M, M), hermitian=True)
    Q2l = cvx.Variable((M, M), hermitian=True)
    Q3l = cvx.Variable((M, M), hermitian=True)
    Q1u = cvx.Variable((M, M), hermitian=True)
    Q2u = cvx.Variable((M, M), hermitian=True)
    Q3u = cvx.Variable((M, M), hermitian=True)
    Ds1u = cvx.Variable()
    Ds2u = cvx.Variable()
    Ds3u = cvx.Variable()
    Ds1l = cvx.Variable()
    Ds2l = cvx.Variable()
    Ds3l = cvx.Variable()
    z = cvx.Variable()
    Cs1u = cvx.Variable((1, M), complex=True)
    Cs2u = cvx.Variable((1, M), complex=True)
    Cs3u = cvx.Variable((1, M), complex=True)
    Cs1l = cvx.Variable((1, M), complex=True)
    Cs2l = cvx.Variable((1, M), complex=True)
    Cs3l = cvx.Variable((1, M), complex=True)
    r1u = cvx.Variable((1, M), complex=True)
    r2u = cvx.Variable((1, M), complex=True)
    r3u = cvx.Variable((1, M), complex=True)
    r1l = cvx.Variable((1, M), complex=True)
    r2l = cvx.Variable((1, M), complex=True)
    r3l = cvx.Variable((1, M), complex=True)

    constraints = []
    constraints = constraints + [
        G @ (cvx.hstack((p1, q1, p2, q2, p3, q3)).T) == np.vstack(
            (1, np.zeros([4 * M, 1])))
    ]
    constraints = constraints + [
        cvx.real(r1u) == -p1 / 2,
        cvx.imag(r1u) == q1 / 2
    ]
    constraints = constraints + [
        cvx.real(r2u) == -p2 / 2,
        cvx.imag(r2u) == q2 / 2
    ]
    constraints = constraints + [
        cvx.real(r3u) == -p3 / 2,
        cvx.imag(r3u) == q3 / 2
    ]
    constraints = constraints + [
        cvx.real(r1l) == p1 / 2,
        cvx.imag(r1l) == -q1 / 2
    ]
    constraints = constraints + [
        cvx.real(r2l) == p2 / 2,
        cvx.imag(r2l) == -q2 / 2
    ]
    constraints = constraints + [
        cvx.real(r3l) == p3 / 2,
        cvx.imag(r3l) == -q3 / 2
    ]
    constraints = constraints + [Cs1u == r1u]
    constraints = constraints + [Cs2u == r2u]
    constraints = constraints + [Cs3u == r3u]
    constraints = constraints + [Cs1l == r1l]
    constraints = constraints + [Cs2l == r2l]
    constraints = constraints + [Cs3l == r3l]
    constraints = constraints + [Ds1u == z / 2]
    constraints = constraints + [Ds2u == z / 2]
    constraints = constraints + [Ds3u == z / 2]
    constraints = constraints + [Ds1l == z / 2]
    constraints = constraints + [Ds2l == z / 2]
    constraints = constraints + [Ds3l == z / 2]
    constraints = constraints + [
        cvx.vstack(
            (cvx.hstack((Q1u - (As1.T) @ Q1u @ As1, -(As1.T) @ Q1u @ Bs1)),
             cvx.hstack((-cvx.conj(
                 (As1.T) @ Q1u @ Bs1).T, -(Bs1.T) @ Q1u @ Bs1)))) + cvx.vstack(
                     (cvx.hstack((np.zeros([M, M]), -cvx.conj(Cs1u).T)),
                      cvx.hstack(
                          (-Cs1u, cvx.reshape(Ds1u + Ds1u, [1, 1]))))) >> 0
    ]
    constraints = constraints + [
        cvx.vstack(
            (cvx.hstack((Q2u - (As2.T) @ Q2u @ As2, -(As2.T) @ Q2u @ Bs2)),
             cvx.hstack((-cvx.conj(
                 (As2.T) @ Q2u @ Bs2).T, -(Bs2.T) @ Q2u @ Bs2)))) + cvx.vstack(
                     (cvx.hstack((np.zeros([M, M]), -cvx.conj(Cs2u).T)),
                      cvx.hstack(
                          (-Cs2u, cvx.reshape(Ds2u + Ds2u, [1, 1]))))) >> 0
    ]
    constraints = constraints + [
        cvx.vstack(
            (cvx.hstack((Q3u - (As3.T) @ Q3u @ As3, -(As3.T) @ Q3u @ Bs3)),
             cvx.hstack((-cvx.conj(
                 (As3.T) @ Q3u @ Bs3).T, -(Bs3.T) @ Q3u @ Bs3)))) + cvx.vstack(
                     (cvx.hstack((np.zeros([M, M]), -cvx.conj(Cs3u).T)),
                      cvx.hstack(
                          (-Cs3u, cvx.reshape(Ds3u + Ds3u, [1, 1]))))) >> 0
    ]
    constraints = constraints + [
        cvx.vstack(
            (cvx.hstack((Q1l - (As1.T) @ Q1l @ As1, -(As1.T) @ Q1l @ Bs1)),
             cvx.hstack((-cvx.conj(
                 (As1.T) @ Q1l @ Bs1).T, -(Bs1.T) @ Q1l @ Bs1)))) + cvx.vstack(
                     (cvx.hstack((np.zeros([M, M]), -cvx.conj(Cs1l).T)),
                      cvx.hstack(
                          (-Cs1l, cvx.reshape(Ds1l + Ds1l, [1, 1]))))) >> 0
    ]
    constraints = constraints + [
        cvx.vstack(
            (cvx.hstack((Q2l - (As2.T) @ Q2l @ As2, -(As2.T) @ Q2l @ Bs2)),
             cvx.hstack((-cvx.conj(
                 (As2.T) @ Q2l @ Bs2).T, -(Bs2.T) @ Q2l @ Bs2)))) + cvx.vstack(
                     (cvx.hstack((np.zeros([M, M]), -cvx.conj(Cs2l).T)),
                      cvx.hstack(
                          (-Cs2l, cvx.reshape(Ds2l + Ds2l, [1, 1]))))) >> 0
    ]
    constraints = constraints + [
        cvx.vstack(
            (cvx.hstack((Q3l - (As3.T) @ Q3l @ As3, -(As3.T) @ Q3l @ Bs3)),
             cvx.hstack((-cvx.conj(
                 (As3.T) @ Q3l @ Bs3).T, -(Bs3.T) @ Q3l @ Bs3)))) + cvx.vstack(
                     (cvx.hstack((np.zeros([M, M]), -cvx.conj(Cs3l).T)),
                      cvx.hstack(
                          (-Cs3l, cvx.reshape(Ds3l + Ds3l, [1, 1]))))) >> 0
    ]

    prob = cvx.Problem(cvx.Minimize(z), constraints)
    prob.solve(solver=cvx.SCS, verbose=True)

    return p, q, z.value, p1.value, p2.value, p3.value, q1.value, q2.value, q3.value