{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# RTK: fixed, float, and standalone\n",
    "\n",
    "EECE 5554 Robotics Sensing and Navigation, Lab 2. Put `rtk_modes.py` in the same folder as this notebook, then run the cells in order."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Each dark box below is a code cell. Edit it, then press **Run** or **Shift+Enter**. Cells share variables from top to bottom, like Jupyter."
   ]
  },
  {
   "cell_type": "code",
   "metadata": {},
   "execution_count": null,
   "outputs": [],
   "source": [
    "from rtk_modes import simulate, stats, MODES\n",
    "\n",
    "truth, sims = simulate('open', 'still')\n",
    "{mode: stats(d['pos'], d['truth']) for mode, d in sims.items()}"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 1. Plot the three clouds"
   ]
  },
  {
   "cell_type": "code",
   "metadata": {},
   "execution_count": null,
   "outputs": [],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "\n",
    "for mode, d in sims.items():\n",
    "    plt.scatter(d['pos'][:, 0], d['pos'][:, 1], s=4, label=mode)\n",
    "plt.scatter([0], [0], marker='+', s=300, c='k', label='truth')\n",
    "plt.axis('equal'); plt.legend()\n",
    "plt.xlabel('East (m)'); plt.ylabel('North (m)')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 2. Precision and accuracy by hand\n",
    "Precision: the spread around the cloud's own center. Accuracy: how far that center is from the truth."
   ]
  },
  {
   "cell_type": "code",
   "metadata": {},
   "execution_count": null,
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "\n",
    "p = sims['standalone']['pos']\n",
    "center = p.mean(axis=0)\n",
    "precision = np.sqrt(((p - center) ** 2).sum(axis=1).mean())\n",
    "accuracy = np.hypot(*center)\n",
    "print(f'precision {precision:.2f} m, accuracy {accuracy:.2f} m')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 3. Why 5 minutes is not enough\n",
    "The standalone error wanders slowly, so each 5-minute run has its own center. Run ten of them:"
   ]
  },
  {
   "cell_type": "code",
   "metadata": {},
   "execution_count": null,
   "outputs": [],
   "source": [
    "centers = []\n",
    "for seed in range(10):\n",
    "    _, s = simulate('open', 'still', seed=seed)\n",
    "    centers.append(s['standalone']['pos'].mean(axis=0))\n",
    "np.round(np.array(centers), 2)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 4. Your turn\n",
    "Make the standalone error white (no memory) by setting its correlation time to 1 s, then rerun the cell above. What happens to the ten centers? Set it back to 40.0 when you are done."
   ]
  },
  {
   "cell_type": "code",
   "metadata": {},
   "execution_count": null,
   "outputs": [],
   "source": [
    "MODES['open']['standalone']['tau'] = 1.0\n",
    "print(MODES['open']['standalone'])"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "name": "python"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}