From 741f2c158d09afe1cc196cfd222fa2f410c0fe17 Mon Sep 17 00:00:00 2001
From: mcarfagno <m.carfagno@outlook.com>
Date: Sat, 12 Dec 2020 18:15:08 +0000
Subject: [PATCH] correted function for numerical jacobian

---
 notebooks/numericalJacobian.ipynb | 85 ++++++++++++++++++++++---------
 1 file changed, 60 insertions(+), 25 deletions(-)

diff --git a/notebooks/numericalJacobian.ipynb b/notebooks/numericalJacobian.ipynb
index ea68822..dd60b5d 100644
--- a/notebooks/numericalJacobian.ipynb
+++ b/notebooks/numericalJacobian.ipynb
@@ -15,13 +15,38 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 12,
+   "execution_count": 31,
    "metadata": {},
    "outputs": [],
    "source": [
     "import numpy as np\n",
     "\n",
-    "def f(x,u):\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",
@@ -38,59 +63,69 @@
     "    \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",
+    "        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 J(f,x,u,epsilon=1e-6):\n",
+    "def J(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",
     "    \n",
-    "    x_minus = x-epsilon\n",
-    "    u_minus = u-epsilon\n",
+    "    for i in range(x.shape[0]):\n",
+    "        perturb = np.zeros(x.shape[0])\n",
+    "        perturb[i] = epsilon\n",
+    "        \n",
+    "        #each loumn of the jac is given by perturbing a variable\n",
+    "        jac_x[:,i]= (f(x+perturb, u)-f(x-perturb, u))/2*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",
+    "    jac_u = np.zeros((4,2))\n",
     "    \n",
-    "    jac = (f_plus - f_minus)/2*epsilon\n",
-    "    \n",
-    "    return jac\n",
+    "    for i in range(u.shape[0]):\n",
+    "        perturb = np.zeros(u.shape[0])\n",
+    "        perturb[i] = epsilon\n",
+    "        \n",
+    "        #each loumn of the jac is given by perturbing a variable\n",
+    "        jac_u[:,i]= (f(x, u+perturb)-f(x, u-perturb))/2*epsilon\n",
+    "        \n",
+    "    return jac_x, jac_u\n",
     "    "
    ]
   },
   {
    "cell_type": "code",
-   "execution_count": 13,
+   "execution_count": 32,
    "metadata": {},
    "outputs": [
     {
      "data": {
       "text/plain": [
-       "array([[1.e-12],\n",
-       "       [0.e+00],\n",
-       "       [1.e-12],\n",
-       "       [0.e+00]])"
+       "(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": 13,
+     "execution_count": 32,
      "metadata": {},
      "output_type": "execute_result"
     }
    ],
    "source": [
     "#starting condition\n",
-    "x=np.array([0,0,0,0])\n",
-    "u=np.array([1,0])\n",
+    "x=np.array([0,0,1,0])\n",
+    "u=np.array([1,0.2])\n",
     "\n",
     "J(f,x,u)"
    ]