How to use external information in prediction horizon

[xiaolou]

Hello everyone,
when I deal with my model, I found a problem that was not solved. The specific explanation is as follows: When the vehicle is operating on the road, the road information (such as road slope) of furture 80m is needed to adjust the vehicle state in advance. However, what has been achieved now is that the vehicle only starts to change state when it reaches the position of the slope. Not achieving the spatial prediction effect I expected。

Best regards,
xiaoloujiang

[Andreas Völz]

Dear xiaoloujiang,

thank you for your interest in the GRAMPC toolbox. Unfortunately, your problem description is not detailed enough for targeted help. Exploiting the predictive character of MPC as you intend is definitely possible and has been demonstrated in many publications using GRAMPC. However, based on the limited information, all I can say is that you need the future values (e.g. road slope depending on position) in the problem functions (often lfct, dldx, Vfct, dVdx). Typically, userparam is used either as array (Matlab interface) or as struct (direct C usage) for this task. If the values are time-dependent, note that the parameter t is the internal time [0, T] in all functions except the system dynamics ffct, dfdx_vec, dfdu_vec, where t is the global time [t0, t0+T]. Depending on the usecase, you have to pass t0 via userparam to the problem functions to convert between internal and global time.

If you need further help, then you have to share more information on problem formulation and implementation.

Best regards,
Andreas Völz

[xiaolou]

Dear Andreas Völz,

thank you for your answer. But I still not solve this problem. In my problem, the main purpose is to use the future road slope information to do speed prediction based on the vehicle longitudinal dynamics model, where displacement and velocity are two state quantities, and slope is a function of displacement, which is not directly related to time. In the cost function, I define the demand power, i.e. P=FV, F is the control quantity. Therefore, according to the information of the slope within the predicted range, the minimum cost for this period is calculated, so that the vehicle state can be adjusted in advance. However, the results I came up with failed to reflect this feature, and the change was only made when the vehicle reached slope. The following article is what I am referring to so far.

Best regards,
xiaoloujiang

[Andreas Völz]

Dear xiaolou,

with this information, it is still difficult to help you. Can you share your implementation of the probfct_TEMPLATE.c, a subset of it or provide a minimal working example that reproduces the problem?

Best regards,
Andreas Völz

[xiaolou]

Dear Andreas Völz,

the main code is described as follows, where h and delt_h mean the slope angle and change rate of slope, respectively. They are entered through the userparam interface in simulink. vdes is desired velocity.

void ffct(typeRNum *out, ctypeRNum t, ctypeRNum *x, ctypeRNum *u, ctypeRNum *p, typeUSERPARAM *userparam)
{

    ctypeRNum *param=(typeRNum*)userparam;
    typeRNum h = param[14];
    typeRNum delt_h = param[15];

    out[0]=x[1];
    out[1]=(u[0]-param[1]*param[2]*(param[3]*COS(h)+SIN(h))-0.5*param[4]*param[5]*param[6]*POW2(x[1]))/(param[1]);


}
/** Jacobian df/dx multiplied by vector vec, i.e. (df/dx)^T*vec or vec^T*(df/dx) **/
void dfdx_vec(typeRNum *out, ctypeRNum t, ctypeRNum *x, ctypeRNum *vec, ctypeRNum *u, ctypeRNum *p, typeUSERPARAM *userparam)
{

    ctypeRNum *param=(typeRNum*)userparam;
    typeRNum h = param[14];
    typeRNum delt_h = param[15];

    out[0]=vec[1]*param[2]*(COS(h)-param[3]*SIN(h))*delt_h;
    out[1]=vec[0]-(vec[1]*param[4]*param[5]*param[6]*x[1])/(param[1]);
} 
/** Jacobian df/du multiplied by vector vec, i.e. (df/du)^T*vec or vec^T*(df/du) **/
void dfdu_vec(typeRNum *out, ctypeRNum t, ctypeRNum *x, ctypeRNum *vec, ctypeRNum *u, ctypeRNum *p, typeUSERPARAM *userparam)
{
    ctypeRNum *param=(typeRNum*)userparam;
    out[0] = vec[1]/(param[1]);
}
/** Jacobian df/dp multiplied by vector vec, i.e. (df/dp)^T*vec or vec^T*(df/dp) **/
void dfdp_vec(typeRNum *out, ctypeRNum t, ctypeRNum *x, ctypeRNum *vec, ctypeRNum *u, ctypeRNum *p, typeUSERPARAM *userparam)
{
}


