I couldn’t run your code so I don’t know the reason for the infeasibility. Shouldn’t the function linear_mps_control
return something?
To get the most help out of posts on discourse, it helps to simplify the problem into a minimal reproducible example. Take a read of Please read: make it easier to help you.
You probably have something wrong with your data or there might be a typo somewhere. Try simplifying the problem until you’re confident that it works, then add complexity. For example, just try running linear_mpc_control
. Do the outputs match what you expect?
I’d also write your JuMP model slightly differently. Here’s how I would write it:
function linear_mpc_control(xref, xbar, x0, dref)
model = Model(Gurobi.Optimizer)
@variable(model, x[1:NX, 1:(T+1)])
@variable(model, u[1:NU, 1:T])
for i in 1:NX
fix(x[i, 1], x0[i])
end
for t in 1:(T+1)
set_upper_bound(x[3, t], MAX_SPEED)
set_lower_bound(x[3, t], MIN_SPEED)
set_upper_bound(u[1, t], MAX_ACCEL)
set_lower_bound(u[1, t], -MAX_ACCEL)
set_upper_bound(u[2, t], MAX_STEER)
set_lower_bound(u[2, t], -MAX_STEER)
end
for t in 1:T
A, B, C = get_linear_model_matrix(xbar[3, t], xbar[4, t], dref[1, t])
@constraint(model, x[:, t + 1] .== A * x[:, t] + B * u[:, t] + C)
end
for t in 1:(T-2)
@constraint(model, u[2, t + 1] - u[2, t] <= MAX_DSTEER * DT)
@constraint(model, u[2, t + 1] - u[2, t] >= -MAX_DSTEER * DT)
end
@objective(
model,
Min,
sum(u[:, t]' * R * u[:, t] for t in 1:T) +
sum((xref[:, t] - x[:, t])' * Q * (xref[:, t] - x[:, t]) for t in 2:T) +
sum((u[:, t+1] - u[:, t])' * Rd * (u[:, t+1] - u[:, t]) for t in 1:(T-2)) +
(xref[:, T] - x[:, T])' * Qf * (xref[:, T] - x[:, T]),
)
optimize!(model)
solution_summary(model)
end