def compute_A(self):
    Ad = []
    for i in range(self.horizon - 1):
        f_i = np.array([[1.0, self.ds[i], 0.0],
                        [0.0, 1.0, self.ds[i]]])
        Ad.append(f_i)
    # print("Ad \n", Ad)
    Ax = sparse.csr_matrix(block_diag(*Ad))
    # print("Ax \n", Ax.toarray())
    f_2 = np.array([[-1.0, 0.0, 0.0],
                    [0.0, -1.0, 0.0]])
    Ay = sparse.kron(sparse.eye(self.horizon - 1), f_2)
    off_set = np.zeros(((self.horizon - 1)*2, 3))
    Ax = sparse.hstack([Ax, off_set])
    print("Ax  \n", Ax)

    Ay = sparse.hstack([off_set, Ay])
    Aeq = Ax + Ay  # 数学的加法
    print("Aeq.shape  \n", Aeq.toarray())