MPC trajectory tracking controller for a bicycle model #840

Hi,
I have so far followed the examples in the documentation, and I am trying to implement a simple state-space MPC-based trajectory tracking controller based on the kinematic bicycle model shown in figure.1

Fig.1 Model
I have a reference trajectory for the states (x_ref(t),y_ref(t), v_ref(t), psi_ref(t)) obtained from a cubic polynomial and would want to optimize a similar objective given in Fig.2

where

I have understood that I can encode the bicycle model as NonlinearIOSystem as

 def vehicle_update(t,x,u, params):
    # x = [x,y,v,yaw]
    v = x[2]
    psi = x[3]
    a = u[0]
    delta = np.clip(u[1],-np.deg2rad(45), np.deg2rad(45))
    xdot =  v * np.cos(psi)
    ydot =  v * np.sin(psi)
    psidot = (v /P.WB) * np.tan(delta)
    vdot = np.clip( a * P.dt, P.speed_min, P.speed_min)
    return np.array([xdot, ydot, psidot, vdot])

def vehicle_output(t,x,u, params):
    return x

def solve_nonlinear_mpc(z_ref, x):
    vehicle = ct.NonlinearIOSystem(
        vehicle_update, vehicle_output, states=4, name='vehicle',
        inputs=['a', 'delta'], outputs=['x', 'y', 'v', 'psi'], dt=0.2)
   #what goes next?##

I now have to code the cost function which essentially computes sum_T_steps((x(t)-x_ref(t))^2 +(y(t)-y_ref(t))^2 +(v(t)-v_ref(t))^2+((psi(t)-psi_ref(t))^2) I'm really not understanding how to proceed from here how do I pass the reference trajectory to opt.quadratic_cost function? or do I have to pass a cost function handle, if so, how do I handle the computation of x(t) using the vehicle_update function in the cost function?

Thank you.