@@ -0,0 +1,1970 @@
+#! /usr/bin/python%0A# -*- coding: utf-8 -*%0A%22%22%22%0A%0Aauthor: Atsushi Sakai%0A%0A%22%22%22%0A%0Aimport numpy as np%0Aimport matplotlib.pyplot as plt%0Aimport scipy.linalg%0A%0Afrom cvxopt import matrix%0Aimport cvxopt%0A%0A%0Adef get_mat_psi(A, N):%0A    psi = np.matrix(np.zeros((0, A.shape%5B1%5D)))%0A%0A    for i in range(1, N + 1):%0A        psi = np.vstack((psi, A ** i))%0A%0A    return psi%0A%0A%0Adef get_mat_gamma(A, B, N):%0A%0A    gamma = B%0A%0A    for i in range(1, N):%0A        gamma = np.vstack((gamma, (A ** i) * B + gamma%5B-1, :%5D))%0A%0A    #  print(gamma)%0A%0A    return gamma%0A%0A%0Adef get_mat_theta(A, B, N):%0A    AiB = B%0A    theta = np.kron(np.eye(N), AiB)%0A    for i in range(1, N):%0A        AiB = A * AiB%0A        theta += np.kron(np.diag(np.ones(N - i), -i), AiB)%0A%0A    #  print(theta)%0A    return theta%0A%0A%0Adef model_predictive_control(A, B, N, Q, R, T, x0, u0):%0A%0A    (nx, nu) = B.shape%0A%0A    du = np.matrix(%5B0.0%5D * N).T%0A%0A    psi = get_mat_psi(A, N)%0A    gamma = get_mat_gamma(A, B, N)%0A    theta = get_mat_theta(A, B, N)%0A%0A    QQ = scipy.linalg.block_diag(np.kron(np.eye(N), Q))%0A    RR = scipy.linalg.block_diag(np.kron(np.eye(N), R))%0A%0A    H = theta.T * QQ * theta + RR%0A    #  print(H)%0A    g = -2.0 * theta.T * QQ * (T - psi * x0 - gamma * u0)%0A    #  print(g)%0A%0A    P = matrix(H)%0A    q = matrix(g)%0A    sol = cvxopt.solvers.qp(P, q)%0A    #  print(sol%5B%22x%22%5D)%0A    du = np.matrix(sol%5B%22x%22%5D)%0A%0A    fx = psi * x0 + gamma * u0 + theta * du%0A%0A    ffx = fx.reshape(N, nx)%0A    ffx = np.vstack((x0.T, ffx))%0A    #  print(ffx)%0A%0A    plt.plot(ffx%5B:, 0%5D)%0A    plt.plot(ffx%5B:, 1%5D)%0A    plt.grid(True)%0A    plt.show()%0A%0A%0Adef test1():%0A    print(%22start!!%22)%0A    A = np.matrix(%5B%5B0.8, 1.0%5D, %5B0, 0.9%5D%5D)%0A    B = np.matrix(%5B%5B-1.0%5D, %5B2.0%5D%5D)%0A    (nx, nu) = B.shape%0A%0A    N = 100  # number of horizon%0A    Q = np.diag(%5B1, 1.0%5D)%0A    R = np.eye(nu)%0A%0A    x0 = np.matrix(%5B2.0, 1.0%5D).T%0A    u0 = np.matrix(%5B0.0%5D)%0A%0A    T = np.matrix(%5B1.0, 0.25%5D * N).T%0A    #  print(T)%0A%0A    model_predictive_control(A, B, N, Q, R, T, x0, u0)%0A%0A%0Aif __name__ == '__main__':%0A    DEBUG_ = True%0A    test1()%0A

add mpc tracking sample