cost function about mpcc

[alexliniger]

Q_k contains basically the contouring error + lag error term (plus some regularization cost on u_k the actual inputs, and the yaw rate and the slip angle (see the AMZ paper))

f_k contains c_k and γ (the γ v_k T_s term)

R_k is R -> U in the code is the derivative of u and not the difference as in the paper

Q_N+1 and f_N+1 are the terminal cost terms, which use higher values for the contouring cost

Generally, the Matlab code and especially the C++ code are improved versions of the code from the paper.