Added a bit of theory

.ipynb_checkpoints/MPC_cvxpy-checkpoint.ipynb

@@ -138,11 +138,11 @@
     "\n",
     "with:\n",
     "\n",
-    "A' = I+dtA\n",
+    "$ A' = I+dtA $\n",
     "\n",
-    "B' = dtB\n",
+    "$ B' = dtB $\n",
     "\n",
-    "C' = dt(f(\\bar{x},\\bar{u}) - A\\bar{x} - B\\bar{u})"
+    "$ C' = dt(f(\\bar{x},\\bar{u}) - A\\bar{x} - B\\bar{u}) $"
    ]
   },
   {
@@ -150,8 +150,8 @@
    "metadata": {},
    "source": [
     "------------------\n",
-    "NB: psi and cte are expressed w.r.t. the reference frame.\n",
+    "NB: psi and cte are expressed w.r.t. the track as reference frame.\n",
-    "in the reference frame of the veicle the equtions would be:\n",
+    "In the reference frame of the veicle the equtions would be:\n",
     "* psi_dot = w\n",
     "* cte_dot = sin(psi)\n",
     "-----------------"
@@ -745,12 +745,67 @@
    "cell_type": "markdown",
    "metadata": {},
    "source": [
-    "### MPC Problem formulation\n"
+    "### MPC Problem formulation\n",
+    "\n",
+    "**Model Predictive Control** refers to the control approach of **numerically** solving a optimization problem at each time step. \n",
+    "\n",
+    "The controller generates a control signal over a fixed lenght T (Horizon) at each time step."
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "![mpc](img/mpc_block_diagram.png)\n",
+    "![mpc](img/mpc_t.png)"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "#### Linear MPC Formulation\n",
+    "\n",
+    "Linear MPC makes use of the **LTI** (Linear time invariant) discrete state space model, wich represents a motion model used for Prediction.\n",
+    "\n",
+    "$x_{t+1} = Ax_t + Bu_t$\n",
+    "\n",
+    "The LTI formulation means that **future states** are linearly related to the current state and actuator signal. Hence, the MPC seeks to find a **control policy** U over a finite lenght horizon.\n",
+    "\n",
+    "$U={u_{t|t}, u_{t+1|t}, ...,u_{t+T|t}}$\n",
+    "\n",
+    "The objective function used minimize (drive the state to 0) is:\n",
+    "\n",
+    "$J(x(t),U) = \\sum^{t+T-1}_{j=t} x^T_{j|t}Qx_{j|t} + u^T_{j|t}Ru_{j|t}$\n",
+    "\n",
+    "This accounts for quadratic error on deviation from 0 of the state and the control inputs sequences. Q and R are the **weight matrices** and are used to tune the response.\n",
+    "\n",
+    "Becouse the goal is tracking a **reference signal** such as a trajectory, the objective function is rewritten as:\n",
+    "\n",
+    "$J(x(t),U) = \\sum^{t+T-1}_{j=t} \\delta x^T_{j|t}Q\\delta x_{j|t} + u^T_{j|t}Ru_{j|t}$\n",
+    "\n",
+    "where the error w.r.t desired state is accounted for:\n",
+    "\n",
+    "$\\delta x = x_{j,t,ref} - x_{j,t}$\n",
+    "\n",
+    "#### Non-Linear MPC Formulation\n",
+    "\n",
+    "In general cases, the objective function is any non-differentiable non-linear function of states and inputs over a finite horizon T. In this case the constrains include nonlinear dynamics of motion.\n",
+    "\n",
+    "$J(x(t),U) = \\sum^{t+T}_{j=t} C(x_{j|t},{j|t})$\n",
+    "\n",
+    "s.t.\n",
+    "\n",
+    "$ x_{j+1|t} = f(x_{j|t},{j|t}) t<j<t+T-1 $\n",
+    "\n",
+    "Other nonlinear constrains may be applied:\n",
+    "\n",
+    "$ g(x_{j|t},{j|t})<0           t<j<t+T-1 $"
    ]
   },
   {
    "cell_type": "code",
-   "execution_count": 14,
+   "execution_count": 18,
    "metadata": {},
    "outputs": [
     {
@@ -764,8 +819,8 @@
      "name": "stdout",
      "output_type": "stream",
      "text": [
-      "CPU times: user 335 ms, sys: 173 ms, total: 508 ms\n",
+      "CPU times: user 277 ms, sys: 0 ns, total: 277 ms\n",
-      "Wall time: 279 ms\n"
+      "Wall time: 276 ms\n"
      ]
     }
    ],
@@ -861,7 +916,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 15,
+   "execution_count": 19,
    "metadata": {},
    "outputs": [
     {

MPC_cvxpy.ipynb

@@ -138,11 +138,11 @@
     "\n",
     "with:\n",
     "\n",
-    "A' = I+dtA\n",
+    "$ A' = I+dtA $\n",
     "\n",
-    "B' = dtB\n",
+    "$ B' = dtB $\n",
     "\n",
-    "C' = dt(f(\\bar{x},\\bar{u}) - A\\bar{x} - B\\bar{u})"
+    "$ C' = dt(f(\\bar{x},\\bar{u}) - A\\bar{x} - B\\bar{u}) $"
    ]
   },
   {
@@ -150,8 +150,8 @@
    "metadata": {},
    "source": [
     "------------------\n",
-    "NB: psi and cte are expressed w.r.t. the reference frame.\n",
+    "NB: psi and cte are expressed w.r.t. the track as reference frame.\n",
-    "in the reference frame of the veicle the equtions would be:\n",
+    "In the reference frame of the veicle the equtions would be:\n",
     "* psi_dot = w\n",
     "* cte_dot = sin(psi)\n",
     "-----------------"
@@ -745,12 +745,67 @@
    "cell_type": "markdown",
    "metadata": {},
    "source": [
-    "### MPC Problem formulation\n"
+    "### MPC Problem formulation\n",
+    "\n",
+    "**Model Predictive Control** refers to the control approach of **numerically** solving a optimization problem at each time step. \n",
+    "\n",
+    "The controller generates a control signal over a fixed lenght T (Horizon) at each time step."
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "![mpc](img/mpc_block_diagram.png)\n",
+    "![mpc](img/mpc_t.png)"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "#### Linear MPC Formulation\n",
+    "\n",
+    "Linear MPC makes use of the **LTI** (Linear time invariant) discrete state space model, wich represents a motion model used for Prediction.\n",
+    "\n",
+    "$x_{t+1} = Ax_t + Bu_t$\n",
+    "\n",
+    "The LTI formulation means that **future states** are linearly related to the current state and actuator signal. Hence, the MPC seeks to find a **control policy** U over a finite lenght horizon.\n",
+    "\n",
+    "$U={u_{t|t}, u_{t+1|t}, ...,u_{t+T|t}}$\n",
+    "\n",
+    "The objective function used minimize (drive the state to 0) is:\n",
+    "\n",
+    "$J(x(t),U) = \\sum^{t+T-1}_{j=t} x^T_{j|t}Qx_{j|t} + u^T_{j|t}Ru_{j|t}$\n",
+    "\n",
+    "This accounts for quadratic error on deviation from 0 of the state and the control inputs sequences. Q and R are the **weight matrices** and are used to tune the response.\n",
+    "\n",
+    "Becouse the goal is tracking a **reference signal** such as a trajectory, the objective function is rewritten as:\n",
+    "\n",
+    "$J(x(t),U) = \\sum^{t+T-1}_{j=t} \\delta x^T_{j|t}Q\\delta x_{j|t} + u^T_{j|t}Ru_{j|t}$\n",
+    "\n",
+    "where the error w.r.t desired state is accounted for:\n",
+    "\n",
+    "$\\delta x = x_{j,t,ref} - x_{j,t}$\n",
+    "\n",
+    "#### Non-Linear MPC Formulation\n",
+    "\n",
+    "In general cases, the objective function is any non-differentiable non-linear function of states and inputs over a finite horizon T. In this case the constrains include nonlinear dynamics of motion.\n",
+    "\n",
+    "$J(x(t),U) = \\sum^{t+T}_{j=t} C(x_{j|t},{j|t})$\n",
+    "\n",
+    "s.t.\n",
+    "\n",
+    "$ x_{j+1|t} = f(x_{j|t},{j|t}) t<j<t+T-1 $\n",
+    "\n",
+    "Other nonlinear constrains may be applied:\n",
+    "\n",
+    "$ g(x_{j|t},{j|t})<0           t<j<t+T-1 $"
    ]
   },
   {
    "cell_type": "code",
-   "execution_count": 14,
+   "execution_count": 18,
    "metadata": {},
    "outputs": [
     {
@@ -764,8 +819,8 @@
      "name": "stdout",
      "output_type": "stream",
      "text": [
-      "CPU times: user 335 ms, sys: 173 ms, total: 508 ms\n",
+      "CPU times: user 277 ms, sys: 0 ns, total: 277 ms\n",
-      "Wall time: 279 ms\n"
+      "Wall time: 276 ms\n"
      ]
     }
    ],
@@ -861,7 +916,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 15,
+   "execution_count": 19,
    "metadata": {},
    "outputs": [
     {