small bugs on linear discrete model of MPC

Author: caseypen

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
 

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,