update notation
parent
0abc666052
commit
279625b4c1
|
@ -75,7 +75,7 @@ class MPC:
|
||||||
)
|
)
|
||||||
|
|
||||||
self.dt = P.DT
|
self.dt = P.DT
|
||||||
self.control_horizon = P.T
|
self.control_horizon = P.T / P.DT
|
||||||
self.Q = np.diag(state_cost)
|
self.Q = np.diag(state_cost)
|
||||||
self.Qf = np.diag(final_state_cost)
|
self.Qf = np.diag(final_state_cost)
|
||||||
self.R = np.diag(input_cost)
|
self.R = np.diag(input_cost)
|
||||||
|
@ -112,18 +112,18 @@ class MPC:
|
||||||
constr = []
|
constr = []
|
||||||
|
|
||||||
for k in range(self.control_horizon):
|
for k in range(self.control_horizon):
|
||||||
cost += opt.quad_form(target[:, k] - x[:, k], self.Q)
|
cost += opt.quad_form(target[:, k] - x[:, k + 1], self.Q)
|
||||||
cost += opt.quad_form(u[:, k], self.R)
|
cost += opt.quad_form(u[:, k], self.R)
|
||||||
|
|
||||||
# Actuation rate of change
|
# Actuation rate of change
|
||||||
if k < (control_horizon - 1):
|
if k < (self.control_horizon - 1):
|
||||||
cost += opt.quad_form(u[:, k + 1] - u[:, k], self.P)
|
cost += opt.quad_form(u[:, k + 1] - u[:, k], self.P)
|
||||||
|
|
||||||
# Kinematics Constrains
|
# Kinematics Constrains
|
||||||
constr += [x[:, k + 1] == A @ x[:, k] + B @ u[:, k] + C]
|
constr += [x[:, k + 1] == A @ x[:, k] + B @ u[:, k] + C]
|
||||||
|
|
||||||
# Actuation rate of change limit
|
# Actuation rate of change limit
|
||||||
if t < (control_horizon - 1):
|
if k < (self.control_horizon - 1):
|
||||||
constr += [
|
constr += [
|
||||||
opt.abs(u[0, k + 1] - u[0, k]) / self.dt <= self.du_bounds[0]
|
opt.abs(u[0, k + 1] - u[0, k]) / self.dt <= self.du_bounds[0]
|
||||||
]
|
]
|
||||||
|
|
|
@ -5,13 +5,13 @@ class Params:
|
||||||
def __init__(self):
|
def __init__(self):
|
||||||
self.N = 4 # number of state variables
|
self.N = 4 # number of state variables
|
||||||
self.M = 2 # number of control variables
|
self.M = 2 # number of control variables
|
||||||
self.T = 10 # Prediction Horizon
|
self.T = 10 # Prediction Horizon [s]
|
||||||
self.DT = 0.2 # discretization step
|
self.DT = 0.2 # discretization step [s]
|
||||||
self.path_tick = 0.05
|
self.path_tick = 0.05 # [m]
|
||||||
self.L = 0.3 # vehicle wheelbase
|
self.L = 0.3 # vehicle wheelbase [m]
|
||||||
self.TARGET_SPEED = 1.0 # m/s
|
self.TARGET_SPEED = 1.0 # [m/s
|
||||||
self.MAX_SPEED = 1.5 # m/s
|
self.MAX_SPEED = 1.5 # [m/s
|
||||||
self.MAX_ACC = 1.0 # m/ss
|
self.MAX_ACC = 1.0 # [m/ss
|
||||||
self.MAX_D_ACC = 1.0 # m/sss
|
self.MAX_D_ACC = 1.0 # [m/sss
|
||||||
self.MAX_STEER = np.radians(30) # rad
|
self.MAX_STEER = np.radians(30) # [rad]
|
||||||
self.MAX_D_STEER = np.radians(30) # rad/s
|
self.MAX_D_STEER = np.radians(30) # [rad/s]
|
||||||
|
|
Loading…
Reference in New Issue