MPC for car platooning, implementation

MPC Discussions
1

Hello,

I am trying to implement the centralized MPC formulation in: https://www.diva-portal.org/smash/get/diva2:1299131/FULLTEXT01.pdf.

I was able to implement in acados the speed and timegap costs but I do not understand how to implement the cost on the slack variable.
Moreover, how can I definite a constraint that depend on the system states or slack variables?

What I did so far:

    Qv = (1-delta)*np.diagflat( np.ones((cars_n, 1)) )  #speed cost
    Qt = delta*np.diagflat( np.ones((nx-cars_n, 1)) )   #timegap cost
    Q = scipy.linalg.block_diag(Qv, Qt)
    Q = np.diagflat( np.ones((nx, 1)) )
    R = np.diagflat( 1e-2 * np.ones((nu, 1)) )
    
    ocp.cost.W = scipy.linalg.block_diag(Q, R)
    ocp.cost.W_e = Q
    
    ocp.cost.Vx = np.zeros((ny, nx))
    ocp.cost.Vx[:nx,:nx] = np.eye(nx)
    ocp.cost.Vx[2,1] = -tau
    Vu = np.zeros((ny, nu))
    Vu[nx:nx+nu, :] = np.eye(nu)
    ocp.cost.Vu = Vu

    ocp.cost.Vx_e = np.eye(nx)

Thanks,
Alessandro