def opt_mpc_with_state_constr(A, B, N, Q, R, P, x0, xmin=None, xmax=None, umax=None, umin=None):
"""
optimize MPC problem with state and (or) input constraints
return
x: state
u: input
"""
(nx, nu) = B.shape
H = scipy.linalg.block_diag(np.kron(np.eye(N), R), np.kron(np.eye(N - 1), Q), np.eye(P.shape[0]))
# print(H)
# calc Ae
Aeu = np.kron(np.eye(N), -B)
# print(Aeu)
# print(Aeu.shape)
Aex = scipy.linalg.block_diag(np.eye((N - 1) * nx), P)
Aex -= np.kron(np.diag([1.0] * (N - 1), k=-1), A)
# print(Aex)
# print(Aex.shape)
Ae = np.hstack((Aeu, Aex))
# print(Ae.shape)
# calc be
be = np.vstack((A, np.zeros(((N - 1) * nx, nx)))) * x0
# print(be)
# np.set_printoptions(precision=3)
# print(H.shape)
# print(H)
# print(np.zeros((N * nx + N * nu, 1)))
# print(Ae)
# print(be)
# === optimization ===
q = np.zeros((N * nx + N * nu, 1))
if umax is None and umin is None:
sol = pyecosqp.ecosqp(H, q, Aeq=Ae, Beq=be)
else:
G, h = generate_inequalities_constraints_mat(N, nx, nu, xmin, xmax, umin, umax)
sol = pyecosqp.ecosqp(H, q, A=G, B=h, Aeq=Ae, Beq=be)
# print(sol)
fx = np.matrix(sol["x"])
u = fx[0, 0:N * nu].reshape(N, nu).T
x = fx[0, -N * nx:].reshape(N, nx).T
x = np.hstack((x0, x))
# print(x)
# print(u)
return x, u
def model_predictive_control(A, B, N, Q, R, T, x0, u0,
mindu=None, maxdu=None, minu=None, maxu=None, maxx=None, minx=None):
(nx, nu) = B.shape
du = np.matrix([0.0] * N).T
psi = get_mat_psi(A, N)
gamma = get_mat_gamma(A, B, N)
theta = get_mat_theta(A, B, N)
QQ = scipy.linalg.block_diag(np.kron(np.eye(N), Q))
RR = scipy.linalg.block_diag(np.kron(np.eye(N), R))
H = theta.T * QQ * theta + RR
g = - theta.T * QQ * (T - psi * x0 - gamma * u0)
# print(H)
# print(g)
# print(u0)
G = np.zeros((0, nu * N))
h = np.zeros((0, nu))
G, h = generate_du_constraints_mat(G, h, N, nu, mindu, maxdu)
G, h = generate_u_constraints_mat(G, h, N, nu, u0, minu, maxu)
kappa = psi * x0 + gamma * u0
G, h = generate_x_constraints_mat(
G, h, N, nx, u0, theta, kappa, minx, maxx)
# print(H)
# print(g)
# print(G)
# print(h)
sol = pyecosqp.ecosqp(H, g, A=G, B=h)
du = np.matrix(sol["x"]).T
# print(du)
fx = psi * x0 + gamma * u0 + theta * du
ffx = fx.reshape(N, nx)
ffx = np.vstack((x0.T, ffx))
# print(ffx)
u = np.cumsum(du).T + u0
# print(u)
return ffx, u, du
def model_predictive_control(A, B, N, Q, R, T, x0, u0, mindu=None, maxdu=None, minu=None, maxu=None, maxx=None, minx=None):
(nx, nu) = B.shape
du = np.matrix([0.0] * N).T
psi = get_mat_psi(A, N)
gamma = get_mat_gamma(A, B, N)
theta = get_mat_theta(A, B, N)
QQ = scipy.linalg.block_diag(np.kron(np.eye(N), Q))
RR = scipy.linalg.block_diag(np.kron(np.eye(N), R))
H = theta.T * QQ * theta + RR
g = - theta.T * QQ * (T - psi * x0 - gamma * u0)
# print(H)
# print(g)
# print(u0)
G = np.zeros((0, nu * N))
h = np.zeros((0, nu))
G, h = generate_du_constraints_mat(G, h, N, nu, mindu, maxdu)
G, h = generate_u_constraints_mat(G, h, N, nu, u0, minu, maxu)
kappa = psi * x0 + gamma * u0
G, h = generate_x_constraints_mat(G, h, N, nx, u0, theta, kappa, minx, maxx)
# print(H)
# print(g)
# print(G)
# print(h)
sol = pyecosqp.ecosqp(H, g, A=G, B=h)
du = np.matrix(sol["x"]).T
# print(du)
fx = psi * x0 + gamma * u0 + theta * du
ffx = fx.reshape(N, nx)
ffx = np.vstack((x0.T, ffx))
# print(ffx)
u = np.cumsum(du).T + u0
# print(u)
return ffx, u, du
if mpc_mode == 0:
x_hat = x
elif mpc_mode == 1:
x_hat = A * x_hat + B * u + Kob * (y - Cy * x_hat)
err = refHrzn - (psi * x_hat + gamma * u)
# err = ref[:N*nz, :] - (psi * x_hat + gamma * u) # tracking error
q = -theta.T * QQ * err # gradient term in QP
# print('q=', q.shape)
G, h = GenConstMatrix(A, B, Cc, Np, Nu, x_hat, u, psi_c, gamma_c, theta_c,
minu, maxu, minz, maxz, mindu, maxdu)
# print('G=', G.shape)
# print('h=', h.shape)
sol = pyecosqp.ecosqp(P, q, A=G, B=h)
du_OneCol = np.matrix(sol["x"]).T
# print('du=',du_OneCol.shape)
zz = psi * x_hat + gamma * u + theta * du_OneCol
du = du_OneCol.reshape(Nu, nu)
# print('zz=',zz)
# print('zz=',zz.shape)
zPred = zz.reshape(Np, ny)
uPred = np.cumsum(du, axis=0) + u.T
# print('uPred=', uPred.shape)
# print('du=',du.shape)
z = zz[:ny, :]
u = u + du[0, :].T
# print('z=',z.shape)