/** 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)
{
    ctypeRNum* param=(ctypeRNum*)userparam;
    typeRNum Sref = param[16];
     typeRNum h = param[14];


 if (u[0]>0)
    { 
         out[0] = param[11]*POW2(u[0]*x[1])+param[12]*POW2(x[1]-vdes);
        }
    else if (u[0]<=0 )
        { 
           out[0] = param[12]*POW2(x[1]-vdes);   
      }
}
/** Gradient dl/dx **/
void dldx(typeRNum *out, ctypeRNum t, ctypeRNum *x, ctypeRNum *u, ctypeRNum *p, ctypeRNum *xdes, ctypeRNum *udes, typeUSERPARAM *userparam)
{
    ctypeRNum* param=(ctypeRNum*)userparam;
    typeRNum Sref = param[16];
    typeRNum h = param[14];

   if (u[0]>0 )
   { 
     out[0] = 0;
     out[1] = 2*param[11]*x[1]*POW2(u[0])+2*param[12]*(x[1]-vdes);
   }
    else 
    { 
    out[0] = 0;
    out[1] = 2*param[12]*(x[1]-vdes);
    }
}
/** Gradient dl/du **/
void dldu(typeRNum *out, ctypeRNum t, ctypeRNum *x, ctypeRNum *u, ctypeRNum *p, ctypeRNum *xdes, ctypeRNum *udes, typeUSERPARAM *userparam)
{
    ctypeRNum* param=(ctypeRNum*)userparam;
    typeRNum h = param[14];

    if (u[0]>0 )

    {
        out[0] =2*param[11]*u[0]*POW2(x[1]);
    }
    else 
    {
        out[0] =0;
    }


}

Best regards,
xiaoloujiang

[Andreas Völz]

Dear xiaolou,

thanks for the code. First of all, the slope h is not used in the cost functions, so no predictive impact here. Then, the slope h (equal to param[14]) is used in the dynamics ffct and its derivative delt_h (equal to param[15]) is used in dfdx_vec. However, you only have one slope parameter that is used for the complete prediction horizon, i.e. it is independent of the actual position / displacement.

What you need to pass in via userparam is a representation of the slope depending on the displacement, e.g. equidistant samples or coefficients of an interpolating polynomial. Then, you have to compute the slope h(x[0]) depending on the position x[0]. Since the position is predicted along the horizon, also the slope is predicted and used for the optimization.

Best regards,
Andreas Völz

[xiaolou]

Dear Andreas Völz,

Thanks for your quick replay. Based on your comments, I changed some parts and added the slope to the cost function. This time I put the expression of the slope directly into the main program. Due to my misunderstanding, the result still doesn't seem to be as expected. Although the slope is written as a function of the position x[0], it is still an independent of position. I sincerely hope you can give further advice.

/** System function f(t,x,u,p,userparam) 
    ------------------------------------ **/
