{
 "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": 19,
   "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",
    "        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",
    "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]= (f(x+perturb_x[i,:], u)-f(x-perturb_x[i,:], u))/2*epsilon\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]= (f(x, u+perturb_u[i,:])-f(x, u-perturb_u[i,:]))/2*epsilon\n",
    "\n",
    "    return jac_x, jac_u\n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "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": 20,
     "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.5"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}