add jupyter notebooks to documentation + updated notebooks

murrayrm · murrayrm · commit 7821e2b62ad4 · 2022-03-30T08:06:30.000-07:00
diff --git a/doc/examples.rst b/doc/examples.rst
@@ -44,6 +44,9 @@ using running examples in FBS2e.
 
    cruise
    describing_functions
+   kincar-fusion
    mpc_aircraft
    steering
    pvtol-lqr-nested
+   pvtol-outputfbk
+   stochresp
diff --git a/doc/kincar-fusion.ipynb b/doc/kincar-fusion.ipynb
@@ -0,0 +1 @@
+../examples/kincar-fusion.ipynb
diff --git a/doc/pvtol-outputfbk.ipynb b/doc/pvtol-outputfbk.ipynb
@@ -0,0 +1 @@
+../examples/pvtol-outputfbk.ipynb
diff --git a/doc/stochresp.ipynb b/doc/stochresp.ipynb
@@ -0,0 +1 @@
+../examples/stochresp.ipynb
diff --git a/examples/kincar-fusion.ipynb b/examples/kincar-fusion.ipynb
diff --git a/examples/pvtol-lqr-nested.ipynb b/examples/pvtol-lqr-nested.ipynb
@@ -532,7 +532,7 @@
  ],
  "metadata": {
   "kernelspec": {
-   "display_name": "Python 3",
+   "display_name": "Python 3 (ipykernel)",
    "language": "python",
    "name": "python3"
   },
diff --git a/examples/pvtol-outputfbk.ipynb b/examples/pvtol-outputfbk.ipynb
@@ -5,15 +5,15 @@
    "id": "c017196f",
    "metadata": {},
    "source": [
-    "# PVTOL LQR + EQF example\n",
+    "# Output feedback control using LQR and extended Kalman filtering\n",
     "RMM, 14 Feb 2022\n",
     "\n",
-    "This notebook illustrates the implementation of an extended Kalman filter and the use of the estimated state for LQR feedback."
+    "This notebook illustrates the implementation of an extended Kalman filter and the use of the estimated state for LQR feedback of a vectored thrust aircraft model."
    ]
   },
   {
    "cell_type": "code",
-   "execution_count": 1,
+   "execution_count": 11,
    "id": "544525ab",
    "metadata": {},
    "outputs": [],
@@ -30,26 +30,40 @@
    "metadata": {},
    "source": [
     "## System definition\n",
-    "The dynamics of the system\n",
-    "with disturbances on the $x$ and $y$ variables is given by\n",
+    "We consider a (planar) vertical takeoff and landing aircraf model:\n",
+    "\n",
+    "![PVTOL diagram](https://murray.cds.caltech.edu/images/murray.cds/7/7d/Pvtol-diagram.png)\n",
+    "\n",
+    "The dynamics of the system with disturbances on the $x$ and $y$ variables are given by\n",
+    "\n",
     "$$\n",
     "  \\begin{aligned}\n",
     "    m \\ddot x &= F_1 \\cos\\theta - F_2 \\sin\\theta - c \\dot x + d_x, \\\\\n",
     "    m \\ddot y &= F_1 \\sin\\theta + F_2 \\cos\\theta - c \\dot y - m g + d_y, \\\\\n",
     "    J \\ddot \\theta &= r F_1.\n",
     "  \\end{aligned}\n",
     "$$\n",
+    "\n",
     "The measured values of the system are the position and orientation,\n",
     "with added noise $n_x$, $n_y$, and $n_\\theta$:\n",
+    "\n",
     "$$\n",
     "  \\vec y = \\begin{bmatrix} x \\\\ y \\\\ \\theta \\end{bmatrix} + \n",
     "  \\begin{bmatrix} n_x \\\\ n_y \\\\ n_z \\end{bmatrix}.\n",
-    "$$\n"
+    "$$"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "id": "198a068d",
+   "metadata": {},
+   "source": [
+    "The dynamics are defined in the `pvtol` module:"
    ]
   },
   {
    "cell_type": "code",
-   "execution_count": 2,
+   "execution_count": 12,
    "id": "ffafed74",
    "metadata": {},
    "outputs": [
@@ -93,9 +107,17 @@
     "print(noisy_pvtol)"
    ]
   },
+  {
+   "cell_type": "markdown",
+   "id": "be6ec05c",
+   "metadata": {},
+   "source": [
+    "We also define the properties of the disturbances, noise, and initial conditions:"
+   ]
+  },
   {
    "cell_type": "code",
-   "execution_count": 3,
+   "execution_count": 13,
    "id": "1e1ee7c9",
    "metadata": {},
    "outputs": [],
@@ -114,20 +136,22 @@
    "id": "e4c52c73",
    "metadata": {},
    "source": [
-    "## Control system design"
+    "## Control system design\n",
+    "\n",
+    "We start be defining an extended Kalman filter to estimate the state of the system from the measured outputs."
    ]
   },
   {
    "cell_type": "code",
-   "execution_count": 4,
+   "execution_count": 14,
    "id": "3647bf15",
    "metadata": {},
    "outputs": [
     {
      "name": "stdout",
      "output_type": "stream",
      "text": [
-      "Object: sys[1]\n",
+      "Object: sys[3]\n",
       "Inputs (5): x0, x1, x2, F1, F2, \n",
       "Outputs (6): xh0, xh1, xh2, xh3, xh4, xh5, \n",
       "States (42): x[0], x[1], x[2], x[3], x[4], x[5], x[6], x[7], x[8], x[9], x[10], x[11], x[12], x[13], x[14], x[15], x[16], x[17], x[18], x[19], x[20], x[21], x[22], x[23], x[24], x[25], x[26], x[27], x[28], x[29], x[30], x[31], x[32], x[33], x[34], x[35], x[36], x[37], x[38], x[39], x[40], x[41], \n"
@@ -175,9 +199,17 @@
     "print(estimator)"
    ]
   },
+  {
+   "cell_type": "markdown",
+   "id": "8c97626d",
+   "metadata": {},
+   "source": [
+    "We now define an LQR controller, using a physically motivated weighting:"
+   ]
+  },
   {
    "cell_type": "code",
-   "execution_count": 5,
+   "execution_count": 15,
    "id": "9787db61",
    "metadata": {},
    "outputs": [
@@ -209,14 +241,11 @@
       "Object: xh5\n",
       "Inputs (13): xd[0], xd[1], xd[2], xd[3], xd[4], xd[5], ud[0], ud[1], Dx, Dy, Nx, Ny, Nth, \n",
       "Outputs (14): x0, x1, x2, x3, x4, x5, F1, F2, xh0, xh1, xh2, xh3, xh4, xh5, \n",
-      "States (48): noisy_pvtol_x0, noisy_pvtol_x1, noisy_pvtol_x2, noisy_pvtol_x3, noisy_pvtol_x4, noisy_pvtol_x5, sys[1]_x[0], sys[1]_x[1], sys[1]_x[2], sys[1]_x[3], sys[1]_x[4], sys[1]_x[5], sys[1]_x[6], sys[1]_x[7], sys[1]_x[8], sys[1]_x[9], sys[1]_x[10], sys[1]_x[11], sys[1]_x[12], sys[1]_x[13], sys[1]_x[14], sys[1]_x[15], sys[1]_x[16], sys[1]_x[17], sys[1]_x[18], sys[1]_x[19], sys[1]_x[20], sys[1]_x[21], sys[1]_x[22], sys[1]_x[23], sys[1]_x[24], sys[1]_x[25], sys[1]_x[26], sys[1]_x[27], sys[1]_x[28], sys[1]_x[29], sys[1]_x[30], sys[1]_x[31], sys[1]_x[32], sys[1]_x[33], sys[1]_x[34], sys[1]_x[35], sys[1]_x[36], sys[1]_x[37], sys[1]_x[38], sys[1]_x[39], sys[1]_x[40], sys[1]_x[41], \n"
+      "States (48): noisy_pvtol_x0, noisy_pvtol_x1, noisy_pvtol_x2, noisy_pvtol_x3, noisy_pvtol_x4, noisy_pvtol_x5, sys[3]_x[0], sys[3]_x[1], sys[3]_x[2], sys[3]_x[3], sys[3]_x[4], sys[3]_x[5], sys[3]_x[6], sys[3]_x[7], sys[3]_x[8], sys[3]_x[9], sys[3]_x[10], sys[3]_x[11], sys[3]_x[12], sys[3]_x[13], sys[3]_x[14], sys[3]_x[15], sys[3]_x[16], sys[3]_x[17], sys[3]_x[18], sys[3]_x[19], sys[3]_x[20], sys[3]_x[21], sys[3]_x[22], sys[3]_x[23], sys[3]_x[24], sys[3]_x[25], sys[3]_x[26], sys[3]_x[27], sys[3]_x[28], sys[3]_x[29], sys[3]_x[30], sys[3]_x[31], sys[3]_x[32], sys[3]_x[33], sys[3]_x[34], sys[3]_x[35], sys[3]_x[36], sys[3]_x[37], sys[3]_x[38], sys[3]_x[39], sys[3]_x[40], sys[3]_x[41], \n"
      ]
     }
    ],
    "source": [
-    "#\n",
-    "# LQR design w/ physically motivated weighting\n",
-    "#\n",
     "# Shoot for 1 cm error in x, 10 cm error in y.  Try to keep the angle\n",
     "# less than 5 degrees in making the adjustments.  Penalize side forces\n",
     "# due to loss in efficiency.\n",
@@ -256,12 +285,14 @@
    "id": "7bf558a0",
    "metadata": {},
    "source": [
-    "## Simulations"
+    "## Simulations\n",
+    "\n",
+    "We now simulate the response of the system, starting with an instantiation of the noise:"
    ]
   },
   {
    "cell_type": "code",
-   "execution_count": 6,
+   "execution_count": 16,
    "id": "c2583a0e",
    "metadata": {},
    "outputs": [
@@ -302,7 +333,7 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 7,
+   "execution_count": 17,
    "id": "ad7a9750",
    "metadata": {},
    "outputs": [
@@ -337,17 +368,17 @@
   },
   {
    "cell_type": "code",
-   "execution_count": 8,
+   "execution_count": 18,
    "id": "c5f24119",
    "metadata": {},
    "outputs": [
     {
      "data": {
       "text/plain": [
-       "<matplotlib.legend.Legend at 0x7f85f8d6ebb0>"
+       "<matplotlib.legend.Legend at 0x7fa431d8b3a0>"
       ]
      },
-     "execution_count": 8,
+     "execution_count": 18,
      "metadata": {},
      "output_type": "execute_result"
     },
@@ -389,12 +420,14 @@
    "id": "0c0d5c99",
    "metadata": {},
    "source": [
-    "### Full state feedback"
+    "### Full state feedback\n",
+    "\n",
+    "As a comparison, we can investigate the response of the system if the exact state was available:"
    ]
   },
   {
    "cell_type": "code",
-   "execution_count": 9,
+   "execution_count": 19,
    "id": "3b6a1f1c",
    "metadata": {},
    "outputs": [
diff --git a/examples/stochresp.ipynb b/examples/stochresp.ipynb
diff --git a/examples/vehicle-steering.png b/examples/vehicle-steering.png
