In [1]:
import numpy as np
import matplotlib.pyplot as plt
from mpl_toolkits import mplot3d
import cvxpy as cp
In [2]:
np.random.seed(2)
n_x = 2 # State dimension
n_u = 1 # Control dimension
T = 2 # Time horizon
dt = .1 # Time step
t_eval = np.linspace(0, T+1, int(T/dt)) # Time evaluation
N = len(t_eval) -1 # Number of time steps
T = N
x_bf = np.zeros((T*n_x, 1)) # State trajectory
u_bf = np.zeros((T*n_u, 1)) # Input trajectory
mu_0 = np.array([1, 1]) # Initial state mean
Sigma_0 = 4*np.eye(n_x) # Initial state covariance
mu_f = np.array([0, 0]) # Final state mean
Sigma_f = 0.05*np.eye(n_x) # Final state covariance
mu_w = 0.0*np.ones(n_x*(T+1)) # Process mean
Sigma_w = 0.001*np.eye(n_x*(T+1)) # Process covariance
w_bf = np.random.multivariate_normal(mean=mu_w, cov=Sigma_w) # Process noise
w_bf[:n_x] = np.random.multivariate_normal(mean=mu_0, cov=Sigma_0) # Initial state
x_bf[:n_x, 0] = w_bf[:n_x]
Q = np.eye(n_x)*100 # State cost
R = np.eye(n_u)*0.1 # Control cost
A = np.array([[1., dt], [0, 1]]) # System dynamics
B = np.array([[0], [dt]]) # Control dynamics
Q_cal = np.kron(np.eye(T+1), Q) # State block diagonal matrix
R_cal = np.kron(np.eye(T+1), R) # Control block diagonal matrix
A_cal = np.kron(np.eye(T+1), A) # System block diagonal matrix
B_cal = np.kron(np.eye(T+1), B) # Control block diagonal matrix
Z = np.diag(np.ones(n_x*(T+1)-n_x), k=-n_x) # Block-downshift matrix
I = np.eye(n_x*(T+1)) # Block identity matrix
Phi_x = cp.Variable((n_x * (T+1), n_x * (T+1))) # State system response
Phi_u = cp.Variable((n_u * (T+1), n_x * (T+1))) # Control system response
constraint = [] # Constraints
for t in range(T+1):
start_idx = t * n_x
end_idx = (t + 1) * n_x
constraint += [Phi_x[start_idx:end_idx, start_idx:end_idx] == np.eye(n_x)]
for block_row in range(1, T+1):
for block_col in range(block_row):
start_i = block_row * n_x
end_i = (block_row + 1) * n_x
start_j = block_col * n_x
end_j = (block_col + 1) * n_x
constraint += [Phi_x[start_j:end_j, start_i:end_i] == 0]
for block_row in range(1, T+1):
for block_col in range(block_row):
start_i = block_row * n_x
end_i = (block_row + 1) * n_x
start_j = block_col * n_u
end_j = (block_col + 1) * n_u
constraint += [Phi_u[start_j:end_j, start_i:end_i] == 0]
V = cp.bmat([[Sigma_f, Phi_x[-n_x:, :]@Sigma_w**0.5],
[(Phi_x[-n_x:, :]@Sigma_w**0.5).T, np.eye(n_x*(T+1))]])
constraint += [
(I - Z @ A_cal) @ Phi_x - Z @ B_cal @ Phi_u == I,
Phi_x[-n_x:, :] @ mu_w == mu_f,
V >> 0,
]
objective = cp.Minimize(
cp.norm(cp.vstack([
cp.hstack((Q_cal**0.5, np.zeros((n_x*(T+1), n_u*(T+1))))),
cp.hstack((np.zeros((n_u*(T+1), n_x*(T+1))), R_cal**0.5))
]) @ cp.vstack((Phi_x, Phi_u)) @ Sigma_w**0.5, "fro")**2)
In [3]:
prob = cp.Problem(objective, constraint)
prob.solve(solver=cp.MOSEK, verbose=True)
===============================================================================
CVXPY
v1.3.1
===============================================================================
(CVXPY) May 03 11:15:24 PM: Your problem has 2400 variables, 403 constraints, and 0 parameters.
(CVXPY) May 03 11:15:24 PM: It is compliant with the following grammars: DCP, DQCP
(CVXPY) May 03 11:15:24 PM: (If you need to solve this problem multiple times, but with different data, consider using parameters.)
(CVXPY) May 03 11:15:24 PM: CVXPY will first compile your problem; then, it will invoke a numerical solver to obtain a solution.
-------------------------------------------------------------------------------
Compilation
-------------------------------------------------------------------------------
(CVXPY) May 03 11:15:24 PM: Compiling problem (target solver=MOSEK).
(CVXPY) May 03 11:15:24 PM: Reduction chain: Dcp2Cone -> CvxAttr2Constr -> ConeMatrixStuffing -> MOSEK
(CVXPY) May 03 11:15:24 PM: Applying reduction Dcp2Cone
(CVXPY) May 03 11:15:24 PM: Applying reduction CvxAttr2Constr
(CVXPY) May 03 11:15:24 PM: Applying reduction ConeMatrixStuffing
(CVXPY) May 03 11:15:24 PM: Applying reduction MOSEK
(CVXPY) May 03 11:15:25 PM: Finished problem compilation (took 7.957e-01 seconds).
-------------------------------------------------------------------------------
Numerical solver
-------------------------------------------------------------------------------
(CVXPY) May 03 11:15:25 PM: Invoking solver MOSEK to obtain a solution.
(CVXPY) May 03 11:15:25 PM: Problem
(CVXPY) May 03 11:15:25 PM: Name :
(CVXPY) May 03 11:15:25 PM: Objective sense : maximize
(CVXPY) May 03 11:15:25 PM: Type : CONIC (conic optimization problem)
(CVXPY) May 03 11:15:25 PM: Constraints : 2402
(CVXPY) May 03 11:15:25 PM: Affine conic cons. : 0
(CVXPY) May 03 11:15:25 PM: Disjunctive cons. : 0
(CVXPY) May 03 11:15:25 PM: Cones : 2
(CVXPY) May 03 11:15:25 PM: Scalar variables : 5226
(CVXPY) May 03 11:15:25 PM: Matrix variables : 1 (scalarized: 903)
(CVXPY) May 03 11:15:25 PM: Integer variables : 0
(CVXPY) May 03 11:15:25 PM:
(CVXPY) May 03 11:15:25 PM: Optimizer started.
(CVXPY) May 03 11:15:25 PM: Presolve started.
(CVXPY) May 03 11:15:25 PM: Eliminator started.
(CVXPY) May 03 11:15:25 PM: Freed constraints in eliminator : 342
(CVXPY) May 03 11:15:25 PM: Eliminator terminated.
(CVXPY) May 03 11:15:25 PM: Linear dependency checker started.
(CVXPY) May 03 11:15:25 PM: Linear dependency checker terminated.
(CVXPY) May 03 11:15:25 PM: Eliminator started.
(CVXPY) May 03 11:15:25 PM: Freed constraints in eliminator : 4
(CVXPY) May 03 11:15:25 PM: Eliminator terminated.
(CVXPY) May 03 11:15:25 PM: Eliminator - tries : 2 time : 0.00
(CVXPY) May 03 11:15:25 PM: Lin. dep. - tries : 1 time : 0.00
(CVXPY) May 03 11:15:25 PM: Lin. dep. - primal attempts : 1 successes : 1
(CVXPY) May 03 11:15:25 PM: Lin. dep. - dual attempts : 0 successes : 0
(CVXPY) May 03 11:15:25 PM: Lin. dep. - primal deps. : 0 dual deps. : 0
(CVXPY) May 03 11:15:25 PM: Presolve terminated. Time: 0.02
(CVXPY) May 03 11:15:25 PM: Optimizer - threads : 8
(CVXPY) May 03 11:15:25 PM: Optimizer - solved problem : the primal
(CVXPY) May 03 11:15:25 PM: Optimizer - Constraints : 388
(CVXPY) May 03 11:15:25 PM: Optimizer - Cones : 3
(CVXPY) May 03 11:15:25 PM: Optimizer - Scalar variables : 1052 conic : 1052
(CVXPY) May 03 11:15:25 PM: Optimizer - Semi-definite variables: 1 scalarized : 903
(CVXPY) May 03 11:15:25 PM: Factor - setup time : 0.00
(CVXPY) May 03 11:15:25 PM: Factor - dense det. time : 0.00 GP order time : 0.00
(CVXPY) May 03 11:15:25 PM: Factor - nonzeros before factor : 4671 after factor : 4671
(CVXPY) May 03 11:15:25 PM: Factor - dense dim. : 2 flops : 2.17e+05
(CVXPY) May 03 11:15:25 PM: ITE PFEAS DFEAS GFEAS PRSTATUS POBJ DOBJ MU TIME
(CVXPY) May 03 11:15:25 PM: 0 2.5e-01 3.3e+00 4.2e+01 0.00e+00 -4.110000000e+01 -0.000000000e+00 1.0e+00 0.03
(CVXPY) May 03 11:15:25 PM: 1 2.0e-01 2.7e+00 3.4e+01 3.93e-01 -3.762222060e+01 5.738509607e-02 8.1e-01 0.05
(CVXPY) May 03 11:15:25 PM: 2 1.7e-01 2.3e+00 2.9e+01 1.38e-01 -3.493156736e+01 6.765368493e-01 7.0e-01 0.05
(CVXPY) May 03 11:15:25 PM: 3 1.5e-01 2.0e+00 2.4e+01 1.81e-01 -2.997359244e+01 2.296345102e+00 6.1e-01 0.05
(CVXPY) May 03 11:15:25 PM: 4 1.2e-01 1.6e+00 1.9e+01 2.47e-01 -2.592941893e+01 3.143232649e+00 4.7e-01 0.06
(CVXPY) May 03 11:15:25 PM: 5 5.3e-02 7.0e-01 4.8e+00 4.41e-01 -9.019750196e-01 1.214778153e+01 2.1e-01 0.06
(CVXPY) May 03 11:15:25 PM: 6 1.2e-02 1.6e-01 7.7e-01 6.50e-01 1.130964510e+01 1.506846074e+01 4.9e-02 0.06
(CVXPY) May 03 11:15:25 PM: 7 1.4e-03 1.9e-02 2.6e-02 9.58e-01 1.703421308e+01 1.746404985e+01 5.7e-03 0.07
(CVXPY) May 03 11:15:25 PM: 8 1.4e-05 1.8e-04 2.2e-05 1.02e+00 1.766135294e+01 1.766547582e+01 5.5e-05 0.07
(CVXPY) May 03 11:15:25 PM: 9 1.1e-07 1.4e-06 1.5e-08 1.00e+00 1.766804171e+01 1.766807347e+01 4.2e-07 0.07
(CVXPY) May 03 11:15:25 PM: 10 2.1e-09 1.8e-07 4.5e-11 9.96e-01 1.766809446e+01 1.766809512e+01 8.8e-09 0.08
(CVXPY) May 03 11:15:25 PM: 11 3.4e-10 4.7e-06 9.1e-13 9.99e-01 1.766809552e+01 1.766809557e+01 6.6e-10 0.08
(CVXPY) May 03 11:15:25 PM: Optimizer terminated. Time: 0.10
(CVXPY) May 03 11:15:25 PM:
(CVXPY) May 03 11:15:25 PM:
(CVXPY) May 03 11:15:25 PM: Interior-point solution summary
(CVXPY) May 03 11:15:25 PM: Problem status : PRIMAL_AND_DUAL_FEASIBLE
(CVXPY) May 03 11:15:25 PM: Solution status : OPTIMAL
(CVXPY) May 03 11:15:25 PM: Primal. obj: 1.7668095525e+01 nrm: 9e+00 Viol. con: 1e-09 var: 0e+00 barvar: 0e+00 cones: 0e+00
(CVXPY) May 03 11:15:25 PM: Dual. obj: 1.7668095573e+01 nrm: 2e+01 Viol. con: 0e+00 var: 3e-09 barvar: 2e-15 cones: 0e+00
-------------------------------------------------------------------------------
Summary
-------------------------------------------------------------------------------
(CVXPY) May 03 11:15:25 PM: Problem status: optimal
(CVXPY) May 03 11:15:25 PM: Optimal value: 1.767e+01
(CVXPY) May 03 11:15:25 PM: Compilation took 7.957e-01 seconds
(CVXPY) May 03 11:15:25 PM: Solver (including time spent in interface) took 1.338e-01 seconds
Out[3]:
17.6680956115126
In [4]:
optimal_Phi_x = Phi_x.value
optimal_Phi_u = Phi_u.value
x_bf = optimal_Phi_x @ w_bf
u_bf = optimal_Phi_u @ w_bf
x_bf_m = x_bf.reshape((T+1, n_x))
u_bf_m = u_bf.reshape((T+1, n_u))
In [5]:
plt.rcParams.update({'font.size': 16})
plt.figure(figsize=(12, 12))
plt.subplot(3, 1, 1)
plt.plot(t_eval, x_bf_m[:, 0])
plt.ylabel('x')
plt.legend(['x_sls'])
plt.grid()
plt.subplot(3, 1, 2)
plt.plot(t_eval, x_bf_m[:, 1])
plt.ylabel('v')
plt.legend(['v_sls'])
plt.grid()
plt.subplot(3, 1, 3)
plt.plot(t_eval, u_bf_m)
plt.xlabel('Time [s]')
plt.ylabel('u')
plt.legend(['u_sls'])
plt.tight_layout()
plt.grid()
plt.show()
In [6]:
fig = plt.figure(figsize=(20, 12))
ax = fig.add_subplot(121, projection='3d')
ax.view_init(25, 35)
ax.plot(x_bf_m[:, 0], t_eval, x_bf_m[:, 1])
ax.scatter(x_bf_m[:, 0], t_eval, x_bf_m[:, 1], color='red', s=30)
ax.set_xlabel('x')
ax.set_ylabel('Time [s]')
ax.set_zlabel('v')
ax = fig.add_subplot(122, projection='3d')
ax.view_init(25, -35)
ax.plot(x_bf_m[:, 0], t_eval, x_bf_m[:, 1])
ax.scatter(x_bf_m[:, 0], t_eval, x_bf_m[:, 1], color='red', s=30)
ax.set_xlabel('x')
ax.set_ylabel('Time [s]')
ax.set_zlabel('v')
plt.show()
