Fusion-EKF-Variances.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "collapsed": false
   },
   "outputs": [],
   "source": [
    "from kalmanfilter import KalmanFilter\n",
    "from datapoint import DataPoint\n",
    "from fusionekf import FusionEKF\n",
    "from tools import get_RMSE, cartesian_to_polar\n",
    "from helpers import parse_data, print_EKF_data, get_state_estimations\n",
    "import numpy as np"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def get_all_differences(all_sensor_data, all_ground_truths):\n",
    "\n",
    "  pxs, pys, vxs, vys, rhos, phis, drhos = [], [], [], [], [], [], []\n",
    "\n",
    "  for s, t in zip(all_sensor_data, all_ground_truths):\n",
    "        \n",
    "    if s.get_name() == 'lidar':\n",
    "    \n",
    "      spx, spy, _, _ = s.get()\n",
    "      tpx, tpy, _, _ = t.get()\n",
    "\n",
    "      pxs += [spx - tpx]\n",
    "      pys += [spy - tpy]\n",
    "      \n",
    "    else:\n",
    "        \n",
    "      spx, spy, svx, svy = s.get()\n",
    "      tpx, tpy, tvx, tvy = t.get()\n",
    "\n",
    "      srho, sphi, sdrho = s.get_raw()\n",
    "      trho, tphi, tdrho = cartesian_to_polar(tpx, tpy, tvx, tvy)\n",
    "        \n",
    "      pxs += [spx - tpx]\n",
    "      pys += [spy - tpy]        \n",
    "      vxs += [svx - tvx]\n",
    "      vys += [svy - tvy]\n",
    "      rhos += [srho - trho]\n",
    "      phis += [sphi - tphi]\n",
    "      drhos += [sdrho - tdrho]\n",
    "\n",
    "    \n",
    "  return pxs, pys, vxs, vys, rhos, phis, drhos\n",
    "\n",
    "def get_variance(x):\n",
    "  return np.var(np.array(x))\n",
    "\n",
    "def print_variances(pxs, pys, vxs, vys, rhos, phis, drhos):\n",
    "  print(\"x:\", get_variance(pxs))\n",
    "  print(\"y:\", get_variance(pys))\n",
    "  print(\"vx:\", get_variance(vxs))\n",
    "  print(\"vy:\", get_variance(vys))\n",
    "  print(\"rho:\", get_variance(rhos))\n",
    "  print(\"phi:\", get_variance(phis))\n",
    "  print(\"drho:\", get_variance(drhos))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "x: 0.0030318456883\n",
      "y: 0.00232796032072\n",
      "vx: 1.75231650122\n",
      "vy: 2.81928216089\n",
      "rho: 0.0103696181683\n",
      "phi: 1.0680397691e-06\n",
      "drho: 0.011294795278\n"
     ]
    }
   ],
   "source": [
    "all_sensor_data1, all_ground_truths1 = parse_data(\"data/data-1.txt\")\n",
    "pxs1, pys1, vxs1, vys1, rhos1, phis1, drhos1 = get_all_differences(all_sensor_data1, all_ground_truths1)\n",
    "print_variances(pxs1, pys1, vxs1, vys1, rhos1, phis1, drhos1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "WARNING: in cartesian_to_polar(): d_squared < THRESH\n",
      "x: 0.0432845677688\n",
      "y: 0.0478147050908\n",
      "vx: 0.0225024900134\n",
      "vy: 0.290537073523\n",
      "rho: 0.0391404813605\n",
      "phi: 3.15121868185e-06\n",
      "drho: 0.00970452550137\n"
     ]
    }
   ],
   "source": [
    "all_sensor_data2, all_ground_truths2 = parse_data(\"data/data-2.txt\")\n",
    "pxs2, pys2, vxs2, vys2, rhos2, phis2, drhos2 = get_all_differences(all_sensor_data2, all_ground_truths2)\n",
    "print_variances(pxs2, pys2, vxs2, vys2, rhos2, phis2, drhos2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {
    "collapsed": false
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "x: 0.00872031903422\n",
      "y: 0.00871732038878\n",
      "vx: 1.51063829744\n",
      "vy: 2.46571865544\n",
      "rho: 0.0144125890908\n",
      "phi: 1.36108366223e-06\n",
      "drho: 0.0110733569443\n"
     ]
    }
   ],
   "source": [
    "print_variances(pxs1 + pxs2, pys1 + pys2, vxs1 + vxs2, \n",
    "                vys1 + vys2, rhos1 + rhos2, phis1 + phis2, drhos1 + drhos2)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "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.5.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}