void ffct(typeRNum *out, ctypeRNum t, ctypeRNum *x, ctypeRNum *u, ctypeRNum *p, typeUSERPARAM *userparam)
{

    ctypeRNum *param=(typeRNum*)userparam;
     typeRNum delt_x = 20;
     typeRNum altitude= 10.28*exp(-POW2((x[0]-3050)/258.5));
     typeRNum h= (10.28*exp(-POW2((x[0]+delt_x-3050)/258.5))-10.28*exp(-POW2((x[0]-delt_x-3050)/258.5)))/(2*delt_x); //


    out[0]=x[1];
    out[1]=(u[0]-param[1]*param[2]*(param[3]*COS(h)+SIN(h))-0.5*param[4]*param[5]*param[6]*POW2(x[1]))/(param[1]);


}
/** Jacobian df/dx multiplied by vector vec, i.e. (df/dx)^T*vec or vec^T*(df/dx) **/
void dfdx_vec(typeRNum *out, ctypeRNum t, ctypeRNum *x, ctypeRNum *vec, ctypeRNum *u, ctypeRNum *p, typeUSERPARAM *userparam)
{

    ctypeRNum *param=(typeRNum*)userparam;
   typeRNum delt_x = 20;
   typeRNum altitude= 10.28*exp(-POW2((x[0]-3050)/258.5)); // Elevation of the road;
  typeRNum h= (10.28*exp(-POW2((x[0]+delt_x-3050)/258.5))-10.28*exp(-POW2((x[0]-    delt_x-3050)/258.5)))/(2*delt_x);   // Road slope

   typeRNum delt_h=((10.28*exp(-POW2((x[0]+delt_x+delt_x-3050)/258.5))-10.28*exp(-POW2((x[0]-3050)/258.5)))/(2*delt_x)-(10.28*exp(-POW2((x[0]-3050)/258.5))-10.28*exp(-POW2((x[0]-delt_x-delt_x-3050)/258.5)))/(2*delt_x))/(2*delt_x);  // Slope change rate

    out[0]=vec[1]*param[2]*(COS(h)-param[3]*SIN(h))*delt_h;
    out[1]=vec[0]-(vec[1]*param[4]*param[5]*param[6]*x[1])/(param[1]);
} 
/** Jacobian df/du multiplied by vector vec, i.e. (df/du)^T*vec or vec^T*(df/du) **/
void dfdu_vec(typeRNum *out, ctypeRNum t, ctypeRNum *x, ctypeRNum *vec, ctypeRNum *u, ctypeRNum *p, typeUSERPARAM *userparam)
{
    ctypeRNum *param=(typeRNum*)userparam;
    out[0] = vec[1]/(param[1]);
}
/** Jacobian df/dp multiplied by vector vec, i.e. (df/dp)^T*vec or vec^T*(df/dp) **/
void dfdp_vec(typeRNum *out, ctypeRNum t, ctypeRNum *x, ctypeRNum *vec, ctypeRNum *u, ctypeRNum *p, typeUSERPARAM *userparam)
{
}


