MPC算法

主要还是针对四舵轮模型进行的算法与仿真。