MPC: Get working on Sim

New euclidean distance costmap, generates pretty well, need to figure out how to incorporate
Localized polynomial seems to be generating valid along track / cross track, need to figure out how to speed up and how to incorporate the values
State updating and how to translate into trajectory following caused some trouble, gotta think about