From 5e3d7966518b4a9041a6ae55e58e192c5a157196 Mon Sep 17 00:00:00 2001
From: pengchen <penchen@ucdavis.edu>
Date: Fri, 2 Nov 2018 16:28:24 -0700
Subject: [PATCH] small bugs on linear discrete model of MPC

---
 .../model_predictive_speed_and_steer_control.py        |  2 +-
 .../notebook.ipynb                                     | 10 +++++-----
 2 files changed, 6 insertions(+), 6 deletions(-)

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 4125aca9..c4e31e76 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
@@ -97,7 +97,7 @@ def get_linear_model_matrix(v, phi, delta):
     C = np.zeros(NX)
     C[0] = DT * v * math.sin(phi) * phi
     C[1] = - DT * v * math.cos(phi) * phi
-    C[3] = v * delta / (WB * math.cos(delta) ** 2)
+    C[3] = - v * delta / (WB * math.cos(delta) ** 2)
 
     return A, B, C
 
diff --git a/PathTracking/model_predictive_speed_and_steer_control/notebook.ipynb b/PathTracking/model_predictive_speed_and_steer_control/notebook.ipynb
index bf91aad7..81214aa2 100644
--- a/PathTracking/model_predictive_speed_and_steer_control/notebook.ipynb
+++ b/PathTracking/model_predictive_speed_and_steer_control/notebook.ipynb
@@ -175,9 +175,9 @@
     "$$z_{k+1}=z_k+f(z_k,u_k)dt$$\n",
     "\n",
     "Using first degree Tayer expantion around zbar and ubar\n",
-    "$$z_{k+1}=z_k+(f(\\bar{z},\\bar{u})+A'z_k+B'u_k)dt$$\n",
+    "$$z_{k+1}=z_k+(f(\\bar{z},\\bar{u})+A'z_k+B'u_k-A'\\bar{z}-B'\\bar{u})dt$$\n",
     "\n",
-    "$$z_{k+1}=(I + dtA')z_k+(dtB')u_k + (f(\\bar{z},\\bar{u})-A'\\bar{z}+B'\\bar{u})dt$$\n"
+    "$$z_{k+1}=(I + dtA')z_k+(dtB')u_k + (f(\\bar{z},\\bar{u})-A'\\bar{z}-B'\\bar{u})dt$$\n"
    ]
   },
   {
@@ -251,7 +251,7 @@
     "\\bar{v}sin(\\bar{\\phi})\\bar{\\phi}dt\\\\\n",
     "-\\bar{v}cos(\\bar{\\phi})\\bar{\\phi}dt\\\\\n",
     "0\\\\\n",
-    "\\frac{\\bar{v}\\bar{\\delta}}{Lcos^2(\\bar{\\delta})}dt\\\\\n",
+    "-\\frac{\\bar{v}\\bar{\\delta}}{Lcos^2(\\bar{\\delta})}dt\\\\\n",
     "\\end{bmatrix}\n",
     "\\end{equation*}"
    ]
@@ -279,7 +279,7 @@
  ],
  "metadata": {
   "kernelspec": {
-   "display_name": "Python 3",
+   "display_name": "Python [default]",
    "language": "python",
    "name": "python3"
   },
@@ -293,7 +293,7 @@
    "name": "python",
    "nbconvert_exporter": "python",
    "pygments_lexer": "ipython3",
-   "version": "3.6.6"
+   "version": "3.6.4"
   }
  },
  "nbformat": 4,