diff --git a/.gitmodules b/.gitmodules
index d2d2e940..4c63b290 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -31,3 +31,6 @@
 [submodule "PathTracking/lqr_speed_steer_control/pycubicspline"]
 	path = PathTracking/lqr_speed_steer_control/pycubicspline
 	url = https://github.com/AtsushiSakai/pycubicspline
+[submodule "PathTracking/model_predictive_speed_and_steer_control/pycubicspline"]
+	path = PathTracking/model_predictive_speed_and_steer_control/pycubicspline
+	url = https://github.com/AtsushiSakai/pycubicspline
diff --git a/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py b/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py
index 26a77bd9..e78a44bc 100644
--- a/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py
+++ b/PathTracking/model_predictive_speed_and_steer_control/model_predictive_speed_and_steer_control.py
@@ -1,15 +1,553 @@
 """
 
-Path tracking simulation with model predictive control for speed and steer control
+Path tracking simulation with iterative linear model predictive control for speed and steer control
 
 author: Atsushi Sakai (@Atsushi_twi)
 
 """
 
+import numpy as np
+import math
+import cvxpy
+import matplotlib.pyplot as plt
+from pycubicspline import pycubicspline
+#  from matplotrecorder import matplotrecorder
+
+nx = 4  # x = x, y, v, yaw
+nu = 2  # a = [accel, steer]
+T = 5  # horizon length
+
+# mpc parameters
+R = np.diag([1.0, 0.01])
+Rd = np.diag([0.01, 0.01])
+Q = np.diag([1.0, 1.0, 0.5, 10.0])
+Qf = Q
+goal_dis = 1.5  # goal distance
+stop_speed = 0.5 / 3.6  # stop speed
+maxtime = 500.0  # max simulation time
+max_accel = 1.0  # maximum accel
+
+# iterative paramter
+maxiter = 3
+du_th = 0.1
+
+target_speed = 20.0 / 3.6
+
+show_animation = True
+#  show_animation = False
+
+dt = 0.2  # [s] time tick
+WB = 6.0  # [m] Wheel base
+
+maxsteer = math.radians(45.0)  # maximum steering angle
+maxdsteer = math.radians(7.0)  # maximum steering speed
+maxspeed = 55.0 / 3.6  # maximum speed
+minspeed = -20.0 / 3.6  # minmum speed
+
+predelta = None
+
+
+class State:
+    """
+    vehicle state class
+    """
+
+    def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0):
+        self.x = x
+        self.y = y
+        self.yaw = yaw
+        self.v = v
+
+
+def pi_2_pi(angle):
+    while(angle > math.pi):
+        angle = angle - 2.0 * math.pi
+
+    while(angle < -math.pi):
+        angle = angle + 2.0 * math.pi
+
+    return angle
+
+
+def get_linear_model_matrix(v, phi, delta):
+
+    A = np.matrix(np.zeros((nx, nx)))
+    A[0, 0] = 1.0
+    A[1, 1] = 1.0
+    A[2, 2] = 1.0
+    A[3, 3] = 1.0
+    A[0, 2] = dt * math.cos(phi)
+    A[0, 3] = - dt * v * math.sin(phi)
+    A[1, 2] = dt * math.sin(phi)
+    A[1, 3] = dt * v * math.cos(phi)
+    A[3, 2] = dt * math.tan(delta)
+
+    B = np.matrix(np.zeros((nx, nu)))
+    B[2, 0] = dt
+    B[3, 1] = dt * v / (WB * math.cos(delta) ** 2)
+
+    C = np.matrix(np.zeros((nx, 1)))
+    C[0, 0] = dt * v * math.sin(phi) * phi
+    C[1, 0] = - dt * v * math.cos(phi) * phi
+    C[3, 0] = v * delta / (WB * math.cos(delta) ** 2)
+
+    return A, B, C
+
+
+def plot_car(x, y, yaw, steer=0.0, cabcolor="-r", truckcolor="-k"):
+
+    LENGTH = 12.0
+    WIDTH = 6.0
+    BACKTOWHEEL = 2.5
+    WHEEL_LEN = 1.0
+    WHEEL_WIDTH = 0.5
+    TREAD = 2.0
+
+    outline = np.matrix([[-BACKTOWHEEL, (LENGTH - BACKTOWHEEL), (LENGTH - BACKTOWHEEL), -BACKTOWHEEL, -BACKTOWHEEL],
+                         [WIDTH / 2, WIDTH / 2, - WIDTH / 2, -WIDTH / 2, WIDTH / 2]])
+
+    fr_wheel = np.matrix([[WHEEL_LEN, -WHEEL_LEN, -WHEEL_LEN, WHEEL_LEN, WHEEL_LEN],
+                          [-WHEEL_WIDTH - TREAD, -WHEEL_WIDTH - TREAD, WHEEL_WIDTH - TREAD, WHEEL_WIDTH - TREAD, -WHEEL_WIDTH - TREAD]])
+
+    rr_wheel = np.copy(fr_wheel)
+
+    fl_wheel = np.copy(fr_wheel)
+    fl_wheel[1, :] *= -1
+    rl_wheel = np.copy(rr_wheel)
+    rl_wheel[1, :] *= -1
+
+    Rot1 = np.matrix([[math.cos(yaw), math.sin(yaw)],
+                      [-math.sin(yaw), math.cos(yaw)]])
+    Rot2 = np.matrix([[math.cos(steer), math.sin(steer)],
+                      [-math.sin(steer), math.cos(steer)]])
+
+    fr_wheel = (fr_wheel.T * Rot2).T
+    fl_wheel = (fl_wheel.T * Rot2).T
+    fr_wheel[0, :] += WB
+    fl_wheel[0, :] += WB
+
+    fr_wheel = (fr_wheel.T * Rot1).T
+    fl_wheel = (fl_wheel.T * Rot1).T
+
+    outline = (outline.T * Rot1).T
+    rr_wheel = (rr_wheel.T * Rot1).T
+    rl_wheel = (rl_wheel.T * Rot1).T
+
+    outline[0, :] += x
+    outline[1, :] += y
+    fr_wheel[0, :] += x
+    fr_wheel[1, :] += y
+    rr_wheel[0, :] += x
+    rr_wheel[1, :] += y
+    fl_wheel[0, :] += x
+    fl_wheel[1, :] += y
+    rl_wheel[0, :] += x
+    rl_wheel[1, :] += y
+
+    plt.plot(np.array(outline[0, :]).flatten(),
+             np.array(outline[1, :]).flatten(), truckcolor)
+    plt.plot(np.array(fr_wheel[0, :]).flatten(),
+             np.array(fr_wheel[1, :]).flatten(), truckcolor)
+    plt.plot(np.array(rr_wheel[0, :]).flatten(),
+             np.array(rr_wheel[1, :]).flatten(), truckcolor)
+    plt.plot(np.array(fl_wheel[0, :]).flatten(),
+             np.array(fl_wheel[1, :]).flatten(), truckcolor)
+    plt.plot(np.array(rl_wheel[0, :]).flatten(),
+             np.array(rl_wheel[1, :]).flatten(), truckcolor)
+    plt.plot(x, y, "*")
+
+
+def update_state(state, a, delta):
+
+    # input check
+    if delta >= maxsteer:
+        delta = maxsteer
+    elif delta <= -maxsteer:
+        delta = -maxsteer
+
+    global predelta
+    if predelta is not None:
+        if (delta - predelta) >= (maxdsteer * dt):
+            delta = predelta + maxdsteer * dt
+        elif (delta - predelta) <= -(maxdsteer * dt):
+            delta = predelta - maxdsteer * dt
+    predelta = delta
+
+    state.x = state.x + state.v * math.cos(state.yaw) * dt
+    state.y = state.y + state.v * math.sin(state.yaw) * dt
+    state.yaw = state.yaw + state.v / WB * math.tan(delta) * dt
+    state.v = state.v + a * dt
+
+    if state. v > maxspeed:
+        state.v = maxspeed
+    elif state. v < minspeed:
+        state.v = minspeed
+
+    return state
+
+
+def get_nparray_from_matrix(x):
+    """
+    get build-in list from matrix
+    """
+    return np.array(x).flatten()
+
+
+def calc_nearest_index(state, cx, cy, cyaw):
+    dx = [state.x - icx for icx in cx]
+    dy = [state.y - icy for icy in cy]
+
+    d = [abs(math.sqrt(idx ** 2 + idy ** 2)) for (idx, idy) in zip(dx, dy)]
+
+    mind = min(d)
+
+    ind = d.index(mind)
+
+    dxl = cx[ind] - state.x
+    dyl = cy[ind] - state.y
+
+    angle = pi_2_pi(cyaw[ind] - math.atan2(dyl, dxl))
+    if angle < 0:
+        mind *= -1
+
+    return ind, mind
+
+
+def predict_motion(x0, oa, od, xref):
+    xbar = xref * 0.0
+    for i in range(len(x0)):
+        xbar[i, 0] = x0[i]
+
+    state = State(x=x0[0], y=x0[1], yaw=x0[3], v=x0[2])
+    for (ai, di, i) in zip(oa, od, range(1, T + 1)):
+        state = update_state(state, ai, di)
+        xbar[0, i] = state.x
+        xbar[1, i] = state.y
+        xbar[2, i] = state.v
+        xbar[3, i] = state.yaw
+
+    return xbar
+
+
+def iterative_linear_mpc_control(xref, x0, dref, oa, od):
+    """
+    MPC contorl with updating operational point iteraitvely
+    """
+
+    if oa is None or od is None:
+        oa = [0.0] * T
+        od = [0.0] * T
+
+    for i in range(maxiter):
+        xbar = predict_motion(x0, oa, od, xref)
+        poa, pod = oa[:], od[:]
+        oa, od, ox, oy, oyaw, ov = linear_mpc_control(xref, xbar, x0, dref)
+        du = sum(abs(oa - poa)) + sum(abs(od - pod))  # calc u change value
+        if du <= du_th:
+            break
+    else:
+        print("Iterative is max iter")
+
+    return oa, od, ox, oy, oyaw, ov
+
+
+def linear_mpc_control(xref, xbar, x0, dref):
+    """
+    linear mpc control
+
+    xref: reference point
+    xbar: operational point
+    x0: initial state
+    dref: reference steer angle
+    """
+
+    x = cvxpy.Variable(nx, T + 1)
+    u = cvxpy.Variable(nu, T)
+
+    cost = 0.0
+    constraints = []
+
+    for t in range(T):
+        cost += cvxpy.quad_form(u[:, t], R)
+
+        if t != 0:
+            cost += cvxpy.quad_form(xref[:, t] - x[:, t], Q)
+
+        A, B, C = get_linear_model_matrix(
+            xbar[2, t], xbar[3, t], dref[0, t])
+        constraints += [x[:, t + 1] == A * x[:, t] + B * u[:, t] + C]
+
+        if t < (T - 1):
+            cost += cvxpy.quad_form(u[:, t + 1] - u[:, t], Rd)
+            constraints += [cvxpy.abs(u[1, t + 1] - u[1, t])
+                            < maxdsteer * dt]
+
+    cost += cvxpy.quad_form(xref[:, T] - x[:, T], Qf)
+
+    constraints += [x[:, 0] == x0]
+    constraints += [x[2, :] <= maxspeed]
+    constraints += [x[2, :] >= minspeed]
+    constraints += [cvxpy.abs(u[0, :]) < max_accel]
+    constraints += [cvxpy.abs(u[1, :]) < maxsteer]
+
+    prob = cvxpy.Problem(cvxpy.Minimize(cost), constraints)
+    prob.solve(verbose=False)
+
+    if prob.status == cvxpy.OPTIMAL or prob.status == cvxpy.OPTIMAL_INACCURATE:
+        ox = get_nparray_from_matrix(x.value[0, :])
+        oy = get_nparray_from_matrix(x.value[1, :])
+        ov = get_nparray_from_matrix(x.value[2, :])
+        oyaw = get_nparray_from_matrix(x.value[3, :])
+        oa = get_nparray_from_matrix(u.value[0, :])
+        odelta = get_nparray_from_matrix(u.value[1, :])
+
+    else:
+        print("Error: Cannot solve mpc..")
+        oa, odelta, ox, oy, oyaw, ov = None, None, None, None, None, None
+
+    return oa, odelta, ox, oy, oyaw, ov
+
+
+def calc_ref_trajectory(state, cx, cy, cyaw, ck, sp, dl, pind):
+    xref = np.zeros((nx, T + 1))
+    dref = np.zeros((1, T + 1))
+    ncourse = len(cx)
+
+    ind, _ = calc_nearest_index(state, cx, cy, cyaw)
+
+    if pind >= ind:
+        ind = pind
+
+    xref[0, 0] = cx[ind]
+    xref[1, 0] = cy[ind]
+    xref[2, 0] = sp[ind]
+    xref[3, 0] = cyaw[ind]
+    dref[0, 0] = 0.0  # steer operational point should be 0
+
+    travel = 0.0
+
+    for i in range(T + 1):
+        travel += abs(state.v) * dt
+        dind = int(round(travel / dl))
+
+        if (ind + dind) < ncourse:
+            xref[0, i] = cx[ind + dind]
+            xref[1, i] = cy[ind + dind]
+            xref[2, i] = sp[ind + dind]
+            xref[3, i] = cyaw[ind + dind]
+            dref[0, i] = 0.0
+        else:
+            xref[0, i] = cx[ncourse - 1]
+            xref[1, i] = cy[ncourse - 1]
+            xref[2, i] = sp[ncourse - 1]
+            xref[3, i] = cyaw[ncourse - 1]
+            dref[0, i] = 0.0
+
+    return xref, ind, dref
+
+
+def check_goal(state, goal, tind, nind):
+
+    # check goal
+    dx = state.x - goal[0]
+    dy = state.y - goal[1]
+    d = math.sqrt(dx ** 2 + dy ** 2)
+    #  print(d)
+
+    if (d <= goal_dis):
+        isgoal = True
+    else:
+        isgoal = False
+
+    if abs(tind - nind) >= 5:
+        isgoal = False
+
+    if (abs(state.v) <= stop_speed):
+        isstop = True
+    else:
+        isstop = False
+
+    if isgoal and isstop:
+        return True
+
+    return False
+
+
+def do_simulation(cx, cy, cyaw, ck, sp, dl):
+    """
+    Simulation
+
+    cx: course x position list
+    cy: course y position list
+    cy: course yaw position list
+    ck: course curvature list
+    sp: speed profile
+    dl: course tick [m]
+
+    """
+
+    goal = [cx[-1], cy[-1]]
+    state = State(x=cx[0], y=cy[0], yaw=cyaw[0], v=0.0)
+
+    time = 0.0
+    x = [state.x]
+    y = [state.y]
+    yaw = [state.yaw]
+    v = [state.v]
+    t = [0.0]
+    d = [0.0]
+    a = [0.0]
+    target_ind, _ = calc_nearest_index(state, cx, cy, cyaw)
+
+    odelta, oa = None, None
+
+    cyaw = smooth_yaw(cyaw)
+
+    while maxtime >= time:
+        xref, target_ind, dref = calc_ref_trajectory(
+            state, cx, cy, cyaw, ck, sp, dl, target_ind)
+
+        x0 = [state.x, state.y, state.v, state.yaw]  # current state
+
+        oa, odelta, ox, oy, oyaw, ov = iterative_linear_mpc_control(
+            xref, x0, dref, oa, odelta)
+
+        if odelta is not None:
+            di, ai = odelta[0], oa[0]
+
+        state = update_state(state, ai, di)
+        time = time + dt
+
+        x.append(state.x)
+        y.append(state.y)
+        yaw.append(state.yaw)
+        v.append(state.v)
+        t.append(time)
+        d.append(di)
+        a.append(ai)
+
+        if check_goal(state, goal, target_ind, len(cx)):
+            print("Goal")
+            break
+
+        if show_animation:
+            plt.cla()
+            if ox is not None:
+                plt.plot(ox, oy, "xr", label="MPC")
+            plt.plot(cx, cy, "-r", label="course")
+            plt.plot(x, y, "ob", label="trajectory")
+            plt.plot(xref[0, :], xref[1, :], "xk", label="xref")
+            plt.plot(cx[target_ind], cy[target_ind], "xg", label="target")
+            plot_car(state.x, state.y, state.yaw, steer=di)
+            plt.axis("equal")
+            plt.grid(True)
+
+            #  wsize = 50.0
+            #  set_xlim([-wsize + state.x, wsize + state.x])
+            #  set_ylim([-wsize + state.y, wsize + state.y])
+            plt.title("Time[s]:" + str(round(time, 2)) +
+                      ", speed[km/h]:" + str(round(state.v * 3.6, 2)))
+            plt.pause(0.0001)
+
+    return t, x, y, yaw, v, d, a
+
+
+def calc_speed_profile(cx, cy, cyaw, target_speed):
+
+    speed_profile = [target_speed] * len(cx)
+    direction = 1.0  # forward
+
+    # Set stop point
+    for i in range(len(cx) - 1):
+        dx = cx[i + 1] - cx[i]
+        dy = cy[i + 1] - cy[i]
+
+        move_direction = math.atan2(dy, dx)
+
+        if dx != 0.0 and dy != 0.0:
+            dangle = abs(pi_2_pi(move_direction - cyaw[i]))
+            if dangle >= math.pi / 4.0:
+                direction = -1.0
+            else:
+                direction = 1.0
+
+        if direction != 1.0:
+            speed_profile[i] = - target_speed
+        else:
+            speed_profile[i] = target_speed
+
+    speed_profile[-1] = 0.0
+
+    return speed_profile
+
+
+def smooth_yaw(yaw):
+
+    for i in range(len(yaw) - 1):
+        dyaw = yaw[i + 1] - yaw[i]
+        while dyaw >= math.pi / 2.0:
+            yaw[i + 1] -= math.pi * 2.0
+            dyaw = yaw[i + 1] - yaw[i]
+        while dyaw <= -math.pi / 2.0:
+            yaw[i + 1] += math.pi * 2.0
+            dyaw = yaw[i + 1] - yaw[i]
+
+    return yaw
+
+
+def get_forward_course(dl):
+    ax = [0.0, 60.0, 125.0, 50.0, 75.0, 30.0, -10.0]
+    ay = [0.0, 0.0, 50.0, 65.0, 30.0, 50.0, -20.0]
+    cx, cy, cyaw, ck, s = pycubicspline.calc_spline_course(ax, ay, ds=dl)
+
+    return cx, cy, cyaw, ck
+
+
+def get_switch_back_course(dl):
+    ax = [0.0, 60.0, 125.0, 50.0, 75.0]
+    ay = [0.0, 0.0, 50.0, 65.0, 30.0]
+    cx, cy, cyaw, ck, s = pycubicspline.calc_spline_course(ax, ay, ds=dl)
+    ax = [75.0, 30.0, 0.0, 0.0]
+    ay = [30.0, 50.0, 10.0, 1.0]
+    cx2, cy2, cyaw2, ck2, s2 = pycubicspline.calc_spline_course(ax, ay, ds=dl)
+    cyaw2 = [i - math.pi for i in cyaw2]
+    cx.extend(cx2)
+    cy.extend(cy2)
+    cyaw.extend(cyaw2)
+    ck.extend(ck2)
+
+    return cx, cy, cyaw, ck
+
 
 def main():
     print(__file__ + " start!!")
 
