{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Satellite geometry and DOP\n",
    "\n",
    "EECE 5554 Robotics Sensing and Navigation, Lecture 04. Put `sat_dop.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": [
    "import numpy as np\n",
    "from sat_dop import geometry_matrix, dop, preset\n",
    "\n",
    "sats = preset('spread', n=7)\n",
    "sats"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 1. The geometry matrix H\n",
    "One row per satellite: minus the unit vector to the satellite (east, north, up), and a 1 for the receiver clock."
   ]
  },
  {
   "cell_type": "code",
   "metadata": {},
   "execution_count": null,
   "outputs": [],
   "source": [
    "H = geometry_matrix(sats)\n",
    "np.round(H, 3)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 2. DOP by hand\n",
    "Q = (HᵀH)⁻¹. DOP values are square roots of sums on its diagonal."
   ]
  },
  {
   "cell_type": "code",
   "metadata": {},
   "execution_count": null,
   "outputs": [],
   "source": [
    "Q = np.linalg.inv(H.T @ H)\n",
    "d = np.diag(Q)\n",
    "print('HDOP', np.sqrt(d[0] + d[1]))\n",
    "print('VDOP', np.sqrt(d[2]))\n",
    "print('PDOP', np.sqrt(d[:3].sum()))\n",
    "print('TDOP', np.sqrt(d[3]))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 3. Check it by simulation\n",
    "Add random range errors with σ = 3 m, solve for the position error with least squares (just like linear regression), and compare the spread with σ × HDOP."
   ]
  },
  {
   "cell_type": "code",
   "metadata": {},
   "execution_count": null,
   "outputs": [],
   "source": [
    "rng = np.random.default_rng(0)\n",
    "sigma = 3.0\n",
    "errs = np.array([np.linalg.lstsq(H, rng.normal(0, sigma, len(sats)), rcond=None)[0] for _ in range(3000)])\n",
    "print('simulated horizontal 1-sigma:', np.sqrt(errs[:, 0].var() + errs[:, 1].var()))\n",
    "print('sigma x HDOP:                ', sigma * dop(sats)['HDOP'])"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 4. Compare skies"
   ]
  },
  {
   "cell_type": "code",
   "metadata": {},
   "execution_count": null,
   "outputs": [],
   "source": [
    "for name in ('spread', 'high', 'clustered', 'canyon'):\n",
    "    r = dop(preset(name), sigma_uere=3.0)\n",
    "    print(f\"{name:10s} HDOP {r['HDOP']:6.2f}   VDOP {r['VDOP']:6.2f}\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 5. Your turn\n",
    "Place your own satellites (azimuth and elevation in degrees). How low can you get HDOP with only 5 satellites? What happens when you move them all high in the sky?"
   ]
  },
  {
   "cell_type": "code",
   "metadata": {},
   "execution_count": null,
   "outputs": [],
   "source": [
    "my_sats = [{'az': 0, 'el': 85}, {'az': 0, 'el': 20}, {'az': 120, 'el': 20}, {'az': 240, 'el': 20}, {'az': 60, 'el': 45}]\n",
    "r = dop(my_sats)\n",
    "print(f\"HDOP {r['HDOP']:.2f}, VDOP {r['VDOP']:.2f}, PDOP {r['PDOP']:.2f}\")"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "name": "python"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}