Differential wheel drive steering and speed PID controller model in MATLAB or similar

I have build a large differential drive electric Rover. It drives between waypoints using RTK gps for location and bearing. Steering is managed by varying the power from the motor controller to the 2 independent electric drive wheels. The code is run mainly on a Arduino Due with housekeeping on a raspberry pi. We have previously incorporated 2 different steering PID controllers however I am not sure that they are working properly. One PID tries to reduce the cross track error and the other reduces the error between the AIMpoint and the actual heading (Pure pursuit algorithm ) I want someone to develop a model and then the implement the PID on our Arduino code to reduce the cross track error to be within 5 to 10cm at all times. I also want developed a speed PID controller to control the max speed especially when moving down slope and getting too fast. We do record data from 2 wheel encoders however as yet we do not use the data This is NOT a project for beginners. So unless you really understand PID, Arduino C++ code , MATLAB or SIMULINK or similar please do not waste your time to apply. Do to the complexity of this issue only native English speakers based in the USA and Europe can be considered. This is a private projects so cost need to be reasonable .