{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Frames\n",
    "\n",
    "- **WGS-84 [lat,lon,alt]:** World Geodetic System 1984 standard for maps. This standard defines the oblate spheroid typically used to model Earth’s shape\n",
    "    - Semi-major: a = 6,378,137 m\n",
    "    - Semi-minor: b = 6,356,752.3142 m\n",
    "    - This is the frame GPS reports position in\n",
    "- **The Earth-Centred, Earth-Fixed Frame (ECEF)[x,y,z]:** a global, attached to Earth itself, and always uses cartesian coordinates\n",
    "- **Local Geographic Frame (LGF)[x,y,z]:**  At any point on Earth’s surface, the local geographic frame is defined by the almost flat ground and the vertical direction, and is relevant because it is the basic reference the aircraft flies against, defining straight and level flight and of course the direction of down. This is an intuitive frame for any scenario whose total extent is no more than some tens of kilometres. The conversion is $p_{NED} = R(p_{ECEF} - p_{ref})$, where $R$ is the rotation from ECEF to either NED or ENU.\n",
    "    - NED: North,  East, Down\n",
    "    - ENU: East, North, Up\n",
    "- **Body Frame [x,y,z]:** A frame attached at the center of mass of the vehicle with the *x-axis* pointing out the front, *y-axis* out the right side, and the *z-axis* pointing down *typically*. However, various authors/engineers use different orientations.\n",
    "\n",
    "## Convention\n",
    "\n",
    "Moving from referene frame body ($\\textbf{F}^b$) to the navigation frame ($\\textbf{F}^n$) is done via a rotation matrix. All rotation matrices are special orthogonal groups in three dimensions or SO(3). Thus they have the following properties:\n",
    "\n",
    "$$\n",
    "R^{-1}(\\theta) = R^T(\\theta) \\\\\n",
    "det(R(\\theta)) = 1\n",
    "$$\n",
    "\n",
    "All rotations can be broken up into their corresponding Euler angles. By convention, rotating vectors in the body frame to the navigation frame is done by:\n",
    "\n",
    "$$\n",
    "R_b^n = R_Z(\\psi) R_y(\\theta) R_x(\\phi) = R_{123}(\\phi, \\theta,\\psi) \n",
    "$$\n",
    "\n",
    "Now we can use this to rotate vectors between reference frames:\n",
    "\n",
    "$$\n",
    "p^n = R_b^n p^b = R_{123}(\\phi, \\theta,\\psi) p^b \\\\\n",
    "v^n = R_{123}(\\phi, \\theta,\\psi) v^b \\\\\n",
    "\\omega^n = R_{123}(\\phi, \\theta,\\psi) \\omega^b \\\\\n",
    "F^n = R_{123}(\\phi, \\theta,\\psi) F^b\n",
    "$$\n",
    "\n",
    "where $n$ refers to a navigation frame and $b$ refers to a body frame.\n",
    "\n",
    "## References\n",
    "\n",
    "- Wikipedia: [Euler angles](https://en.wikipedia.org/wiki/Euler_angles)\n",
    "- Wikipedia: [Local tangental coordinate frames](https://en.wikipedia.org/wiki/Local_tangent_plane_coordinates)\n",
    "- MIT Open Courseware: [Designing Electromechanical Robotic Systems](https://ocw.mit.edu/courses/mechanical-engineering/2-017j-design-of-electromechanical-robotic-systems-fall-2009/course-text/)\n",
    "- StackExchange: [Rotational Kinematics and Angular Velocity Vector Transformation\n",
    "](https://physics.stackexchange.com/a/429094)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "%load_ext autoreload\n",
    "%autoreload 2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 36,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from numpy import sin, cos, pi, sqrt\n",
    "from numpy.testing import assert_allclose\n",
    "deg2rad = pi/180\n",
    "rad2deg = 180/pi"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "from rotations import frames\n",
    "from rotations import rotations"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 41,
   "metadata": {},
   "outputs": [],
   "source": [
    "class Nav:\n",
    "    def __init__(self, orig,):\n",
    "        self.r = self.rot_ecef2ned(*orig[:2])\n",
    "        self.o = self.wgs2ecef(*orig)\n",
    "    \n",
    "    def ecef2ned(self, pt):\n",
    "        \"\"\"ECEF(x,y,z) to NED(x,y,z)\"\"\"\n",
    "        #to_nav()\n",
    "        return self.r.dot(pt-self.o)\n",
    "    \n",
    "    def ned2ecef(self, pt):\n",
    "        \"\"\"ECEF(x,y,z) to NED(x,y,z)\"\"\"\n",
    "        #to_ecef\n",
    "        return self.r.T.dot(pt)+self.o\n",
    "    \n",
    "    def wgs2ned(self, lat, lon, alt):\n",
    "        x = self.wgs2ecef(lat,lon,alt)\n",
    "        return self.ecef2ned(x)\n",
    "    \n",
    "    def rot_ecef2ned(self, lat, lon, degrees=True):\n",
    "        \"\"\"ECEF to NED rotation matrix\"\"\"\n",
    "        if degrees:\n",
    "            lat *= deg2rad\n",
    "            lon *= deg2rad\n",
    "        return np.array([\n",
    "            [-sin(lat)*cos(lon), -sin(lat)*sin(lon), cos(lat)],\n",
    "            [-sin(lon), cos(lon), 0],\n",
    "            [-cos(lat)*cos(lon), -cos(lat)*sin(lon), -sin(lat)]\n",
    "        ])\n",
    "    \n",
    "    def wgs2ecef(self, lat,lon, alt, degrees=True):\n",
    "        \"\"\"wgs84(lat,lon,alt) to ECEF(x,y,z)\"\"\"\n",
    "        if degrees:\n",
    "            lat *= deg2rad\n",
    "            lon *= deg2rad\n",
    "\n",
    "        a = 6378137.0 # WGS semi-major axis\n",
    "        b = 6356752.314245 # WGS semi-minor axis\n",
    "        e = sqrt(1-(b/a)**2)\n",
    "        d = sqrt(1-e**2*sin(lat)**2)\n",
    "        f = (a/d+alt)*cos(lat)\n",
    "\n",
    "        x = f*cos(lon)\n",
    "        y = f*sin(lon)\n",
    "        z = (a*(1-e*e)/d+alt)*sin(lat)\n",
    "\n",
    "        return np.array([x,y,z])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 42,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[[-0.5        -0.5         0.70710678]\n",
      " [-0.70710678  0.70710678  0.        ]\n",
      " [-0.5        -0.5        -0.70710678]]\n",
      "[3194419.14506062 3194419.14506062 4487348.40886573]\n",
      "6367489.543863376\n"
     ]
    }
   ],
   "source": [
    "ned = Nav((45,45,0))\n",
    "print(ned.r)\n",
    "print(ned.o)\n",
    "print(np.linalg.norm(ned.o))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 43,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[3194419.14506062 3194419.14506062 4487348.40886573]\n",
      "[0. 0. 0.]\n"
     ]
    }
   ],
   "source": [
    "p = ned.wgs2ecef(45,45,0)\n",
    "print(p)\n",
    "m=ned.ecef2ned(p)\n",
    "print(m)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 44,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[3194419.14506062 3194419.14506062 4487348.40886573]\n"
     ]
    }
   ],
   "source": [
    "pp = ned.ned2ecef(m)\n",
    "print(pp)\n",
    "assert_allclose(pp,p)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 47,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([ 1.11132698e+04, -9.09494702e-13,  9.69818833e+00])"
      ]
     },
     "execution_count": 47,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "ned.wgs2ned(45.1,45,0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "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.7.7"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}