{
 "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": 12,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "\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",
    "def J(f,x,u,epsilon=1e-6):\n",
    "    \"\"\"\n",
    "    :param f:\n",
    "    :param x:\n",
    "    :param u:\n",
    "    \"\"\"\n",
    "    \n",
    "    \n",
    "    x_minus = x-epsilon\n",
    "    u_minus = u-epsilon\n",
    "    \n",
    "    x_plus = x+epsilon\n",
    "    u_plus = u+epsilon\n",
    "    \n",
    "    # compute finite differences\n",
    "    f_plus = f(x_plus, u_plus)\n",
    "    f_minus = f(x_minus, u_minus)\n",
    "    \n",
    "    jac = (f_plus - f_minus)/2*epsilon\n",
    "    \n",
    "    return jac\n",
    "    "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([[1.e-12],\n",
       "       [0.e+00],\n",
       "       [1.e-12],\n",
       "       [0.e+00]])"
      ]
     },
     "execution_count": 13,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "#starting condition\n",
    "x=np.array([0,0,0,0])\n",
    "u=np.array([1,0])\n",
    "\n",
    "J(f,x,u)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "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.7.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}