def LTIinit(self, x, LapTime): self.TimeMPC = LapTime self.points = int(self.TimeMPC/self.dt) self.v0 = x[0] self.ClosedLoopDataLTI_MPC = ClosedLoopData(self.points, self.v0) self.Controller_PathFollowingLTI_MPC = PathFollowingLTI_MPC(self.A, self.B, self.Q, self.R, self.N, self.vt, self.inputConstr) # simulator.Sim(ClosedLoopDataLTI_MPC, Controller_PathFollowingLTI_MPC) self.count = 0
plotTrajectory(map, ClosedLoopDataPID.x, ClosedLoopDataPID.x_glob, ClosedLoopDataPID.u) plt.show() # ====================================================================================================================== # ====================================== LINEAR REGRESSION ============================================================ # ====================================================================================================================== raw_input("Finished PID - Start TI-MPC?") print("Starting TI-MPC") lamb = 0.0000001 #fit linear dynamics to the closed loop data: x2 = A*x1 + b*u1; lamb is weight on frob norm of W A, B, Error = Regression(ClosedLoopDataPID.x, ClosedLoopDataPID.u, lamb) if RunMPC == 1: ClosedLoopDataLTI_MPC = ClosedLoopData( dt, TimeMPC, v0) #form (empty) matrices for experiment data Controller_PathFollowingLTI_MPC = PathFollowingLTI_MPC(A, B, Q, R, N, vt) simulator.Sim(ClosedLoopDataLTI_MPC, Controller_PathFollowingLTI_MPC) #file_data = open(sys.path[0]+'/data/ClosedLoopDataLTI_MPC.obj', 'wb') file_data = open('data/ClosedLoopDataLTI_MPC.obj', 'wb') pickle.dump(ClosedLoopDataLTI_MPC, file_data) file_data.close() else: #file_data = open(sys.path[0]+'/data/ClosedLoopDataLTI_MPC.obj', 'rb') file_data = open('data/ClosedLoopDataLTI_MPC.obj', 'rb') ClosedLoopDataLTI_MPC = pickle.load(file_data) file_data.close() print("===== TI-MPC terminated") if plotFlagMPC == 1: plotTrajectory(map, ClosedLoopDataLTI_MPC.x, ClosedLoopDataLTI_MPC.x_glob, ClosedLoopDataLTI_MPC.u)
else: file_data = open(sys.path[0] + '/data/ClosedLoopDataPID1.obj', 'rb') ClosedLoopDataPID = pickle.load(file_data) file_data.close() print "===== PID terminated" # ====================================================================================================================== # ====================================== LINEAR REGRESSION ============================================================ # ====================================================================================================================== print "Starting MPC" lamb = 0.0000001 #应该是增量 A, B, Error = Regression(ClosedLoopDataPID.x, ClosedLoopDataPID.u, lamb) if RunMPC == 1: ClosedLoopDataLTI_MPC = ClosedLoopData(dt, TimeMPC, v0) Controller_PathFollowingLTI_MPC = PathFollowingLTI_MPC( A, B, Q, R, N, vt, inputConstr) simulator.Sim(ClosedLoopDataLTI_MPC, Controller_PathFollowingLTI_MPC) file_data = open(sys.path[0] + '/data/ClosedLoopDataLTI_MPC1.obj', 'w') pickle.dump(ClosedLoopDataLTI_MPC, file_data) file_data.close() else: file_data = open(sys.path[0] + '/data/ClosedLoopDataLTI_MPC1.obj', 'r') ClosedLoopDataLTI_MPC = pickle.load(file_data) file_data.close() print "===== MPC terminated" # ====================================================================================================================== # =================================== LOCAL LINEAR REGRESSION ========================================================= # ====================================================================================================================== print "Starting TV-MPC"