control.py 文件源码

def compute_control(self):
        """
        Compute the optimal control p=[p[0] ... p[K]].
        """
        K, Phi, Psi, X_init = self.K, self.Phi, self.Psi, self.X_init

        # Cost 1: sum_i 1/K * (x_i - p_i) ** 2
        B0 = array([[1, 0], [0, 0]])
        C0 = array([1, 0])
        B = block_diag(*[B0 for i in xrange(K)])
        C = block_diag(*[C0 for i in xrange(K)])
        P1 = 1. / K * (dot(Psi.T, dot(B, Psi)) - 2 * dot(C, Psi) + eye(K))
        q1 = 1. / K * dot(dot(Phi, X_init).T, dot(B, Psi) - C.T)

        # Cost 2: sum_i (p_i - p_{i - 1}) ** 2
        N1, N2 = zeros((K, K)), zeros((K, K))
        N1[0:K - 1, 0:K - 1] = eye(K - 1)
        N2[0:K - 1, 1:K] = eye(K - 1)
        P2 = dot((N2 - N1).T, (N2 - N1))
        q2 = zeros(K)

        w1, w2 = 1., 100.
        P = w1 * P1 + w2 * P2
        q = w1 * q1 + w2 * q2
        G = vstack([self.I, -self.I])
        h = hstack([self.p_max, -self.p_min])
        A = vstack([self.Psi_last, hstack([zeros(K - 1), [1]])])
        b = hstack([self.X_target, [self.X_target[0]]]) \
            - hstack([dot(self.Phi_last, X_init), [0]])

        p = cvxopt_solve_qp(P, q, G, h, A, b)
        return p