{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "import numpy as np\n",
    "\n",
    "# create a sinewave with a frequency of 1Hz\n",
    "t = np.arange(0, 4, 0.01)\n",
    "pure = np.sin(2 * np.pi * t)\n",
    "\n",
    "# add random noise to the sinewave\n",
    "noise = np.random.normal(0, 0.1, len(t))\n",
    "noisy = pure + noise"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "[encoded data, 30260 characters]",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "# plot the sinewave\n",
    "plt.plot(t, pure, label='pure')\n",
    "plt.plot(t, noisy, label='noisy')\n",
    "plt.legend()\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "def derivative(x, y, gain=0.1):\n",
    "    return np.gradient(y, x) * gain"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "<matplotlib.legend.Legend at 0x7fde00941b10>"
      ]
     },
     "execution_count": 5,
     "metadata": {},
     "output_type": "execute_result"
    },
    {
     "data": {
      "image/png": "[encoded data, 36704 characters]",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "# plot derivative of pure sinewave\n",
    "plt.plot(t, derivative(t, pure, 1), label='derivative')\n",
    "# plot derivative of noisy sinewave\n",
    "plt.plot(t, derivative(t, noisy, 1), label='derivative')\n",
    "plt.legend()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "[encoded data, 33608 characters]",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "# low pass filter on the noisy sinewave\n",
    "filtered = np.convolve(noisy, np.ones(10)/10, mode='same')\n",
    "# plot filtered noisy\n",
    "plt.plot(t, filtered, label='filtered')\n",
    "# plot derivative of filtered noisy\n",
    "plt.plot(t, derivative(t, filtered, 1), label='filtered derivative')\n",
    "# # plot derivative of original noisy\n",
    "# plt.plot(t, derivative(t, noisy, 1), label='noisy derivative')\n",
    "\n",
    "plt.legend()\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Create pseudo-derivative function\n",
    "def pseudo_derivative(x, y, gain=0.1):\n",
    "  # apply a low pass filter to x\n",
    "  filtered = np.convolve(y, np.ones(10)/10, mode='same')\n",
    "  # now take the derivative of x_filt\n",
    "  return np.gradient(filtered, x) * gain"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "[encoded data, 31108 characters]",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "# plot the pseudo-derivative of noisy sinewave\n",
    "plt.plot(t, pseudo_derivative(t, noisy, 1), label='pseudo-derivative')\n",
    "\n",
    "# plot derivative of noisy sinewave\n",
    "# plt.plot(t, derivative(t, noisy, 1), label='derivative')\n",
    "\n",
    "plt.legend()\n",
    "plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "image/png": "[encoded data, 58960 characters]",
      "text/plain": [
       "<Figure size 432x288 with 1 Axes>"
      ]
     },
     "metadata": {
      "needs_background": "light"
     },
     "output_type": "display_data"
    }
   ],
   "source": [
    "# try a kalman filter on the noisy sinewave\n",
    "\n",
    "def kalman_filtered(x, kalman_gain=0.05):\n",
    "  current_value = x[0]\n",
    "  kalman_output = [current_value]\n",
    "  for item in x[1:]:\n",
    "    current_value = current_value + kalman_gain * (item - current_value)\n",
    "    kalman_output.append(current_value)\n",
    "  return kalman_output\n",
    "\n",
    "# plot kalman filtered noisy\n",
    "plt.plot(t, kalman_filtered(noisy), label='lp filtered')\n",
    "plt.plot(t, noisy, label='noisy')\n",
    "plt.plot(t, pure, label='pure')\n",
    "plt.plot(t, np.gradient(pure), label='pure derivative')\n",
    "plt.plot(t, pseudo_derivative(t, noisy, 1), label='pseudo-derivative')\n",
    "# plot derivative of kalman filtered noisy\n",
    "plt.plot(t, derivative(t, kalman_filtered(noisy), 1), label='lp filtered derivative')\n",
    "plt.legend()\n",
    "plt.show()"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3.7.4 ('.venv': venv)",
   "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.4"
  },
  "orig_nbformat": 4,
  "vscode": {
   "interpreter": {
    "hash": "97190208b4a6552dc9493965a1caf8d39c9d1897fe9495f2fe88af90c5a07bd6"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
