How to add measured disturbance in the predicted horizon

[Ethan]

Hi everyone, first of all, thanks to all developers for providing us with this toolbox. I want to add external measured disturbances to my model (MATLAB interface) in the form of discrete points over a period of time in the predicted horizon. I 've already read the discussion of https://sourceforge.net/p/grampc/discussion/general/thread/bac9bb8d7b/. But things are a little difference, and I have no idea what to do next.

/** System function f(t,x,u,p,userparam) 
    ------------------------------------ **/
void ffct(typeRNum *out, ctypeRNum t, ctypeRNum *x, ctypeRNum *u, ctypeRNum *p, typeUSERPARAM *userparam)
{
    /***omission part***/
    out[0] = x[1];
    out[1] = m * (  - k * x[0] - c * x[1] -  Cr_11 * x[2] - Cr_12 * x[3] -  Cr_13 * x[4] - Cr_14 * x[5] + MD + u[0]);
    out[2] = Br_11 * x[1] + Ar_11 * x[2] + Ar_12 * x[3] + Ar_13 * x[4] + Ar_14 * x[5] ;
    out[3] = Br_21 * x[1] + Ar_21 * x[2] + Ar_22 * x[3] + Ar_23 * x[4] + Ar_24 * x[5] ;
    out[4] = Br_31 * x[1] + Ar_31 * x[2] + Ar_32 * x[3] + Ar_33 * x[4] + Ar_34 * x[5] ;
    out[5] = Br_41 * x[1] + Ar_41 * x[2] + Ar_42 * x[3] + Ar_43 * x[4] + Ar_44 * x[5] ;
}
/** Integral cost l(t,x(t),u(t),p,xdes,udes,userparam) 
    -------------------------------------------------- **/
void lfct(typeRNum *out, ctypeRNum t, ctypeRNum *x, ctypeRNum *u, ctypeRNum *p, ctypeRNum *xdes, ctypeRNum *udes, typeUSERPARAM *userparam)
{
    out[0] =  x[1] * u[0] ;      
}

Here is the main part of my code and the variable MD is the external measured disturbances. Looking forward to your reply.

Best regards,
Wong.

[Andreas Völz]

Dear Wong,

the actual / true disturbance should be added to the simulation. For example, in startMPC.m when integrating the system to obtain the next state.

If you can measure the current disturbance and assume that it remains constant over the prediction horizon, you can store it within the MPC loop in a userparam field and consider it in the ffct and possibly dfdx_vec, dfdu_vec. This is what you should test first.

If you have a prediction model for the disturbance, meaning that you can get estimates of future disturbances, then you can compute the disturbance for each of the Nhor sampling points along the prediction horizon and pass them via userparam to the problem functions. Inside ffct, dfdx_vec and dfdu_vec you have to use the current time t to find the respective field in userparam (attention: these functions get the global time t0+t). Take a look at GRAMPC's MHE example to see how to work with different values for each sampling step.

Best regards,
Andreas Völz

[Ethan]

Dear Andreas Völz,

Thanks very much for your help. With your guidance, my program seems to be working well.

Best regards,
Wong.

[Ethan]

Dear Andreas Völz,

Some new problems appear in my program. My program works well without adding any inequality constraints. However, an error occurs when I add some simple inequality constraints in my program. After the pobfct is compiled, the grampc seems cannot be Initialized and Matalb gives an error of ' PenaltyMin : Invalid value for option. '.

/** Inequality constraints h(t,x(t),u(t),p,uperparam) <= 0 
    ------------------------------------------------------ **/
void hfct(typeRNum *out, ctypeRNum t, ctypeRNum *x, ctypeRNum *u, ctypeRNum *p, typeUSERPARAM *userparam)
{
    out[0] = x[0] - 0.3  ;
    out[1] = -0.3 - x[0] ;
}
/** Jacobian dh/dx multiplied by vector vec, i.e. (dh/dx)^T*vec or vec^T*(dg/dx) **/
void dhdx_vec(typeRNum *out, ctypeRNum t, ctypeRNum *x, ctypeRNum *u, ctypeRNum *p, ctypeRNum *vec, typeUSERPARAM *userparam)
{
    out[0] = vec[0] - vec[1] ;
    out[1] = 0 ;
}
/** Jacobian dh/du multiplied by vector vec, i.e. (dh/du)^T*vec or vec^T*(dg/du) **/
void dhdu_vec(typeRNum *out, ctypeRNum t, ctypeRNum *x, ctypeRNum *u, ctypeRNum *p, ctypeRNum *vec, typeUSERPARAM *userparam)
{
}
/** Jacobian dh/dp multiplied by vector vec, i.e. (dh/dp)^T*vec or vec^T*(dg/dp) **/
void dhdp_vec(typeRNum *out, ctypeRNum t, ctypeRNum *x, ctypeRNum *u, ctypeRNum *p, ctypeRNum *vec, typeUSERPARAM *userparam)
{
}

So do you know why this happens？

[Andreas Völz]

Dear Wong,

it is difficult to help with the few information available. My first guess would be that grampc_estim_penmin (typically called from initData.m) computes an invalid value for your problem (maybe inf or nan). Please remove the call of grampc_estim_penmin_Cmex in initData to check this. Note that this function internally calls grampc_run, which requires that the problem is well-defined after the initialization. If your problem relies on some user parameters that are only set in the MPC loop in startMPC and not in initData, this could be the cause of the error.

Best regards,
Andreas Völz

[Ethan]

Dear Andreas Völz,

Thanks very much for your reply. Just as you guess, when the call of grampc_estim_penmin_Cmex in initData is removed, the program works well. However, the problem still exists (the parameters of PenaltyMin which is in the setting of Multipliers & penalties cannot be determined for a suitable value) after I checked the whole program carefully again and even reduced the order of the state-space model. Now, I am uncertain about the underlying issue, and I would greatly appreciate if you can kindly help to check the code. The brief introduction of the model is in the attachment. Any insights you can provide would be invaluable. If you are currently unavailable, please feel free to disregard this request.

Best regards,
Wong.

[Andreas Völz]

Dear Wong,

as I have said, grampc_estim_penmin internally calls grampc_run which requires that the userparameters are well defined. In your case, the disturbance-related fields are only set in the Simulink model but not within initData prior to the call of grampc_estim_penmin. You should change line 98 of initData.m to something like

Wf = zeros(1, user.opt.Nhor);
userparam = [ pSys , pCost , boxz, Wf ] ;

and then the call of

[grampc, ~] = CmexFiles.grampc_estim_penmin_Cmex(grampc,1);

should work without error.

Best regards,
Andreas Völz