mpc_python_learn/notebooks/1.0-lti-system-modelling.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 1 System Modelling\n",
    "\n",
    "This notebook contains the theory on using the vehicle Kinematics Equations to derive the linearized state space model"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-10-22T08:05:56.118290Z",
     "start_time": "2024-10-22T08:05:46.550696Z"
    }
   },
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "Matplotlib is building the font cache; this may take a moment.\n"
     ]
    }
   ],
   "source": [
    "import numpy as np\n",
    "from scipy.integrate import odeint\n",
    "from scipy.interpolate import interp1d\n",
    "import cvxpy as cp\n",
    "\n",
    "import matplotlib.pyplot as plt\n",
    "\n",
    "plt.style.use(\"ggplot\")\n",
    "\n",
    "import time"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## kinematics model equations\n",
    "\n",
    "The variables of the model are:\n",
    "\n",
    "* $x$ coordinate of the robot\n",
    "* $y$ coordinate of the robot\n",
    "* $v$ velocity of the robot\n",
    "* $\\theta$ heading of the robot\n",
    "\n",
    "The inputs of the model are:\n",
    "\n",
    "* $a$ acceleration of the robot\n",
    "* $\\delta$ steering of the robot\n",
    "\n",
    "These are the differential equations f(x,u) of the model:\n",
    "\n",
    "$\\dot{x} = f(x,u)$\n",
    "\n",
    "* $\\dot{x} = v\\cos{\\theta}$ \n",
    "* $\\dot{y} = v\\sin{\\theta}$\n",
    "* $\\dot{v} = a$\n",
    "* $\\dot{\\theta} = \\frac{v\\tan{\\delta}}{L}$\n",
    "\n",
    "Discretize with forward Euler Integration for time step dt:\n",
    "\n",
    "${x_{t+1}} = x_{t} + f(x,u)dt$\n",
    "\n",
    "* ${x_{t+1}} = x_{t} + v_t\\cos{\\theta}dt$\n",
    "* ${y_{t+1}} = y_{t} + v_t\\sin{\\theta}dt$\n",
    "* ${v_{t+1}} = v_{t} + a_tdt$\n",
    "* ${\\theta_{t+1}} = \\theta_{t} + \\frac{v\\tan{\\delta}}{L} dt$\n",
    "\n",
    "----------------------\n",
    "\n",
    "The Model is **non-linear** and **time variant**, but the Numerical Optimizer requires a Linear sets of equations. To approximate the equivalent **LTI** State space model, the **Taylor's series expansion** is used around $\\bar{x}$ and $\\bar{u}$ (at each time step):\n",
    "\n",
    "$ f(x,u) \\approx f(\\bar{x},\\bar{u}) +  \\frac{\\partial f(x,u)}{\\partial x}|_{x=\\bar{x},u=\\bar{u}}(x-\\bar{x}) + \\frac{\\partial f(x,u)}{\\partial u}|_{x=\\bar{x},u=\\bar{u}}(u-\\bar{u})$\n",
    "\n",
    "This can be rewritten usibg the State Space model form Ax+Bu :\n",
    "\n",
    "$ f(\\bar{x},\\bar{u}) +  A|_{x=\\bar{x},u=\\bar{u}}(x-\\bar{x}) + B|_{x=\\bar{x},u=\\bar{u}}(u-\\bar{u})$\n",
    "\n",
    "Where:\n",
    "\n",
    "$\n",
    "A =\n",
    "\\quad\n",
    "\\begin{bmatrix}\n",
    "\\frac{\\partial f(x,u)}{\\partial x} & \\frac{\\partial f(x,u)}{\\partial y} & \\frac{\\partial f(x,u)}{\\partial v} & \\frac{\\partial f(x,u)}{\\partial \\theta} \\\\\n",
    "\\end{bmatrix}\n",
    "\\quad\n",
    "=\n",
    "\\displaystyle \\left[\\begin{matrix}0 & 0 & \\cos{\\left(\\theta \\right)} & - v \\sin{\\left(\\theta \\right)}\\\\0 & 0 & \\sin{\\left(\\theta \\right)} & v \\cos{\\left(\\theta \\right)}\\\\0 & 0 & 0 & 0\\\\0 & 0 & \\frac{\\tan{\\left(\\delta \\right)}}{L} & 0\\end{matrix}\\right]\n",
    "$\n",
    "\n",
    "and\n",
    "\n",
    "$\n",
    "B = \n",
    "\\quad\n",
    "\\begin{bmatrix}\n",
    "\\frac{\\partial f(x,u)}{\\partial a} & \\frac{\\partial f(x,u)}{\\partial \\delta} \\\\\n",
    "\\end{bmatrix}\n",
    "\\quad\n",
    "= \n",
    "\\displaystyle \\left[\\begin{matrix}0 & 0\\\\0 & 0\\\\1 & 0\\\\0 & \\frac{v \\left(\\tan^{2}{\\left(\\delta \\right)} + 1\\right)}{L}\\end{matrix}\\right]\n",
    "$\n",
    "\n",
    "are the *Jacobians*.\n",
    "\n",
    "\n",
    "\n",
    "So the discretized model is given by:\n",
    "\n",
    "$ x_{t+1} = x_t + (f(\\bar{x},\\bar{u}) +  A|_{x=\\bar{x}}(x_t-\\bar{x}) + B|_{u=\\bar{u}}(u_t-\\bar{u}) )dt $\n",
    "\n",
    "$ x_{t+1} = (I+dtA)x_t + dtBu_t +dt(f(\\bar{x},\\bar{u}) - A\\bar{x} - B\\bar{u}))$\n",
    "\n",
    "The LTI-equivalent kinematics model is:\n",
    "\n",
    "$ x_{t+1} = A'x_t + B' u_t + C' $\n",
    "\n",
    "with:\n",
    "\n",
    "$ A' = I+dtA|_{x=\\bar{x},u=\\bar{u}} $\n",
    "\n",
    "$ B' = dtB|_{x=\\bar{x},u=\\bar{u}} $\n",
    "\n",
    "$ C' = dt(f(\\bar{x},\\bar{u}) - A|_{x=\\bar{x},u=\\bar{u}}\\bar{x} - B|_{x=\\bar{x},u=\\bar{u}}\\bar{u}) $"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "-----------------\n",
    "[About Taylor Series Expansion](https://courses.engr.illinois.edu/ece486/fa2017/documents/lecture_notes/state_space_p2.pdf):\n",
    "\n",
    "In order to linearize general nonlinear systems, we will use the Taylor Series expansion of functions.\n",
    "\n",
    "Typically it is possible to assume that the system is operating about some nominal\n",
    "state solution $\\bar{x}$ (possibly requires a nominal input $\\bar{u}$) called **equilibrium point**.\n",
    "\n",
    "Recall that the Taylor Series expansion of f(x) around the\n",
    "point $\\bar{x}$ is given by:\n",
    "\n",
    "$f(x)=f(\\bar{x}) + \\frac{df(x)}{dx}|_{x=\\bar{x}}(x-\\bar{x})$ + higher order terms...\n",
    "\n",
    "For x sufficiently close to $\\bar{x}$, these higher order terms will be very close to zero, and so we can drop them.\n",
    "\n",
    "The extension to functions of multiple states and inputs is very similar to the above procedure.Suppose the evolution of state x\n",
    "is given by:\n",
    "\n",
    "$\\dot{x} = f(x1, x2, . . . , xn, u1, u2, . . . , um) = Ax+Bu$\n",
    "\n",
    "Where:\n",
    "\n",
    "$ A =\n",
    "\\quad\n",
    "\\begin{bmatrix}\n",
    "\\frac{\\partial f(x,u)}{\\partial x1} & ... & \\frac{\\partial f(x,u)}{\\partial xn} \\\\\n",
    "\\end{bmatrix}\n",
    "\\quad\n",
    "$ and $ B = \\quad\n",
    "\\begin{bmatrix}\n",
    "\\frac{\\partial f(x,u)}{\\partial u1} & ... & \\frac{\\partial f(x,u)}{\\partial um} \\\\\n",
    "\\end{bmatrix}\n",
    "\\quad $\n",
    "\n",
    "Then:\n",
    "\n",
    "$f(x,u)=f(\\bar{x},\\bar{u}) + \\frac{df(x,u)}{dx}|_{x=\\bar{x}}(x-\\bar{x}) + \\frac{df(x,u)}{du}|_{u=\\bar{u}}(u-\\bar{u}) = f(\\bar{x},\\bar{u}) + A_{x=\\bar{x}}(x-\\bar{x}) + B_{u=\\bar{u}}(u-\\bar{u})$\n",
    "\n",
    "-----------------"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## ODE Model\n",
    "Motion Prediction: using scipy intergration"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-10-22T09:21:29.100064Z",
     "start_time": "2024-10-22T09:21:29.094806Z"
    }
   },
   "outputs": [],
   "source": [
    "# Define process model\n",
    "# This uses the continuous model\n",
    "def kinematics_model(x, t, u):\n",
    "    \"\"\"\n",
    "    Returns the set of ODE of the vehicle model.\n",
    "    \"\"\"\n",
    "\n",
    "    L = 0.3  # vehicle wheelbase\n",
    "    dxdt = x[2] * np.cos(x[3])\n",
    "    dydt = x[2] * np.sin(x[3])\n",
    "    dvdt = u[0]\n",
    "    dthetadt = x[2] * np.tan(u[1]) / L\n",
    "\n",
    "    dqdt = [dxdt, dydt, dvdt, dthetadt]\n",
    "\n",
    "    return dqdt\n",
    "\n",
    "\n",
    "def predict(x0, u):\n",
    "    \"\"\" \"\"\"\n",
    "\n",
    "    x_ = np.zeros((N, T + 1))\n",
    "\n",
    "    x_[:, 0] = x0\n",
    "\n",
    "    # solve ODE\n",
    "    for t in range(1, T + 1):\n",
    "\n",
    "        tspan = [0, DT]\n",
    "        x_next = odeint(kinematics_model, x0, tspan, args=(u[:, t - 1],))\n",
    "\n",
    "        x0 = x_next[1]\n",
    "        x_[:, t] = x_next[1]\n",
    "\n",
    "    return x_"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-10-22T09:25:39.603454Z",
     "start_time": "2024-10-22T09:25:39.598889Z"
    }
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "CPU times: user 2.66 ms, sys: 55 μs, total: 2.71 ms\n",
      "Wall time: 2.77 ms\n"
     ]
    }
   ],
   "source": [
    "%%time\n",
    "\n",
    "u_bar = np.zeros((M, T))\n",
    "u_bar[0, :] = 0.2  # m/ss\n",
    "u_bar[1, :] = np.radians(-np.pi / 4)  # rad\n",
    "\n",
    "x0 = np.zeros(N)\n",
    "x0[0] = 0\n",
    "x0[1] = 1\n",
    "x0[2] = 0\n",
    "x0[3] = np.radians(0)\n",
    "\n",
    "x_bar = predict(x0, u_bar)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-10-22T09:25:42.569050Z",
     "start_time": "2024-10-22T09:25:42.507079Z"
    }
   },
   "outputs": [
    {
     "data": {
      "text/plain": "<Figure size 640x480 with 2 Axes>",
      "image/png": "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
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# plot trajectory\n",
    "plt.subplot(2, 2, 1)\n",
    "plt.plot(x_bar[0, :], x_bar[1, :])\n",
    "plt.plot(np.linspace(0, 10, T + 1), np.zeros(T + 1), \"b-\")\n",
    "plt.axis(\"equal\")\n",
    "plt.ylabel(\"y\")\n",
    "plt.xlabel(\"x\")\n",
    "\n",
    "plt.subplot(2, 2, 2)\n",
    "plt.plot(np.degrees(x_bar[2, :]))\n",
    "plt.ylabel(\"theta(t) [deg]\")\n",
    "# plt.xlabel('time')\n",
    "\n",
    "\n",
    "plt.tight_layout()\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## State Space Linearized Model"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-10-22T09:30:50.489618Z",
     "start_time": "2024-10-22T09:30:50.483390Z"
    }
   },
   "outputs": [],
   "source": [
    "\"\"\"\n",
    "Control problem statement.\n",
    "\"\"\"\n",
    "\n",
    "N = 4  # number of state variables\n",
    "M = 2  # number of control variables\n",
    "T = 20  # Prediction Horizon\n",
    "DT = 0.2  # discretization step"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-10-22T09:30:50.962772Z",
     "start_time": "2024-10-22T09:30:50.957952Z"
    }
   },
   "outputs": [],
   "source": [
    "def get_linear_model(x_bar, u_bar):\n",
    "    \"\"\"\n",
    "    Computes the LTI approximated state space model x' = Ax + Bu + C\n",
    "    \"\"\"\n",
    "\n",
    "    L = 0.3  # vehicle wheelbase\n",
    "\n",
    "    x = x_bar[0]\n",
    "    y = x_bar[1]\n",
    "    v = x_bar[2]\n",
    "    theta = x_bar[3]\n",
    "\n",
    "    a = u_bar[0]\n",
    "    delta = u_bar[1]\n",
    "\n",
    "    A = np.zeros((N, N))\n",
    "    A[0, 2] = np.cos(theta)\n",
    "    A[0, 3] = -v * np.sin(theta)\n",
    "    A[1, 2] = np.sin(theta)\n",
    "    A[1, 3] = v * np.cos(theta)\n",
    "    A[3, 2] = v * np.tan(delta) / L\n",
    "    A_lin = np.eye(N) + DT * A\n",
    "\n",
    "    B = np.zeros((N, M))\n",
    "    B[2, 0] = 1\n",
    "    B[3, 1] = v / (L * np.cos(delta) ** 2)\n",
    "    B_lin = DT * B\n",
    "\n",
    "    f_xu = np.array(\n",
    "        [v * np.cos(theta), v * np.sin(theta), a, v * np.tan(delta) / L]\n",
    "    ).reshape(N, 1)\n",
    "    C_lin = DT * (\n",
    "        f_xu - np.dot(A, x_bar.reshape(N, 1)) - np.dot(B, u_bar.reshape(M, 1))\n",
    "    )\n",
    "\n",
    "    return np.round(A_lin, 4), np.round(B_lin, 4), np.round(C_lin, 4)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-10-22T09:30:53.505536Z",
     "start_time": "2024-10-22T09:30:53.502223Z"
    }
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "CPU times: user 3.48 ms, sys: 2.16 ms, total: 5.64 ms\n",
      "Wall time: 4.17 ms\n"
     ]
    },
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_33582/46870782.py:17: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
      "  A[0, 2] = np.cos(theta)\n",
      "/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_33582/46870782.py:18: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
      "  A[0, 3] = -v * np.sin(theta)\n",
      "/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_33582/46870782.py:19: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
      "  A[1, 2] = np.sin(theta)\n",
      "/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_33582/46870782.py:20: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
      "  A[1, 3] = v * np.cos(theta)\n",
      "/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_33582/46870782.py:21: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
      "  A[3, 2] = v * np.tan(delta) / L\n",
      "/var/folders/hd/8kg_jtmd6svgg_sc384pbcdm0000gn/T/ipykernel_33582/46870782.py:26: DeprecationWarning: Conversion of an array with ndim > 0 to a scalar is deprecated, and will error in future. Ensure you extract a single element from your array before performing this operation. (Deprecated NumPy 1.25.)\n",
      "  B[3, 1] = v / (L * np.cos(delta) ** 2)\n"
     ]
    }
   ],
   "source": [
    "%%time\n",
    "\n",
    "u_bar = np.zeros((M, T))\n",
    "u_bar[0, :] = 0.2  # m/s\n",
    "u_bar[1, :] = np.radians(-np.pi / 4)  # rad\n",
    "\n",
    "x0 = np.zeros(N)\n",
    "x0[0] = 0\n",
    "x0[1] = 1\n",
    "x0[2] = 0\n",
    "x0[3] = np.radians(0)\n",
    "\n",
    "x_bar = np.zeros((N, T + 1))\n",
    "x_bar[:, 0] = x0\n",
    "\n",
    "for t in range(1, T + 1):\n",
    "    xt = x_bar[:, t - 1].reshape(N, 1)\n",
    "    ut = u_bar[:, t - 1].reshape(M, 1)\n",
    "\n",
    "    A, B, C = get_linear_model(xt, ut)\n",
    "\n",
    "    xt_plus_one = np.dot(A, xt) + np.dot(B, ut) + C\n",
    "\n",
    "    x_bar[:, t] = np.squeeze(xt_plus_one)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {
    "ExecuteTime": {
     "end_time": "2024-10-22T09:30:54.635354Z",
     "start_time": "2024-10-22T09:30:54.569920Z"
    }
   },
   "outputs": [
    {
     "data": {
      "text/plain": "<Figure size 640x480 with 2 Axes>",
      "image/png": "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
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "# plot trajectory\n",
    "plt.subplot(2, 2, 1)\n",
    "plt.plot(x_bar[0, :], x_bar[1, :])\n",
    "plt.plot(np.linspace(0, 10, T + 1), np.zeros(T + 1), \"b-\")\n",
    "plt.axis(\"equal\")\n",
    "plt.ylabel(\"y\")\n",
    "plt.xlabel(\"x\")\n",
    "\n",
    "plt.subplot(2, 2, 2)\n",
    "plt.plot(np.degrees(x_bar[2, :]))\n",
    "plt.ylabel(\"theta(t)\")\n",
    "# plt.xlabel('time')\n",
    "\n",
    "\n",
    "plt.tight_layout()\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The results are the same as expected, so the linearized model is equivalent as expected."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "name": "python3",
   "language": "python",
   "display_name": "Python 3 (ipykernel)"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.5"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}