raiot
/
PythonLinearNonlinearControl
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

add models

This commit is contained in:
Shunichi09 2019-02-08 09:47:25 +09:00
parent 10b5b9011a
commit f1fc57a219
2 changed files with 8 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
mpc/with_disturbance/iterative_MPC.py
View File

@ -4,6 +4,8 @@ import math
import copy import copy
from cvxopt import matrix, solvers from cvxopt import matrix, solvers
import cvxopt
cvxopt.solvers.options['show_progress'] = False # not display
class IterativeMpcController(): class IterativeMpcController():
""" """

11
mpc/with_disturbance/main_track.py
View File

@ -236,7 +236,7 @@ def main():
# parameters # parameters
dt = 0.01 dt = 0.01
simulation_time = 20 # in seconds simulation_time = 20 # in seconds
PREDICT_STEP = 20 PREDICT_STEP = 15
iteration_num = int(simulation_time / dt) iteration_num = int(simulation_time / dt)
# make simulator with coninuous matrix # make simulator with coninuous matrix
@ -258,14 +258,15 @@ def main():
# get traj's curvature # get traj's curvature
NUM_SKIP = 5 NUM_SKIP = 5
angles, curvatures = calc_curvatures(traj_ref[:, index_min:index_min + PREDICT_STEP + 2 * NUM_SKIP], PREDICT_STEP, NUM_SKIP) MARGIN = 10
angles, curvatures = calc_curvatures(traj_ref[:, index_min + MARGIN:index_min + PREDICT_STEP + 2 * NUM_SKIP + MARGIN], PREDICT_STEP, NUM_SKIP)
# evaluation function weight # evaluation function weight
Q = np.diag([100., 1., 1.]) Q = np.diag([100., 1., 1.])
R = np.diag([0.01]) R = np.diag([0.01])
# System model update # System model update
V = 4.0 # in pratical we should calc from the state V = 3.0 # in pratical we should calc from the state
lead_car_system_model.calc_predict_sytem_model(V, curvatures, PREDICT_STEP) lead_car_system_model.calc_predict_sytem_model(V, curvatures, PREDICT_STEP)
follow_car_system_model.calc_predict_sytem_model(V, curvatures, PREDICT_STEP) follow_car_system_model.calc_predict_sytem_model(V, curvatures, PREDICT_STEP)
@ -292,12 +293,12 @@ def main():
# nearest point # nearest point
index_min, nearest_point = search_nearest_point(traj_ref, lead_car.xs[:2].reshape(2, 1)) index_min, nearest_point = search_nearest_point(traj_ref, lead_car.xs[:2].reshape(2, 1))
# end check # end check
if len(traj_ref_ys) <= index_min + PREDICT_STEP + 2 * NUM_SKIP: if len(traj_ref_ys) <= index_min + PREDICT_STEP + 2 * NUM_SKIP + MARGIN:
print("break") print("break")
break break
# get traj's curvature # get traj's curvature
angles, curvatures = calc_curvatures(traj_ref[:, index_min:index_min + PREDICT_STEP + 2 * NUM_SKIP], PREDICT_STEP, NUM_SKIP) angles, curvatures = calc_curvatures(traj_ref[:, index_min+MARGIN:index_min + PREDICT_STEP + 2 * NUM_SKIP + MARGIN], PREDICT_STEP, NUM_SKIP)
# System model update # System model update
V = 4.0 # in pratical we should calc from the state V = 4.0 # in pratical we should calc from the state