Thank you for implementing S-function as it becomes very easy to implement everything on Simulink. However, it would be very helpful if we can retrieve some more information from S-function controller for analysis and debugging.
To be specific, I am looking to get Cost function value, full horizon of state and input and all the times. Only full horizon will be enough to calculate Cost function value manually. I was looking into the files from which S-function is generated to make changes myself for my work but it didn’t seem straightforward.
It would be really appreciated if you could add these functionalities or could specify the sections of code where we can make changes to extract information.
I think it is a trade-off what is added as input/output to the S-function.
If all possible inputs & outputs were added, I think that the block would get hard to use and probably it would slow down Simulink.
However, please feel encouraged to extent the S-function for your needs.
For example, you can enlarge the port with x_1 to contain the full state trajectory.
For that you would have to change the size of the port here (to size N*nx).:
and add a loop that extracts x for stage 1 up to N here:
Thank you for the suggestions on how to get the full horizon. I tried to run a for loop over the line 382 as you mentioned, but the problem is that, I dont know what is the size, data type and where exactly out_x1 is declared. I found that out_x1 is defined as a pointer of type real_t. But no idea what is real_t
I implemented a for loop as follows,
int z[ {{dims.nx}} ][ {{dims.N}} ];
int *p;
for (int i = 1; i < {{ dims.N + 1 }}; i++)
{
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, i, "x", (void *) p);
for(int m = 1; m < {{dims.nx +1}}; m++ )
{
z[m][i] = &p[m];
}
}
Now how should I assign values from z to out_x1.? If i knew more about out_x1 then I could fill it with vectors I get from ocp_nlp_out_get.
Likewise, I also want slack variable values if possible.
This is just a reminder again. In addition to the above, i also tried this (infact this was my initial try and since it didnt work i tried the above):
ssSetOutputPortVectorDimension(S, 3, {{ dims.nx*dims.N }} );
.....
// get next state
for (int i=1; i <= {{ dims.N +1 }}; i++)
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, i, "x", (void *) out_x1);
}
This does work and gives the full horizon, but except for the first state in the horizon, everything else is zero.
It would be really helpful if you could help get the full horizon. It will help me analyse what MPC is doing for the changes i make in my problem. I am in the last stages of my thesis and stuck with this. If you can just put the working solution here and not add it on github then also it will enough for me.
adapting the loop in your latest post to the following should work:
for (int i=1; i <= {{ dims.N }}; i++)
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, i, "x", (void *) (out_x1+ i *{{ dims.nx }}));
Note: the last argument is the pointer to the memory where the values you request are written to.
Thus, this should vary in the loop to not overwrite the result always.
Also: i should go from 0 or 1 to N, since there are N+1 nodes, which are counted starting with 0.
Thank you for your solution. It does give the full horizon but something is unusual.
If I run for loop for i starting from 1, then the first state always stays equal to 0.
If I run for loop for i starting from 0, then 2 of my first state elements freeze and do not increase in value as expected.
in both the cases the length of the horizon vector is equal to nx*N. which is no brainer since we set this as the size of the output port.
I dont understand why this is happening. I have checked everything from my end it looks okay. If i revert back the changes to the original then, it works perfect. Your insights will be really valuable.
Right, I realized I made a mistake, it should be (out_x1+(i-1) *{{ dims.nx }}), since the first state should be written into out_x1 without offset.
So, this should be it.
for (int i=1; i <= {{ dims.N }}; i++)
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, i, "x", (void *) (out_x1+(i-1) *{{ dims.nx }}));
No, I didn’t add cost as an output of S-function.
But using the horizon values and the reference (which I generate by myself) for each time step, I am calculating the cost function values for the entire simulation afterwards i.e. offline.
Works the same way with the control output btw.
Just change to ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, i-1, "u", (void *) (out_u0+(i-1) *{{ dims.nu }}));
@FreyJo can you agree? I wasnt sure about the out_u0+(i-1), but the results look good.
I will have a look.
