How to tune MPC of designed MPC controller built by matlab

Hello,

I have created my own road case scenario and wantes to test it on designed MPC controller with const speed of 15m/s. I have followed the lecture ( Understanding Model Predictive Control, Part 6: How to Design an MPC Controller with Simulink and Model Predictive Control Toolbox) . But my output is not looking good. the reference lateral position i started is around 4m but staring poistion of plant model output of lateral poistion is at 0. I am providing my model and files below. Can anyone let me know how to tune this MPC controller to get better results.

P.S. not just theoritically, if possible with results.

The left image is for yaw angles and right is for lateral positions

We can get the params file and others if download the files from lecture video...