{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# IMU noise and Allan variance\n",
    "\n",
    "EECE 5554 Robotics Sensing and Navigation, Lab 3. Put `allan_gyro.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 allan_gyro import simulate_gyro, allan_deviation, read_noise, DEG\n",
    "\n",
    "fs = 40.0\n",
    "rate = simulate_gyro(arw=0.21, bias_inst=8.0, rrw=0.0, fs=fs, hours=1.0)   # rad/s, at rest\n",
    "print(len(rate), 'samples, mean', rate.mean() / DEG * 3600, 'deg/hr')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 1. One point of the Allan deviation, by hand\n",
    "Integrate the rate into an angle, take clusters of m samples, and look at how much neighboring cluster averages differ."
   ]
  },
  {
   "cell_type": "code",
   "metadata": {},
   "execution_count": null,
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "\n",
    "theta = np.concatenate([[0.0], np.cumsum(rate) / fs])   # angle, rad\n",
    "m = 40                                                  # 40 samples = 1 s\n",
    "tau = m / fs\n",
    "d = theta[2 * m:] - 2 * theta[m:-m] + theta[:-2 * m]\n",
    "adev = np.sqrt((d ** 2).sum() / (2 * tau ** 2 * len(d)))\n",
    "print(f'Allan deviation at tau = {tau} s: {adev / DEG * 3600:.2f} deg/hr')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 2. The whole curve\n",
    "`allan_deviation` repeats that for many cluster sizes. `read_noise` reads N and B off the curve."
   ]
  },
  {
   "cell_type": "code",
   "metadata": {},
   "execution_count": null,
   "outputs": [],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "\n",
    "taus, ad = allan_deviation(rate, fs)\n",
    "ad_dph = ad / DEG * 3600\n",
    "plt.loglog(taus, ad_dph)\n",
    "plt.xlabel('tau (s)'); plt.ylabel('Allan deviation (deg/hr)'); plt.grid(True, which='both')\n",
    "read_noise(taus, ad_dph)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 3. Your turn\n",
    "Add rate random walk and compare the two curves. Where does the +1/2 tail start?"
   ]
  },
  {
   "cell_type": "code",
   "metadata": {},
   "execution_count": null,
   "outputs": [],
   "source": [
    "rate2 = simulate_gyro(arw=0.21, bias_inst=8.0, rrw=100.0, fs=fs, hours=1.0)\n",
    "taus2, ad2 = allan_deviation(rate2, fs)\n",
    "plt.loglog(taus, ad / DEG * 3600, label='no RRW')\n",
    "plt.loglog(taus2, ad2 / DEG * 3600, label='RRW = 100 deg/hr/sqrt(hr)')\n",
    "plt.xlabel('tau (s)'); plt.ylabel('Allan deviation (deg/hr)'); plt.grid(True, which='both'); plt.legend()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "On your own computer, you can load your Lab 3 gyro data instead, for example `rate = np.loadtxt('gyro_z.csv')`, in rad/s at your IMU's sample rate."
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "name": "python"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}