/** 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)
{
    ctypeRNum* param=(ctypeRNum*)userparam;
    typeRNum Sref = param[16];

   typeRNum delt_x = 20;
   typeRNum altitude= 10.28*exp(-POW2((x[0]-3050)/258.5));
   typeRNum h= (10.28*exp(-POW2((x[0]+delt_x-3050)/258.5))-10.28*exp(-POW2((x[0]- delt_x-3050)/258.5)))/(2*delt_x); 

   typeRNum delt_h=((10.28*exp(-POW2((x[0]+delt_x+delt_x-3050)/258.5))-10.28*exp(-POW2((x[0]-3050)/258.5)))/(2*delt_x)-(10.28*exp(-POW2((x[0]-3050)/258.5))-10.28*exp(-POW2((x[0]-delt_x-delt_x-3050)/258.5)))/(2*delt_x))/(2*delt_x);


    out[0] = param[11]*POW2(u[0]*x[1])+param[12]*POW2(x[1]-15)+ param[10]*0.5*POW2((u[0]-param[1]*param[2]*(param[3]*COS(h)+SIN(h))-0.5*param[4]*param[5]*param[6]*POW2(x[1]))/(param[1])+param[2]*SIN(h));
}
/** Gradient dl/dx **/
void dldx(typeRNum *out, ctypeRNum t, ctypeRNum *x, ctypeRNum *u, ctypeRNum *p, ctypeRNum *xdes, ctypeRNum *udes, typeUSERPARAM *userparam)
{
    ctypeRNum* param=(ctypeRNum*)userparam;
    typeRNum Sref = param[16];
   typeRNum delt_x = 20;
   typeRNum altitude= 10.28*exp(-POW2((x[0]-3050)/258.5));
   typeRNum h= (10.28*exp(-POW2((x[0]+delt_x-3050)/258.5))-10.28*exp(-POW2((x[0]-delt_x-3050)/258.5)))/(2*delt_x); //

   typeRNum delt_h=((10.28*exp(-POW2((x[0]+delt_x+delt_x-3050)/258.5))-10.28*exp(-POW2((x[0]-3050)/258.5)))/(2*delt_x)-(10.28*exp(-POW2((x[0]-3050)/258.5))-10.28*exp(-POW2((x[0]-delt_x-delt_x-3050)/258.5)))/(2*delt_x))/(2*delt_x);


     out[0] = param[10]*((u[0]-param[1]*param[2]*(param[3]*COS(h)+SIN(h))-0.5*param[4]*param[5]*param[6]*POW2(x[1]))/(param[1])+param[2]*SIN(h))*(param[2]*param[3]*SIN(h))*delt_h;
     out[1] = 2*param[11]*POW2(u[0])*x[1]+2*param[12]*(x[1]-15)+param[10]*((u[0]-param[1]*param[2]*(param[3]*COS(h)+SIN(h))-0.5*param[4]*param[5]*param[6]*POW2(x[1]))/param[1]+param[2]*SIN(h))*(-param[4]*param[5]*param[6]*x[1]/param[1]);
}
/** Gradient dl/du **/
void dldu(typeRNum *out, ctypeRNum t, ctypeRNum *x, ctypeRNum *u, ctypeRNum *p, ctypeRNum *xdes, ctypeRNum *udes, typeUSERPARAM *userparam)
{
    ctypeRNum* param=(ctypeRNum*)userparam;
    /*typeRNum h = param[14];*/
   typeRNum delt_x = 20;
  typeRNum altitude= 10.28*exp(-POW2((x[0]-3050)/258.5));
     typeRNum h= (10.28*exp(-POW2((x[0]+delt_x-3050)/258.5))-10.28*exp(-POW2((x[0]-delt_x-3050)/258.5)))/(2*delt_x); //

   typeRNum delt_h=((10.28*exp(-POW2((x[0]+delt_x+delt_x-3050)/258.5))-10.28*exp(-POW2((x[0]-3050)/258.5)))/(2*delt_x)-(10.28*exp(-POW2((x[0]-3050)/258.5))-10.28*exp(-POW2((x[0]-delt_x-delt_x-3050)/258.5)))/(2*delt_x))/(2*delt_x);


    out[0] =2*param[11]*POW2(x[1])*u[0]+param[10]*((u[0]-param[1]*param[2]*(param[3]*COS(h)+SIN(h))-0.5*param[4]*param[5]*param[6]*POW2(x[1]))/param[1]+param[2]*SIN(h))/param[1];

}

Best regards,
xiaoloujiang

[Andreas Völz]

Dear xiaolou,

thank you for the update, this looks already better, but also more complicated. It seems to me that your are computing the slope h and the change of slope delt_h using numerical differentiation of the altitude a, although the altitude is given as an analytical function. Is this correct? Then, why don't you compute slope and change of slope also analytically?

Let's say the position is s=x[0], and you have an altitude function a(s), where the slope is defined is h(s)=da(s) / ds and the change of slope as dh(s) / ds = d^2 a(s) / ds^2. This should work in GRAMPC if you correctly implement the gradients, that is, if ffct or lfct contains h(s) somewhere, then dfdx_vec and dldx must take the respective gradients dh(s) / ds into account.

Maybe try to test your code step by step. For example, let the vehicle drive with constant velocity along a road with varying altitude. Inspect the simulated and the predicted trajectories, whether the desired effect of the altitude is visible. Analytical derivatives can be validated by implementing numerical differentiation and checking multiple test cases.

Best regards,
Andreas Völz

[xiaolou]

Dear Andreas Völz,

Tank you very much for your guidance and help. Now, it can work efficiently.
In the end of my letter,I put my best wishes for you！

Yours，
xiaoloujiang

[Andreas Völz]

Dear xiaolou,

I am glad to hear that you were able to get it running as desired. If you noticed some critical steps, that could also be of help to other users of GRAMPC, feel free to share your experience.

Best regards,
Andreas Völz