I’m using zoRO inside a robust tube MPC setup. I’ve customized the zoRO C template for my use case and have it working in MATLAB on a toy example. I’m now trying to port the same approach to Simulink, and before I invest more time, I’d like to confirm that this is feasible and that my workflow makes sense.
What I’m doing in MATLAB
An estimator provides a time-varying parameter estimate and an associated uncertainty.
The parameter estimate goes into the prediction model in the usual way (via p).
I modified the zoRO custom update so that parameter uncertainty is handled as additive process uncertainty via multiplication by the model sensitivity w.r.t. the parameter
At each call I pass state covariance (P_0), and the parameter-uncertainty payload into ‘custom_update()’ to propagate uncertainty and apply constraint backoffs.
What I’m trying to do in Simulink
The parameter estimate itself can already be streamed through the parameter_traj inport.
I’m stuck on streaming the uncertainty payload for zoRO. My current plan is:
Provide a dedicated Simulink inport to the OCP for the zoRO payload (separate from parameter_traj).
Extend the acados_solver_sfun_*.c template to:
declare the extra inport and its dimensions,
read that payload in mdlOutputs,
call *_acados_custom_update(capsule, data, data_len) between the two RTI phases,
then proceed with the normal *_acados_solve() path.
Keep the backoff computation and (P_k) propagation inside the custom update C template, so the S-function only passes the raw payload through.
I this a correct way to intergate zoRO into Simulink? If not, what steps should I take instead?
I implemented a custom version of acados_solver_sfun.in.c (and the matching make_sfun.in.m) that adds an extra input to the OCP S-function for an uncertainty payload. This payload is used inside a custom_update function between RTI phase 1 and phase 2 to compute and apply a constraint back-off before the feedback solve.
Setup summary
Toy problem: a nonlinear inverted pendulum simulated in Simulink. The controller directly commands the joint torque to make the pendulum track a sinusoidal reference angle.
Prediction and plant models are identical, except the plant uses a smooth continuous approximation of the sign() function for coulomb friction.
Sampling time: 1 ms (deliberately extreme).
Horizon: N = 30.
Constraint: a maximum pendulum angle that the sinusoidal reference intentionally exceeds.
Observation: after feeding a non-zero uncertainty payload, the controller appears to back off the state from the constraint, as shown below.
your approach of extending the Simulink wrapper and the custom update function make sense!
I would only rather add an optional input to the S-function, rather than copying it. You can also make a PR with that.
Great that you already have promising results!
For testing:
You can compare the closed-loop simulation results from MATLAB and Simulink.
For debugging, you can just print values you pass to the custom update function to to check if they are what you expect. This works if you start MATLAB/Simulink from a terminal on Linux.
Like I previously mentioned, I wish to handle parametric uncertainty within zoRO. I have currently extended the custom_update_function_zoro_template.in.c to accept parameter sensitvities F_k = ∂x_{k+1}/∂p_k payload externally, but I’d like acados to compute per shooting node internally, analogous to how A_k and B_k are formed, so zoRO can fetch it via ocp_nlp_get_at_stage(...).
After each sim call, read S_p from sim_out and store it as the discrete F_k, alongside the existing A and B.
C interface getter
File: interfaces/acados_c/ocp_nlp_interface.c
Add a "F" branch to ocp_nlp_get_at_stage(...) and its dims mapping.
Source the data from the continuous dynamics memory (not from the QP matrices).
Templates and model externals
Files: interfaces/acados_template/c_templates_tera/model.in.h, interfaces/acados_template/c_templates_tera/acados_solver.in.c
Generate CasADi code for df/dp (e.g., expl_ode_fun_jac_p.c/h).
Declare the external in model.in.h.
Create/free and pass it to the sim in acados_solver.in.c so ERK can call it.
For each stage, call ocp_nlp_get_at_stage(..., "F", ...) and use that in the uncertainty logic.
Questions
Is this the correct and minimal approach to get F_k that is consistent with the A,B produced by the continuous simulator? Or is there an easier way to compute F_k for use in zoRO.
Should S_p be added as extra columns to S_forw, or is it cleaner to add a separate sim_out field?
that sounds interesting!
I think there are a few features that you plan to implement.
Simulator sensitivities:
It would be great to add support for that.
I understand that you need the sensitivities for stage varying parameter, not p_{\mathrm{global}}, right?
You could add it for discrete dynamics as a first step, if using this kind of simulator works for you.
Otherwise, adding it in one of the integrators is certainly also nice.
Please make it optional adding something like opts->sens_forw_p.
Also, I would opt for a separate field S_p instead of reshaping S_forw.
I guess, you are doing zoRO-RTI, i.e. alternate one SQP iteration with the uncertainty update.
In this, this is indeed pretty much what you want.
More generally, we perform multiple SQP iterations which dont need F_k, or maybe call it S_{p,k}.
Thus, I would rather add a function to the OCP solver, that triggers the computation of S_{p,k}, and corresponding getters similar to what you described in 3).
I think, you can directly call the sim getter in the dynamics getter to avoid copying and creating extra memory.
Indeed this is what you need to do, but a part of the point 1).
Yes, I am not exactly sure what you need to change in the template. It can either be implemented as an option in the ZoroDescription or you indeed copy the template and create an custom_update_function_zoro_adapted.in.c.
Overall, these changes are welcome acados PRs.
Submitting them as acados PRs has the benefits of: visibility, other people checking correctness of your code, getting feedback and ideally getting it merged into the main acados branch.
As PRs should be self-contained and “minimal”, I think this should be at least 3 separate ones for 1), 2) and 5).
Also, creating a test for the sensitivity computation with standalone integrator instead of the OCP solver is an important intermediate step.
Thank you for your comments! I’d love to open a PR and hopefully get it merged so that others can benefit from it as well. However, I don’t have much experience with writing PRs. Is there a page or set of guidelines that outlines best practices or tips for Acados PRs?
As I mentioned in my earlier messages, I’ve also worked on getting zoRO to run in Simulink. Specifically, I modified the acados_solver_sfun.in.c file so that zoRO is used whenever {%- if custom_update_filename != "" -%}. An additional input port to the S-function is generated when {%- if custom_update_filename != "" and simulink_opts.inputs.zoRO_payload -%}. The custom_update function is then executed using the data from this payload port. The make_sfun.in.m file was modified accordingly.
If the port is enabled but left unwired, it falls back to the build-time matrices; the same behavior applies when the zoRO_payload option is disabled.
While I’m currently working on the sensitivities feature, I could already prepare a PR for the above changes if that’s useful?
Regarding your first comment: my goal is to feed parameter estimates and their associated uncertainties into the framework. I therefore update the parameters between control iterations while keeping them constant over the prediction horizon. Do you think this setup simplifies the overall approach?
Yes, you can make a PR just with that first change you describe.
I just added some brief guidelines on PRs here.
In Simulink, zoRO already works with RTI, as you probably saw.
You can just use {{ zoro_description.data_size }} in the templates when creating the port.
I didn’t know that unwired ports actually work in Simulink.
For me it would be fine to always require the input data, if it is required by the custom update function.
Regarding your first comment: my goal is to feed parameter estimates and their associated uncertainties into the framework. I therefore update the parameters between control iterations while keeping them constant over the prediction horizon. Do you think this setup simplifies the overall approach?
I think it doesn’t change much, in what needs to be implemented overall. Or what do you mean specifically?
