diff --git a/.ipynb_checkpoints/MPC_cvxpy-checkpoint.ipynb b/.ipynb_checkpoints/MPC_cvxpy-checkpoint.ipynb index ad687ea..8088e2a 100644 --- a/.ipynb_checkpoints/MPC_cvxpy-checkpoint.ipynb +++ b/.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", - "in the reference frame of the veicle the equtions would be:\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", "* 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