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)