martinelli_2010_simple_localization_mobileRobot2D.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import sympy as sp\n",
    "sp.init_printing()\n",
    "from pynoa import *"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Set `autosave` to **True** if you want to save the object files automatically.\n",
    "\n",
    "Set the directory where you want to save the file in `NOA_obj_save_dir`"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "autosave = False\n",
    "NOA_obj_save_dir = \"backup/example_martinelli_mobile_robot\""
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# A Simple Example of Localization\n",
    "From Equation (3) of Martinelli's paper.   \n",
    "Original process model $\\dot{\\mathbf{x}}=\\mathbf{f}(\\mathbf{x},\\mathbf{u})$:\n",
    "\n",
    "\\begin{equation}\n",
    "    \\begin{aligned}\n",
    "        \\dot{D} &= v \\cos(\\theta_R - \\phi_R) \\\\\n",
    "        \\dot{\\phi_R} &= \\frac{v}{D} \\sin(\\theta_R - \\phi_R) \\\\\n",
    "        \\dot{\\theta_R} &= \\omega\n",
    "    \\end{aligned}\n",
    "\\end{equation}\n",
    "and measurement model $\\mathbf{y}=\\mathbf{h}(\\mathbf{x},\\mathbf{u})$:\n",
    "\\begin{equation}\n",
    "    y = \\pi - \\theta_R + \\phi_R\n",
    "\\end{equation}\n",
    "where:\n",
    "* the state vector $\\mathbf{x} = \\begin{bmatrix} \n",
    "                                    D & \\phi_R & \\theta_R\n",
    "                                    \\end{bmatrix}^\\top$\n",
    "* the control inputs $\\mathbf{u} =  \\begin{bmatrix} \n",
    "                                    v & \\omega\n",
    "                                    \\end{bmatrix}^\\top$\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "mobile_robot_symbolic NOA object initialized\n"
     ]
    }
   ],
   "source": [
    "# State vector\n",
    "D, phi_R, theta_R = sp.symbols(\"D, phi_R, theta_R\")\n",
    "mobile_robot = NOA(\"mobile_robot_symbolic\")\n",
    "mobile_robot.x = sp.Matrix([D, phi_R, theta_R])"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Convert the process model into the control-affine form:\n",
    "\n",
    "\\begin{equation}\n",
    "\\mathbf{\\dot x}=\n",
    "\\mathbf{f}_0(\\mathbf{x})\n",
    "+\n",
    "\\sum_{i=1}^m\\mathbf{f}_i(\\mathbf{x})u_i.\n",
    "\\end{equation}"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "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",
      "text/latex": [
       "$\\displaystyle \\left[ \\left[\\begin{matrix}0\\\\0\\\\0\\end{matrix}\\right], \\  \\left[\\begin{matrix}\\cos{\\left(\\phi_{R} - \\theta_{R} \\right)}\\\\- \\frac{\\sin{\\left(\\phi_{R} - \\theta_{R} \\right)}}{D}\\\\0\\end{matrix}\\right], \\  \\left[\\begin{matrix}0\\\\0\\\\1\\end{matrix}\\right]\\right]$"
      ],
      "text/plain": [
       "⎡     ⎡ cos(φ_R - θ_R) ⎤     ⎤\n",
       "⎢⎡0⎤  ⎢                ⎥  ⎡0⎤⎥\n",
       "⎢⎢ ⎥  ⎢-sin(φ_R - θ_R) ⎥  ⎢ ⎥⎥\n",
       "⎢⎢0⎥, ⎢────────────────⎥, ⎢0⎥⎥\n",
       "⎢⎢ ⎥  ⎢       D        ⎥  ⎢ ⎥⎥\n",
       "⎢⎣0⎦  ⎢                ⎥  ⎣1⎦⎥\n",
       "⎣     ⎣       0        ⎦     ⎦"
      ]
     },
     "execution_count": 4,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# Control-affine vector fields of process model \\dot{x} = f(x,u)\n",
    "f0 = sp.zeros(3,1)\n",
    "f1 = sp.Matrix([[sp.cos(theta_R-phi_R), sp.sin(theta_R-phi_R)/D, 0]]).T\n",
    "f2 = sp.Matrix([[0,0,1]]).T\n",
    "mobile_robot.f = [f0, f1, f2]\n",
    "mobile_robot.f"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAAHoAAAAZCAYAAAD+OToQAAAACXBIWXMAAA7EAAAOxAGVKw4bAAAEOElEQVRoBe2a/1HbMBTHDdcBaLpB2ADoBA0bQDtBYYNy/Sv5Lxc2gE5QyAbQCTiyAWxQjg3a78f1c2VZdhJDEsXk3YknPUnW9/3Qk2yy1e/3r5Mk2VMxOhwMBo/W2PD1s4D81xPqCwf5eEuOflDHriPcVFtkAfm2K3VG2010yiY3mRrFHOHfUTlROYoC0BJANHK0cJ0uAdtClpBzSWscV1csoLab4hC1kpo6ei2NIadyF8HJx6o/q4xVZ2fvrKVCc4Bu6ujfc6wR01CcfIaTPVDs8oWT1j1S+bbwhQILzO1oAcUot4FnRS0S7hMB7IhfBoB2ArJWieZ2tLTn9WuyhlY4E+b0XDbs0sNeK59M1lb+bhbFMoMQ9bxfc2PtimOkierRv3MLI1kIzKRulyxl+6ncHRNNXXqQ9r+r1N0pLjWudFme6mhN4lZ6Iz5WYYF7cZz7KE6di03szjbFT4XV6lIhSR0tWfRHkTCOACz6pELQUrhMogN+oZ5oXNAXtY7WpBvNvRZPH6L6ZxXXKNQBcKySksayMGkSzjzSItkAMATMufiyybJPjhMAwvJHrHQMxaaD8ID/Ttyc+UXtC7XZbNj1lrp4JVU6WhO5vHTF3cvLrtdmEUDkpH4WJdoOxH3D8hXugwqBECT18cxfwc5q4VfNKznMGQ6eQoBpvOEeOuPSqvpeqgPBT6D7hG5cCHGUTzitYC8bIDm6ufrtSWaO/ag+10c2rcArHa1RLOru3sLErHEg7gKwMfuqhOYylq9RdY5+Vj/zX4VkEIwL3f1j+V9SuL1L50Kn8hIdgvoJC7qzeQpB56w5taq5BCg2MsLpbtvkBb5daBUbOPHeRHoYu+LBa7PoT5M5nGj2DUs38lV9ifKNQcYq7WZAZhSjDkAjQDlSjQgcC2aTlXjdjiY1uA9AcXeX4jBueHZ+pw/PFiUo8rGZjHQ2VL1xNKcLzPlH67FrC05Wm9srqTKIRXL0jkYHUznDRYC+N1nG2ZS5vb2+tFnnaNIPzjRj5OezFiSiMBTR5ROLQj31Exz8Z4xL3L7adq6ouVS60mpkHzt7wX1YgyBGHYCLk7G7G7jU0aWZo/UwjMKrE84mhXMWsBNwHJ8RQ2ezutJFeb+2AElUR05wrOrfoQTtSDgMP0HnGgt8LmG42HQAH7j887/WwUyC6nY0DsKZvHuSxmhPvd1pWLpzGO8QgUI0roSEG6eGsk8Vnuh0AKj0KGUhyYI3dV+xWkc7g0nBM0WOxjHWv3Bxg63bQeqOihalw6w2fHVjzOro/HyuQ6DoYidAvkIdyZ7oyMZwQVrVeQ2MSlqkDnr2yoJ9u1LjYkf+WlUU/29JCW7V9i35h9ppus9GDMW5RHBG9sRjdfLa65DZu8Q2vxkrmaRdgmzDNfvNWLtM8Ta0mTV1vw1rtFjLNHV7+m1+1+0ZZN2aStf+W8PkL3lFmu5sWhxDAAAAAElFTkSuQmCC",
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}\\phi_{R} - \\theta_{R} + \\pi\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "[φ_R - θ_R + π]"
      ]
     },
     "execution_count": 5,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# Measurement model y=h(x,u)\n",
    "mobile_robot.h = sp.Matrix([sp.pi - theta_R + phi_R])\n",
    "mobile_robot.h"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Permutation of Vector Fields\n",
      "[(0,), (1,), (2,)]\n",
      "current Lie derivative:  k1f0  not appended to observability matrix due to null vector\n",
      "current Lie derivative:  k1f1 appended to observability matrix\n",
      "current Lie derivative:  k1f2  not appended to observability matrix due to null vector\n",
      "[(0, 0), (0, 1), (0, 2), (1, 0), (1, 1), (1, 2), (2, 0), (2, 1), (2, 2)]\n",
      "current Lie derivative:  k2f0f0  not appended to observability matrix due to null vector\n",
      "current Lie derivative:  k2f0f1  not appended to observability matrix due to null vector\n",
      "current Lie derivative:  k2f0f2  not appended to observability matrix due to null vector\n",
      "current Lie derivative:  k2f1f0  not appended to observability matrix due to null vector\n",
      "current Lie derivative:  k2f1f1 appended to observability matrix\n",
      "current Lie derivative:  k2f1f2 appended to observability matrix\n",
      "current Lie derivative:  k2f2f0  not appended to observability matrix due to null vector\n",
      "current Lie derivative:  k2f2f1  not appended to observability matrix due to null vector\n",
      "current Lie derivative:  k2f2f2  not appended to observability matrix due to null vector\n",
      "\n",
      "Calculating rank of symbolic observability matrix ...\n",
      "The system is NOT weakly locally observable (WLO)!\n",
      "Dimension of largest WLO subsystem (Observable & Jointly Observable):  2\n",
      "Dimension of Undistinguishable Region (Unobservable):  1\n"
     ]
    },
    {
     "data": {
      "image/png": "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",
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}0 & 1 & -1\\\\\\frac{\\sin{\\left(\\phi_{R} - \\theta_{R} \\right)}}{D^{2}} & - \\frac{\\cos{\\left(\\phi_{R} - \\theta_{R} \\right)}}{D} & \\frac{\\cos{\\left(\\phi_{R} - \\theta_{R} \\right)}}{D}\\\\- \\frac{4 \\sin{\\left(\\phi_{R} - \\theta_{R} \\right)} \\cos{\\left(\\phi_{R} - \\theta_{R} \\right)}}{D^{3}} & - \\frac{2 \\sin^{2}{\\left(\\phi_{R} - \\theta_{R} \\right)}}{D^{2}} + \\frac{2 \\cos^{2}{\\left(\\phi_{R} - \\theta_{R} \\right)}}{D^{2}} & \\frac{2 \\sin^{2}{\\left(\\phi_{R} - \\theta_{R} \\right)}}{D^{2}} - \\frac{2 \\cos^{2}{\\left(\\phi_{R} - \\theta_{R} \\right)}}{D^{2}}\\\\- \\frac{\\cos{\\left(\\phi_{R} - \\theta_{R} \\right)}}{D^{2}} & - \\frac{\\sin{\\left(\\phi_{R} - \\theta_{R} \\right)}}{D} & \\frac{\\sin{\\left(\\phi_{R} - \\theta_{R} \\right)}}{D}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "⎡                0                                     1                      \n",
       "⎢                                                                             \n",
       "⎢         sin(φ_R - θ_R)                       -cos(φ_R - θ_R)                \n",
       "⎢         ──────────────                       ────────────────               \n",
       "⎢                2                                    D                       \n",
       "⎢               D                                                             \n",
       "⎢                                                                             \n",
       "⎢                                          2                   2              \n",
       "⎢-4⋅sin(φ_R - θ_R)⋅cos(φ_R - θ_R)     2⋅sin (φ_R - θ_R)   2⋅cos (φ_R - θ_R)  2\n",
       "⎢─────────────────────────────────  - ───────────────── + ─────────────────  ─\n",
       "⎢                 3                            2                   2          \n",
       "⎢                D                            D                   D           \n",
       "⎢                                                                             \n",
       "⎢        -cos(φ_R - θ_R)                       -sin(φ_R - θ_R)                \n",
       "⎢        ────────────────                      ────────────────               \n",
       "⎢                2                                    D                       \n",
       "⎣               D                                                             \n",
       "\n",
       "                -1                  ⎤\n",
       "                                    ⎥\n",
       "          cos(φ_R - θ_R)            ⎥\n",
       "          ──────────────            ⎥\n",
       "                D                   ⎥\n",
       "                                    ⎥\n",
       "                                    ⎥\n",
       "    2                   2           ⎥\n",
       "⋅sin (φ_R - θ_R)   2⋅cos (φ_R - θ_R)⎥\n",
       "──────────────── - ─────────────────⎥\n",
       "        2                   2       ⎥\n",
       "       D                   D        ⎥\n",
       "                                    ⎥\n",
       "          sin(φ_R - θ_R)            ⎥\n",
       "          ──────────────            ⎥\n",
       "                D                   ⎥\n",
       "                                    ⎦"
      ]
     },
     "execution_count": 6,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "# Observability rank criterion using NOA object\n",
    "mobile_robot.ORC()\n",
    "mobile_robot.obsv_mat"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "The observable modes are g(x) which satisfiy these partial differential equations (PDE): \n"
     ]
    },
    {
     "data": {
      "image/png": "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",
      "text/latex": [
       "$\\displaystyle \\frac{{\\partial g(\\mathbf{x})}}{\\partial \\theta_{R}} + \\frac{{\\partial g(\\mathbf{x})}}{\\partial \\phi_{R}} = 0$"
      ],
      "text/plain": [
       "{\\partial g(\\mathbf{x})}   {\\partial g(\\mathbf{x})}    \n",
       "──────────────────────── + ──────────────────────── = 0\n",
       "      \\partial⋅θ_R               \\partial⋅φ_R          "
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Observable modes: \n"
     ]
    },
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAABEAAAAOCAYAAADJ7fe0AAAACXBIWXMAAA7EAAAOxAGVKw4bAAABDUlEQVQoFY2T4Q2CMBCFwQmII+AGGjfQDTRu4Ar85S8jqBMY2QDcQBmBEQwb4Pdqm4BQ5ZLr3b27vrbHEbZtG3QlTdMj8RbdWbzENtaPrFWcUFsrDr9JbFFAgdgr7MphzoLpoBO6x89nLtG1JJY2vnZx55M/40svwkZJwDdKInqKT24kIt3KR6KeBBRUPgZw0w/swkeim/y6hbhjLUg8IOF014/iU+NdXV09IGHLlH6IeW3pizGSKf3Qfs1Rw83LMZK//WCj5kSSaOmRTOkHNZraDC3xNSt9EuKf/bAEd+pqfPPsMZKDQAoG8wGmA57oA7/3K5h/B1DTF6Pus+X4L3SOSvQEDVdGrRsy4UbemFVVEoJZxAAAAAAASUVORK5CYII=",
      "text/latex": [
       "$\\displaystyle D$"
      ],
      "text/plain": [
       "D"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "mobile_robot.null_calc_opt = \"symbolic\"\n",
    "mobile_robot.observable_mode()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [],
   "source": [
    "if(autosave):\n",
    "    mobile_robot.save(NOA_obj_save_dir)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "mobile_robot_numeric NOA object initialized\n",
      "Drift 2nd Order Vector Fields\n",
      "[(0,), (1,), (2,)]\n",
      "current Lie derivative:  k1f0  not appended to observability matrix due to null vector\n",
      "current Lie derivative:  k1f1 appended to observability matrix\n",
      "current Lie derivative:  k1f2  not appended to observability matrix due to null vector\n",
      "[(0, 1), (0, 2)]\n",
      "current Lie derivative:  k2f0f1  not appended to observability matrix due to null vector\n",
      "current Lie derivative:  k2f0f2  not appended to observability matrix due to null vector\n",
      "Insufficient obsv_mat rows. Auto-construct obsv_mat using Permutation of vector fields ...\n",
      "Permutation of Vector Fields\n",
      "[(0,), (1,), (2,)]\n",
      "current Lie derivative:  k1f0  not appended to observability matrix due to null vector\n",
      "current Lie derivative:  k1f1 appended to observability matrix\n",
      "current Lie derivative:  k1f2  not appended to observability matrix due to null vector\n",
      "[(0, 0), (0, 1), (0, 2), (1, 0), (1, 1), (1, 2), (2, 0), (2, 1), (2, 2)]\n",
      "current Lie derivative:  k2f0f0  not appended to observability matrix due to null vector\n",
      "current Lie derivative:  k2f0f1  not appended to observability matrix due to null vector\n",
      "current Lie derivative:  k2f0f2  not appended to observability matrix due to null vector\n",
      "current Lie derivative:  k2f1f0  not appended to observability matrix due to null vector\n",
      "current Lie derivative:  k2f1f1 appended to observability matrix\n",
      "current Lie derivative:  k2f1f2 appended to observability matrix\n",
      "current Lie derivative:  k2f2f0  not appended to observability matrix due to null vector\n",
      "current Lie derivative:  k2f2f1  not appended to observability matrix due to null vector\n",
      "current Lie derivative:  k2f2f2  not appended to observability matrix due to null vector\n",
      "Substituted parameters with values from same-order prime numbers\n",
      "printing self.numeric_params_dict:\n",
      "{D: 5.0, phi_R: 3.0, theta_R: 2.0}\n",
      "\n",
      "Calculating rank of numerical observability matrix ...\n",
      "The system is NOT weakly locally observable (WLO)!\n",
      "Dimension of largest WLO subsystem (Observable & Jointly Observable):  2\n",
      "Dimension of Undistinguishable Region (Unobservable):  1\n",
      "Substituted parameters with values from same-order prime numbers\n",
      "printing self.numeric_params_dict:\n",
      "{D: 3.0, phi_R: 2.0, theta_R: 5.0}\n",
      "\n",
      "Calculating rank of numerical observability matrix ...\n",
      "The system is NOT weakly locally observable (WLO)!\n",
      "Dimension of largest WLO subsystem (Observable & Jointly Observable):  2\n",
      "Dimension of Undistinguishable Region (Unobservable):  1\n"
     ]
    }
   ],
   "source": [
    "mobile_robot_num = NOA(\"mobile_robot_numeric\")\n",
    "mobile_robot_num.x = mobile_robot.x\n",
    "mobile_robot_num.f = mobile_robot.f\n",
    "mobile_robot_num.h = mobile_robot.h\n",
    "# mobile_robot_num.LD_order = 2\n",
    "mobile_robot_num.combn_permn_opt = \"drift2ndOrder\"\n",
    "mobile_robot_num.rank_calc_opt = \"numeric\"\n",
    "mobile_robot_num.params_config_subs = sp.Matrix([mobile_robot_num.x])\n",
    "# new_params_dict = {D: 2, phi_R: sp.pi/5, theta_R: sp.pi/7}\n",
    "# mobile_robot_num.update_params_dict(new_params_dict)\n",
    "mobile_robot_num.ORC()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "The observable modes are g(x) which satisfiy these partial differential equations (PDE): \n"
     ]
    },
    {
     "data": {
      "image/png": "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",
      "text/latex": [
       "$\\displaystyle \\frac{{\\partial g(\\mathbf{x})}}{\\partial \\theta_{R}} + \\frac{{\\partial g(\\mathbf{x})}}{\\partial \\phi_{R}} = 0$"
      ],
      "text/plain": [
       "{\\partial g(\\mathbf{x})}   {\\partial g(\\mathbf{x})}    \n",
       "──────────────────────── + ──────────────────────── = 0\n",
       "      \\partial⋅θ_R               \\partial⋅φ_R          "
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Observable modes: \n"
     ]
    },
    {
     "data": {
      "image/png": "iVBORw0KGgoAAAANSUhEUgAAABEAAAAOCAYAAADJ7fe0AAAACXBIWXMAAA7EAAAOxAGVKw4bAAABDUlEQVQoFY2T4Q2CMBCFwQmII+AGGjfQDTRu4Ar85S8jqBMY2QDcQBmBEQwb4Pdqm4BQ5ZLr3b27vrbHEbZtG3QlTdMj8RbdWbzENtaPrFWcUFsrDr9JbFFAgdgr7MphzoLpoBO6x89nLtG1JJY2vnZx55M/40svwkZJwDdKInqKT24kIt3KR6KeBBRUPgZw0w/swkeim/y6hbhjLUg8IOF014/iU+NdXV09IGHLlH6IeW3pizGSKf3Qfs1Rw83LMZK//WCj5kSSaOmRTOkHNZraDC3xNSt9EuKf/bAEd+pqfPPsMZKDQAoG8wGmA57oA7/3K5h/B1DTF6Pus+X4L3SOSvQEDVdGrRsy4UbemFVVEoJZxAAAAAAASUVORK5CYII=",
      "text/latex": [
       "$\\displaystyle D$"
      ],
      "text/plain": [
       "D"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "mobile_robot_num.observable_mode()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [],
   "source": [
    "if(autosave):\n",
    "    mobile_robot_num.save(NOA_obj_save_dir)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3.6.9 64-bit",
   "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.6.9"
  },
  "orig_nbformat": 4,
  "vscode": {
   "interpreter": {
    "hash": "31f2aee4e71d21fbe5cf8b01ff0e069b9275f58929596ceb00d14d90e3e16cd6"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}