{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Kálmán filter in 2D\n",
    "GPS+IMU fusion"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "import numpy as np\n",
    "from scipy.stats import norm\n",
    "from sympy import Symbol, Matrix\n",
    "from sympy.interactive import printing"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Example"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "mean:           12.2149585383803\n",
      "std deviation:  4.1213744424815175\n"
     ]
    },
    {
     "data": {
      "image/png": "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",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "example = np.random.normal(12, 4, 500)\n",
    "plt.hist(example, 20, density = True, label=\"example sensor readings\")\n",
    "e_mu, e_std = norm.fit(example)\n",
    "print(\"mean:          \", e_mu)\n",
    "print(\"std deviation: \", e_std)\n",
    "xmin, xmax = plt.xlim()\n",
    "e_x = np.linspace(xmin, xmax, 100)\n",
    "p = norm.pdf(e_x, e_mu, e_std)\n",
    "plt.plot(e_x, p, 'k', linewidth=2, label=\"norm\")\n",
    "plt.show()\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "\n",
    "x = np.matrix([[0.0, 0.0, 0.0, 0.0, 0.0, 0.0]]).T\n",
    "n = x.size  # States\n",
    "P = np.matrix(\n",
    "    [\n",
    "        [10.0, 0.0, 0.0, 0.0, 0.0, 0.0],\n",
    "        [0.0, 10.0, 0.0, 0.0, 0.0, 0.0],\n",
    "        [0.0, 0.0, 10.0, 0.0, 0.0, 0.0],\n",
    "        [0.0, 0.0, 0.0, 10.0, 0.0, 0.0],\n",
    "        [0.0, 0.0, 0.0, 0.0, 10.0, 0.0],\n",
    "        [0.0, 0.0, 0.0, 0.0, 0.0, 10.0],\n",
    "    ]\n",
    ")\n",
    "#print(P)\n",
    "\n",
    "fig = plt.figure(figsize=(6, 6))\n",
    "im = plt.imshow(P, interpolation=\"none\", cmap=plt.get_cmap(\"BuGn\"))\n",
    "plt.title(\"Initial Covariance Matrix $P$\")\n",
    "ylocs, ylabels = plt.yticks()\n",
    "# set the locations of the yticks\n",
    "plt.yticks(np.arange(7))\n",
    "# set the locations and labels of the yticks\n",
    "plt.yticks(\n",
    "    np.arange(6),\n",
    "    (\"$x$\", \"$y$\", \"$\\dot x$\", \"$\\dot y$\", \"$\\ddot x$\", \"$\\ddot y$\"),\n",
    "    fontsize=22,\n",
    ")\n",
    "\n",
    "xlocs, xlabels = plt.xticks()\n",
    "# set the locations of the yticks\n",
    "plt.xticks(np.arange(7))\n",
    "# set the locations and labels of the yticks\n",
    "plt.xticks(\n",
    "    np.arange(6),\n",
    "    (\"$x$\", \"$y$\", \"$\\dot x$\", \"$\\dot y$\", \"$\\ddot x$\", \"$\\ddot y$\"),\n",
    "    fontsize=22,\n",
    ")\n",
    "\n",
    "plt.xlim([-0.5, 5.5])\n",
    "plt.ylim([5.5, -0.5])\n",
    "\n",
    "from mpl_toolkits.axes_grid1 import make_axes_locatable\n",
    "\n",
    "divider = make_axes_locatable(plt.gca())\n",
    "cax = divider.append_axes(\"right\", \"5%\", pad=\"3%\")\n",
    "plt.colorbar(im, cax=cax)\n",
    "plt.tight_layout()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": []
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "'\\nfig = plt.figure(figsize=(6, 6))\\nim = plt.imshow(A, interpolation=\"none\", cmap=plt.get_cmap(\"BuGn\"))\\nplt.title(\"Matrix $A$\")\\ndivider = make_axes_locatable(plt.gca())\\ncax = divider.append_axes(\"right\", \"5%\", pad=\"3%\")\\nplt.colorbar(im, cax=cax)\\nplt.tight_layout()\\n'"
      ]
     },
     "execution_count": 23,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "dt = 0.1  # Time Step between Filter Steps\n",
    "\n",
    "A = np.matrix(\n",
    "    [\n",
    "        [1.0, 0.0, dt, 0.0, 1 / 2.0 * dt ** 2, 0.0],\n",
    "        [0.0, 1.0, 0.0, dt, 0.0, 1 / 2.0 * dt ** 2],\n",
    "        [0.0, 0.0, 1.0, 0.0, dt, 0.0],\n",
    "        [0.0, 0.0, 0.0, 1.0, 0.0, dt],\n",
    "        [0.0, 0.0, 0.0, 0.0, 1.0, 0.0],\n",
    "        [0.0, 0.0, 0.0, 0.0, 0.0, 1.0],\n",
    "    ]\n",
    ")\n",
    "\"\"\"\n",
    "fig = plt.figure(figsize=(6, 6))\n",
    "im = plt.imshow(A, interpolation=\"none\", cmap=plt.get_cmap(\"BuGn\"))\n",
    "plt.title(\"Matrix $A$\")\n",
    "divider = make_axes_locatable(plt.gca())\n",
    "cax = divider.append_axes(\"right\", \"5%\", pad=\"3%\")\n",
    "plt.colorbar(im, cax=cax)\n",
    "plt.tight_layout()\n",
    "\"\"\""
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "metadata": {},
   "outputs": [],
   "source": [
    "H = np.matrix(\n",
    "    [\n",
    "        [0.0, 0.0, 0.0, 0.0, 1.0, 0.0],\n",
    "        [0.0, 0.0, 0.0, 0.0, 0.0, 1.0],\n",
    "        [1.0, 0.0, 0.0, 0.0, 0.0, 0.0],\n",
    "        [0.0, 1.0, 0.0, 0.0, 0.0, 0.0],\n",
    "    ]\n",
    ")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 25,
   "metadata": {},
   "outputs": [],
   "source": [
    "ra = 10.0 ** 2\n",
    "rp = 2.0 ** 2\n",
    "\n",
    "R = np.matrix(\n",
    "    [[ra, 0.0, 0.0, 0.0], [0.0, ra, 0.0, 0.0], [0.0, 0.0, rp, 0.0], [0.0, 0.0, 0.0, rp]]\n",
    ")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 32,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "'\\nfig = plt.figure(figsize=(6, 6))\\nim = plt.imshow(Q, interpolation=\"none\", cmap=plt.get_cmap(\"BuGn\"))\\nplt.title(\"Matrix $Q$\")\\ndivider = make_axes_locatable(plt.gca())\\ncax = divider.append_axes(\"right\", \"5%\", pad=\"3%\")\\nplt.colorbar(im, cax=cax)\\nplt.tight_layout()\\nprint(Q)\\n'"
      ]
     },
     "execution_count": 32,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "sj = 0.1\n",
    "dts = Symbol(\"\\Delta t\")\n",
    "Q = (\n",
    "    np.matrix(\n",
    "        [\n",
    "            [(dt ** 6) / 36, 0, (dt ** 5) / 12, 0, (dt ** 4) / 6, 0],\n",
    "            [0, (dt ** 6) / 36, 0, (dt ** 5) / 12, 0, (dt ** 4) / 6],\n",
    "            [(dt ** 5) / 12, 0, (dt ** 4) / 4, 0, (dt ** 3) / 2, 0],\n",
    "            [0, (dt ** 5) / 12, 0, (dt ** 4) / 4, 0, (dt ** 3) / 2],\n",
    "            [(dt ** 4) / 6, 0, (dt ** 3) / 2, 0, (dt ** 2), 0],\n",
    "            [0, (dt ** 4) / 6, 0, (dt ** 3) / 2, 0, (dt ** 2)],\n",
    "        ]\n",
    "    )\n",
    "    * sj ** 2\n",
    ")\n",
    "\"\"\"\n",
    "fig = plt.figure(figsize=(6, 6))\n",
    "im = plt.imshow(Q, interpolation=\"none\", cmap=plt.get_cmap(\"BuGn\"))\n",
    "plt.title(\"Matrix $Q$\")\n",
    "divider = make_axes_locatable(plt.gca())\n",
    "cax = divider.append_axes(\"right\", \"5%\", pad=\"3%\")\n",
    "plt.colorbar(im, cax=cax)\n",
    "plt.tight_layout()\n",
    "print(Q)\n",
    "\"\"\""
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Used sources:\n",
    "- https://github.com/udacity/robot_pose_ekf\n",
    "- https://github.com/TUMFTM\n",
    "- https://github.com/cggos/imu_x_fusion\n",
    "- https://github.com/PRBonn/LiDAR-MOS"
   ]
  }
 ],
 "metadata": {
  "interpreter": {
   "hash": "31f2aee4e71d21fbe5cf8b01ff0e069b9275f58929596ceb00d14d90e3e16cd6"
  },
  "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
 },
 "nbformat": 4,
 "nbformat_minor": 2
}