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"