{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Compute the jacobian numerically\n",
    "\n",
    "link: --> http://www.maths.lth.se/na/courses/FMN081/FMN081-06/lecture7.pdf\n",
    "\n",
    "Often the Jacobian is not **analytically** available and it has to be computed numerically.\n",
    "It can be computed column wise by finite differences:\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "\n",
    "# #CONTINUOUS\n",
    "# def f(x,u):\n",
    "#     \"\"\"\n",
    "#     :param x:\n",
    "#     :param u:\n",
    "#     \"\"\"\n",
    "#     xx = x[0]\n",
    "#     xy = x[1]\n",
    "#     v =  x[2]\n",
    "#     theta =x[3]\n",
    "\n",
    "#     a = u[0]\n",
    "#     delta = u[1]\n",
    "\n",
    "#     L=0.3\n",
    "\n",
    "#     #vector of ackerman equations\n",
    "#     return np.array([\n",
    "#         np.cos(theta)*v,\n",
    "#         np.sin(theta)*v,\n",
    "#         a,\n",
    "#         v*np.arctan(delta)/L\n",
    "#         ])\n",
    "\n",
    "# DISCRETE\n",
    "def f(x, u, dt=0.1):\n",
    "    \"\"\"\n",
    "    :param x:\n",
    "    :param u:\n",
    "    \"\"\"\n",
    "    xx = x[0]\n",
    "    xy = x[1]\n",
    "    v = x[2]\n",
    "    theta = x[3]\n",
    "\n",
    "    a = u[0]\n",
    "    delta = u[1]\n",
    "\n",
    "    L = 0.3\n",
    "\n",
    "    # vector of ackerman equations\n",
    "    return np.array(\n",
    "        [\n",
    "            xx + np.cos(theta) * v * dt,\n",
    "            xy + np.sin(theta) * v * dt,\n",
    "            v + a * dt,\n",
    "            theta + v * np.arctan(delta) / L * dt,\n",
    "        ]\n",
    "    )\n",
    "\n",
    "\n",
    "def Jacobians(f, x, u, epsilon=1e-4):\n",
    "    \"\"\"\n",
    "    :param f:\n",
    "    :param x:\n",
    "    :param u:\n",
    "    \"\"\"\n",
    "\n",
    "    jac_x = np.zeros((4, 4))\n",
    "    jac_u = np.zeros((4, 2))\n",
    "\n",
    "    perturb_x = np.eye(4) * epsilon\n",
    "    perturb_u = np.eye(2) * epsilon\n",
    "\n",
    "    # each row is state vector where one variable has been perturbed\n",
    "    x_perturbed_plus = np.tile(x, (4, 1)) + perturb_x\n",
    "    x_perturbed_minus = np.tile(x, (4, 1)) - perturb_x\n",
    "\n",
    "    # each row is state vector where one variable has been perturbed\n",
    "    u_perturbed_plus = np.tile(u, (2, 1)) + perturb_u\n",
    "    u_perturbed_minus = np.tile(u, (2, 1)) - perturb_u\n",
    "\n",
    "    for i in range(x.shape[0]):\n",
    "\n",
    "        # each coloumn of the jac is given by perturbing a variable\n",
    "        jac_x[:, i] = (\n",
    "            (f(x + perturb_x[i, :], u) - f(x - perturb_x[i, :], u)) / 2 * epsilon\n",
    "        )\n",
    "\n",
    "    for i in range(u.shape[0]):\n",
    "\n",
    "        # each coloumn of the jac is given by perturbing a variable\n",
    "        jac_u[:, i] = (\n",
    "            (f(x, u + perturb_u[i, :]) - f(x, u - perturb_u[i, :])) / 2 * epsilon\n",
    "        )\n",
    "\n",
    "    return jac_x, jac_u"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(array([[1.00000000e-08, 0.00000000e+00, 1.00000000e-09, 0.00000000e+00],\n",
       "        [0.00000000e+00, 1.00000000e-08, 0.00000000e+00, 9.99999998e-10],\n",
       "        [0.00000000e+00, 0.00000000e+00, 1.00000000e-08, 0.00000000e+00],\n",
       "        [0.00000000e+00, 0.00000000e+00, 6.57985199e-10, 1.00000000e-08]]),\n",
       " array([[0.0000000e+00, 0.0000000e+00],\n",
       "        [0.0000000e+00, 0.0000000e+00],\n",
       "        [1.0000000e-09, 0.0000000e+00],\n",
       "        [0.0000000e+00, 3.2051282e-09]]))"
      ]
     },
     "execution_count": 2,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# starting condition\n",
    "x = np.array([0, 0, 1, 0])\n",
    "u = np.array([1, 0.2])\n",
    "\n",
    "Jacobians(f, x, u)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "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.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}