System dynamics as constraints of fmin_slsqp

Question

I've been trying to develop a nonlinear Model Predictive Control (MPC) in python. The goal of this controller is to minimize a cost function, with the system dynamics as constraints of it, as described in 1.
For those unfamiliar with it, each time step, the controller makes a prediction N timesteps ahead and tries to minimize a cost function having the control inputs (u) and the dynamics (x) of the predictions as independent variables of the optimization problem.

One problem is that the independent variables will be dependent between them, i.e. x(k+1) depends on x(k) and u(k) and so on.
As I said, I have to implement the dynamics as constraints. I've been doing so with the following code:

def constraint(self, ux0, x0):
    u = ux0[0:self.N]
    x = ux0[self.N:].reshape(self.N, 4).T
    x_next = x0
    for i in range(0, self.N-1):
        x0 = self.next_state(x0, u[i])
        x_next = np.c_[x_next, x0]
    return (x-x_next).T.flatten()

And then using this as a f_eqcons on fmin_slsqp.

Basically, the difference between the dynamics x_next and the state belonging to the independent variables x must be zero.

The real problem is that with N=100, for example, I get 500 independent variables and it takes forever to compute the optimization. I mean, I can't even get results and I will have to use, at least N=500 or so. I think it is because of the way I'm implementing the constraints.

I'm not sure if here is the best way to post this but do any of you have some idea on a proper way to implement this?

Answer 1

If your dynamics are linear, you could simply formulate the problem in CVXPY and solve it as a QP. It is very intuitive (the code reads as the math. You can follow this example example to start.

If the dynamics are nonlinear I suggest you to use CASADI. It allows to define the nonlinear dynamics and cast them in a nonlinear MPC problem. Check out this small example, their wiki for more details.