From 45a64eada467a16a6216398213c9d2b8adcf8dd5 Mon Sep 17 00:00:00 2001 From: Shunichi09 Date: Wed, 6 Feb 2019 19:22:58 +0900 Subject: [PATCH] add --- mpc/with_disturbance/main_track.py | 25 ++++++++++++++++++------- 1 file changed, 18 insertions(+), 7 deletions(-) diff --git a/mpc/with_disturbance/main_track.py b/mpc/with_disturbance/main_track.py index fbbafdd..ac06f9f 100644 --- a/mpc/with_disturbance/main_track.py +++ b/mpc/with_disturbance/main_track.py @@ -136,15 +136,26 @@ def main(): # you must be care about this matrix # these A and B are for continuos system if you want to use discret system matrix please skip this step # lineared car system - V = 5.0 - Ad = np.array([[1., 0., 0., 0.], - [0., 1, V, 0.], - [0., 0., 1., 0.], - [0., 0., 1., 0.]]) * dt + WHEEL_BASE = 2.2 - Bd = np.array([[0.], [0.], [0.], [0.3]]) * dt + V = 0.1 # initialize - W_D = np.array([[V], [0.], [0.], [0.]]) * dt + alpha = 0.1 + R = 1.0 / 2 * math.sin(alpha) + delta_r = math.atan2(WHEEL_BASE / R) + + A12 = (V / WHEEL_BASE) / math.cos(delta_r) + A22 = (1. - 1. / tau) + + Ad = np.array([[1., V, 0.], + [0., 1., A12], + [0., 0., A22]]) * dt + + Bd = np.array([[0.], [0.], [1. / tau]]) * dt + + W_D_0 = - (V / WHEEL_BASE) * delta_r / (math.cos(delta_r)**2) + + W_D = np.array([[0.], [W_D_0], [0.]]) * dt # make simulator with coninuous matrix init_xs_lead = np.array([5., 0., 0. ,0.])