# NMPC with custom cost function and input rate constraints

Hi,

I am currently working on implementing the following cost function for a nonlinear Model Predictive Control (NMPC) system. This function aims to address two simultaneous objectives:

1. Minimizing tracking error while enforcing input rate constraints within the cost function.
2. Maximizing the “F” information metric to enhance the vehicle’s state estimation in an unknown environment.

``````        xr = x_ref[0:12, :]
x0 = x[0:12, :]
cost = 0
for k in range(self.horizon):
cost += (X[0:12, k] - xr).T @ self.P @ (X[0:12, k] - xr)
cost += (U[:, k + 1] - U[:, k]).T @ self.R @ (U[:, k + 1] - U[:, k])
cost += -alpha * F

opt.subject_to(
X[:, k + 1]
== self.forward_dynamics(
X[:, k], U[:, k], flow_bf, flow_acc_bf, self.model_type
)
)
opt.subject_to(opt.bounded(self.xmin, X[0:12, k], self.xmax))
opt.subject_to(opt.bounded(self.umin, U[:, k], self.umax))

cost += (X[0:12, -1] - xr).T @ self.P @ (X[0:12, -1] - xr)

opt.subject_to(opt.bounded(self.xmin, X[0:12, -1], self.xmax))
opt.subject_to(X[0:12, 0] == X0)

opt.set_value(X0, x0)
``````

I have successfully implemented the mentioned formulation using CasADi, and it performs well. However, the computation time is relatively high. Consequently, I’ve decided to switch to using acados with a Python interface to potentially improve efficiency.

As an initial step, I’ve broken down the problem and opted to implement only a portion of it, focusing solely on minimizing tracking error:

Reduced cost function - Casadi implementation:

``````        xr = x_ref[0:12, :]
x0 = x[0:12, :]
cost = 0
for k in range(self.horizon):
cost += (X[0:12, k] - xr).T @ self.P @ (X[0:12, k] - xr)
cost += (U[:, k + 1] - U[:, k]).T @ self.R @ (U[:, k + 1] - U[:, k])

opt.subject_to(
X[:, k + 1]
== self.forward_dynamics(
X[:, k], U[:, k], flow_bf, flow_acc_bf, self.model_type
)
)
opt.subject_to(opt.bounded(self.xmin, X[0:12, k], self.xmax))
opt.subject_to(opt.bounded(self.umin, U[:, k], self.umax))

cost += (X[0:12, -1] - xr).T @ self.P @ (X[0:12, -1] - xr)

opt.subject_to(opt.bounded(self.xmin, X[0:12, -1], self.xmax))
opt.subject_to(X[0:12, 0] == X0)

opt.set_value(X0, x0)
``````

However, I’ve encountered difficulties defining (u_{k+1} - u{k}).T * R * (u_{k+1} - u{k}) when using “NONLINEAR_LS”. This has resulted in discontinuous output from the optimizer, as illustrated in the figure below:

I would greatly appreciate guidance on how to implement this using the “EXTERNAL” cost type, as I haven’t found any examples addressing a similar challenge.

I’ve attached the acados implementation that produces the discontinuous output as shown above:

model:

``````    def auv_model(self):
x = SX.sym("x", (12, 1))
x_dot = SX.sym("x_dot", (12, 1))
u = SX.sym("u", (self.thrusters, 1))

f_expl_expr = self.nonlinear_dynamics(x, u, self.model_type)
f_impl_expr = x_dot - f_expl_expr

p = []

``````

solver description:

``````    def create_ocp_solver_description(self):

# set options

"GAUSS_NEWTON"  # 'GAUSS_NEWTON', 'EXACT'
)
self.acados_ocp.solver_options.nlp_solver_type = "SQP"  # SQP_RTI, SQP
``````

optimize:

``````def optimize(self, x, x_ref):
xr = x_ref[0:12, :]
x0 = x[0:12, :]
N = self.horizon

# initialize solver
for stage in range(N + 1):
for stage in range(N):

# set initial state constraint

for k in range(N):

if status != 0:

# self.previous_control = np.array([force_axes]).T

u_next = evalf(mtimes(pinv(self.auv.tam), force_axes)).full()

return force_axes, u_next
``````

Any assistance would be greatly appreciated! Thank you!

Hi and welcome to the acados forum!

the acados OCP formulation allows you to define costs as a function of the state and controls associated with a single shooting node only, i.e. the cost may only depend on x_k and u_k. To include a cost on the rate of change you will therefore need to augment your system dynamics. Please check out this post.

The external cost module is straight forward to use, you just provide a casadi expression defining the cost term, you may want to check this example.

Best, Katrin