324 lines
203 KiB
Plaintext
324 lines
203 KiB
Plaintext
|
{
|
||
|
|
"cells": [
|
||
|
|
{
|
||
|
|
"cell_type": "markdown",
|
||
|
|
"metadata": {},
|
||
|
|
"source": [
|
||
|
|
"## Kalman filter\n",
|
||
|
|
"\n",
|
||
|
|
"We will go through this notebook after a Power Point lecture.\n",
|
||
|
|
"\n",
|
||
|
|
"In the Power Point lecture, we showed -- amongst other imates -- figures from [this PDF](https://synapticlab.co.kr/attachment/cfile1.uf@2737C54B590907BA0D46CE.pdf) ([doi:10.1109/MSP.2012.2203621](https://doi.org/10.1109/MSP.2012.2203621)).\n",
|
||
|
|
"\n",
|
||
|
|
"As further reading, I recommend this webpage: [How a Kalman filter works, in pictures](https://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/).\n",
|
||
|
|
"\n",
|
||
|
|
"For a wonderful, if hardcore, Python-based Kalman filter library and documentation, please see [FilterPy](https://filterpy.readthedocs.io/en/latest/)."
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 1,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [
|
||
|
|
{
|
||
|
|
"name": "stdout",
|
||
|
|
"output_type": "stream",
|
||
|
|
"text": [
|
||
|
|
"Requirement already satisfied: adskalman in /Users/andrew/anaconda3/envs/pm21-dragon/lib/python3.11/site-packages (0.3.11)\n"
|
||
|
|
]
|
||
|
|
}
|
||
|
|
],
|
||
|
|
"source": [
|
||
|
|
"!pip install adskalman"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "markdown",
|
||
|
|
"metadata": {},
|
||
|
|
"source": [
|
||
|
|
"# Kalman filter example in Python"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 2,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [],
|
||
|
|
"source": [
|
||
|
|
"import numpy as np\n",
|
||
|
|
"import adskalman.adskalman as adskalman\n",
|
||
|
|
"import matplotlib.pyplot as plt"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 3,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [],
|
||
|
|
"source": [
|
||
|
|
"def column(arr):\n",
|
||
|
|
" \"\"\"convert 1D array-like to a 2D vertical array\n",
|
||
|
|
"\n",
|
||
|
|
" >>> column((1,2,3))\n",
|
||
|
|
"\n",
|
||
|
|
" array([[1],\n",
|
||
|
|
" [2],\n",
|
||
|
|
" [3]])\n",
|
||
|
|
" \"\"\"\n",
|
||
|
|
" arr = np.array(arr)\n",
|
||
|
|
" assert arr.ndim == 1\n",
|
||
|
|
" a2 = arr[:, np.newaxis]\n",
|
||
|
|
" return a2"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 4,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [],
|
||
|
|
"source": [
|
||
|
|
"# Create a 4-dimensional state space model:\n",
|
||
|
|
"# (x, y, xvel, yvel).\n",
|
||
|
|
"dt = 0.01\n",
|
||
|
|
"true_initial_state = column([0.0, 0.0, 10.0, -5.0])\n",
|
||
|
|
"# This is F in wikipedia language.\n",
|
||
|
|
"motion_model = np.array([[1.0, 0.0, dt, 0.0],\n",
|
||
|
|
" [0.0, 1.0, 0.0, dt],\n",
|
||
|
|
" [0.0, 0.0, 1.0, 0.0],\n",
|
||
|
|
" [0.0, 0.0, 0.0, 1.0]])\n",
|
||
|
|
"\n",
|
||
|
|
"# This is Q in wikipedia language. For a constant velocity form, it must take this specific form to be correct.\n",
|
||
|
|
"T3 = dt**3/3\n",
|
||
|
|
"T2 = dt**2/2\n",
|
||
|
|
"motion_noise_covariance = 1000.0*np.array([[T3, 0.0, T2, 0.0],\n",
|
||
|
|
" [0.0, T3, 0.0, T2],\n",
|
||
|
|
" [T2, 0.0, dt, 0.0],\n",
|
||
|
|
" [0.0, T2, 0.0, dt]])"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 5,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [],
|
||
|
|
"source": [
|
||
|
|
"duration = 0.5\n",
|
||
|
|
"t = np.arange(0.0, duration, dt)"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 6,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [],
|
||
|
|
"source": [
|
||
|
|
"# Create some fake data with our model.\n",
|
||
|
|
"current_state = true_initial_state\n",
|
||
|
|
"state = []\n",
|
||
|
|
"for _ in t:\n",
|
||
|
|
" state.append(current_state[:, 0])\n",
|
||
|
|
" noise_sample = adskalman.rand_mvn(np.zeros(4), motion_noise_covariance, 1).T\n",
|
||
|
|
" current_state = np.dot(motion_model, current_state) + noise_sample\n",
|
||
|
|
"state = np.array(state)"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 7,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [
|
||
|
|
{
|
||
|
|
"data": {
|
||
|
|
"image/png": "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
|
||
|
|
"text/plain": [
|
||
|
|
"<Figure size 640x480 with 1 Axes>"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
"metadata": {},
|
||
|
|
"output_type": "display_data"
|
||
|
|
}
|
||
|
|
],
|
||
|
|
"source": [
|
||
|
|
"plt.plot(state[:, 0], state[:, 1], '.-')\n",
|
||
|
|
"plt.xlabel('x')\n",
|
||
|
|
"_ = plt.ylabel('y')"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 8,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [],
|
||
|
|
"source": [
|
||
|
|
"# Create observation model. We only observe the position.\n",
|
||
|
|
"observation_model = np.array([[1.0, 0.0, 0.0, 0.0],\n",
|
||
|
|
" [0.0, 1.0, 0.0, 0.0]])\n",
|
||
|
|
"observation_noise_covariance = np.array([[0.01, 0.0],\n",
|
||
|
|
" [0.0, 0.01]])"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 9,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [],
|
||
|
|
"source": [
|
||
|
|
"# Create noisy observations.\n",
|
||
|
|
"observation = []\n",
|
||
|
|
"for current_state in state:\n",
|
||
|
|
" noise_sample = adskalman.rand_mvn(np.zeros(2), observation_noise_covariance, 1).T\n",
|
||
|
|
" current_observation = np.dot(observation_model, column(current_state)) + noise_sample\n",
|
||
|
|
" observation.append(current_observation[:, 0])\n",
|
||
|
|
"observation = np.array(observation)"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 10,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [
|
||
|
|
{
|
||
|
|
"data": {
|
||
|
|
"image/png": "iVBORw0KGgoAAAANSUhEUgAAAkMAAAGwCAYAAACq12GxAAAAOXRFWHRTb2Z0d2FyZQBNYXRwbG90bGliIHZlcnNpb24zLjkuMiwgaHR0cHM6Ly9tYXRwbG90bGliLm9yZy8hTgPZAAAACXBIWXMAAA9hAAAPYQGoP6dpAAB3pklEQVR4nO3dd3iT5foH8O+bpE13undLy2jLKBvaslUEVBygCKJ1c1w40OMR8HfO4QzBcY4bt0cUURAZbvamFFqgzFJW957pbtPk/f2RJtA906zv57p6SZL3TZ7GF3Lnee7nvgVRFEUQERERWSmJsQdAREREZEwMhoiIiMiqMRgiIiIiq8ZgiIiIiKwagyEiIiKyagyGiIiIyKoxGCIiIiKrJjP2AEydRqNBTk4OnJ2dIQiCsYdDREREnSCKIioqKuDv7w+JpP25HwZDHcjJyUFQUJCxh0FERETdkJmZicDAwHaPYTDUAWdnZwDaN9PFxcXIoyEiIqLOKC8vR1BQkP5zvD0MhjqgWxpzcXFhMERERGRmOpPiwgRqIiIismoMhoiIiMiqMRgiIiIiq8ZgiIiIiKwagyEiIiKyagyGiIiIyKoxGCIiIiKrxmCIiIiIrBqDISIiIrJqDIaIiIjIqjEYIiIiIqvGYMhM5SprEHelCLnKGmMPhYiIyKyxUasZ2pCQgWWbz0AjAhIBWDU3EvPHBRt7WERERGaJM0NmJldZow+EAEAjAss3n+UMERERUTcxGDIzqUVV+kBIRy2KSCuqNs6AiIiIzByDITMT6ukIQWh6n1QQEOLpYJwBERERmTkGQ2bGT2GPwb4uTe5bfmsE/BT2RhoRERGReWMwZGaKKutwMb8CAOAklwIAgtw5K0RERNRdDIbMzNaT2WjQiBgRqMDtIwIAAInppUYeFRERkfliMGRGRFHEj8ezAAD3jA3CuBA3AEBiWokxh0VERGTWWGfIjJzNLseFvArYyiS4Y7g/ymtVAIAz2UrUqtSws5EaeYRERETmhzNDZmTj8UwAwIwhPlA42CDQzR7eznKo1CJOZZYZd3BERERmisGQmahVqfFTUg4AYN7YIACAIAgYF+IOgHlDRERE3cVgyEzsSs6HskYFP4UdJg301N8/ph/zhoiIiHqCwZCZ2JioTZyeOzoAUsm1qou6maHj6aXQNC9NTURERB1iMGQG8pS1OHipEABwz5igJo8N9nOGg60U5bUNuFRQaYzhERERmTUGQ2Zg04ksaERgXIgbQj0dmzwmk0owKtgVAJDApTIiIqIuYzBk4kRRxKbG2kLzms0K6Yztd22pjIiIiLqGwZCJO5FRiqtFVbC3keLW4X6tHjO2sfgiZ4aIiIi6jsGQidMlTt8a6Qcnees1MkcFu0EiAFmlNchT1vbl8IiIiMwegyETVl3fgF9P5wIA5o0NbPM4J7kMQ/y1newT0zk7RERE1BUMhkzYtrN5qKxrQLC7A6JC3ds9Vpc3lJjGvCEiIqKuYDBkwnRLZPeMCYQgCO0eq8sb4swQERFR1zAYMlGZJdU4crUYgqAttNgR3czQ+ZxyVNY1GHp4REREFoPBkIn6sXE7/YQBHgh0c+jweF+FHQLd7KERgZMZXCojIiLqLAZDJkijEfXBUFu1hVqjb9rKvCEiIqJOYzBkguKvFiO7rAbOchlmDvXt9Hn6pq3MGyIiIuo0BkMmaGPjrNDsEf6wt5V2+jzdzNDJjDI0qDUGGRsREZGlYTBkYsprVfjjbMe1hVozyNsJLnYyVNerkZxbYYjhERERWRyzCYZKS0sRGxsLhUIBhUKB2NhYlJWVtXm8SqXCK6+8gsjISDg6OsLf3x8PPvggcnJy+m7Q3fD76VzUqjQY4OWIUUGuXTpXIhH0S2VszUFERNQ5ZhMMLVy4EElJSdi2bRu2bduGpKQkxMbGtnl8dXU1Tpw4gb/+9a84ceIENm/ejIsXL+KOO+7ow1F3nW6JbN7YoA5rC7VmbAibthIREXVF682uTExycjK2bduG+Ph4REVFAQA+//xzxMTEICUlBeHh4S3OUSgU2LlzZ5P7PvjgA4wfPx4ZGRkIDg7uk7F3xZXCShxPL4VUImDuqI5rC7Vm7HUzQ6IodiugIiIisiZmMTN05MgRKBQKfSAEANHR0VAoFIiLi+v08yiVSgiCAFdX1zaPqaurQ3l5eZOfvqLbTj81zAveLnbdeo4RQa6wkQooqKhDZklNbw6PiIjIIplFMJSXlwdvb+8W93t7eyMvL69Tz1FbW4ulS5di4cKFcHFxafO4VatW6fOSFAoFgoI6X+enJ9QaEZtP6GoLdS1x+np2NlJEBigAcIs9ERFRZxg1GFqxYgUEQWj3JzExEQBaXe7p7DKQSqXCggULoNFo8NFHH7V77LJly6BUKvU/mZmZ3fvluujApULkl9fBzcEGNw326dFz6fKGElh8kYiIqENGzRlavHgxFixY0O4xISEhOH36NPLz81s8VlhYCB+f9gMHlUqFe++9F6mpqdizZ0+7s0IAIJfLIZfLOx58L/uxsSnrnSMDYCvrWYw6tp8bPgNwnDNDREREHTJqMOTp6QlPT88Oj4uJiYFSqcSxY8cwfvx4AMDRo0ehVCoxYcKENs/TBUKXLl3C3r174eHh0Wtj701l1fXYeV4b7N3TgyUyHd32+ov5lSirroerg22Pn5OIiMhSmUXO0ODBgzFr1iwsWrQI8fHxiI+Px6JFizB79uwmO8kiIiKwZcsWAEBDQwPuueceJCYmYt26dVCr1cjLy0NeXh7q6+uN9au06qekHNSrNRjs54Jhjfk+PeHhJEd/L0cA3GJPRETUEbMIhgBg3bp1iIyMxIwZMzBjxgwMHz4ca9eubXJMSkoKlEolACArKws///wzsrKyMHLkSPj5+el/urIDrS9sPK7NS+pJ4nRz4/o1Nm1lMERERNQus6gzBADu7u749ttv2z1GFEX9n0NCQprcNlXJueU4m10OG6mAu7pZW6g1Y0LcsCExE4msRE1ERNQus5kZslS62kI3RfjA3bH3cnt0TVtPZSlR16DuteclIiKyNAyGjEil1mDryWwAXW/K2pEQDwd4ONqivkGDs9nKXn1uIiIiS8JgyIj2XChAcVU9vJzlmBrm1avPLQgCxoboWnMwb4iIiKgtDIaMaO2RdADAjCE+kEl7/3+FbqkskcEQERFRmxgMGckXB6/i0OUiAMD3xzKwISGj119DV2/oeHoJNBrTTyYnIiIyBgZDRpCrrMFrvyXrb2tEYPnms8hV9m5j1aH+CtjZSFBarcLVospefW4iIiJLwWDICFKLqtB8nkYtikgrqu7V17GVSTAi0BUAsCEhs9eDLSIiIkvAYMgIQj0dIWnWX1YqCAjxdOj113K01ZaS+vxgKia+vscgy3FERETmzGyKLloSP4U9Vs2NxPLNZ6EWRUgFASvnDoOfwr5XXydXWYO9Fwv0tzUi8MqmM4i/UoxBvs4IcLVHoJs9Alwd4OUsh7R5hEZERGQFGAwZyfxxwZgS5oW0omqEeDr0eiAENC7HtZI3vSUpp8V9NlIBvgo7BLhqg6MAN3sEutojwM0eAa728HO1g1wm7fA1c5U1SC2qQqino0F+JyIiot7GYMiI/BT2Bg0YdMtx128kEwTg4ZgQKGtUyCqrQXZpDfLKa6FSi8gsqUFmSQ2Ali08BAHwcpLrg6OmwZI2ePrtdA6WbT4DjQhIBGDV3EjMHxdssN+PiIioNwiiOTTwMqLy8nIoFAoolUq4uLgYezhdtiEho8VyXPMApUGtQX5FHbJLa5BdVt343xpkldYgp0z751qVpsuvLRUEHFp6A2eIiIioz3Xl85szQxauM8txMqmkcXnMHoB7i8dFUURJVT2yG2eSdIHS9beVNaoW5+l2yDEYIiIiU8ZgyAr0dDlOEAR4OMnh4STH8Mat+s1dLqjEjHf2o3ltRw0nHomIyMRxaz31ioHeTlg1NxJSoemOtCfXHsfBS4VGGhUREVHHmDPUAXPPGeprucoapBVVw83BBn/76RyOpZVAKhHwrzuHYWEUk6mJiKhvdOXzmzND1Kv8FPaIGeCBCD8XrH1
|
||
|
|
"text/plain": [
|
||
|
|
"<Figure size 640x480 with 1 Axes>"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
"metadata": {},
|
||
|
|
"output_type": "display_data"
|
||
|
|
}
|
||
|
|
],
|
||
|
|
"source": [
|
||
|
|
"plt.plot(observation[:, 0], observation[:, 1], '.-')\n",
|
||
|
|
"plt.xlabel('x')\n",
|
||
|
|
"_ = plt.ylabel('y')"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 11,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [],
|
||
|
|
"source": [
|
||
|
|
"# Run kalman filter on the noisy observations.\n",
|
||
|
|
"y = observation\n",
|
||
|
|
"F = motion_model\n",
|
||
|
|
"H = observation_model\n",
|
||
|
|
"Q = motion_noise_covariance\n",
|
||
|
|
"R = observation_noise_covariance\n",
|
||
|
|
"initx = true_initial_state[:, 0]\n",
|
||
|
|
"initV = 0.1*np.eye(4)"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 12,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [],
|
||
|
|
"source": [
|
||
|
|
"kfilt = adskalman.KalmanFilter(F, H, Q, R, initx, initV)\n",
|
||
|
|
"xfilt = []\n",
|
||
|
|
"Vfilt = []\n",
|
||
|
|
"for i, y_i in enumerate(y):\n",
|
||
|
|
" is_initial = i == 0\n",
|
||
|
|
" xfilt_i, Vfilt_i = kfilt.step(y=y_i, isinitial=is_initial)\n",
|
||
|
|
" xfilt.append(xfilt_i)\n",
|
||
|
|
" Vfilt.append(Vfilt_i)\n",
|
||
|
|
"xfilt = np.array(xfilt)\n",
|
||
|
|
"Vfilt = np.array(Vfilt)"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 13,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [
|
||
|
|
{
|
||
|
|
"data": {
|
||
|
|
"image/png": "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
|
||
|
|
"text/plain": [
|
||
|
|
"<Figure size 640x480 with 1 Axes>"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
"metadata": {},
|
||
|
|
"output_type": "display_data"
|
||
|
|
}
|
||
|
|
],
|
||
|
|
"source": [
|
||
|
|
"plt.plot(state[:, 0], state[:, 1], '.-', label='true')\n",
|
||
|
|
"plt.plot(observation[:, 0], observation[:, 1], '.-', label='observed')\n",
|
||
|
|
"plt.plot(xfilt[:, 0], xfilt[:, 1], '.-', label='kalman filtered')\n",
|
||
|
|
"plt.xlabel('x')\n",
|
||
|
|
"plt.ylabel('y')\n",
|
||
|
|
"_ = plt.legend()"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 14,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [],
|
||
|
|
"source": [
|
||
|
|
"# Now run again with missing data\n",
|
||
|
|
"y[20:30, :] = np.nan\n",
|
||
|
|
"kfilt = adskalman.KalmanFilter(F, H, Q, R, initx, initV)\n",
|
||
|
|
"xfilt = []\n",
|
||
|
|
"Vfilt = []\n",
|
||
|
|
"for i, y_i in enumerate(y):\n",
|
||
|
|
" is_initial = i == 0\n",
|
||
|
|
" xfilt_i, Vfilt_i = kfilt.step(y=y_i, isinitial=is_initial)\n",
|
||
|
|
" xfilt.append(xfilt_i)\n",
|
||
|
|
" Vfilt.append(Vfilt_i)\n",
|
||
|
|
"xfilt = np.array(xfilt)\n",
|
||
|
|
"Vfilt = np.array(Vfilt)"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 15,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [
|
||
|
|
{
|
||
|
|
"data": {
|
||
|
|
"image/png": "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
|
||
|
|
"text/plain": [
|
||
|
|
"<Figure size 640x480 with 1 Axes>"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
"metadata": {},
|
||
|
|
"output_type": "display_data"
|
||
|
|
}
|
||
|
|
],
|
||
|
|
"source": [
|
||
|
|
"plt.plot(state[:, 0], state[:, 1], '.-', label='true')\n",
|
||
|
|
"plt.plot(observation[:, 0], observation[:, 1], '.-', label='observed')\n",
|
||
|
|
"plt.plot(xfilt[:, 0], xfilt[:, 1], '.-', label='kalman filtered')\n",
|
||
|
|
"plt.xlabel('x')\n",
|
||
|
|
"plt.ylabel('y')\n",
|
||
|
|
"_ = plt.legend()"
|
||
|
|
]
|
||
|
|
}
|
||
|
|
],
|
||
|
|
"metadata": {
|
||
|
|
"kernelspec": {
|
||
|
|
"display_name": "Python 3 (ipykernel)",
|
||
|
|
"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.11.10"
|
||
|
|
}
|
||
|
|
},
|
||
|
|
"nbformat": 4,
|
||
|
|
"nbformat_minor": 4
|
||
|
|
}
|