Side-projects/1D-Kalman-filter.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 1D Kalman Filter\n",
    "\n",
    "Now, you're ready to implement a 1D Kalman Filter by putting all these steps together. Let's take the case of a robot that moves through the world. As a robot moves through the world it locates itself by performing a cycle of:\n",
    "1. sensing and performing a measurement update and\n",
    "2. moving and performing a motion update\n",
    "\n",
    "You've programmed each of these steps individually, so now let's combine them in a cycle!\n",
    "\n",
    "After implementing this filter, you should see that you can go from a very uncertain location Gaussian to a more and more certain Gaussian, as pictured below. The code in this notebooks is really just a simplified version of the Kalman filter that runs in the Google self-driving car that is used to track surrounding vehicles and other objects.\n",
    "\n",
    "---"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Below is our usual Gaussian equation and imports."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "# import math functions\n",
    "from math import *\n",
    "import matplotlib.pyplot as plt\n",
    "import numpy as np\n",
    "\n",
    "# gaussian function\n",
    "def f(mu, sigma2, x):\n",
    "    ''' f takes in a mean and squared variance, and an input x\n",
    "       and returns the gaussian value.'''\n",
    "    coefficient = 1.0 / sqrt(2.0 * pi *sigma2)\n",
    "    exponential = exp(-0.5 * (x-mu) ** 2 / sigma2)\n",
    "    return coefficient * exponential\n",
    " "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You've also been given the complete `update` code that performs a parameter update when an initial belief and new measurement information are merged. And the complete `predict` code that performs an update to a Gaussian after a motion is incorporated."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "# the update function\n",
    "def update(mean1, var1, mean2, var2):\n",
    "    ''' This function takes in two means and two squared variance terms,\n",
    "        and returns updated gaussian parameters.'''\n",
    "    # Calculate the new parameters\n",
    "    new_mean = (var2*mean1 + var1*mean2)/(var2+var1)\n",
    "    new_var = 1/(1/var2 + 1/var1)\n",
    "    \n",
    "    return [new_mean, new_var]\n",
    "\n",
    "\n",
    "# the motion update/predict function\n",
    "def predict(mean1, var1, mean2, var2):\n",
    "    ''' This function takes in two means and two squared variance terms,\n",
    "        and returns updated gaussian parameters, after motion.'''\n",
    "    # Calculate the new parameters\n",
    "    new_mean = mean1 + mean2\n",
    "    new_var = var1 + var2\n",
    "    \n",
    "    return [new_mean, new_var]"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### QUIZ: For the given measurements and motions, write complete 1D Kalman filter code that loops through all of these in order.\n",
    "\n",
    "Your complete code should look at sensor measurements then motions in that sequence until all updates are done!\n",
    "\n",
    "### Initial Uncertainty\n",
    "\n",
    "You'll see that you are given initial parameters below, and this includes the initial location estimation, `mu` and squared variance, `sig`. Note that the initial estimate is set to the location 0, and the variance is extremely large; this is a state of high confusion much like the *uniform* distribution we used in the histogram filter. There are also values given for the squared variance associated with the sensor measurements and the motion, since neither of those readings are perfect, either.\n",
    "\n",
    "You should see that even though the initial estimate for location (the initial `mu`) is far from the first measurement, it should catch up fairly quickly as you cycle through measurements and motions."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Update, [4.998000799680128, 3.9984006397441023]\n",
      "Predict, [5.998000799680128, 5.998400639744102]\n",
      "Update, [5.999200191953932, 2.399744061425258]\n",
      "Predict, [6.999200191953932, 4.399744061425258]\n",
      "Update, [6.999619127420922, 2.0951800575117594]\n",
      "Predict, [8.999619127420921, 4.09518005751176]\n",
      "Update, [8.999811802788143, 2.0235152416216957]\n",
      "Predict, [9.999811802788143, 4.023515241621696]\n",
      "Update, [9.999906177177365, 2.0058615808441944]\n",
      "Predict, [10.999906177177365, 4.005861580844194]\n"
     ]
    }
   ],
   "source": [
    "# measurements for mu and motions, U\n",
    "measurements = [5., 6., 7., 9., 10.]\n",
    "motions = [1., 1., 2., 1., 1.]\n",
    "\n",
    "# initial parameters\n",
    "measurement_sig = 4.\n",
    "motion_sig = 2.\n",
    "mu = 0.\n",
    "sig = 10000.\n",
    "\n",
    "## TODO: Loop through all measurements/motions\n",
    "## Print out and display the resulting Gaussian \n",
    "for i in range(len(measurements)):\n",
    "    mu, sig = update(mu, sig, measurements[i], measurement_sig)\n",
    "    print(f'Update, [{mu}, {sig}]')\n",
    "    mu, sig = predict(mu, sig, motions[i], motion_sig)\n",
    "    print(f'Predict, [{mu}, {sig}]')\n",
    "    \n"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "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.5"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}