Extend S-function to handle MHE Problems

Hi acados-Team!

Unfortunately I am having issues to solve a MHE problem with acados when using simulink. What I have done is implemented a simple PT2 model containing two s-Functions, one MPC and one MHE (similar to the MHE example in python). The formulation works fine as an m-Code. When executing the simulink model, Matlab instantly crashes with an error message that a mex-Function has crashed.

I have a suspicion, that the crash is caused by the choice of constraints, as I have observed the following behaviour:

  1. According to the pdf-document problem_formulation_ocp_mex.pdf the initial state x0 should not be set for MHE problems. When formulating an ocp without constraints either MATLAB crashes when compiling the mex functions or I receive error messages from acados, that x0 needs to be set.
Error: Failed to render 'main_sim.in.c' Reason: Variable `constraints.lbx_0[i]` not found in context while rendering 'main_sim.in.c': the evaluated version was
`constraints.lbx_0.0`. Maybe the index is out of bounds?

Therefore I constrain x0 via constr_Jbx_0 within a range of -1e6 and +1e6.

  1. When setting only constr_Jbx_0 as mentioned in point 1. the simulink MHE s-functions does not crash. However no solution is found in the s-Function (ocp status ~=0), although the acados_ocp object is able to find a solution when calling acados_ocp.solve() in an m-script with the same input values. I checked the code in the c-Code of the s-Function. It seems to me, that x0 is constrained with lower and upper bounds of the same value, which is in contrast to point 1.
  2. So what I have tried next is to set constraints via constr_expr_h. In this case the ocp formulation as a m-script is execetud fine, but the corresponding compiled s-Function causes simulink to crash.

Do you know what causes these problems? If you are interested in my example to reconstruct the problem, I will upload it.

These issues happen when using the acados version of the 3rd of November with the commit: 993c92291af17a787865ce36167e2e1011d75d3e

Thanks a lot for helping! :slight_smile:

Hi @chris,

The S-function was indeed never tested with an MHE problem, more specifically, the first input is always the initial state and sets lbx0, ubx0 internally, which is assumed there to be of size nx.

One would have to generalize the templated S-function Code with respect to MHE problems, such that x0 is not an input of the S-Function, if nbx0 != nbx see these lines:
https://github.com/acados/acados/blob/da58ccce733f03e6cd6b2447899c57a0abfc9db0/interfaces/acados_template/acados_template/c_templates_tera/acados_solver_sfun.in.c#L91-L94 https://github.com/acados/acados/blob/da58ccce733f03e6cd6b2447899c57a0abfc9db0/interfaces/acados_template/acados_template/c_templates_tera/acados_solver_sfun.in.c#L212-L217

Moreover, one should change the Outputs of the Sfunction.
These are hard coded here for now https://github.com/acados/acados/blob/da58ccce733f03e6cd6b2447899c57a0abfc9db0/interfaces/acados_template/acados_template/c_templates_tera/acados_solver_sfun.in.c#L358-L363

I guess the following outputs are also relevant in the MHE case:
out_status, out_KKT_res, out_cpu_time, out_sqp_iter.
Additionally, one would at the very least want the last value of x I guess.
What do you think?

I think, at least what I stated above should be added in the Sfunction template logic.
I would like to encourage you to try this yourself and make a pull request.
I am not sure when I will find time to do this myself.

General disclaimer: the S-function is not as flexible in terms of interacting with the solver. One might have to adapt the templated S-function code in order to add the inputs / outputs one wants for the specific application. For example, what was requested here: Code Generation for S-function - Matlab Interface

Best,
Jonathan

Hi Jonathan,

thanks for your quick reply. I would be glad, if I could contribute to acados! Let me just ask if I got everything right, before starting to code:

  • The in- and oututs of the s-function will depend on whether it is an MPC or MHE case
  • To detect the MHE case one will check nbx_0, because one does not set a fixed initial state in MHE.
  • The MPC s-Function stays as it is, the MHE s-Function is the new feature
  • The output of the MHE will be out_status, out_KKT_res, out_cpu_time, out_sqp_iter and x

I also think that we need the last value of x. I even think this is the most important output of an MHE s-Function.

There is still some parts, that I am not sure about:

  • What happens when you want to constrain all elements of x_0? For example I have x=(x1,x2) and want to have [-1,-1] < x0 < [1,1]. In this case it would be nbx = nbx0 and the detection wouldn’t work.
  • How can I leave out the constraints for x0 completely? I would like to execute mhe_model.set('dim_nbx_0',0);, but then I get the following message:
Error using acados_template_mex.render_acados_templates>render_file (line 232)
rendering main_sim.in.c failed.
 command: D:\acados_v013\examples\acados_matlab_octave\..\..\bin\t_renderer.exe
 "D:\acados_v013\examples\acados_matlab_octave\..\..\interfaces\acados_template\acados_template\c_templates_tera\*"
 "main_sim.in.c"
 "C:\..\Systemidentifikation\MHE\PT2_parameterestimation\acados_ocp_nlp.json"
 "main_sim_MHE_PT2_ode.c"
 returned status 1, got result:
Error: Failed to render 'main_sim.in.c'
Reason: Variable `constraints.lbx_0[i]` not found in context while rendering 'main_sim.in.c': the evaluated version was
`constraints.lbx_0.0`. Maybe the index is out of bounds?

To overcome this issue until now I have always set constr_Jbx_0 as an identity matrix and set ‘lbx_0’ and ‘ubx_0’ very low and very high. Somehow this feels more like a workaround… So what is the right way to do so?

Cheers
Christian

Hey Chris,

Great that you are willing to contribute here.

You are right. We should do this in a cleaner way.

We will discuss how to implement this nicely privately and update the topic as soon as there is a PR.