+    dl = 1.0  # course tick
+    #  cx, cy, cyaw, ck = get_forward_course(dl)
+    cx, cy, cyaw, ck = get_switch_back_course(dl)
+
+    sp = calc_speed_profile(cx, cy, cyaw, target_speed)
+
+    t, x, y, yaw, v, d, a = do_simulation(cx, cy, cyaw, ck, sp, dl)
+
+    if show_animation:
+        plt.subplots()
+        plt.plot(cx, cy, "-r", label="spline")
+        plt.plot(x, y, "-g", label="tracking")
+        plt.grid(True)
+        plt.axis("equal")
+        plt.xlabel("x[m]")
+        plt.ylabel("y[m]")
+        plt.legend()
+
+        plt.subplots()
+        plt.plot(t, v, "-r", label="speed")
+        plt.grid(True)
+        plt.xlabel("Time [s]")
+        plt.ylabel("Speed [kmh]")
+
+        plt.show()
+
 
 if __name__ == '__main__':
     main()
diff --git a/PathTracking/model_predictive_speed_and_steer_control/pycubicspline b/PathTracking/model_predictive_speed_and_steer_control/pycubicspline
new file mode 160000
index 00000000..34b741f7
--- /dev/null
+++ b/PathTracking/model_predictive_speed_and_steer_control/pycubicspline
@@ -0,0 +1 @@
+Subproject commit 34b741f7eee6cfbe851317ca36dab59fc560ba9a