w1.Q = np.array([[5, 0, 0, 0], [0, 5, 0, 0], [0, 0, 1, 0],
[0, 0, 0, 1]]) # State weight matrix Q
w1.R = np.array([[1, 0], [0, 1]]) # Control input weight matrix R
w1.P = control.dare(G1.A, G1.B, w1.Q,
w1.R)[0] # Final state matrix P, based off DARE
w2 = mpc.Weight() # Empty weights class
w2.Q = np.array([[5, 0, 0, 0], [0, 5, 0, 0], [0, 0, 1, 0],
[0, 0, 0, 1]]) # State weight matrix Q
w2.R = np.array([[1, 0], [0, 1]]) # Control input weight matrix R
w2.P = control.dare(G2.A, G2.B, w2.Q,
w2.R)[0] # Final state matrix P, based off DARE
predMat1 = mpc.generate_prediction_matrices(G1,
N) # Prediction matrices P and S
costMat1 = mpc.generate_cost_matrices(
predMat1, w1, N) # Cost matrices Q, inverse Q (Qi) and c
predMat2 = mpc.generate_prediction_matrices(G2,
N) # Prediction matrices P and S
costMat2 = mpc.generate_cost_matrices(
predMat2, w2, N) # Cost matrices Q, inverse Q (Qi) and c
costMatT = mpc.CostMatrix()
costMatT.H = scipy.linalg.block_diag(costMat1.H, costMat2.H)
costMatT.Hi = np.linalg.inv(costMatT.H)
costMatT.F = scipy.linalg.block_diag(costMat1.F, costMat2.F)
vMax1 = 5 # Bound on velocity
xBound1 = 5 # Bound on position
vMax2 = 5 # Bound on velocity
G.B = np.array([[0, 0], [0, 0], [1, 0], [0, 1]])
G.C = np.array([[1, 0, 0, 0], [0, 1, 0, 0]])
G.D = np.array([[0, 0], [0, 0]])
x0 = np.array([[0], [0], [0], [0]]) # Initial state
WP = np.array([[5], [0], [0], [0]]) # Waypoint
w = mpc.Weight() # Empty weights class
w.Q = np.array([[5, 0, 0, 0], [0, 5, 0, 0], [0, 0, 1, 0],
[0, 0, 0, 1]]) # State weight matrix Q
w.R = np.array([[1, 0], [0, 1]]) # Control input weight matrix R
w.P = control.dare(G.A, G.B, w.Q,
w.R)[0] # Final state matrix P, based off DARE
predMat = mpc.generate_prediction_matrices(G, N) # Prediction matrices P and S
costMat = mpc.generate_cost_matrices(
predMat, w, N) # Cost matrices Q, inverse Q (Qi) and c
vMax = 5 # Bound on velocity
xBound = 5 # Bound on position
Ac = np.array([[0, 1, 0, 0], [0, -1, 0, 0], [0, 0, 1, 0], [0, 0, -1, 0],
[0, 0, 0, 1], [0, 0, 0, -1]]) # Constraint matrix A (Ax <= b)
bc = np.array([[xBound], [xBound], [vMax], [vMax], [vMax],
[vMax]]) # Constraint vector b
#%% MPC simulation
x = np.zeros((4, T + 1)) # States
x[:, [0]] = x0 # Initial state
for k in range(T):
c = costMat.c @ (x[:, [k]] - WP)