mpc控制动力学模型

我想问一下北理那mpc控制那本书第五章的三自由度动力学模型如何跑任意轨迹啊？
我改他代码跑的轨迹和我给的没啥关系